Fix seg fault with attached objects during motion execution (#3466)

(cherry picked from commit c81ac91)

Author: MarqRazz

moveit_core/robot_state/src/robot_state.cpp

@@ -1217,14 +1217,24 @@ void RobotState::getAttachedBodies(std::vector<const AttachedBody*>& attached_bo
   attached_bodies.clear();
   attached_bodies.reserve(attached_body_map_.size());
   for (const auto& it : attached_body_map_)
-    attached_bodies.push_back(it.second.get());
+  {
+    if (it.second)
+    {
+      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();
+  {
+    if (it.second && !it.first.empty())
+    {
+      attached_bodies[it.first] = it.second.get();
+    }
+  }
 }
 
 void RobotState::getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const JointModelGroup* group) const

moveit_ros/planning/plan_execution/src/plan_execution.cpp

@@ -283,17 +283,20 @@ 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();
+    moveit::core::RobotState start_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];
+    start_state.getAttachedBodies(current_attached_objects);
+    if (plan_components_attached_objects_.size() > static_cast<size_t>(path_segment.first))
+      waypoint_attached_objects = plan_components_attached_objects_[path_segment.first];
+    moveit::core::RobotState waypoint_state(start_state);
     for (std::size_t i = std::max(path_segment.second - 1, 0); i < wpc; ++i)
     {
       collision_detection::CollisionResult res;
-      state = t.getWayPoint(i);
+      waypoint_attached_objects.clear();  // clear out the last waypoints attached objects
+      waypoint_state = t.getWayPoint(i);
       if (plan_components_attached_objects_[path_segment.first].empty())
       {
-        state.getAttachedBodies(waypoint_attached_objects);
+        waypoint_state.getAttachedBodies(waypoint_attached_objects);
       }
 
       // If sample state has attached objects that are not in the current state, remove them from the sample state
@@ -302,7 +305,7 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP
         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);
+          waypoint_state.clearAttachedBody(name);
         }
       }
 
@@ -312,35 +315,35 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP
         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));
+          waypoint_state.attachBody(std::make_unique<moveit::core::AttachedBody>(*object));
         }
       }
 
       if (acm)
       {
-        plan.planning_scene->checkCollision(req, res, state, *acm);
+        plan.planning_scene->checkCollision(req, res, waypoint_state, *acm);
       }
       else
       {
-        plan.planning_scene->checkCollision(req, res, state);
+        plan.planning_scene->checkCollision(req, res, waypoint_state);
       }
 
-      if (res.collision || !plan.planning_scene->isStateFeasible(state, false))
+      if (res.collision || !plan.planning_scene->isStateFeasible(waypoint_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(state, true);
+        plan.planning_scene->isStateFeasible(waypoint_state, true);
         req.verbose = true;
         res.clear();
         if (acm)
         {
-          plan.planning_scene->checkCollision(req, res, state, *acm);
+          plan.planning_scene->checkCollision(req, res, waypoint_state, *acm);
         }
         else
         {
-          plan.planning_scene->checkCollision(req, res, state);
+          plan.planning_scene->checkCollision(req, res, waypoint_state);
         }
         return false;
       }
@@ -469,7 +472,7 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni
   {
     const auto& trajectory = component.trajectory;
     std::map<std::string, const moveit::core::AttachedBody*> trajectory_attached_objects;
-    if (trajectory)
+    if (trajectory && trajectory->getWayPointCount() > 0)
     {
       std::map<std::string, const moveit::core::AttachedBody*> attached_objects;
       trajectory->getWayPoint(0).getAttachedBodies(trajectory_attached_objects);
@@ -483,7 +486,8 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni
         }
       }
     }
-    plan_components_attached_objects_.push_back(trajectory_attached_objects);
+    if (!trajectory_attached_objects.empty())
+      plan_components_attached_objects_.push_back(trajectory_attached_objects);
   }
 
   while (rclcpp::ok() && !execution_complete_ && !path_became_invalid_)