From 9bdfcc8c232260107536f3095dbf4e8143334e92 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 6 Feb 2018 09:05:00 +0100 Subject: [PATCH] expose Task::initScene() ... to allow use of local scene --- core/include/moveit/task_constructor/task.h | 12 ++++-- core/src/task.cpp | 46 +++++++++++---------- 2 files changed, 33 insertions(+), 25 deletions(-) diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index b0affc3f..931ea828 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -91,11 +91,18 @@ public: /// remove function callback void erase(TaskCallbackList::const_iterator which); + /// initialize planning scene from get_planning_scene service (waiting given timeout for it) + /// if service is not available or timeout is zero, use an empty planning scene + planning_scene::PlanningScenePtr initScene(ros::Duration timeout = ros::Duration(-1)); + + /// reset all stages void reset() override; + /// initialize all stages with given scene void init(const planning_scene::PlanningSceneConstPtr &scene) override; + /// reset, init scene (if not yet done), and init all stages, then start planning bool plan(); - /// print current state std::cout + /// print current task state (number of found solutions and propagated states) to std::cout static void printState(const Task &t); size_t numSolutions() const override; @@ -117,7 +124,6 @@ public: protected: void initModel(); - void initScene(); bool canCompute() const override; bool compute() override; void onNewSolution(const SolutionBase &s) override; @@ -125,7 +131,7 @@ protected: private: std::string id_; robot_model_loader::RobotModelLoaderPtr rml_; - planning_scene::PlanningSceneConstPtr scene_; // initial scene + planning_scene::PlanningScenePtr scene_; // initial scene // introspection and monitoring std::unique_ptr introspection_; diff --git a/core/src/task.cpp b/core/src/task.cpp index 2d19755e..eddce014 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -68,34 +68,36 @@ void Task::initModel () { if( !rml_->getModel() ) throw Exception("Task failed to construct RobotModel"); } -void Task::initScene() { + +planning_scene::PlanningScenePtr Task::initScene(ros::Duration timeout) { assert(rml_); + scene_ = std::make_shared(rml_->getModel()); + ros::NodeHandle h; ros::ServiceClient client = h.serviceClient("get_planning_scene"); - client.waitForExistence(); + if (timeout != ros::Duration() && client.waitForExistence(timeout)) { + moveit_msgs::GetPlanningScene::Request req; + moveit_msgs::GetPlanningScene::Response res; - 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; - 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 acquire current PlanningScene"); + if (client.call(req, res)) + std::const_pointer_cast(scene_)->setPlanningSceneMsg(res.scene); + return scene_; } - - scene_ = std::make_shared(rml_->getModel()); - std::const_pointer_cast(scene_)->setPlanningSceneMsg(res.scene); + ROS_WARN("failed to acquire current PlanningScene, using empty one"); + return scene_; } planning_pipeline::PlanningPipelinePtr @@ -205,7 +207,7 @@ bool Task::compute() bool Task::plan() { reset(); - initScene(); + if (!scene_) initScene(); init(scene_); while(ros::ok() && canCompute()) {