From 4c1805de2c7b52def3c5f215c66a2d536e04a292 Mon Sep 17 00:00:00 2001 From: marqrazz Date: Tue, 6 May 2025 09:03:36 -0600 Subject: [PATCH 1/6] Fix seg fault with attached objects during motion execution --- moveit_core/robot_state/src/robot_state.cpp | 6 ++++-- moveit_ros/planning/plan_execution/src/plan_execution.cpp | 8 +++++--- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 246417d77a..b9b4ddee54 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -1217,14 +1217,16 @@ void RobotState::getAttachedBodies(std::vector& 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& 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& attached_bodies, const JointModelGroup* group) const diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index 1a5d3b6ae2..eadae5458e 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -286,7 +286,8 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP moveit::core::RobotState state = plan.planning_scene->getCurrentState(); std::map current_attached_objects, waypoint_attached_objects; state.getAttachedBodies(current_attached_objects); - waypoint_attached_objects = plan_components_attached_objects_[path_segment.first]; + if (plan_components_attached_objects_.size() > path_segment.first) + waypoint_attached_objects = plan_components_attached_objects_[path_segment.first]; for (std::size_t i = std::max(path_segment.second - 1, 0); i < wpc; ++i) { collision_detection::CollisionResult res; @@ -469,7 +470,7 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni { const auto& trajectory = component.trajectory; std::map trajectory_attached_objects; - if (trajectory) + if (trajectory && trajectory->getWayPointCount() > 0) { std::map attached_objects; trajectory->getWayPoint(0).getAttachedBodies(trajectory_attached_objects); @@ -483,7 +484,8 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni } } } - plan_components_attached_objects_.push_back(trajectory_attached_objects); + if (trajectory_attached_objects.size() > 0) + plan_components_attached_objects_.push_back(trajectory_attached_objects); } while (rclcpp::ok() && !execution_complete_ && !path_became_invalid_) From d0a5149d6c2299a814f98e26b81381ec3f038cd7 Mon Sep 17 00:00:00 2001 From: marqrazz Date: Tue, 6 May 2025 09:47:07 -0600 Subject: [PATCH 2/6] fix sign-compare warning --- moveit_ros/planning/plan_execution/src/plan_execution.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index eadae5458e..3a6f7a8ab4 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -286,7 +286,7 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP moveit::core::RobotState state = plan.planning_scene->getCurrentState(); std::map current_attached_objects, waypoint_attached_objects; state.getAttachedBodies(current_attached_objects); - if (plan_components_attached_objects_.size() > path_segment.first) + if (plan_components_attached_objects_.size() > static_cast(path_segment.first)) waypoint_attached_objects = plan_components_attached_objects_[path_segment.first]; for (std::size_t i = std::max(path_segment.second - 1, 0); i < wpc; ++i) { From 727866f4c4674c357347cb814edcb1a0f7f1ae30 Mon Sep 17 00:00:00 2001 From: marqrazz Date: Tue, 6 May 2025 12:12:59 -0600 Subject: [PATCH 3/6] use unique RobotState to keep raw pointers valid --- moveit_ros/planning/plan_execution/src/plan_execution.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index 3a6f7a8ab4..f7b0c7304d 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -283,15 +283,15 @@ 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 current_attached_objects, waypoint_attached_objects; - state.getAttachedBodies(current_attached_objects); + start_state.getAttachedBodies(current_attached_objects); if (plan_components_attached_objects_.size() > static_cast(path_segment.first)) waypoint_attached_objects = plan_components_attached_objects_[path_segment.first]; for (std::size_t i = std::max(path_segment.second - 1, 0); i < wpc; ++i) { collision_detection::CollisionResult res; - state = t.getWayPoint(i); + moveit::core::RobotState state = t.getWayPoint(i); if (plan_components_attached_objects_[path_segment.first].empty()) { state.getAttachedBodies(waypoint_attached_objects); From d5ef5db4f8ac9b75b47c132a0a9cd34bdfc6ad6d Mon Sep 17 00:00:00 2001 From: marqrazz Date: Tue, 6 May 2025 13:45:32 -0600 Subject: [PATCH 4/6] pre-commit --- moveit_core/robot_state/src/robot_state.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index b9b4ddee54..ddb3fa830c 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -1217,16 +1217,24 @@ void RobotState::getAttachedBodies(std::vector& attached_bo attached_bodies.clear(); attached_bodies.reserve(attached_body_map_.size()); for (const auto& it : attached_body_map_) + { if (it.second) + { attached_bodies.push_back(it.second.get()); + } + } } void RobotState::getAttachedBodies(std::map& 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(); + } + } } void RobotState::getAttachedBodies(std::vector& attached_bodies, const JointModelGroup* group) const From 97d1a1e80e07f6082b9a49dd96cc7ad111c34aba Mon Sep 17 00:00:00 2001 From: marqrazz Date: Tue, 6 May 2025 14:04:39 -0600 Subject: [PATCH 5/6] missed one --- moveit_ros/planning/plan_execution/src/plan_execution.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index f7b0c7304d..96d97ce9cf 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -484,7 +484,7 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni } } } - if (trajectory_attached_objects.size() > 0) + if (!trajectory_attached_objects.empty()) plan_components_attached_objects_.push_back(trajectory_attached_objects); } From 598dd30f0d8233af15cad47bfeefd894a93e0df1 Mon Sep 17 00:00:00 2001 From: marqrazz Date: Mon, 12 May 2025 08:20:18 -0600 Subject: [PATCH 6/6] declare waypoint_state outside loop --- .../plan_execution/src/plan_execution.cpp | 22 ++++++++++--------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index 96d97ce9cf..d6f67e89f2 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -288,13 +288,15 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP start_state.getAttachedBodies(current_attached_objects); if (plan_components_attached_objects_.size() > static_cast(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; - moveit::core::RobotState 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 @@ -303,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); } } @@ -313,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(*object)); + waypoint_state.attachBody(std::make_unique(*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; }