From a531b2679b3c534cdec15b48120eded660a9d002 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jafar=20Uru=C3=A7?= <cafer.abdi@gmail.com>
Date: Tue, 31 Dec 2024 15:27:17 +0000
Subject: [PATCH] Remove ACM entries when removing collision objects

Backport of https://github.com/moveit/moveit2/pull/3183
---
 .../planning_scene/src/planning_scene.cpp     |  3 +
 .../test/test_planning_scene.cpp              | 55 +++++++++++++++++++
 2 files changed, 58 insertions(+)

diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp
index 332d41f4e0..7cd4565a90 100644
--- a/moveit_core/planning_scene/src/planning_scene.cpp
+++ b/moveit_core/planning_scene/src/planning_scene.cpp
@@ -442,6 +442,7 @@ void PlanningScene::pushDiffs(const PlanningScenePtr& scene)
         scene->world_->removeObject(it.first);
         scene->removeObjectColor(it.first);
         scene->removeObjectType(it.first);
+        scene->getAllowedCollisionMatrixNonConst().removeEntry(it.first);
       }
       else
       {
@@ -1417,6 +1418,7 @@ void PlanningScene::removeAllCollisionObjects()
       world_->removeObject(object_id);
       removeObjectColor(object_id);
       removeObjectType(object_id);
+      getAllowedCollisionMatrixNonConst().removeEntry(object_id);
     }
 }
 
@@ -1875,6 +1877,7 @@ bool PlanningScene::processCollisionObjectRemove(const moveit_msgs::CollisionObj
 
     removeObjectColor(object.id);
     removeObjectType(object.id);
+    getAllowedCollisionMatrixNonConst().removeEntry(object.id);
   }
   return true;
 }
diff --git a/moveit_core/planning_scene/test/test_planning_scene.cpp b/moveit_core/planning_scene/test/test_planning_scene.cpp
index bfe3b133a5..aea24438e7 100644
--- a/moveit_core/planning_scene/test/test_planning_scene.cpp
+++ b/moveit_core/planning_scene/test/test_planning_scene.cpp
@@ -542,6 +542,61 @@ TEST(PlanningScene, RobotStateDiffBug)
   }
 }
 
+TEST(PlanningScene, UpdateACMAfterObjectRemoval)
+{
+  auto robot_model = moveit::core::loadTestingRobotModel("panda");
+  auto ps = std::make_shared<planning_scene::PlanningScene>(robot_model);
+
+  const auto object_name = "object";
+  collision_detection::CollisionRequest collision_request;
+  collision_request.group_name = "hand";
+  collision_request.verbose = true;
+
+  // Helper function to add an object to the planning scene
+  auto add_object = [&] {
+    const auto ps1 = create_planning_scene_diff(*ps, object_name, moveit_msgs::CollisionObject::ADD);
+    ps->usePlanningSceneMsg(ps1);
+    EXPECT_EQ(get_collision_objects_names(*ps), (std::set<std::string>{ object_name }));
+  };
+
+  // Modify the allowed collision matrix and make sure it is updated
+  auto modify_acm = [&] {
+    collision_detection::AllowedCollisionMatrix& acm = ps->getAllowedCollisionMatrixNonConst();
+    acm.setEntry(object_name, ps->getRobotModel()->getJointModelGroup("hand")->getLinkModelNamesWithCollisionGeometry(),
+                 true);
+    EXPECT_TRUE(ps->getAllowedCollisionMatrix().hasEntry(object_name));
+  };
+
+  // Check collision
+  auto check_collision = [&] {
+    collision_detection::CollisionResult res;
+    ps->checkCollision(collision_request, res);
+    return res.collision;
+  };
+
+  // Test removing a collision object using a diff
+  add_object();
+  EXPECT_TRUE(check_collision());
+  modify_acm();
+  EXPECT_FALSE(check_collision());
+  {
+    const auto ps1 = create_planning_scene_diff(*ps, object_name, moveit_msgs::CollisionObject::REMOVE);
+    ps->usePlanningSceneMsg(ps1);
+    EXPECT_EQ(get_collision_objects_names(*ps), (std::set<std::string>{}));
+    EXPECT_FALSE(ps->getAllowedCollisionMatrix().hasEntry(object_name));
+  }
+
+  // Test removing all objects
+  add_object();
+  // This should report a collision since it's a completely new object
+  EXPECT_TRUE(check_collision());
+  modify_acm();
+  EXPECT_FALSE(check_collision());
+  ps->removeAllCollisionObjects();
+  EXPECT_EQ(get_collision_objects_names(*ps), (std::set<std::string>{}));
+  EXPECT_FALSE(ps->getAllowedCollisionMatrix().hasEntry(object_name));
+}
+
 #ifndef INSTANTIATE_TEST_SUITE_P  // prior to gtest 1.10
 #define INSTANTIATE_TEST_SUITE_P(...) INSTANTIATE_TEST_CASE_P(__VA_ARGS__)
 #endif