From 8ccb62682f1496acbdee995fc31edb829febd6a5 Mon Sep 17 00:00:00 2001 From: Marco Magri Date: Sat, 8 Feb 2025 11:31:09 +0100 Subject: [PATCH] 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();