From a0a9738a004a200042287e1b23ca75495644a467 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 20 Oct 2018 22:02:05 +0200 Subject: [PATCH] update PlanningScene's RobotState before storing it --- core/include/moveit/task_constructor/storage.h | 2 +- core/src/stages/generate_grasp_pose.cpp | 2 +- core/src/stages/generate_pose.cpp | 2 +- core/src/storage.cpp | 12 ++++++++++-- 4 files changed, 13 insertions(+), 5 deletions(-) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 781008d1..7cf6d9c1 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -98,7 +98,7 @@ public: typedef std::deque Solutions; /// create an InterfaceState from a planning scene - InterfaceState(const planning_scene::PlanningSceneConstPtr& ps); + InterfaceState(const planning_scene::PlanningScenePtr& ps); /// copy an existing InterfaceState, but not including incoming/outgoing trajectories InterfaceState(const InterfaceState& other); diff --git a/core/src/stages/generate_grasp_pose.cpp b/core/src/stages/generate_grasp_pose.cpp index bdb740a7..f8011449 100644 --- a/core/src/stages/generate_grasp_pose.cpp +++ b/core/src/stages/generate_grasp_pose.cpp @@ -112,7 +112,7 @@ void GenerateGraspPose::onNewSolution(const SolutionBase& s) void GenerateGraspPose::compute() { if (scenes_.empty()) return; - planning_scene::PlanningSceneConstPtr scene = scenes_[0]; + planning_scene::PlanningScenePtr scene = scenes_[0]; scenes_.pop_front(); const auto& props = properties(); diff --git a/core/src/stages/generate_pose.cpp b/core/src/stages/generate_pose.cpp index 65bb4bd7..fe8d07d2 100644 --- a/core/src/stages/generate_pose.cpp +++ b/core/src/stages/generate_pose.cpp @@ -67,7 +67,7 @@ bool GeneratePose::canCompute() const { void GeneratePose::compute() { if (scenes_.empty()) return; - planning_scene::PlanningSceneConstPtr scene = scenes_[0]; + planning_scene::PlanningScenePtr scene = scenes_[0]; scenes_.pop_front(); geometry_msgs::PoseStamped target_pose = properties().get("pose"); diff --git a/core/src/storage.cpp b/core/src/storage.cpp index e7cb53cd..9ae9352a 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -45,8 +45,16 @@ namespace moveit { namespace task_constructor { -InterfaceState::InterfaceState(const planning_scene::PlanningSceneConstPtr &ps) - : scene_(ps) +const planning_scene::PlanningScenePtr& +ensureUpdated(const planning_scene::PlanningScenePtr& scene) { + // ensure scene's state is updated + if (scene->getCurrentState().dirty()) + scene->getCurrentStateNonConst().update(); + return scene; +} + +InterfaceState::InterfaceState(const planning_scene::PlanningScenePtr& ps) + : scene_(ensureUpdated(ps)) { }