feat: automatically determine whether attached objects need to be re-…

…queried based on their consistency in the planned trajectory.

---
Before entering the monitoring phase, it checks whether the attached objects remain consistent throughout the trajectory. If they do, they are stored and later used by the isRemainingPathValid method without needing to be queried again. If the attached objects change during the planned trajectory, the map is left empty, signaling isRemainingPathValid to query them at each waypoint.

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::map<std::string, const moveit::core::AttachedBody*> trajectory_attached_objects_;
 
   unsigned int default_max_replan_attempts_;
 

moveit_ros/planning/plan_execution/src/plan_execution.cpp

@@ -286,11 +286,15 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP
     moveit::core::RobotState state = plan.planning_scene->getCurrentState();
     std::map<std::string, const moveit::core::AttachedBody*> current_attached_objects, sample_attached_objects;
     state.getAttachedBodies(current_attached_objects);
+    sample_attached_objects = trajectory_attached_objects_;
     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(sample_attached_objects);
+      state = t.getWayPoint(i);
+      if (trajectory_attached_objects_.empty())
+      {
+        state.getAttachedBodies(sample_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] : sample_attached_objects)
@@ -456,6 +460,29 @@ 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.
+  trajectory_attached_objects_.clear();
+  for (const auto& component : plan.plan_components)
+  {
+    if (component.trajectory)
+    {
+      const auto& trajectory = component.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;
+        }
+      }
+    }
+  }
+
   while (rclcpp::ok() && !execution_complete_ && !path_became_invalid_)
   {
     r.sleep();