fix: ensure attached objects update during motion execution (#3327) (#3414)

(cherry picked from commit 89acadd)

Co-authored-by: Marco Magri <94347649+MarcoMagriDev@users.noreply.github.com>

Author: mergify[bot]

moveit_core/robot_state/include/moveit/robot_state/robot_state.hpp

@@ -1519,6 +1519,9 @@ class RobotState
   /** \brief Get all bodies attached to the model corresponding to this state */
   void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies) const;
 
+  /** \brief Get all bodies attached to the model corresponding to this state */
+  void getAttachedBodies(std::map<std::string, const AttachedBody*>& attached_bodies) const;
+
   /** \brief Get all bodies attached to a particular group the model corresponding to this state */
   void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const JointModelGroup* group) const;
 

moveit_core/robot_state/src/robot_state.cpp

@@ -1220,6 +1220,13 @@ void RobotState::getAttachedBodies(std::vector<const AttachedBody*>& attached_bo
     attached_bodies.push_back(it.second.get());
 }
 
+void RobotState::getAttachedBodies(std::map<std::string, const AttachedBody*>& attached_bodies) const
+{
+  attached_bodies.clear();
+  for (const auto& it : attached_body_map_)
+    attached_bodies[it.first] = it.second.get();
+}
+
 void RobotState::getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const JointModelGroup* group) const
 {
   attached_bodies.clear();

moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.hpp

@@ -156,6 +156,7 @@ class PlanExecution
   planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
   trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_;
   planning_scene_monitor::TrajectoryMonitorPtr trajectory_monitor_;
+  std::vector<std::map<std::string, const moveit::core::AttachedBody*>> plan_components_attached_objects_;
 
   unsigned int default_max_replan_attempts_;
 

moveit_ros/planning/plan_execution/src/plan_execution.cpp

@@ -283,34 +283,64 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP
     collision_detection::CollisionRequest req;
     req.group_name = t.getGroupName();
     req.pad_environment_collisions = false;
+    moveit::core::RobotState state = plan.planning_scene->getCurrentState();
+    std::map<std::string, const moveit::core::AttachedBody*> current_attached_objects, waypoint_attached_objects;
+    state.getAttachedBodies(current_attached_objects);
+    waypoint_attached_objects = plan_components_attached_objects_[path_segment.first];
     for (std::size_t i = std::max(path_segment.second - 1, 0); i < wpc; ++i)
     {
       collision_detection::CollisionResult res;
+      state = t.getWayPoint(i);
+      if (plan_components_attached_objects_[path_segment.first].empty())
+      {
+        state.getAttachedBodies(waypoint_attached_objects);
+      }
+
+      // If sample state has attached objects that are not in the current state, remove them from the sample state
+      for (const auto& [name, object] : waypoint_attached_objects)
+      {
+        if (current_attached_objects.find(name) == current_attached_objects.end())
+        {
+          RCLCPP_DEBUG(logger_, "Attached object '%s' is not in the current scene. Removing it.", name.c_str());
+          state.clearAttachedBody(name);
+        }
+      }
+
+      // If current state has attached objects that are not in the sample state, add them to the sample state
+      for (const auto& [name, object] : current_attached_objects)
+      {
+        if (waypoint_attached_objects.find(name) == waypoint_attached_objects.end())
+        {
+          RCLCPP_DEBUG(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, 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;
       }
@@ -430,6 +460,32 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni
   path_became_invalid_ = false;
   bool preempt_requested = false;
 
+  // Check that attached objects remain consistent throughout the trajectory and store them.
+  // This avoids querying the scene for attached objects at each waypoint whenever possible.
+  // If a change in attached objects is detected, they will be queried at each waypoint.
+  plan_components_attached_objects_.clear();
+  plan_components_attached_objects_.reserve(plan.plan_components.size());
+  for (const auto& component : plan.plan_components)
+  {
+    const auto& trajectory = component.trajectory;
+    std::map<std::string, const moveit::core::AttachedBody*> trajectory_attached_objects;
+    if (trajectory)
+    {
+      std::map<std::string, const moveit::core::AttachedBody*> attached_objects;
+      trajectory->getWayPoint(0).getAttachedBodies(trajectory_attached_objects);
+      for (std::size_t i = 1; i < trajectory->getWayPointCount(); ++i)
+      {
+        trajectory->getWayPoint(i).getAttachedBodies(attached_objects);
+        if (attached_objects != trajectory_attached_objects)
+        {
+          trajectory_attached_objects.clear();
+          break;
+        }
+      }
+    }
+    plan_components_attached_objects_.push_back(trajectory_attached_objects);
+  }
+
   while (rclcpp::ok() && !execution_complete_ && !path_became_invalid_)
   {
     r.sleep();