diff --git a/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.cpp index 9b74af8dea..c74337de36 100644 --- a/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.cpp @@ -55,7 +55,7 @@ bool move_group::ApplyPlanningSceneService::applyScene(moveit_msgs::ApplyPlannin return true; } context_->planning_scene_monitor_->updateFrameTransforms(); - context_->planning_scene_monitor_->newPlanningSceneMessage(req.scene); + res.success = context_->planning_scene_monitor_->newPlanningSceneMessage(req.scene); return true; } diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h index d271d3baf2..b138e75035 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h @@ -356,7 +356,7 @@ class PlanningSceneMonitor : private boost::noncopyable void clearOctomap(); // Called to update the planning scene with a new message. - void newPlanningSceneMessage(const moveit_msgs::PlanningScene& scene); + bool newPlanningSceneMessage(const moveit_msgs::PlanningScene& scene); protected: diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index 7a3e1f2a5a..aea119c033 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp @@ -476,10 +476,12 @@ void planning_scene_monitor::PlanningSceneMonitor::clearOctomap() octomap_monitor_->getOcTreePtr()->unlockWrite(); } -void planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::PlanningScene& scene) +bool planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::PlanningScene& scene) { if (!scene_) - return; + return false; + + bool result; SceneUpdateType upd = UPDATE_SCENE; std::string old_scene_name; @@ -490,7 +492,7 @@ void planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneMessage(const last_update_time_ = ros::Time::now(); old_scene_name = scene_->getName(); - scene_->usePlanningSceneMsg(scene); + result = scene_->usePlanningSceneMsg(scene); if (octomap_monitor_) { if (!scene.is_diff && scene.world.octomap.octomap.data.empty()) @@ -544,6 +546,7 @@ void planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneMessage(const } } triggerSceneUpdateEvent(upd); + return result; } void planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr &world)