Don't destroy objects on attach (#3205) (#3214)

(cherry picked from commit 639e214)

Co-authored-by: Marq Rasmussen <[email protected]>

moveit_core/planning_scene/src/planning_scene.cpp

@@ -374,7 +374,11 @@ void PlanningScene::pushDiffs(const PlanningScenePtr& scene)
         scene->world_->removeObject(it.first);
         scene->removeObjectColor(it.first);
         scene->removeObjectType(it.first);
-        scene->getAllowedCollisionMatrixNonConst().removeEntry(it.first);
+        // if object is attached, it should not be removed from the ACM
+        if (!scene->getCurrentState().hasAttachedBody(it.first))
+        {
+          scene->getAllowedCollisionMatrixNonConst().removeEntry(it.first);
+        }
       }
       else
       {

moveit_core/planning_scene/test/test_planning_scene.cpp

@@ -607,6 +607,20 @@ TEST(PlanningScene, UpdateACMAfterObjectRemoval)
     EXPECT_EQ(getCollisionObjectsNames(*ps), (std::set<std::string>{ object_name }));
   };
 
+  // Helper function to attach the object to the robot
+  auto attach_object = [&] {
+    const auto ps1 = createPlanningSceneDiff(*ps, object_name, moveit_msgs::msg::CollisionObject::ADD, true);
+    ps->usePlanningSceneMsg(ps1);
+    EXPECT_EQ(getAttachedCollisionObjectsNames(*ps), (std::set<std::string>{ object_name }));
+  };
+
+  // Helper function to detach the object from the robot
+  auto detach_object = [&] {
+    const auto ps1 = createPlanningSceneDiff(*ps, object_name, moveit_msgs::msg::CollisionObject::REMOVE, true);
+    ps->usePlanningSceneMsg(ps1);
+    EXPECT_EQ(getAttachedCollisionObjectsNames(*ps), (std::set<std::string>{}));
+  };
+
   // Modify the allowed collision matrix and make sure it is updated
   auto modify_acm = [&] {
     collision_detection::AllowedCollisionMatrix& acm = ps->getAllowedCollisionMatrixNonConst();
@@ -627,6 +641,11 @@ TEST(PlanningScene, UpdateACMAfterObjectRemoval)
   EXPECT_TRUE(check_collision());
   modify_acm();
   EXPECT_FALSE(check_collision());
+  // Attach and detach the object from the robot to make sure that collision are still allowed
+  attach_object();
+  EXPECT_FALSE(check_collision());
+  detach_object();
+  EXPECT_FALSE(check_collision());
   {
     const auto ps1 = createPlanningSceneDiff(*ps, object_name, moveit_msgs::msg::CollisionObject::REMOVE);
     ps->usePlanningSceneMsg(ps1);

moveit_ros/planning/plan_execution/src/plan_execution.cpp

@@ -83,7 +83,7 @@ plan_execution::PlanExecution::PlanExecution(
   : node_(node)
   , planning_scene_monitor_(planning_scene_monitor)
   , trajectory_execution_manager_(trajectory_execution)
-  , logger_(moveit::getLogger("moveit.ros.add_time_optimal_parameterization"))
+  , logger_(moveit::getLogger("moveit.ros.plan_execution"))
 {
   if (!trajectory_execution_manager_)
   {
@@ -297,9 +297,8 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP
 
       if (res.collision || !plan.planning_scene->isStateFeasible(t.getWayPoint(i), false))
       {
-        // Dave's debacle
-        RCLCPP_INFO(logger_, "Trajectory component '%s' is invalid",
-                    plan.plan_components[path_segment.first].description.c_str());
+        RCLCPP_INFO(logger_, "Trajectory component '%s' is invalid for waypoint %ld out of %ld",
+                    plan.plan_components[path_segment.first].description.c_str(), i, wpc);
 
         // call the same functions again, in verbose mode, to show what issues have been detected
         plan.planning_scene->isStateFeasible(t.getWayPoint(i), true);