From f1a607129a6b5183c9296b29e0d126d9b48e7ea8 Mon Sep 17 00:00:00 2001 From: Captain Yoshi Date: Thu, 31 Oct 2024 08:56:40 -0400 Subject: [PATCH] Replace adapter ResolveConstraintFrames with direct function call (#2975) The ResolveConstraintFrames adapter was only changing the planning request used within the planning pipeline, while the subsequent trajectory validation was still using the original, unresolved request, causing at best a warning and at worst acceptance of paths not satisfying the path constraints. We make the adapter obsolete and just call the resolver function once in the beginning of the planning pipeline. --- .../planning_pipeline/src/planning_pipeline.cpp | 11 +++++++++-- .../src/resolve_constraint_frames.cpp | 8 ++------ .../planning_request_adapters_plugin_description.xml | 2 +- .../launch/chomp_planning_pipeline.launch.xml | 1 - .../launch/ompl_planning_pipeline.launch.xml | 1 - 5 files changed, 12 insertions(+), 11 deletions(-) diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index 30562515df..57b817f517 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -216,7 +217,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla } bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, + const planning_interface::MotionPlanRequest& request, planning_interface::MotionPlanResponse& res, std::vector& adapter_added_state_index) const { @@ -225,7 +226,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla // broadcast the request we are about to work on, if needed if (publish_received_requests_) - received_request_publisher_.publish(req); + received_request_publisher_.publish(request); adapter_added_state_index.clear(); if (!planner_instance_) @@ -236,6 +237,12 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla return false; } + // resolve constraint frames + planning_interface::MotionPlanRequest req = request; + kinematic_constraints::resolveConstraintFrames(planning_scene->getCurrentState(), req.path_constraints); + for (moveit_msgs::Constraints& constraint : req.goal_constraints) + kinematic_constraints::resolveConstraintFrames(planning_scene->getCurrentState(), constraint); + bool solved = false; try { diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp index 0c5cdb3dd2..570202bf08 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp @@ -46,6 +46,7 @@ class ResolveConstraintFrames : public planning_request_adapter::PlanningRequest public: ResolveConstraintFrames() : planning_request_adapter::PlanningRequestAdapter() { + ROS_WARN("The planning adapter 'ResolveConstraintFrames' is obsolete and can be removed from your config."); } void initialize(const ros::NodeHandle& /*nh*/) override @@ -61,12 +62,7 @@ class ResolveConstraintFrames : public planning_request_adapter::PlanningRequest const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, std::vector& /*added_path_index*/) const override { - ROS_DEBUG("Running '%s'", getDescription().c_str()); - planning_interface::MotionPlanRequest modified = req; - kinematic_constraints::resolveConstraintFrames(planning_scene->getCurrentState(), modified.path_constraints); - for (moveit_msgs::Constraints& constraint : modified.goal_constraints) - kinematic_constraints::resolveConstraintFrames(planning_scene->getCurrentState(), constraint); - return planner(planning_scene, modified, res); + return planner(planning_scene, req, res); } }; diff --git a/moveit_ros/planning/planning_request_adapters_plugin_description.xml b/moveit_ros/planning/planning_request_adapters_plugin_description.xml index 93685f5e43..c14c8ad343 100644 --- a/moveit_ros/planning/planning_request_adapters_plugin_description.xml +++ b/moveit_ros/planning/planning_request_adapters_plugin_description.xml @@ -27,7 +27,7 @@ - Resolves constraints that are defined in collision objects or subframes to robot links, because the former are not known to the planner. + Obsolete. Resolved constraints that are defined w.r.t. collision objects or subframes. diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/chomp_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/chomp_planning_pipeline.launch.xml index 22f2ed74b9..6ba98e9069 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/chomp_planning_pipeline.launch.xml +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/chomp_planning_pipeline.launch.xml @@ -5,7 +5,6 @@