From 6332f31080ec2c2c911ec3ae144cd9601b505e32 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Sat, 5 Aug 2023 15:37:45 +0200 Subject: [PATCH] Use generate parameter library for planning pipeline parameters --- moveit_ros/planning/CMakeLists.txt | 1 + .../planning/planning_pipeline/CMakeLists.txt | 3 ++ .../planning_pipeline/planning_pipeline.h | 14 ++------- .../res/planning_pipeline_parameters.yaml | 11 +++++++ .../src/planning_pipeline.cpp | 31 +++++-------------- .../planning_pipeline_interfaces.hpp | 4 +-- .../src/planning_pipeline_interfaces.cpp | 12 +++---- 7 files changed, 32 insertions(+), 44 deletions(-) create mode 100644 moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml diff --git a/moveit_ros/planning/CMakeLists.txt b/moveit_ros/planning/CMakeLists.txt index 0553e6e2eb0..e0bf25cdc61 100644 --- a/moveit_ros/planning/CMakeLists.txt +++ b/moveit_ros/planning/CMakeLists.txt @@ -47,6 +47,7 @@ set(THIS_PACKAGE_LIBRARIES moveit_kinematics_plugin_loader moveit_robot_model_loader moveit_constraint_sampler_manager_loader + planning_pipeline_parameters moveit_planning_pipeline moveit_planning_pipeline_interfaces moveit_trajectory_execution_manager diff --git a/moveit_ros/planning/planning_pipeline/CMakeLists.txt b/moveit_ros/planning/planning_pipeline/CMakeLists.txt index 6be7790b25b..76c43223e4d 100644 --- a/moveit_ros/planning/planning_pipeline/CMakeLists.txt +++ b/moveit_ros/planning/planning_pipeline/CMakeLists.txt @@ -1,4 +1,7 @@ +generate_parameter_library(planning_pipeline_parameters res/planning_pipeline_parameters.yaml) + add_library(moveit_planning_pipeline SHARED src/planning_pipeline.cpp) +target_link_libraries(moveit_planning_pipeline planning_pipeline_parameters) include(GenerateExportHeader) generate_export_header(moveit_planning_pipeline) target_include_directories(moveit_planning_pipeline PUBLIC $) diff --git a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h index f563f9f2d03..468c75a835a 100644 --- a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h +++ b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h @@ -74,23 +74,15 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline /** \brief Given a robot model (\e model), a node handle (\e pipeline_nh), initialize the planning pipeline. \param model The robot model for which this pipeline is initialized. \param node The ROS node that should be used for reading parameters needed for configuration - \param the parameter namespace where the planner configurations are stored - \param planning_plugin_param_name The name of the ROS parameter under which the name of the planning plugin is - specified - \param adapter_plugins_param_name The name of the ROS parameter under which the names of the request adapter - plugins are specified (plugin names separated by space; order matters) + \param parameter_namespace parameter namespace where the planner configurations are stored */ PlanningPipeline(const moveit::core::RobotModelConstPtr& model, const std::shared_ptr& node, - const std::string& parameter_namespace, - const std::string& planning_plugin_param_name = "planning_plugin", - const std::string& adapter_plugins_param_name = "request_adapters"); + const std::string& parameter_namespace); /** \brief Given a robot model (\e model), a node handle (\e pipeline_nh), initialize the planning pipeline. \param model The robot model for which this pipeline is initialized. \param node The ROS node that should be used for reading parameters needed for configuration - \param the parameter namespace where the planner configurations are stored - \param planning_plugin_name The name of the planning plugin to load - \param adapter_plugins_names The names of the planning request adapter plugins to load + \param parameter_namespace parameter namespace where the planner configurations are stored */ PlanningPipeline(const moveit::core::RobotModelConstPtr& model, const std::shared_ptr& node, const std::string& parameter_namespace, const std::string& planning_plugin_name, diff --git a/moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml b/moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml new file mode 100644 index 00000000000..6b115762ea9 --- /dev/null +++ b/moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml @@ -0,0 +1,11 @@ +planning_pipeline_parameters: + planning_plugin: { + type: string, + description: "Name of the planner plugin used by the pipeline", + default_value: "UNKNOWN", + } + request_adapters: { + type: string, + description: "Names of the planning request adapter plugins (plugin names separated by space).", + default_value: "UNKNOWN", + } diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index 3bb94dc43bc..f51ec03d175 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -42,6 +42,8 @@ #include #include +#include + namespace { const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros_planning.planning_pipeline"); @@ -49,9 +51,7 @@ const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros_planning.planning_p planning_pipeline::PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model, const std::shared_ptr& node, - const std::string& parameter_namespace, - const std::string& planner_plugin_param_name, - const std::string& adapter_plugins_param_name) + const std::string& parameter_namespace) : active_{ false } , node_(node) , parameter_namespace_(parameter_namespace) @@ -60,28 +60,13 @@ planning_pipeline::PlanningPipeline::PlanningPipeline(const moveit::core::RobotM , robot_model_(model) , check_solution_paths_{ false } { - std::string planner_plugin_fullname = parameter_namespace_ + "." + planner_plugin_param_name; - if (parameter_namespace_.empty()) - { - planner_plugin_fullname = planner_plugin_param_name; - } - if (node_->has_parameter(planner_plugin_fullname)) - { - node_->get_parameter(planner_plugin_fullname, planner_plugin_name_); - } - - std::string adapter_plugins_fullname = parameter_namespace_ + "." + adapter_plugins_param_name; - if (parameter_namespace_.empty()) - { - adapter_plugins_fullname = adapter_plugins_param_name; - } - - std::string adapters; - if (node_->has_parameter(adapter_plugins_fullname)) + auto param_listener = planning_pipeline_parameters::ParamListener(node, parameter_namespace); + const auto params = param_listener.get_params(); + planner_plugin_name_ = params.planning_plugin; + if (!params.request_adapters.empty()) { - node_->get_parameter(adapter_plugins_fullname, adapters); boost::char_separator sep(" "); - boost::tokenizer> tok(adapters, sep); + boost::tokenizer> tok(params.request_adapters, sep); for (boost::tokenizer>::iterator beg = tok.begin(); beg != tok.end(); ++beg) { adapter_plugin_names_.push_back(*beg); diff --git a/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp index e4b0ec11e84..21e37633513 100644 --- a/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp +++ b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp @@ -111,8 +111,6 @@ const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipe std::unordered_map createPlanningPipelineMap(const std::vector& pipeline_names, const moveit::core::RobotModelConstPtr& robot_model, const rclcpp::Node::SharedPtr& node, - const std::string& parameter_namespace = std::string(), - const std::string& planning_plugin_param_name = "planning_plugin", - const std::string& adapter_plugins_param_name = "request_adapters"); + const std::string& parameter_namespace = std::string()); } // namespace planning_pipeline_interfaces } // namespace moveit diff --git a/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp b/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp index 2c06d6d9d59..6243e924ed6 100644 --- a/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp +++ b/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp @@ -58,7 +58,8 @@ planWithSinglePipeline(const ::planning_interface::MotionPlanRequest& motion_pla return motion_plan_response; } const planning_pipeline::PlanningPipelinePtr pipeline = it->second; - if(!pipeline->generatePlan(planning_scene, motion_plan_request, motion_plan_response)){ + if (!pipeline->generatePlan(planning_scene, motion_plan_request, motion_plan_response)) + { motion_plan_response.error_code = moveit::core::MoveItErrorCode::FAILURE; } return motion_plan_response; @@ -159,8 +160,7 @@ const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipe std::unordered_map createPlanningPipelineMap(const std::vector& pipeline_names, const moveit::core::RobotModelConstPtr& robot_model, const rclcpp::Node::SharedPtr& node, - const std::string& parameter_namespace, const std::string& planning_plugin_param_name, - const std::string& adapter_plugins_param_name) + const std::string& parameter_namespace) { std::unordered_map planning_pipelines; for (const auto& planning_pipeline_name : pipeline_names) @@ -173,10 +173,8 @@ createPlanningPipelineMap(const std::vector& pipeline_names, } // Create planning pipeline - planning_pipeline::PlanningPipelinePtr pipeline = - std::make_shared(robot_model, node, - parameter_namespace + planning_pipeline_name, - planning_plugin_param_name, adapter_plugins_param_name); + planning_pipeline::PlanningPipelinePtr pipeline = std::make_shared( + robot_model, node, parameter_namespace + planning_pipeline_name); if (!pipeline->getPlannerManager()) {