diff --git a/include/moveit_task_constructor/subtasks/current_state.h b/include/moveit_task_constructor/subtasks/current_state.h index 10d3a072..b5f31d85 100644 --- a/include/moveit_task_constructor/subtasks/current_state.h +++ b/include/moveit_task_constructor/subtasks/current_state.h @@ -13,6 +13,9 @@ public: virtual bool canCompute(); virtual bool compute(); + +protected: + bool succeeded_; }; } diff --git a/src/subtasks/current_state.cpp b/src/subtasks/current_state.cpp index b20bd36b..b3c1b718 100644 --- a/src/subtasks/current_state.cpp +++ b/src/subtasks/current_state.cpp @@ -1,11 +1,47 @@ + #include +#include +#include + bool moveit::task_constructor::subtasks::CurrentState::canCompute(){ - return true; + return !succeeded_; } bool moveit::task_constructor::subtasks::CurrentState::compute(){ - return false; + ros::NodeHandle h; + ros::ServiceClient client = h.serviceClient("get_planning_scene"); + client.waitForExistence(); + + moveit_msgs::GetPlanningScene::Request req; + moveit_msgs::GetPlanningScene::Response res; + + req.components.components = + moveit_msgs::PlanningSceneComponents::SCENE_SETTINGS + | moveit_msgs::PlanningSceneComponents::ROBOT_STATE + | moveit_msgs::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS + | moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES + | moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY + | moveit_msgs::PlanningSceneComponents::OCTOMAP + | moveit_msgs::PlanningSceneComponents::TRANSFORMS + | moveit_msgs::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX + | moveit_msgs::PlanningSceneComponents::LINK_PADDING_AND_SCALING + | moveit_msgs::PlanningSceneComponents::OBJECT_COLORS; + + succeeded_= client.call(req, res); + + if(!succeeded_) + return false; + + // empty trajectory ref -> this node only produces states + robot_trajectory::RobotTrajectoryPtr traj; + + planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(robot_model_)); + ps->setPlanningSceneMsg(res.scene); + + sendBothWays(traj, ps); + + return true; }