Cleanup planning request adapter interface (#2266)

* Use default arguments instead of additional functions * Use generate param lib for default plan request adapters * Small cleanup of ResolveConstraintFrames * Remove dublicate yaml file entry * Move list_planning_adapter_plugins into own directory * Apply suggestions from code review Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> * Fix copy& paste error * Update parameter descriptions Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> * Apply suggestions from code review Co-authored-by: Kyle Cesare <kcesare@gmail.com> * EMPTY_PATH_INDEX_VECTOR -> empty_path_index_vector * Update parameter yaml * Make param listener unique * Fix build error * Use gt_eq instead of deprecated lower_bounds --------- Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Co-authored-by: Kyle Cesare <kcesare@gmail.com>
moveit · Jul 27, 2023 · 830ceda · 830ceda
1 parent cdf7a98
commit 830ceda
Show file tree

Hide file tree

Showing 14 changed files with 138 additions and 181 deletions.
diff --git a/...anning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h b/...anning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h
@@ -46,6 +46,10 @@
 /** \brief Generic interface to adapting motion planning requests */
 namespace planning_request_adapter
 {
+namespace
+{
+std::vector<std::size_t> empty_path_index_vector = {};
+}
 MOVEIT_CLASS_FORWARD(PlanningRequestAdapter);  // Defines PlanningRequestAdapterPtr, ConstPtr, WeakPtr... etc
 
 /** @brief Concept in MoveIt which can be used to modify the planning problem and resulting trajectory (pre-processing
@@ -60,8 +64,6 @@ class PlanningRequestAdapter
       std::function<bool(const planning_scene::PlanningSceneConstPtr&, const planning_interface::MotionPlanRequest&,
                          planning_interface::MotionPlanResponse&)>;
 
-  virtual ~PlanningRequestAdapter() = default;
-
   /** \brief Initialize parameters using the passed Node and parameter namespace.
    *  @param node Node instance used by the adapter
    *  @param parameter_namespace Parameter namespace for adapter
@@ -71,23 +73,7 @@ class PlanningRequestAdapter
   /** \brief Get a description of this adapter
    *  @return Returns a short string that identifies the planning request adapter
    */
-  virtual std::string getDescription() const
-  {
-    return "";
-  }
-
-  /** \brief Adapt the planning request if needed, call the planner
-      function \e planner and update the planning response if
-      needed.
-   *  @param planner Pointer to the planner used to solve the passed problem
-   *  @param planning_scene Representation of the environment for the planning
-   *  @param req Motion planning request with a set of constraints
-   *  @param res Reference to which the generated motion plan response is written to
-   *  @return True if response got generated correctly */
-  bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
-                    const planning_scene::PlanningSceneConstPtr& planning_scene,
-                    const planning_interface::MotionPlanRequest& req,
-                    planning_interface::MotionPlanResponse& res) const;
+  [[nodiscard]] virtual std::string getDescription() const = 0;
 
   /** \brief Adapt the planning request if needed, call the planner
       function \e planner and update the planning response if
@@ -103,10 +89,11 @@ class PlanningRequestAdapter
      current state itself appears to touch obstacles). This is helpful because the added states should not be considered
      invalid in all situations.
    *  @return True if response got generated correctly */
-  bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
-                    const planning_scene::PlanningSceneConstPtr& planning_scene,
-                    const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
-                    std::vector<std::size_t>& added_path_index) const;
+  [[nodiscard]] bool adaptAndPlan(const planning_interface::PlannerManagerPtr& 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;
 
   /** \brief Adapt the planning request if needed, call the planner
       function \e planner and update the planning response if
@@ -122,31 +109,10 @@ class PlanningRequestAdapter
         current state itself appears to touch obstacles). This is helpful because the added states should not be
    considered invalid in all situations.
    *  @return True if response got generated correctly */
-  virtual bool 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) 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;
-    }
-  }
+  [[nodiscard]] virtual bool
+  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;
 };
 
 /** \brief Apply a sequence of adapters to a motion plan */
@@ -157,17 +123,6 @@ class PlanningRequestAdapterChain
    *  @param adapter Adapter to be added */
   void addAdapter(const PlanningRequestAdapterConstPtr& adapter);
 
-  /** \brief Iterate through the chain and call all adapters and planners in the correct order
-   *  @param planner Pointer to the planner used to solve the passed problem
-   *  @param planning_scene Representation of the environment for the planning
-   *  @param req Motion planning request with a set of constraints
-   *  @param res Reference to which the generated motion plan response is written to
-   *  @return True if response got generated correctly */
-  bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
-                    const planning_scene::PlanningSceneConstPtr& planning_scene,
-                    const planning_interface::MotionPlanRequest& req,
-                    planning_interface::MotionPlanResponse& res) const;
-
   /** \brief Iterate through the chain and call all adapters and planners in the correct order
    *  @param planner Pointer to the planner used to solve the passed problem
    *  @param planning_scene Representation of the environment for the planning
@@ -178,10 +133,11 @@ class PlanningRequestAdapterChain
           current state itself appears to touch obstacles). This is helpful because the added states should not be
           considered invalid in all situations.
    *  @return True if response got generated correctly */
-  bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
-                    const planning_scene::PlanningSceneConstPtr& planning_scene,
-                    const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
-                    std::vector<std::size_t>& added_path_index) const;
+  [[nodiscard]] bool adaptAndPlan(const planning_interface::PlannerManagerPtr& 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;
 
 private:
   std::vector<PlanningRequestAdapterConstPtr> 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
@@ -98,29 +98,11 @@ 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
-{
-  std::vector<std::size_t> empty_added_path_index;
-  return adaptAndPlan(planner, planning_scene, req, res, empty_added_path_index);
-}
-
 void PlanningRequestAdapterChain::addAdapter(const PlanningRequestAdapterConstPtr& adapter)
 {
   adapters_.push_back(adapter);
 }
 
-bool 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<std::size_t> empty_added_path_index;
-  return adaptAndPlan(planner, planning_scene, req, res, empty_added_path_index);
-}
-
 bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
                                                const planning_scene::PlanningSceneConstPtr& planning_scene,
                                                const planning_interface::MotionPlanRequest& req,

diff --git a/moveit_ros/planning/CMakeLists.txt b/moveit_ros/planning/CMakeLists.txt
@@ -52,6 +52,7 @@ set(THIS_PACKAGE_LIBRARIES
   moveit_plan_execution
   moveit_planning_scene_monitor
   moveit_collision_plugin_loader
+  default_plan_request_adapter_parameters
   moveit_default_planning_request_adapter_plugins
   moveit_cpp
 )
@@ -79,19 +80,20 @@ include_directories(${THIS_PACKAGE_INCLUDE_DIRS})
 include_directories(SYSTEM
                     ${EIGEN3_INCLUDE_DIRS})
 
-add_subdirectory(rdf_loader)
 add_subdirectory(collision_plugin_loader)
-add_subdirectory(kinematics_plugin_loader)
-add_subdirectory(robot_model_loader)
 add_subdirectory(constraint_sampler_manager_loader)
+add_subdirectory(introspection)
+add_subdirectory(kinematics_plugin_loader)
+add_subdirectory(moveit_cpp)
+add_subdirectory(plan_execution)
+add_subdirectory(planning_components_tools)
 add_subdirectory(planning_pipeline)
 add_subdirectory(planning_pipeline_interfaces)
 add_subdirectory(planning_request_adapter_plugins)
 add_subdirectory(planning_scene_monitor)
-add_subdirectory(planning_components_tools)
+add_subdirectory(rdf_loader)
+add_subdirectory(robot_model_loader)
 add_subdirectory(trajectory_execution_manager)
-add_subdirectory(plan_execution)
-add_subdirectory(moveit_cpp)
 
 install(
   TARGETS ${THIS_PACKAGE_LIBRARIES}

diff --git a/moveit_ros/planning/introspection/CMakeLists.txt b/moveit_ros/planning/introspection/CMakeLists.txt
@@ -0,0 +1,10 @@
+add_executable(moveit_list_planning_adapter_plugins src/list_planning_adapter_plugins.cpp)
+ament_target_dependencies(moveit_list_planning_adapter_plugins
+  moveit_core
+  rclcpp
+  pluginlib
+)
+
+install(TARGETS moveit_list_planning_adapter_plugins
+  DESTINATION lib/${PROJECT_NAME}
+)
diff --git a/...ning_request_adapter_plugins/src/list.cpp → ...ion/src/list_planning_adapter_plugins.cpp b/...ning_request_adapter_plugins/src/list.cpp → ...ion/src/list_planning_adapter_plugins.cpp
@@ -32,7 +32,10 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
 
-/* Author: Ioan Sucan */
+/* Author: Ioan Sucan
+ * Desc:  Simple executable to list the loadable PlanningRequestAdapter. To use it simply run:
+ * `ros2 run moveit_ros_planning moveit_list_planning_adapter_plugins`
+ */
 
 #include <pluginlib/class_loader.hpp>
 #include <moveit/planning_request_adapter/planning_request_adapter.h>

diff --git a/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt b/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/default_plan_request_adapter_params.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
@@ -18,15 +20,3 @@ ament_target_dependencies(moveit_default_planning_request_adapter_plugins
   rclcpp
   pluginlib
 )
-
-add_executable(moveit_list_request_adapter_plugins src/list.cpp)
-ament_target_dependencies(moveit_list_request_adapter_plugins
-  Boost
-  moveit_core
-  rclcpp
-  pluginlib
-)
-
-install(TARGETS moveit_list_request_adapter_plugins
-  DESTINATION lib/${PROJECT_NAME}
-)
diff --git a/...os/planning/planning_request_adapter_plugins/res/default_plan_request_adapter_params.yaml b/...os/planning/planning_request_adapter_plugins/res/default_plan_request_adapter_params.yaml
@@ -0,0 +1,45 @@
+# This files contains the parameters for all of MoveIt's default PlanRequestAdapters
+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 adjacent 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: Minimum joint value change to consider two waypoints unique.",
+    default_value: 0.001,
+  }
+  start_state_max_dt: {
+    type: double,
+    description: "FixStartStateCollision/FixStartStateBounds: Maximum temporal distance of the fixed start state from the original state.",
+    default_value: 0.5,
+  }
+  jiggle_fraction: {
+    type: double,
+    description: "FixStartStateCollision: Joint value perturbation as a percentage of the total range of motion for the joint.",
+    default_value: 0.02,
+  }
+  max_sampling_attempts: {
+    type: int,
+    description: "FixStartStateCollision: Maximum number of attempts to re-sample a valid start state.",
+    default_value: 100,
+    validation: {
+        gt_eq<>: [ 0 ],
+    }
+  }
+  start_state_max_bounds_error: {
+    type: double,
+    description: "FixStartStateBounds: Maximum allowable error outside joint bounds for the starting configuration.",
+    default_value: 0.05,
+  }
+  default_workspace_bounds: {
+    type: double,
+    description: "FixWorkspaceBounds: Default workspace bounds representing a cube around the robot's origin whose edge length this parameter defines.",
+    default_value: 10.0,
+  }
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
@@ -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
   {
   }

diff --git a/...t_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp b/...t_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp
@@ -38,6 +38,8 @@
 #include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
 #include <class_loader/class_loader.hpp>
 
+#include <default_plan_request_adapter_parameters.hpp>
+
 namespace default_planner_request_adapters
 {
 using namespace trajectory_processing;
@@ -48,15 +50,10 @@ 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);
-    resample_dt_ = getParam(node, LOGGER, parameter_namespace, "resample_dt", 0.1);
-    min_angle_change_ = getParam(node, LOGGER, parameter_namespace, "min_angle_change", 0.001);
+    param_listener_ =
+        std::make_unique<default_plan_request_adapter_parameters::ParamListener>(node, parameter_namespace);
   }
 
   std::string getDescription() const override
@@ -72,7 +69,8 @@ class AddTimeOptimalParameterization : public planning_request_adapter::Planning
     if (result && res.trajectory)
     {
       RCLCPP_DEBUG(LOGGER, " Running '%s'", getDescription().c_str());
-      TimeOptimalTrajectoryGeneration totg(path_tolerance_, resample_dt_, min_angle_change_);
+      const auto params = param_listener_->get_params();
+      TimeOptimalTrajectoryGeneration totg(params.path_tolerance, params.resample_dt, params.min_angle_change);
       if (!totg.computeTimeStamps(*res.trajectory, req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor))
       {
         RCLCPP_WARN(LOGGER, " Time parametrization for the solution path failed.");
@@ -84,9 +82,7 @@ class AddTimeOptimalParameterization : public planning_request_adapter::Planning
   }
 
 protected:
-  double path_tolerance_;
-  double resample_dt_;
-  double min_angle_change_;
+  std::unique_ptr<default_plan_request_adapter_parameters::ParamListener> param_listener_;
 };
 
 }  // namespace default_planner_request_adapters