planning_scene updates: expose success state to caller

This is required to get the information back for the ApplyPlanningSceneService.

planning_scene/include/moveit/planning_scene/planning_scene.h

@@ -677,18 +677,18 @@ class PlanningScene : private boost::noncopyable,
   /** \brief Apply changes to this planning scene as diffs, even if the message itself is not marked as being a diff (is_diff
       member). A parent is not required to exist. However, the existing data in the planning instance is not cleared. Data from
       the message is only appended (and in cases such as e.g., the robot state, is overwritten). */
-  void setPlanningSceneDiffMsg(const moveit_msgs::PlanningScene &scene);
+  bool setPlanningSceneDiffMsg(const moveit_msgs::PlanningScene &scene);
 
   /** \brief Set this instance of a planning scene to be the same as the one serialized in the \e scene message, even if the message itself is marked as being a diff (is_diff member) */
-  void setPlanningSceneMsg(const moveit_msgs::PlanningScene &scene);
+  bool setPlanningSceneMsg(const moveit_msgs::PlanningScene &scene);
 
   /** \brief Call setPlanningSceneMsg() or setPlanningSceneDiffMsg() depending on how the is_diff member of the message is set */
-  void usePlanningSceneMsg(const moveit_msgs::PlanningScene &scene);
+  bool usePlanningSceneMsg(const moveit_msgs::PlanningScene &scene);
 
   bool processCollisionObjectMsg(const moveit_msgs::CollisionObject &object);
   bool processAttachedCollisionObjectMsg(const moveit_msgs::AttachedCollisionObject &object);
 
-  void processPlanningSceneWorldMsg(const moveit_msgs::PlanningSceneWorld &world);
+  bool processPlanningSceneWorldMsg(const moveit_msgs::PlanningSceneWorld &world);
 
   void processOctomapMsg(const octomap_msgs::OctomapWithPose &map);
   void processOctomapMsg(const octomap_msgs::Octomap &map);

planning_scene/src/planning_scene.cpp

@@ -1133,8 +1133,10 @@ void planning_scene::PlanningScene::decoupleParent()
   parent_.reset();
 }
 
-void planning_scene::PlanningScene::setPlanningSceneDiffMsg(const moveit_msgs::PlanningScene &scene_msg)
+bool planning_scene::PlanningScene::setPlanningSceneDiffMsg(const moveit_msgs::PlanningScene &scene_msg)
 {
+  bool result = true;
+
   logDebug("moveit.planning_scene: Adding planning scene diff");
   if (!scene_msg.name.empty())
     name_ = scene_msg.name;
@@ -1185,14 +1187,16 @@ void planning_scene::PlanningScene::setPlanningSceneDiffMsg(const moveit_msgs::P
 
   // process collision object updates
   for (std::size_t i = 0 ; i < scene_msg.world.collision_objects.size() ; ++i)
-    processCollisionObjectMsg(scene_msg.world.collision_objects[i]);
+    result &= processCollisionObjectMsg(scene_msg.world.collision_objects[i]);
 
   // if an octomap was specified, replace the one we have with that one
   if (!scene_msg.world.octomap.octomap.data.empty())
     processOctomapMsg(scene_msg.world.octomap);
+
+  return result;
 }
 
-void planning_scene::PlanningScene::setPlanningSceneMsg(const moveit_msgs::PlanningScene &scene_msg)
+bool planning_scene::PlanningScene::setPlanningSceneMsg(const moveit_msgs::PlanningScene &scene_msg)
 {
   logDebug("moveit.planning_scene: Setting new planning scene: '%s'", scene_msg.name.c_str());
   name_ = scene_msg.name;
@@ -1221,22 +1225,24 @@ void planning_scene::PlanningScene::setPlanningSceneMsg(const moveit_msgs::Plann
   for (std::size_t i = 0 ; i < scene_msg.object_colors.size() ; ++i)
     setObjectColor(scene_msg.object_colors[i].id, scene_msg.object_colors[i].color);
   world_->clearObjects();
-  processPlanningSceneWorldMsg(scene_msg.world);
+  return processPlanningSceneWorldMsg(scene_msg.world);
 }
 
-void planning_scene::PlanningScene::processPlanningSceneWorldMsg(const moveit_msgs::PlanningSceneWorld &world)
+bool planning_scene::PlanningScene::processPlanningSceneWorldMsg(const moveit_msgs::PlanningSceneWorld &world)
 {
+  bool result = true;
   for (std::size_t i = 0 ; i < world.collision_objects.size() ; ++i)
-    processCollisionObjectMsg(world.collision_objects[i]);
+    result &= processCollisionObjectMsg(world.collision_objects[i]);
   processOctomapMsg(world.octomap);
+  return result;
 }
 
-void planning_scene::PlanningScene::usePlanningSceneMsg(const moveit_msgs::PlanningScene &scene_msg)
+bool planning_scene::PlanningScene::usePlanningSceneMsg(const moveit_msgs::PlanningScene &scene_msg)
 {
   if (scene_msg.is_diff)
-    setPlanningSceneDiffMsg(scene_msg);
+    return setPlanningSceneDiffMsg(scene_msg);
   else
-    setPlanningSceneMsg(scene_msg);
+    return setPlanningSceneMsg(scene_msg);
 }
 
 void planning_scene::PlanningScene::processOctomapMsg(const octomap_msgs::Octomap &map)