WIP Use generate param lib for default plan request adapters

moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h

@@ -113,27 +113,6 @@ class PlanningRequestAdapter
   adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene,
                const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
                std::vector<std::size_t>& added_path_index = EMPTY_PATH_INDEX_VECTOR) const = 0;
-
-protected:
-  /** \brief Helper param for getting a parameter using a namespace **/
-  template <typename T>
-  T getParam(const rclcpp::Node::SharedPtr& node, const rclcpp::Logger& logger, const std::string& parameter_namespace,
-             const std::string& parameter_name, T default_value = {}) const
-  {
-    std::string full_name = parameter_namespace.empty() ? parameter_name : parameter_namespace + "." + parameter_name;
-    T value;
-    if (!node->get_parameter(full_name, value))
-    {
-      RCLCPP_INFO(logger, "Param '%s' was not set. Using default value: %s", full_name.c_str(),
-                  std::to_string(default_value).c_str());
-      return default_value;
-    }
-    else
-    {
-      RCLCPP_INFO(logger, "Param '%s' was set to %s", full_name.c_str(), std::to_string(value).c_str());
-      return value;
-    }
-  }
 };
 
 /** \brief Apply a sequence of adapters to a motion plan */

moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt

@@ -1,4 +1,6 @@
-set(SOURCE_FILES
+generate_parameter_library(default_plan_request_adapter_parameters res/plan_request_adapter_param.yaml)
+
+add_library(moveit_default_planning_request_adapter_plugins SHARED
   src/empty.cpp
   src/fix_start_state_bounds.cpp
   src/fix_start_state_collision.cpp
@@ -9,7 +11,7 @@ set(SOURCE_FILES
   src/resolve_constraint_frames.cpp
 )
 
-add_library(moveit_default_planning_request_adapter_plugins SHARED ${SOURCE_FILES})
+target_link_libraries(moveit_default_planning_request_adapter_plugins default_plan_request_adapter_parameters)
 
 set_target_properties(moveit_default_planning_request_adapter_plugins PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 ament_target_dependencies(moveit_default_planning_request_adapter_plugins
@@ -27,6 +29,6 @@ ament_target_dependencies(moveit_list_request_adapter_plugins
   pluginlib
 )
 
-install(TARGETS moveit_list_request_adapter_plugins
+install(TARGETS moveit_list_request_adapter_plugins default_plan_request_adapter_parameters
   DESTINATION lib/${PROJECT_NAME}
 )

moveit_ros/planning/planning_request_adapter_plugins/res/plan_request_adapter_param.yaml

@@ -0,0 +1,46 @@
+default_plan_request_adapter_parameters:
+  path_tolerance: {
+    type: double,
+    description: "AddTimeOptimalParameterization: Tolerance per joint in which the time parameterization is allowed to deviate from the original path.",
+    default_value: 0.1,
+  }
+  resample_dt: {
+    type: double,
+    description: "AddTimeOptimalParameterization:Time steps between two waypoints of the parameterized trajectory. The trajectory is re-sampled from the original path.",
+    default_value: 0.1,
+  }
+  min_angle_change: {
+    type: double,
+    description: "AddTimeOptimalParameterization: ?",
+    default_value: 0.001,
+  }
+  start_state_max_dt: {
+    type: double,
+    description: "FixStartStateCollision: Maximum temporal distance of the fixed start state from the original state.",
+    default_value: 0.5,
+  }
+  jiggle_fraction: {
+    type: double,
+    description: "FixStartStateCollision: ?",
+    default_value: 0.02,
+  }
+  max_sampling_attempts: {
+    type: double,
+    description: "FixStartStateCollision: Maximum number of attempts to re-sample a valid start state.",
+    default_value: 100,
+  }
+  start_state_max_bounds_error: {
+    type: double,
+    description: "FixStartStateBounds: ?",
+    default_value: 0.05,
+  }
+  start_state_max_dt: {
+    type: double,
+    description: "FixStartStateBounds: ?",
+    default_value: 0.5,
+  }
+  default_workspace_bounds: {
+    type: double,
+    description: "FixWorkspaceBounds: Default workspace bounds representing a 10m x 10m x 10m box around the robot's origin.",
+    default_value: 10.0,
+  }

moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp

@@ -52,10 +52,6 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.add_traj_smo
 class AddRuckigTrajectorySmoothing : public planning_request_adapter::PlanningRequestAdapter
 {
 public:
-  AddRuckigTrajectorySmoothing() : planning_request_adapter::PlanningRequestAdapter()
-  {
-  }
-
   void initialize(const rclcpp::Node::SharedPtr& /* unused */, const std::string& /* unused */) override
   {
   }

moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp

@@ -48,10 +48,6 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.add_time_opt
 class AddTimeOptimalParameterization : public planning_request_adapter::PlanningRequestAdapter
 {
 public:
-  AddTimeOptimalParameterization() : planning_request_adapter::PlanningRequestAdapter()
-  {
-  }
-
   void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override
   {
     path_tolerance_ = getParam(node, LOGGER, parameter_namespace, "path_tolerance", 0.1);

moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp

@@ -64,6 +64,9 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap
   static const std::string BOUNDS_PARAM_NAME;
   static const std::string DT_PARAM_NAME;
 
+  const std::string FixStartStateBounds::BOUNDS_PARAM_NAME = "start_state_max_bounds_error";
+  const std::string FixStartStateBounds::DT_PARAM_NAME = "start_state_max_dt";
+
   void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override
   {
     node_ = node;
@@ -208,9 +211,6 @@ class FixStartStateBounds : public planning_request_adapter::PlanningRequestAdap
   double bounds_dist_;
   double max_dt_offset_;
 };
-
-const std::string FixStartStateBounds::BOUNDS_PARAM_NAME = "start_state_max_bounds_error";
-const std::string FixStartStateBounds::DT_PARAM_NAME = "start_state_max_dt";
 }  // namespace default_planner_request_adapters
 
 CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::FixStartStateBounds,

moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp

@@ -62,6 +62,10 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA
   static const std::string JIGGLE_PARAM_NAME;
   static const std::string ATTEMPTS_PARAM_NAME;
 
+  const std::string FixStartStateCollision::DT_PARAM_NAME = "start_state_max_dt";
+  const std::string FixStartStateCollision::JIGGLE_PARAM_NAME = "jiggle_fraction";
+  const std::string FixStartStateCollision::ATTEMPTS_PARAM_NAME = "max_sampling_attempts";
+
   void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override
   {
     node_ = node;
@@ -189,10 +193,6 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA
   double jiggle_fraction_;
   int sampling_attempts_;
 };
-
-const std::string FixStartStateCollision::DT_PARAM_NAME = "start_state_max_dt";
-const std::string FixStartStateCollision::JIGGLE_PARAM_NAME = "jiggle_fraction";
-const std::string FixStartStateCollision::ATTEMPTS_PARAM_NAME = "max_sampling_attempts";
 }  // namespace default_planner_request_adapters
 
 CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::FixStartStateCollision,

moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_path_constraints.cpp

@@ -59,10 +59,6 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.fix_start_st
 class FixStartStatePathConstraints : public planning_request_adapter::PlanningRequestAdapter
 {
 public:
-  FixStartStatePathConstraints() : planning_request_adapter::PlanningRequestAdapter()
-  {
-  }
-
   void initialize(const rclcpp::Node::SharedPtr& /* node */, const std::string& /* parameter_namespace */) override
   {
   }

moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp

@@ -42,7 +42,8 @@
 #include <rclcpp/logger.hpp>
 #include <rclcpp/logging.hpp>
 #include <rclcpp/node.hpp>
-#include <rclcpp/parameter_value.hpp>
+
+#include <default_plan_request_adapter_parameters.hpp>
 
 namespace default_planner_request_adapters
 {
@@ -53,12 +54,13 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.fix_workspac
 class FixWorkspaceBounds : public planning_request_adapter::PlanningRequestAdapter
 {
 public:
-  static const std::string WBOUNDS_PARAM_NAME;
-
   void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override
   {
     node_ = node;
-    workspace_extent_ = getParam(node_, LOGGER, parameter_namespace, WBOUNDS_PARAM_NAME, 10.0);
+    const auto param_listener_ =
+        std::make_shared<default_plan_request_adapter_parameters::ParamListener>(node, parameter_namespace);
+    const auto params = param_listener_->get_params();
+    workspace_extent_ = params.default_workspace_bounds;
     workspace_extent_ /= 2.0;
   }
 
@@ -94,8 +96,6 @@ class FixWorkspaceBounds : public planning_request_adapter::PlanningRequestAdapt
   rclcpp::Node::SharedPtr node_;
   double workspace_extent_;
 };
-
-const std::string FixWorkspaceBounds::WBOUNDS_PARAM_NAME = "default_workspace_bounds";
 }  // namespace default_planner_request_adapters
 
 CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::FixWorkspaceBounds,