feat: adds getAttachedBodies override that returns a map instead of a vector

Author: MarcoMagriDev

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,17 @@ 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();
+  std::vector<const AttachedBody*> bodies;
+  getAttachedBodies(bodies);
+  for (const auto& b : bodies)
+  {
+    attached_bodies[b->getName()] = b;
+  }
+}
+
 void RobotState::getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const JointModelGroup* group) const
 {
   attached_bodies.clear();

moveit_ros/planning/plan_execution/src/plan_execution.cpp

@@ -283,24 +283,14 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP
     collision_detection::CollisionRequest req;
     req.group_name = t.getGroupName();
     req.pad_environment_collisions = false;
-    auto getAttachedObjects = [](const moveit::core::RobotState& state) {
-      std::vector<const moveit::core::AttachedBody*> attached_bodies;
-      state.getAttachedBodies(attached_bodies);
-      std::map<std::string, const moveit::core::AttachedBody*> attached_objects;
-      for (const auto& ab : attached_bodies)
-      {
-        attached_objects[ab->getName()] = ab;
-      }
-      return attached_objects;
-    };
-
     moveit::core::RobotState state = plan.planning_scene->getCurrentState();
-    std::map<std::string, const moveit::core::AttachedBody*> current_attached_objects = getAttachedObjects(state);
+    std::map<std::string, const moveit::core::AttachedBody*> current_attached_objects, sample_attached_object;
+    state.getAttachedBodies(current_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;
-      std::map<std::string, const moveit::core::AttachedBody*> sample_attached_object = getAttachedObjects(state);
+      state.getAttachedBodies(sample_attached_object);
 
       // 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_object)