From 039f1e689685c9f33ab2557a439bed1dc3fdb3c4 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 28 Feb 2017 16:47:09 +0100 Subject: [PATCH] Task now holds a PlanningScene instead of just a RobotModel. All generating subtasks need the scene, so avoid every single one of them asking for it. --- include/moveit_task_constructor/storage.h | 12 +++++- include/moveit_task_constructor/subtask.h | 15 ++++--- .../subtasks/current_state.h | 4 +- include/moveit_task_constructor/task.h | 5 ++- src/subtask.cpp | 11 +++--- src/subtasks/current_state.cpp | 39 ++++--------------- src/task.cpp | 31 ++++++++++++++- 7 files changed, 68 insertions(+), 49 deletions(-) diff --git a/include/moveit_task_constructor/storage.h b/include/moveit_task_constructor/storage.h index 246db11b..0233e66a 100644 --- a/include/moveit_task_constructor/storage.h +++ b/include/moveit_task_constructor/storage.h @@ -2,7 +2,17 @@ #pragma once -#include +#include + +#include + +namespace planning_scene { +MOVEIT_CLASS_FORWARD(PlanningScene); +} + +namespace robot_trajectory { +MOVEIT_CLASS_FORWARD(RobotTrajectory); +} namespace moveit::task_constructor { diff --git a/include/moveit_task_constructor/subtask.h b/include/moveit_task_constructor/subtask.h index f71152b1..7b78a912 100644 --- a/include/moveit_task_constructor/subtask.h +++ b/include/moveit_task_constructor/subtask.h @@ -10,8 +10,11 @@ #include #include -namespace moveit::task_constructor { +namespace planning_scene { +MOVEIT_CLASS_FORWARD(PlanningScene); +} +namespace moveit::task_constructor { MOVEIT_CLASS_FORWARD(SubTask); @@ -27,7 +30,7 @@ public: const std::list& getBegin(); const std::list& getTrajectories(); - void setRobotModel(robot_model::RobotModelConstPtr); + void setPlanningScene(planning_scene::PlanningSceneConstPtr); void addPredecessor(SubTaskPtr); void addSuccessor(SubTaskPtr); @@ -35,13 +38,13 @@ public: protected: void sendForward(); void sendBackward(); - void sendBothWays(robot_trajectory::RobotTrajectoryPtr&, planning_scene::PlanningScenePtr&); + void sendBothWays(robot_trajectory::RobotTrajectoryPtr&, planning_scene::PlanningSceneConstPtr&); void connectBegin(InterfaceState&, SubTrajectory*); void connectEnd(InterfaceState&, SubTrajectory*); - InterfaceState* newBegin(planning_scene::PlanningScenePtr, SubTrajectory*); - InterfaceState* newEnd(planning_scene::PlanningScenePtr, SubTrajectory*); + InterfaceState* newBegin(planning_scene::PlanningSceneConstPtr, SubTrajectory*); + InterfaceState* newEnd(planning_scene::PlanningSceneConstPtr, SubTrajectory*); const std::string name_; @@ -54,7 +57,7 @@ protected: std::list beginnings_; std::list endings_; - robot_model::RobotModelConstPtr robot_model_; + planning_scene::PlanningSceneConstPtr scene_; }; } diff --git a/include/moveit_task_constructor/subtasks/current_state.h b/include/moveit_task_constructor/subtasks/current_state.h index b5f31d85..d84ec31e 100644 --- a/include/moveit_task_constructor/subtasks/current_state.h +++ b/include/moveit_task_constructor/subtasks/current_state.h @@ -8,14 +8,14 @@ namespace moveit::task_constructor::subtasks { class CurrentState : public SubTask { public: - using SubTask::SubTask; + CurrentState(std::string name); virtual bool canCompute(); virtual bool compute(); protected: - bool succeeded_; + bool ran_; }; } diff --git a/include/moveit_task_constructor/task.h b/include/moveit_task_constructor/task.h index 395df8bb..57adb590 100644 --- a/include/moveit_task_constructor/task.h +++ b/include/moveit_task_constructor/task.h @@ -6,8 +6,8 @@ #include -namespace moveit::core { - MOVEIT_CLASS_FORWARD(RobotModel); +namespace planning_scene { + MOVEIT_CLASS_FORWARD(PlanningScene); } namespace robot_model_loader { @@ -35,6 +35,7 @@ protected: std::vector subtasks_; + planning_scene::PlanningScenePtr scene_; robot_model_loader::RobotModelLoaderPtr rml_; }; diff --git a/src/subtask.cpp b/src/subtask.cpp index fcc75774..ee6fa66c 100644 --- a/src/subtask.cpp +++ b/src/subtask.cpp @@ -33,13 +33,12 @@ moveit::task_constructor::SubTask::getTrajectories(){ } void -moveit::task_constructor::SubTask::setRobotModel(robot_model::RobotModelConstPtr model){ - robot_model_= model; +moveit::task_constructor::SubTask::setPlanningScene(planning_scene::PlanningSceneConstPtr scene){ + scene_= scene; } - void -moveit::task_constructor::SubTask::sendBothWays(robot_trajectory::RobotTrajectoryPtr& traj, planning_scene::PlanningScenePtr& ps){ +moveit::task_constructor::SubTask::sendBothWays(robot_trajectory::RobotTrajectoryPtr& traj, planning_scene::PlanningSceneConstPtr& ps){ trajectories_.emplace_back(); SubTrajectory& subtraj= trajectories_.back(); subtraj.trajectory= traj; @@ -54,13 +53,13 @@ moveit::task_constructor::SubTask::sendBothWays(robot_trajectory::RobotTrajector } moveit::task_constructor::InterfaceState* -moveit::task_constructor::SubTask::newBegin(planning_scene::PlanningScenePtr ps, SubTrajectory* old_end){ +moveit::task_constructor::SubTask::newBegin(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* old_end){ beginnings_.push_back( InterfaceState(ps, old_end, NULL)); return &beginnings_.back(); } moveit::task_constructor::InterfaceState* -moveit::task_constructor::SubTask::newEnd(planning_scene::PlanningScenePtr ps, SubTrajectory* old_beginning){ +moveit::task_constructor::SubTask::newEnd(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* old_beginning){ endings_.push_back( InterfaceState(ps, NULL, old_beginning)); return &endings_.back(); } diff --git a/src/subtasks/current_state.cpp b/src/subtasks/current_state.cpp index b3c1b718..4a46c520 100644 --- a/src/subtasks/current_state.cpp +++ b/src/subtasks/current_state.cpp @@ -1,47 +1,24 @@ - #include -#include -#include +moveit::task_constructor::subtasks::CurrentState::CurrentState(std::string name) +: moveit::task_constructor::SubTask::SubTask(name) +{ + ran_= false; +} bool moveit::task_constructor::subtasks::CurrentState::canCompute(){ - return !succeeded_; + return !ran_; } bool moveit::task_constructor::subtasks::CurrentState::compute(){ - 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; + ran_= true; // 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); + sendBothWays(traj, scene_); return true; } diff --git a/src/task.cpp b/src/task.cpp index 8f75decf..a7e97714 100644 --- a/src/task.cpp +++ b/src/task.cpp @@ -1,6 +1,9 @@ #include #include +#include + +#include #include moveit::task_constructor::Task::Task() @@ -8,6 +11,32 @@ moveit::task_constructor::Task::Task() rml_.reset(new robot_model_loader::RobotModelLoader); if( !rml_->getModel() ) throw Exception("Task failed to construct RobotModel"); + + 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; + + if(!client.call(req, res)){ + throw Exception("Task failed to aquire current PlanningScene"); + } + + planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(rml_->getModel())); + ps->setPlanningSceneMsg(res.scene); } void moveit::task_constructor::Task::addStart( SubTaskPtr subtask ){ @@ -32,6 +61,6 @@ bool moveit::task_constructor::Task::plan(){ void moveit::task_constructor::Task::addSubTask( SubTaskPtr subtask ){ - subtask->setRobotModel( rml_->getModel() ); + subtask->setPlanningScene( scene_ ); subtasks_.push_back( subtask ); }