From 9be2ab3172e6c62195d43bd164dda812cb3f03ea Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Fri, 31 Mar 2023 17:55:58 +0200 Subject: [PATCH] Add API to set parallel planning callbacks and deprecate functions --- .../solvers/pipeline_planner.h | 38 ++++++++++++++++--- core/src/solvers/pipeline_planner.cpp | 29 ++++++++------ 2 files changed, 50 insertions(+), 17 deletions(-) diff --git a/core/include/moveit/task_constructor/solvers/pipeline_planner.h b/core/include/moveit/task_constructor/solvers/pipeline_planner.h index 414ba7fc9..57d3f5788 100644 --- a/core/include/moveit/task_constructor/solvers/pipeline_planner.h +++ b/core/include/moveit/task_constructor/solvers/pipeline_planner.h @@ -40,6 +40,8 @@ #include #include +#include +#include #include #include @@ -57,23 +59,34 @@ MOVEIT_CLASS_FORWARD(PipelinePlanner); class PipelinePlanner : public PlannerInterface { public: - // TODO: Deprecate + /** Simple Constructor to use only a single pipeline + * \param [in] node ROS 2 node + * \param [in] pipeline_name Name of the planning pipeline to be used. This is also the assumed namespace where the + * parameters of this pipeline can be found \param [in] planner_id Planner id to be used for planning + */ PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name = "ompl", const std::string& planner_id = ""); + [[deprecated("Deprecated: Use new constructor implementations.")]] // clang-format off + PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline){}; + /** \brief Constructor - * \param [in] node ROS 2 node handle + * \param [in] node ROS 2 node * \param [in] pipeline_id_planner_id_map A map containing pairs of planning pipeline name and planner plugin name * for the planning requests * \param [in] planning_pipelines Optional: A map with the pipeline names and initialized corresponding planning * pipelines + * \param [in] stopping_criterion_callback Callback function to stop parallel planning pipeline according to a user defined criteria + * \param [in] solution_selection_function Callback function to choose the best solution when multiple pipelines are used */ PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::unordered_map& pipeline_id_planner_id_map, const std::unordered_map planning_pipelines = - std::unordered_map()); + std::unordered_map(), + const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback = &moveit::planning_pipeline_interfaces::stopAtFirstSolution, + const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function = &moveit::planning_pipeline_interfaces::getShortestSolution); - // TODO: Deprecate + [[deprecated("Replaced with setPlannerId(pipeline_name, planner_id)")]] // clang-format off void setPlannerId(const std::string& /*planner*/) { /* Do nothing */ } @@ -84,9 +97,20 @@ class PipelinePlanner : public PlannerInterface */ bool setPlannerId(const std::string& pipeline_name, const std::string& planner_id); + /** \brief Set stopping criterion function for parallel planning + * \param [in] stopping_criterion_callback New stopping criterion function that will be used + */ + void setStoppingCriterionFunction(const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback); + + /** \brief Set solution selection function for parallel planning + * \param [in] solution_selection_function New solution selection that will be used + */ + void setSolutionSelectionFunction(const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function); + /** \brief This function is called when an MTC task that uses this solver is initialized. If no pipelines are * configured when this function is invoked, the planning pipelines named in the 'pipeline_id_planner_id_map' are - * initialized with the given robot model. \param [in] robot_model A robot model that is used to initialize the + * initialized with the given robot model. + * \param [in] robot_model A robot model that is used to initialize the * planning pipelines of this solver */ void init(const moveit::core::RobotModelConstPtr& robot_model) override; @@ -156,6 +180,10 @@ class PipelinePlanner : public PlannerInterface /** \brief Map of pipelines names and planning pipelines. This map is used to quickly search for a requested motion * planning pipeline when during plan(..) */ std::unordered_map planning_pipelines_; + + moveit::planning_pipeline_interfaces::StoppingCriterionFunction stopping_criterion_callback_; + moveit::planning_pipeline_interfaces::SolutionSelectionFunction solution_selection_function_; + }; } // namespace solvers } // namespace task_constructor diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index f81b522f0..f985d998d 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -40,9 +40,6 @@ #include #include #include -#include -#include -#include #include #include @@ -70,8 +67,13 @@ PipelinePlanner::PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std: PipelinePlanner::PipelinePlanner( const rclcpp::Node::SharedPtr& node, const std::unordered_map& pipeline_id_planner_id_map, - const std::unordered_map planning_pipelines) - : node_(node), pipeline_id_planner_id_map_(pipeline_id_planner_id_map) { + const std::unordered_map planning_pipelines, + const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback, + const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function) + : node_(node) + , pipeline_id_planner_id_map_(pipeline_id_planner_id_map) + , stopping_criterion_callback_(stopping_criterion_callback) + , solution_selection_function_(solution_selection_function) { // If the pipeline name - pipeline map is passed as constructor argument, use it. Otherwise, it will be created in // the init(..) function if (!planning_pipelines.empty()) { @@ -94,11 +96,6 @@ PipelinePlanner::PipelinePlanner( planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC); } -// PipelinePlanner::PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline) -// : PipelinePlanner(rclcpp::Node::SharedPtr()) { -// planning_pipelines_.at(0) = planning_pipeline; -//} - bool PipelinePlanner::setPlannerId(const std::string& pipeline_name, const std::string& planner_id) { // Only set ID if pipeline exists. It is not possible to create new pipelines with this command. if (pipeline_id_planner_id_map_.count(pipeline_name) > 0) { @@ -109,6 +106,15 @@ bool PipelinePlanner::setPlannerId(const std::string& pipeline_name, const std:: return false; } +void PipelinePlanner::setStoppingCriterionFunction( + const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_function) { + stopping_criterion_callback_ = stopping_criterion_function; +} +void PipelinePlanner::setSolutionSelectionFunction( + const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function) { + solution_selection_function_ = solution_selection_function; +} + void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) { // If no planning pipelines exist, create them based on the pipeline names provided in pipeline_id_planner_id_map_. // The assumption here is that all parameters required by the planning pipeline can be found in a namespace that @@ -200,8 +206,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning // planWithParallelPipelines will return a vector with the single best solution std::vector<::planning_interface::MotionPlanResponse> responses = moveit::planning_pipeline_interfaces::planWithParallelPipelines( - requests, planning_scene, planning_pipelines_, &moveit::planning_pipeline_interfaces::stopAtFirstSolution, - &moveit::planning_pipeline_interfaces::getShortestSolution); + requests, planning_scene, planning_pipelines_, stopping_criterion_callback_, solution_selection_function_); // If solutions exist and the first one is successful if (!responses.empty() && responses.at(0)) {