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

(cherry picked from commit c81ac91)

# Conflicts:
#	moveit_core/robot_state/src/robot_state.cpp
#	moveit_ros/planning/plan_execution/src/plan_execution.cpp

Author: MarqRazz

moveit_core/robot_state/src/robot_state.cpp

@@ -1033,9 +1033,29 @@ 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());
+    }
+  }
+}
+
+<<<<<<< HEAD
+=======
+void RobotState::getAttachedBodies(std::map<std::string, const AttachedBody*>& attached_bodies) const
+{
+  attached_bodies.clear();
+  for (const auto& it : attached_body_map_)
+  {
+    if (it.second && !it.first.empty())
+    {
+      attached_bodies[it.first] = it.second.get();
+    }
+  }
 }
 
+>>>>>>> c81ac917e (Fix seg fault with attached objects during motion execution (#3466))
 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

@@ -297,6 +297,7 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP
     std::size_t wpc = t.getWayPointCount();
     collision_detection::CollisionRequest req;
     req.group_name = t.getGroupName();
+<<<<<<< HEAD
     for (std::size_t i = std::max(path_segment.second - 1, 0); i < wpc; ++i)
     {
       collision_detection::CollisionResult res;
@@ -306,18 +307,81 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP
         plan.planning_scene_->checkCollisionUnpadded(req, res, t.getWayPoint(i));
 
       if (res.collision || !plan.planning_scene_->isStateFeasible(t.getWayPoint(i), false))
+=======
+    req.pad_environment_collisions = false;
+    moveit::core::RobotState start_state = plan.planning_scene->getCurrentState();
+    std::map<std::string, const moveit::core::AttachedBody*> current_attached_objects, waypoint_attached_objects;
+    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;
+      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())
+      {
+        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
+      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());
+          waypoint_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());
+          waypoint_state.attachBody(std::make_unique<moveit::core::AttachedBody>(*object));
+        }
+      }
+
+      if (acm)
+      {
+        plan.planning_scene->checkCollision(req, res, waypoint_state, *acm);
+      }
+      else
+      {
+        plan.planning_scene->checkCollision(req, res, waypoint_state);
+      }
+
+      if (res.collision || !plan.planning_scene->isStateFeasible(waypoint_state, false))
+>>>>>>> c81ac917e (Fix seg fault with attached objects during motion execution (#3466))
       {
         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
+<<<<<<< HEAD
         plan.planning_scene_->isStateFeasible(t.getWayPoint(i), true);
         req.verbose = true;
         res.clear();
         if (acm)
           plan.planning_scene_->checkCollisionUnpadded(req, res, t.getWayPoint(i), *acm);
         else
           plan.planning_scene_->checkCollisionUnpadded(req, res, t.getWayPoint(i));
+=======
+        plan.planning_scene->isStateFeasible(waypoint_state, true);
+        req.verbose = true;
+        res.clear();
+        if (acm)
+        {
+          plan.planning_scene->checkCollision(req, res, waypoint_state, *acm);
+        }
+        else
+        {
+          plan.planning_scene->checkCollision(req, res, waypoint_state);
+        }
+>>>>>>> c81ac917e (Fix seg fault with attached objects during motion execution (#3466))
         return false;
       }
     }
@@ -426,6 +490,36 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni
   path_became_invalid_ = false;
   bool preempt_requested = false;
 
+<<<<<<< HEAD
+=======
+  // 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 && trajectory->getWayPointCount() > 0)
+    {
+      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;
+        }
+      }
+    }
+    if (!trajectory_attached_objects.empty())
+      plan_components_attached_objects_.push_back(trajectory_attached_objects);
+  }
+
+>>>>>>> c81ac917e (Fix seg fault with attached objects during motion execution (#3466))
   while (rclcpp::ok() && !execution_complete_ && !path_became_invalid_)
   {
     r.sleep();