Use generate parameter library for planning pipeline parameters

moveit · Aug 16, 2023 · 6332f31 · 6332f31
1 parent b6f9f78
commit 6332f31
Show file tree

Hide file tree

Showing 7 changed files with 32 additions and 44 deletions.
diff --git 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
@@ -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 $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>)

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
@@ -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<rclcpp::Node>& 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<rclcpp::Node>& 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
@@ -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
@@ -42,16 +42,16 @@
 #include <fmt/format.h>
 #include <sstream>
 
+#include <planning_pipeline_parameters.hpp>
+
 namespace
 {
 const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros_planning.planning_pipeline");
 }  // namespace
 
 planning_pipeline::PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model,
                                                       const std::shared_ptr<rclcpp::Node>& 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<char> sep(" ");
-    boost::tokenizer<boost::char_separator<char>> tok(adapters, sep);
+    boost::tokenizer<boost::char_separator<char>> tok(params.request_adapters, sep);
     for (boost::tokenizer<boost::char_separator<char>>::iterator beg = tok.begin(); beg != tok.end(); ++beg)
     {
       adapter_plugin_names_.push_back(*beg);

diff --git a/...e_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp b/...e_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp
@@ -111,8 +111,6 @@ const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipe
 std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr>
 createPlanningPipelineMap(const std::vector<std::string>& 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
@@ -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<std::string, planning_pipeline::PlanningPipelinePtr>
 createPlanningPipelineMap(const std::vector<std::string>& 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<std::string, planning_pipeline::PlanningPipelinePtr> planning_pipelines;
   for (const auto& planning_pipeline_name : pipeline_names)
@@ -173,10 +173,8 @@ createPlanningPipelineMap(const std::vector<std::string>& pipeline_names,
     }
 
     // Create planning pipeline
-    planning_pipeline::PlanningPipelinePtr pipeline =
-        std::make_shared<planning_pipeline::PlanningPipeline>(robot_model, node,
-                                                              parameter_namespace + planning_pipeline_name,
-                                                              planning_plugin_param_name, adapter_plugins_param_name);
+    planning_pipeline::PlanningPipelinePtr pipeline = std::make_shared<planning_pipeline::PlanningPipeline>(
+        robot_model, node, parameter_namespace + planning_pipeline_name);
 
     if (!pipeline->getPlannerManager())
     {