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..246417d77a 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -1220,6 +1220,13 @@ 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(); + 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 { attached_bodies.clear(); 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 173bceccad..26699489f8 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -283,34 +283,64 @@ 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(); + 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) { collision_detection::CollisionResult res; + 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) + { + 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_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)); + } + } + 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; } @@ -430,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();