fix: adds first draft of on the fly attached object checks

Author: MarcoMagriDev

moveit_ros/planning/plan_execution/src/plan_execution.cpp

@@ -285,27 +285,41 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP
     req.pad_environment_collisions = false;
     std::vector<const moveit::core::AttachedBody*> attached_bodies;
     plan.planning_scene->getCurrentState().getAttachedBodies(attached_bodies);
-    std::vector<std::string> current_attached_objects;
-    std::transform(attached_bodies.begin(), attached_bodies.end(), current_attached_objects.begin(),
-                   [](const moveit::core::AttachedBody* ab) { return ab->getName(); });
+    std::map<std::string, const moveit::core::AttachedBody*> current_attached_objects;
+    for (const auto& ab : attached_bodies)
+    {
+      current_attached_objects[ab->getName()] = ab;
+    }
     moveit::core::RobotState state = plan.planning_scene->getCurrentState();
     for (std::size_t i = std::max(path_segment.second - 1, 0); i < wpc; ++i)
     {
       state = t.getWayPoint(i);
       collision_detection::CollisionResult res;
       state.getAttachedBodies(attached_bodies);
-      std::vector<std::string> sample_attached_object;
-      std::transform(attached_bodies.begin(), attached_bodies.end(), sample_attached_object.begin(),
-                     [](const moveit::core::AttachedBody* ab) { return ab->getName(); });
-      for (const auto& attached_object : sample_attached_object)
+      std::map<std::string, const moveit::core::AttachedBody*> sample_attached_object;
+      for (const auto& ab : attached_bodies)
+      {
+        sample_attached_object[ab->getName()] = ab;
+      }
+      for (const auto& [name, object] : sample_attached_object)
+      {
+        if (current_attached_objects.find(name) == current_attached_objects.end())
+        {
+          RCLCPP_INFO(logger_, "Attached object '%s' is not in the current scene. Removing from robot state",
+                      name.c_str());
+          state.clearAttachedBody(name);
+        }
+      }
+
+      for (const auto& [name, object] : current_attached_objects)
       {
-        if (std::find(current_attached_objects.begin(), current_attached_objects.end(), attached_object) ==
-            current_attached_objects.end())
+        if (sample_attached_object.find(name) == sample_attached_object.end())
         {
-          RCLCPP_INFO(logger_, "Attached object '%s' is not in the current scene", attached_object.c_str());
-          state.clearAttachedBody(attached_object);
+          RCLCPP_INFO(logger_, "Attached object '%s' is not in the robot state. Adding it.", name.c_str());
+          state.attachBody(std::make_unique<moveit::core::AttachedBody>(*object));
         }
       }
+
       if (acm)
       {
         plan.planning_scene->checkCollision(req, res, state, *acm);