fix: cleanup code using lamda function

Author: MarcoMagriDev

moveit_ros/planning/plan_execution/src/plan_execution.cpp

@@ -283,39 +283,41 @@ 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::map<std::string, const moveit::core::AttachedBody*> current_attached_objects;
-    for (const auto& ab : attached_bodies)
-    {
-      current_attached_objects[ab->getName()] = ab;
-    }
+    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);
     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::map<std::string, const moveit::core::AttachedBody*> sample_attached_object;
-      for (const auto& ab : attached_bodies)
-      {
-        sample_attached_object[ab->getName()] = ab;
-      }
+      std::map<std::string, const moveit::core::AttachedBody*> sample_attached_object = getAttachedObjects(state);
+
+      // 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)
       {
         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());
+          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 (sample_attached_object.find(name) == sample_attached_object.end())
         {
-          RCLCPP_INFO(logger_, "Attached object '%s' is not in the robot state. Adding it.", name.c_str());
+          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));
         }
       }