From b94e50ba257f5fee8483040492d261f658dcc556 Mon Sep 17 00:00:00 2001 From: Marco Magri Date: Thu, 6 Feb 2025 17:55:36 +0100 Subject: [PATCH 1/4] fix: ensure attached objects update during motion execution - Check that attached objects in the monitored robot match those in the planned trajectory. - If an object disappears from the monitored robot, remove it from the trajectory waypoint robot_state. - If an object is attached to the monitored robot but missing in the trajectory, add it to enable meaningful collision checking. --- .../plan_execution/src/plan_execution.cpp | 48 ++++++++++++++++--- 1 file changed, 42 insertions(+), 6 deletions(-) diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index 173bceccad..82d0b7549d 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -283,34 +283,70 @@ 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 attached_bodies; + state.getAttachedBodies(attached_bodies); + std::map 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 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; + std::map 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_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_DEBUG(logger_, "Attached object '%s' is not in the robot state. Adding it.", name.c_str()); + state.attachBody(std::make_unique(*object)); + } + } + if (acm) { - plan.planning_scene->checkCollision(req, res, t.getWayPoint(i), *acm); + plan.planning_scene->checkCollision(req, res, state, *acm); } else { - plan.planning_scene->checkCollision(req, res, t.getWayPoint(i)); + plan.planning_scene->checkCollision(req, res, state); } - if (res.collision || !plan.planning_scene->isStateFeasible(t.getWayPoint(i), false)) + if (res.collision || !plan.planning_scene->isStateFeasible(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(t.getWayPoint(i), true); + plan.planning_scene->isStateFeasible(state, true); req.verbose = true; res.clear(); if (acm) { - plan.planning_scene->checkCollision(req, res, t.getWayPoint(i), *acm); + plan.planning_scene->checkCollision(req, res, state, *acm); } else { - plan.planning_scene->checkCollision(req, res, t.getWayPoint(i)); + plan.planning_scene->checkCollision(req, res, state); } return false; } From 20df675bdb538820f63528d0836e8ce7390c717e Mon Sep 17 00:00:00 2001 From: Marco Magri Date: Thu, 6 Feb 2025 20:59:23 +0100 Subject: [PATCH 2/4] feat: adds `getAttachedBodies` override that returns a map instead of a vector --- .../include/moveit/robot_state/robot_state.hpp | 3 +++ moveit_core/robot_state/src/robot_state.cpp | 11 +++++++++++ .../plan_execution/src/plan_execution.cpp | 16 +++------------- 3 files changed, 17 insertions(+), 13 deletions(-) diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.hpp b/moveit_core/robot_state/include/moveit/robot_state/robot_state.hpp index 599071f374..26ea89e8e2 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.hpp +++ b/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& attached_bodies) const; + /** \brief Get all bodies attached to the model corresponding to this state */ + void getAttachedBodies(std::map& attached_bodies) const; + /** \brief Get all bodies attached to a particular group the model corresponding to this state */ void getAttachedBodies(std::vector& attached_bodies, const JointModelGroup* group) const; diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index a3fded62bf..32922c098e 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -1220,6 +1220,17 @@ void RobotState::getAttachedBodies(std::vector& attached_bo attached_bodies.push_back(it.second.get()); } +void RobotState::getAttachedBodies(std::map& attached_bodies) const +{ + attached_bodies.clear(); + std::vector bodies; + getAttachedBodies(bodies); + for (const auto& b : bodies) + { + attached_bodies[b->getName()] = b; + } +} + void RobotState::getAttachedBodies(std::vector& attached_bodies, const JointModelGroup* group) const { attached_bodies.clear(); diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index 82d0b7549d..05c1589180 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/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 attached_bodies; - state.getAttachedBodies(attached_bodies); - std::map 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 current_attached_objects = getAttachedObjects(state); + std::map 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 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) From 99bdd411cc3b95f3d1b10dae9096b2044c66edf5 Mon Sep 17 00:00:00 2001 From: Marco Magri Date: Fri, 7 Feb 2025 16:06:43 +0100 Subject: [PATCH 3/4] fix: removes vector allocation in getAttachedBodies map override style: `sample_attached_object` -> `sample_attached_objects` --- moveit_core/robot_state/src/robot_state.cpp | 8 ++------ moveit_ros/planning/plan_execution/src/plan_execution.cpp | 8 ++++---- 2 files changed, 6 insertions(+), 10 deletions(-) diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 32922c098e..246417d77a 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -1223,12 +1223,8 @@ void RobotState::getAttachedBodies(std::vector& attached_bo void RobotState::getAttachedBodies(std::map& attached_bodies) const { attached_bodies.clear(); - std::vector bodies; - getAttachedBodies(bodies); - for (const auto& b : bodies) - { - attached_bodies[b->getName()] = b; - } + for (const auto& it : attached_body_map_) + 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 05c1589180..cf15223238 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -284,16 +284,16 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP req.group_name = t.getGroupName(); req.pad_environment_collisions = false; moveit::core::RobotState state = plan.planning_scene->getCurrentState(); - std::map current_attached_objects, sample_attached_object; + std::map current_attached_objects, sample_attached_objects; 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; - state.getAttachedBodies(sample_attached_object); + state.getAttachedBodies(sample_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] : sample_attached_object) + for (const auto& [name, object] : sample_attached_objects) { if (current_attached_objects.find(name) == current_attached_objects.end()) { @@ -305,7 +305,7 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP // 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()) + if (sample_attached_objects.find(name) == sample_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)); From 8ccb62682f1496acbdee995fc31edb829febd6a5 Mon Sep 17 00:00:00 2001 From: Marco Magri Date: Sat, 8 Feb 2025 11:31:09 +0100 Subject: [PATCH 4/4] feat: automatically determine whether attached objects need to be re-queried based on their consistency in the planned trajectory. --- Before entering the monitoring phase, it checks whether the attached objects remain consistent throughout the trajectory. If they do, they are stored and later used by the isRemainingPathValid method without needing to be queried again. If the attached objects change during the planned trajectory, the map is left empty, signaling isRemainingPathValid to query them at each waypoint. --- .../moveit/plan_execution/plan_execution.hpp | 1 + .../plan_execution/src/plan_execution.cpp | 31 +++++++++++++++++-- 2 files changed, 30 insertions(+), 2 deletions(-) diff --git a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.hpp b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.hpp index e5bcddb190..5ba182ec8b 100644 --- a/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.hpp +++ b/moveit_ros/planning/plan_execution/include/moveit/plan_execution/plan_execution.hpp @@ -156,6 +156,7 @@ class PlanExecution planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_; planning_scene_monitor::TrajectoryMonitorPtr trajectory_monitor_; + std::map trajectory_attached_objects_; unsigned int default_max_replan_attempts_; diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index cf15223238..26699489f8 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -286,11 +286,15 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP moveit::core::RobotState state = plan.planning_scene->getCurrentState(); std::map current_attached_objects, sample_attached_objects; state.getAttachedBodies(current_attached_objects); + sample_attached_objects = trajectory_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; - state.getAttachedBodies(sample_attached_objects); + state = t.getWayPoint(i); + if (trajectory_attached_objects_.empty()) + { + state.getAttachedBodies(sample_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] : sample_attached_objects) @@ -456,6 +460,29 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni path_became_invalid_ = false; bool preempt_requested = false; + // 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. + trajectory_attached_objects_.clear(); + for (const auto& component : plan.plan_components) + { + if (component.trajectory) + { + const auto& trajectory = component.trajectory; + std::map 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; + } + } + } + } + while (rclcpp::ok() && !execution_complete_ && !path_became_invalid_) { r.sleep();