fix: first attempt of fixing attached objects update during motion execution.

checks that attached objects in the monitored robot matches the one in the planned trajectory. If objects are disappeared in the monitored robot remove also from the trajectory one.

TODO add opposite way

Author: MarcoMagriDev

moveit_ros/planning/plan_execution/src/plan_execution.cpp

@@ -283,34 +283,54 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP
     collision_detection::CollisionRequest req;
     req.group_name = t.getGroupName();
     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(); });
+    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)
+      {
+        if (std::find(current_attached_objects.begin(), current_attached_objects.end(), attached_object) ==
+            current_attached_objects.end())
+        {
+          RCLCPP_INFO(logger_, "Attached object '%s' is not in the current scene", attached_object.c_str());
+          state.clearAttachedBody(attached_object);
+        }
+      }
       if (acm)
       {
-        plan.planning_scene->checkCollision(req, res, t.getWayPoint(i), *acm);
+        plan.planning_scene->checkCollision(req, res, state, *acm);
       }
       else
       {
-        plan.planning_scene->checkCollision(req, res, t.getWayPoint(i));
+        plan.planning_scene->checkCollision(req, res, state);
       }
 
-      if (res.collision || !plan.planning_scene->isStateFeasible(t.getWayPoint(i), false))
+      if (res.collision || !plan.planning_scene->isStateFeasible(state, false))
       {
         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);
+        plan.planning_scene->isStateFeasible(state, true);
         req.verbose = true;
         res.clear();
         if (acm)
         {
-          plan.planning_scene->checkCollision(req, res, t.getWayPoint(i), *acm);
+          plan.planning_scene->checkCollision(req, res, state, *acm);
         }
         else
         {
-          plan.planning_scene->checkCollision(req, res, t.getWayPoint(i));
+          plan.planning_scene->checkCollision(req, res, state);
         }
         return false;
       }