From fd88dd63469f2790b7d13864b425c813a2f95aa5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Michael=20G=C3=B6rner?= <me@v4hn.de>
Date: Mon, 16 Aug 2021 05:16:34 +0200
Subject: [PATCH] Preserve metadata when detaching objects (#2814)

* PS: report failure when object could not be removed

This ensures the meta-data will not be removed below.

* PS: add test for meta-data handling
---
 .../planning_scene/src/planning_scene.cpp     |  4 +-
 .../test/test_planning_scene.cpp              | 72 +++++++++++++++++++
 2 files changed, 75 insertions(+), 1 deletion(-)

diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp
index 476c3c193a..38e1cae5fe 100644
--- a/moveit_core/planning_scene/src/planning_scene.cpp
+++ b/moveit_core/planning_scene/src/planning_scene.cpp
@@ -1806,7 +1806,9 @@ bool PlanningScene::processCollisionObjectRemove(const moveit_msgs::CollisionObj
   }
   else
   {
-    world_->removeObject(object.id);
+    if (!world_->removeObject(object.id))
+      return false;
+
     removeObjectColor(object.id);
     removeObjectType(object.id);
   }
diff --git a/moveit_core/planning_scene/test/test_planning_scene.cpp b/moveit_core/planning_scene/test/test_planning_scene.cpp
index 28dece6b9a..1609eaee3f 100644
--- a/moveit_core/planning_scene/test/test_planning_scene.cpp
+++ b/moveit_core/planning_scene/test/test_planning_scene.cpp
@@ -212,6 +212,78 @@ TEST(PlanningScene, loadBadSceneGeometry)
   EXPECT_FALSE(ps->loadGeometryFromStream(malformed_scene_geometry));
 }
 
+TEST(PlanningScene, rememberMetadataWhenAttached)
+{
+  moveit::core::RobotModelPtr robot_model(moveit::core::RobotModelBuilder("empty_robot", "base_link").build());
+  planning_scene::PlanningScene scene(robot_model);
+
+  // prepare planning scene message to add a colored object
+  moveit_msgs::PlanningScene scene_msg;
+  scene_msg.robot_model_name = robot_model->getName();
+  scene_msg.is_diff = true;
+  scene_msg.robot_state.is_diff = true;
+
+  moveit_msgs::CollisionObject co;
+  co.header.frame_id = "base_link";
+  co.id = "blue_sphere";
+  co.operation = moveit_msgs::CollisionObject::ADD;
+  co.pose.orientation.w = 1.0;
+  {
+    // set valid primitive
+    shape_msgs::SolidPrimitive primitive;
+    primitive.type = shape_msgs::SolidPrimitive::SPHERE;
+    primitive.dimensions.push_back(/* SPHERE_RADIUS */ 1.0);
+    co.primitives.push_back(primitive);
+    geometry_msgs::Pose pose;
+    pose.orientation.w = 1.0;
+    co.primitive_poses.push_back(pose);
+  }
+  // meta-data 1: object type
+  co.type.key = "blue_sphere_type";
+  co.type.db = "{'type':'CustomDB'}";
+  scene_msg.world.collision_objects.push_back(co);
+
+  // meta-data 2: object color
+  moveit_msgs::ObjectColor color;
+  color.id = co.id;
+  color.color.b = 1.0;
+  color.color.a = 1.0;
+  scene_msg.object_colors.push_back(color);
+
+  EXPECT_FALSE(scene.hasObjectColor(co.id)) << "scene knows color before adding it(?)";
+  EXPECT_FALSE(scene.hasObjectType(co.id)) << "scene knows type before adding it(?)";
+
+  // add object to scene
+  scene.usePlanningSceneMsg(scene_msg);
+
+  EXPECT_TRUE(scene.hasObjectColor(co.id)) << "scene failed to add object color";
+  EXPECT_EQ(scene.getObjectColor(co.id), color.color) << "scene added wrong object color";
+  EXPECT_TRUE(scene.hasObjectType(co.id)) << "scene failed to add object type";
+  EXPECT_EQ(scene.getObjectType(co.id), co.type) << "scene added wrong object type";
+
+  // attach object
+  moveit_msgs::AttachedCollisionObject aco;
+  aco.object.operation = moveit_msgs::CollisionObject::ADD;
+  aco.object.id = co.id;
+  aco.link_name = robot_model->getModelFrame();
+  scene.processAttachedCollisionObjectMsg(aco);
+
+  EXPECT_EQ(scene.getObjectColor(co.id), color.color) << "scene forgot object color after it got attached";
+  EXPECT_EQ(scene.getObjectType(co.id), co.type) << "scene forgot object type after it got attached";
+
+  // trying to remove object from the scene while it is attached is expected to fail
+  co.operation = moveit_msgs::CollisionObject::REMOVE;
+  EXPECT_FALSE(scene.processCollisionObjectMsg(co))
+      << "scene removed attached object from collision world (although it's not there)";
+
+  // detach again right away
+  aco.object.operation = moveit_msgs::CollisionObject::REMOVE;
+  scene.processAttachedCollisionObjectMsg(aco);
+
+  EXPECT_EQ(scene.getObjectColor(co.id), color.color) << "scene forgot specified color after attach/detach";
+  EXPECT_EQ(scene.getObjectType(co.id), co.type) << "scene forgot specified type after attach/detach";
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);