Add object types when requesting attached bodies in planning scene msg.

moveit · Jul 18, 2016 · f70e290 · f70e290
1 parent 750dd54
commit f70e290
Showing 1 changed file with 10 additions and 3 deletions.
diff --git a/planning_scene/src/planning_scene.cpp b/planning_scene/src/planning_scene.cpp
@@ -909,11 +909,18 @@ void planning_scene::PlanningScene::getPlanningSceneMsg(moveit_msgs::PlanningSce
   if (comp.components & moveit_msgs::PlanningSceneComponents::TRANSFORMS)
     getTransforms().copyTransforms(scene_msg.fixed_frame_transforms);
 
-  if (comp.components & moveit_msgs::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS)
+  if (comp.components & moveit_msgs::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS) {
     robot_state::robotStateToRobotStateMsg(getCurrentState(), scene_msg.robot_state, true);
-  else
-    if (comp.components & moveit_msgs::PlanningSceneComponents::ROBOT_STATE)
+    for(std::vector<moveit_msgs::AttachedCollisionObject>::iterator it
+            = scene_msg.robot_state.attached_collision_objects.begin();
+            it != scene_msg.robot_state.attached_collision_objects.end(); ++it) {
+        if(hasObjectType(it->object.id)) {
+            it->object.type = getObjectType(it->object.id);
+        }
+    }
+  } else if (comp.components & moveit_msgs::PlanningSceneComponents::ROBOT_STATE) {
       robot_state::robotStateToRobotStateMsg(getCurrentState(), scene_msg.robot_state, false);
+  }
 
   if (comp.components & moveit_msgs::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX)
     getAllowedCollisionMatrix().getMessage(scene_msg.allowed_collision_matrix);