PS: do not list object as destroyed when they got attached

The information in the diff is redundant because attaching implies the removal from the PlanningScene. In the unlikely case you relied on the REMOVE entry in the diff message, use the newly attached collision object to indicate the same instead.
moveit · Sep 2, 2021 · cdac8ac · cdac8ac
1 parent e1f6c6a
commit cdac8ac
Showing 1 changed file with 14 additions and 5 deletions.
diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp
@@ -693,11 +693,20 @@ void PlanningScene::getPlanningSceneDiffMsg(moveit_msgs::PlanningScene& scene_ms
         do_omap = true;
       else if (it.second == collision_detection::World::DESTROY)
       {
-        moveit_msgs::CollisionObject co;
-        co.header.frame_id = getPlanningFrame();
-        co.id = it.first;
-        co.operation = moveit_msgs::CollisionObject::REMOVE;
-        scene_msg.world.collision_objects.push_back(co);
+        // if object became attached, it should not be recorded as removed here
+        if (!std::count_if(scene_msg.robot_state.attached_collision_objects.cbegin(),
+                           scene_msg.robot_state.attached_collision_objects.cend(),
+                           [&it](const moveit_msgs::AttachedCollisionObject& aco) {
+                             return aco.object.id == it.first &&
+                                    aco.object.operation == moveit_msgs::CollisionObject::ADD;
+                           }))
+        {
+          moveit_msgs::CollisionObject co;
+          co.header.frame_id = getPlanningFrame();
+          co.id = it.first;
+          co.operation = moveit_msgs::CollisionObject::REMOVE;
+          scene_msg.world.collision_objects.push_back(co);
+        }
       }
       else
       {