apply planning scene: use newly exposed success value of newPlanningS…

…ceneMessage
otamachan · Jun 16, 2016 · 0ec74d4 · 0ec74d4
1 parent 2000333
commit 0ec74d4
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 5 deletions.
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
@@ -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/...ing/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h b/...ing/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
@@ -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)