From 66a64b4a72b6ddef1af2329f20ed8162554d5bcb Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 31 Oct 2022 11:17:24 -0500 Subject: [PATCH] Planning request adapters: short-circuit if failure, return code rather than bool (#1605) * Return code rather than bool * Remove all debug prints * Small fixup * Minor cleanup of comment and error handling * void return from PlannerFn * Control reaches end of non-void function * Use a MoveItErrorCode cast * More efficient callAdapter() * More MoveItErrorCode * CI fixup attempt --- .github/workflows/ci.yaml | 2 +- .../planning_request_adapter.h | 50 +++++++------- .../src/planning_request_adapter.cpp | 67 ++++++++++--------- .../src/chomp_optimizer_adapter.cpp | 21 ++++-- .../src/planning_pipeline.cpp | 3 +- .../add_iterative_spline_parameterization.cpp | 16 +++-- .../src/add_ruckig_traj_smoothing.cpp | 16 +++-- .../src/add_time_optimal_parameterization.cpp | 16 +++-- .../src/add_time_parameterization.cpp | 16 +++-- .../src/empty.cpp | 8 ++- .../src/fix_start_state_bounds.cpp | 18 +++-- .../src/fix_start_state_collision.cpp | 20 +++--- .../src/fix_start_state_path_constraints.cpp | 44 ++++++------ .../src/fix_workspace_bounds.cpp | 12 ++-- .../src/resolve_constraint_frames.cpp | 10 ++- 15 files changed, 183 insertions(+), 136 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 2d01d7f6c0..09e2d13a7b 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -37,7 +37,7 @@ jobs: AFTER_SETUP_UPSTREAM_WORKSPACE: vcs pull $BASEDIR/upstream_ws/src # Uninstall binaries that are duplicated in the .repos file # TODO(andyz): remove this once a sync containing 35b93c8 happens - AFTER_SETUP_UPSTREAM_WORKSPACE_EMBED: sudo apt remove ros-${{ matrix.env.ROS_DISTRO }}-moveit-msgs -y + AFTER_SETUP_UPSTREAM_WORKSPACE_EMBED: sudo apt remove -y ros-${{ matrix.env.ROS_DISTRO }}-moveit-msgs || true AFTER_SETUP_DOWNSTREAM_WORKSPACE: vcs pull $BASEDIR/downstream_ws/src # Clear the ccache stats before and log the stats after the build AFTER_SETUP_CCACHE: ccache --zero-stats --max-size=10.0G diff --git a/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h b/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h index 624370d98a..f2d2aaf8f1 100644 --- a/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h +++ b/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -51,9 +52,9 @@ MOVEIT_CLASS_FORWARD(PlanningRequestAdapter); // Defines PlanningRequestAdapter class PlanningRequestAdapter { public: - using PlannerFn = - std::function; + using PlannerFn = std::function; PlanningRequestAdapter() { @@ -73,25 +74,27 @@ class PlanningRequestAdapter return ""; } - bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const; + moveit::core::MoveItErrorCode adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const; - bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const; + moveit::core::MoveItErrorCode adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, + std::vector& added_path_index) const; /** \brief Adapt the planning request if needed, call the planner function \e planner and update the planning response if needed. If the response is changed, the index values of the states added without planning are added to \e added_path_index */ - virtual bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const = 0; + virtual moveit::core::MoveItErrorCode adaptAndPlan(const PlannerFn& planner, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, + std::vector& added_path_index) const = 0; protected: /** \brief Helper param for getting a parameter using a namespace **/ @@ -128,15 +131,16 @@ class PlanningRequestAdapterChain adapters_.push_back(adapter); } - bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const; + moveit::core::MoveItErrorCode adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) const; - bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const; + moveit::core::MoveItErrorCode adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, + std::vector& added_path_index) const; private: std::vector adapters_; diff --git a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp b/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp index 8a456d3d0d..508c67b81e 100644 --- a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp +++ b/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp @@ -45,22 +45,30 @@ rclcpp::Logger LOGGER = rclcpp::get_logger("moveit").get_child("planning_request namespace { -bool callPlannerInterfaceSolve(const planning_interface::PlannerManager& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) +moveit::core::MoveItErrorCode callPlannerInterfaceSolve(const planning_interface::PlannerManager& planner, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res) { planning_interface::PlanningContextPtr context = planner.getPlanningContext(planning_scene, req, res.error_code_); if (context) - return context->solve(res); + { + // TODO(andyz): consider returning a moveit::core::MoveItErrorCode from context->solve() + bool result = context->solve(res); + return result ? moveit::core::MoveItErrorCode::SUCCESS : moveit::core::MoveItErrorCode::FAILURE; + } else - return false; + { + return moveit::core::MoveItErrorCode::FAILURE; + } } -bool callAdapter(const PlanningRequestAdapter& adapter, const PlanningRequestAdapter::PlannerFn& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) +moveit::core::MoveItErrorCode callAdapter(const PlanningRequestAdapter& adapter, + const PlanningRequestAdapter::PlannerFn& planner, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, + std::vector& added_path_index) { try { @@ -77,11 +85,10 @@ bool callAdapter(const PlanningRequestAdapter& adapter, const PlanningRequestAda } // namespace -bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const +moveit::core::MoveItErrorCode PlanningRequestAdapter::adaptAndPlan( + const planning_interface::PlannerManagerPtr& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, + std::vector& added_path_index) const { return adaptAndPlan( [&planner](const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, @@ -91,29 +98,26 @@ bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManag planning_scene, req, res, added_path_index); } -bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const +moveit::core::MoveItErrorCode PlanningRequestAdapter::adaptAndPlan( + const planning_interface::PlannerManagerPtr& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res) const { std::vector dummy; return adaptAndPlan(planner, planning_scene, req, res, dummy); } -bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const +moveit::core::MoveItErrorCode PlanningRequestAdapterChain::adaptAndPlan( + const planning_interface::PlannerManagerPtr& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res) const { std::vector dummy; return adaptAndPlan(planner, planning_scene, req, res, dummy); } -bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const +moveit::core::MoveItErrorCode PlanningRequestAdapterChain::adaptAndPlan( + const planning_interface::PlannerManagerPtr& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, + std::vector& added_path_index) const { // if there are no adapters, run the planner directly if (adapters_.empty()) @@ -140,15 +144,17 @@ bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::Planner fn = [&adapter = *adapters_[i], fn, &added_path_index = added_path_index_each[i]]( const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res) { + // Abort pipeline and return in case of failure return callAdapter(adapter, fn, scene, req, res, added_path_index); }; } - bool result = fn(planning_scene, req, res); + moveit::core::MoveItErrorCode moveit_code = fn(planning_scene, req, res); added_path_index.clear(); // merge the index values from each adapter for (std::vector& added_states_by_each_adapter : added_path_index_each) + { for (std::size_t& added_index : added_states_by_each_adapter) { for (std::size_t& index_in_path : added_path_index) @@ -156,8 +162,9 @@ bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::Planner index_in_path++; added_path_index.push_back(added_index); } + } std::sort(added_path_index.begin(), added_path_index.end()); - return result; + return moveit_code; } } diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp b/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp index 0c197d83a4..6723dfec9a 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp +++ b/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp @@ -172,16 +172,20 @@ class OptimizerAdapter : public planning_request_adapter::PlanningRequestAdapter return "CHOMP Optimizer"; } - bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& ps, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& /*added_path_index*/) const override + moveit::core::MoveItErrorCode adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& ps, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, + std::vector& /*added_path_index*/) const override { RCLCPP_DEBUG(LOGGER, "CHOMP: adaptAndPlan ..."); // following call to planner() calls the OMPL planner and stores the trajectory inside the MotionPlanResponse res // variable which is then used by CHOMP for optimization of the computed trajectory - if (!planner(ps, req, res)) - return false; + moveit::core::MoveItErrorCode moveit_code = planner(ps, req, res); + if (!bool(moveit_code)) + { + return moveit_code; + } // create a hybrid collision detector to set the collision checker as hybrid collision_detection::CollisionDetectorAllocatorPtr hybrid_cd( @@ -202,10 +206,15 @@ class OptimizerAdapter : public planning_request_adapter::PlanningRequestAdapter { res.trajectory_ = res_detailed.trajectory_[0]; res.planning_time_ += res_detailed.processing_time_[0]; + moveit_code = moveit::core::MoveItErrorCode::FAILURE; + } + else + { + moveit_code = moveit::core::MoveItErrorCode::SUCCESS; } res.error_code_ = res_detailed.error_code_; - return planning_success; + return moveit_code; } private: diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index a33a8f0f34..3fe5ea1bce 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -258,7 +258,8 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla { if (adapter_chain_) { - solved = adapter_chain_->adaptAndPlan(planner_instance_, planning_scene, req, res, adapter_added_state_index); + solved = + bool(adapter_chain_->adaptAndPlan(planner_instance_, planning_scene, req, res, adapter_added_state_index)); if (!adapter_added_state_index.empty()) { std::stringstream ss; diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/add_iterative_spline_parameterization.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/add_iterative_spline_parameterization.cpp index de4df4abd3..2006981963 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/add_iterative_spline_parameterization.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/add_iterative_spline_parameterization.cpp @@ -59,23 +59,25 @@ class AddIterativeSplineParameterization : public planning_request_adapter::Plan return "Add Time Parameterization"; } - bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& /*added_path_index*/) const override + moveit::core::MoveItErrorCode adaptAndPlan(const PlannerFn& planner, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, + std::vector& /*added_path_index*/) const override { - bool result = planner(planning_scene, req, res); - if (result && res.trajectory_) + moveit::core::MoveItErrorCode moveit_code = planner(planning_scene, req, res); + if (bool(moveit_code) && res.trajectory_) { RCLCPP_DEBUG(LOGGER, "Running '%s'", getDescription().c_str()); if (!time_param_.computeTimeStamps(*res.trajectory_, req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor)) { RCLCPP_WARN(LOGGER, "Time parametrization for the solution path failed."); - result = false; + moveit_code = moveit::core::MoveItErrorCode::FAILURE; } } - return result; + return moveit_code; } private: diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp index b8f8f5fc69..013899e689 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp @@ -60,21 +60,23 @@ class AddRuckigTrajectorySmoothing : public planning_request_adapter::PlanningRe return "Add Ruckig trajectory smoothing."; } - bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& /*added_path_index*/) const override + moveit::core::MoveItErrorCode adaptAndPlan(const PlannerFn& planner, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, + std::vector& /*added_path_index*/) const override { - bool result = planner(planning_scene, req, res); - if (result && res.trajectory_) + moveit::core::MoveItErrorCode moveit_code = planner(planning_scene, req, res); + if (bool(moveit_code) && res.trajectory_) { if (!smoother_.applySmoothing(*res.trajectory_, req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor)) { - result = false; + moveit_code = moveit::core::MoveItErrorCode::FAILURE; } } - return result; + return moveit_code; } private: diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp index 8e07d2625a..0cd7a056d6 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp @@ -64,12 +64,14 @@ class AddTimeOptimalParameterization : public planning_request_adapter::Planning return "Add Time Optimal Parameterization"; } - bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& /*added_path_index*/) const override + moveit::core::MoveItErrorCode adaptAndPlan(const PlannerFn& planner, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, + std::vector& /*added_path_index*/) const override { - bool result = planner(planning_scene, req, res); - if (result && res.trajectory_) + moveit::core::MoveItErrorCode moveit_code = planner(planning_scene, req, res); + if (bool(moveit_code) && res.trajectory_) { RCLCPP_DEBUG(LOGGER, " Running '%s'", getDescription().c_str()); TimeOptimalTrajectoryGeneration totg(path_tolerance_, resample_dt_, min_angle_change_); @@ -77,11 +79,11 @@ class AddTimeOptimalParameterization : public planning_request_adapter::Planning req.max_acceleration_scaling_factor)) { RCLCPP_WARN(LOGGER, " Time parametrization for the solution path failed."); - result = false; + moveit_code = moveit::core::MoveItErrorCode::FAILURE; } } - return result; + return moveit_code; } protected: diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_parameterization.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_parameterization.cpp index 387caafdc3..c691ed04c3 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_parameterization.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_parameterization.cpp @@ -58,23 +58,25 @@ class AddTimeParameterization : public planning_request_adapter::PlanningRequest return "Add Time Parameterization"; } - bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& /*added_path_index*/) const override + moveit::core::MoveItErrorCode adaptAndPlan(const PlannerFn& planner, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, + std::vector& /*added_path_index*/) const override { - bool result = planner(planning_scene, req, res); - if (result && res.trajectory_) + moveit::core::MoveItErrorCode moveit_code = planner(planning_scene, req, res); + if (bool(moveit_code) && res.trajectory_) { RCLCPP_DEBUG(LOGGER, "Running '%s'", getDescription().c_str()); if (!time_param_.computeTimeStamps(*res.trajectory_, req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor)) { RCLCPP_WARN(LOGGER, "Time parametrization for the solution path failed."); - result = false; + moveit_code = moveit::core::MoveItErrorCode::FAILURE; } } - return result; + return moveit_code; } private: diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/empty.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/empty.cpp index 1bc73d7c21..edf74bc290 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/empty.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/empty.cpp @@ -47,9 +47,11 @@ class Empty : public planning_request_adapter::PlanningRequestAdapter return "No Op"; } - bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& /*added_path_index*/) const override + moveit::core::MoveItErrorCode adaptAndPlan(const PlannerFn& planner, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, + std::vector& /*added_path_index*/) const override { return planner(planning_scene, req, res); } diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp index 2f1655cdf1..e4f6437487 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp @@ -66,9 +66,11 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap return "Fix Start State Bounds"; } - bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const override + moveit::core::MoveItErrorCode adaptAndPlan(const PlannerFn& planner, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, + std::vector& added_path_index) const override { RCLCPP_DEBUG(LOGGER, "Running '%s'", getDescription().c_str()); @@ -165,16 +167,18 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap } } - bool solved; + moveit::core::MoveItErrorCode moveit_code; // if we made any changes, use them if (change_req) { planning_interface::MotionPlanRequest req2 = req; moveit::core::robotStateToRobotStateMsg(start_state, req2.start_state); - solved = planner(planning_scene, req2, res); + moveit_code = planner(planning_scene, req2, res); } else - solved = planner(planning_scene, req, res); + { + moveit_code = planner(planning_scene, req, res); + } // re-add the prefix state, if it was constructed if (prefix_state && res.trajectory_ && !res.trajectory_->empty()) @@ -190,7 +194,7 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap added_path_index.push_back(0); } - return solved; + return moveit_code; } private: diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp index d9e4b103a6..c9516937f0 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp @@ -72,9 +72,11 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA return "Fix Start State In Collision"; } - bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const override + moveit::core::MoveItErrorCode adaptAndPlan(const PlannerFn& planner, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, + std::vector& added_path_index) const override { RCLCPP_DEBUG(LOGGER, "Running '%s'", getDescription().c_str()); @@ -132,8 +134,8 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA { planning_interface::MotionPlanRequest req2 = req; moveit::core::robotStateToRobotStateMsg(start_state, req2.start_state); - bool solved = planner(planning_scene, req2, res); - if (solved && !res.trajectory_->empty()) + moveit::core::MoveItErrorCode moveit_code = planner(planning_scene, req2, res); + if (bool(moveit_code) && !res.trajectory_->empty()) { // heuristically decide a duration offset for the trajectory (induced by the additional point added as a // prefix to the computed trajectory) @@ -145,15 +147,17 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA added_index++; added_path_index.push_back(0); } - return solved; + return moveit_code; } else { RCLCPP_WARN(LOGGER, "Unable to find a valid state nearby the start state (using jiggle fraction of %lf and %u sampling " - "attempts). Passing the original planning request to the planner.", + "attempts).", jiggle_fraction_, sampling_attempts_); - return planner(planning_scene, req, res); + res.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::START_STATE_IN_COLLISION; + return moveit::core::MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::START_STATE_IN_COLLISION); + ; } } else diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp index b555c86d41..f552d6d51c 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp @@ -63,9 +63,11 @@ class FixStartStatePathConstraints : public planning_request_adapter::PlanningRe return "Fix Start State Path Constraints"; } - bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& added_path_index) const override + moveit::core::MoveItErrorCode adaptAndPlan(const PlannerFn& planner, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, + std::vector& added_path_index) const override { RCLCPP_DEBUG(LOGGER, "Running '%s'", getDescription().c_str()); @@ -90,20 +92,22 @@ class FixStartStatePathConstraints : public planning_request_adapter::PlanningRe // index information from that call std::vector added_path_index_temp; added_path_index_temp.swap(added_path_index); - bool solved1 = planner(planning_scene, req2, res2); + moveit::core::MoveItErrorCode solved1 = planner(planning_scene, req2, res2); added_path_index_temp.swap(added_path_index); - if (solved1) + moveit::core::MoveItErrorCode solved2(moveit_msgs::msg::MoveItErrorCodes::FAILURE); + if (bool(solved1)) { planning_interface::MotionPlanRequest req3 = req; - RCLCPP_INFO(LOGGER, "Planned to path constraints. Resuming original planning request."); + RCLCPP_INFO(LOGGER, "The start state was modified to match path constraints. Now resuming the original " + "planning request."); // extract the last state of the computed motion plan and set it as the new start state moveit::core::robotStateToRobotStateMsg(res2.trajectory_->getLastWayPoint(), req3.start_state); - bool solved2 = planner(planning_scene, req3, res); + solved2 = planner(planning_scene, req3, res); res.planning_time_ += res2.planning_time_; - if (solved2) + if (bool(solved2)) { // since we add a prefix, we need to correct any existing index positions for (std::size_t& added_index : added_path_index) @@ -116,24 +120,22 @@ class FixStartStatePathConstraints : public planning_request_adapter::PlanningRe // we need to append the solution paths. res2.trajectory_->append(*res.trajectory_, 0.0); res2.trajectory_->swap(*res.trajectory_); - return true; + return moveit_msgs::msg::MoveItErrorCodes::SUCCESS; } - else - return false; } - else + + if (!bool(solved1) || !bool(solved2)) { - RCLCPP_WARN(LOGGER, "Unable to plan to path constraints. Running usual motion plan."); - bool result = planner(planning_scene, req, res); - res.planning_time_ += res2.planning_time_; - return result; + RCLCPP_WARN(LOGGER, "Unable to meet path constraints at the start."); + moveit::core::MoveItErrorCode moveit_code( + moveit_msgs::msg::MoveItErrorCodes::START_STATE_VIOLATES_PATH_CONSTRAINTS); + res.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::START_STATE_VIOLATES_PATH_CONSTRAINTS; + return moveit_code; } } - else - { - RCLCPP_DEBUG(LOGGER, "Path constraints are OK. Running usual motion plan."); - return planner(planning_scene, req, res); - } + + RCLCPP_DEBUG(LOGGER, "Path constraints are OK. Continuing without `fix_start_state_path_constraints`."); + return planner(planning_scene, req, res); } }; } // namespace default_planner_request_adapters diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp index 1e2d699cfc..899ef6c0d9 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp @@ -62,9 +62,11 @@ class FixWorkspaceBounds : public planning_request_adapter::PlanningRequestAdapt return "Fix Workspace Bounds"; } - bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& /*added_path_index*/) const override + moveit::core::MoveItErrorCode adaptAndPlan(const PlannerFn& planner, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, + std::vector& /*added_path_index*/) const override { RCLCPP_DEBUG(LOGGER, "Running '%s'", getDescription().c_str()); const moveit_msgs::msg::WorkspaceParameters& wparams = req.workspace_parameters; @@ -79,8 +81,8 @@ class FixWorkspaceBounds : public planning_request_adapter::PlanningRequestAdapt default_wp.max_corner.x = default_wp.max_corner.y = default_wp.max_corner.z = workspace_extent_; return planner(planning_scene, req2, res); } - else - return planner(planning_scene, req, res); + + return planner(planning_scene, req, res); } private: 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 b8c82c2b05..3ece0d8929 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 @@ -58,15 +58,19 @@ class ResolveConstraintFrames : public planning_request_adapter::PlanningRequest return "Resolve constraint frames to robot links"; } - bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& /*added_path_index*/) const override + moveit::core::MoveItErrorCode adaptAndPlan(const PlannerFn& planner, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, + std::vector& /*added_path_index*/) const override { RCLCPP_DEBUG(LOGGER, "Running '%s'", getDescription().c_str()); planning_interface::MotionPlanRequest modified = req; kinematic_constraints::resolveConstraintFrames(planning_scene->getCurrentState(), modified.path_constraints); for (moveit_msgs::msg::Constraints& constraint : modified.goal_constraints) + { kinematic_constraints::resolveConstraintFrames(planning_scene->getCurrentState(), constraint); + } return planner(planning_scene, modified, res); } };