[Planning Pipeline Refactoring] #2 Enable chaining planners (#2457)

* Enable chaining multiple planners

MIGRATION.md

@@ -3,6 +3,13 @@
 API changes in MoveIt releases
 
 ## ROS Rolling
+- [10/2023] The planning pipeline now has a vector of planner plugins rather than a single one. Please update the planner plugin parameter e.g. like this:
+```diff
+- planning_plugin: ompl_interface/OMPLPlanner
++ planning_plugins:
++   - ompl_interface/OMPLPlanner
+```
+
 - [10/2023] Planning request adapters are now separated into PlanRequest (preprocessing) and PlanResponse (postprocessing) adapters. The adapters are configured with ROS parameter vectors (vector order corresponds to execution order). Please update your pipeline configurations for example like this:
 ```diff
 - request_adapters: >-

moveit_configs_utils/default_configs/chomp_planning.yaml

@@ -1,4 +1,5 @@
-planning_plugin: chomp_interface/CHOMPPlanner
+planning_plugins:
+  - chomp_interface/CHOMPPlanner
 enable_failure_recovery: true
 # The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline.
 request_adapters:

moveit_configs_utils/default_configs/ompl_planning.yaml

@@ -1,4 +1,5 @@
-planning_plugin: ompl_interface/OMPLPlanner
+planning_plugins:
+  - ompl_interface/OMPLPlanner
 # The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline.
 request_adapters:
   - default_planning_request_adapters/ResolveConstraintFrames

moveit_configs_utils/default_configs/pilz_industrial_motion_planner_planning.yaml

@@ -1,6 +1,14 @@
-planning_plugin: pilz_industrial_motion_planner/CommandPlanner
-# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline.
+planning_plugins:
+  - pilz_industrial_motion_planner/CommandPlanner
 default_planner_config: PTP
+request_adapters:
+  - default_planning_request_adapters/ResolveConstraintFrames
+  - default_planning_request_adapters/ValidateWorkspaceBounds
+  - default_planning_request_adapters/CheckStartStateBounds
+  - default_planning_request_adapters/CheckStartStateCollision
+response_adapters:
+  - default_planning_response_adapters/ValidateSolution
+  - default_planning_response_adapters/DisplayMotionPath
 capabilities: >-
     pilz_industrial_motion_planner/MoveGroupSequenceAction
     pilz_industrial_motion_planner/MoveGroupSequenceService

moveit_configs_utils/default_configs/stomp_planning.yaml

@@ -1,5 +1,5 @@
-planning_plugin: stomp_moveit/StompPlanner
-# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline.
+planning_plugins:
+  - stomp_moveit/StompPlanner
 request_adapters:
   - default_planning_request_adapters/ResolveConstraintFrames
   - default_planning_request_adapters/ValidateWorkspaceBounds

moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h

@@ -164,8 +164,8 @@ class PlannerManager
   virtual bool initialize(const moveit::core::RobotModelConstPtr& model, const rclcpp::Node::SharedPtr& node,
                           const std::string& parameter_namespace);
 
-  /// Get \brief a short string that identifies the planning interface
-  virtual std::string getDescription() const;
+  /// \brief Get a short string that identifies the planning interface.
+  virtual std::string getDescription() const = 0;
 
   /// \brief Get the names of the known planning algorithms (values that can be filled as planner_id in the planning
   /// request)

moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_pilz_industrial_motion_planner.test.py

@@ -18,8 +18,8 @@ def generate_test_description():
     # Load the context
     test_config = load_moveit_config()
 
-    planning_plugin = {
-        "planning_plugin": "pilz_industrial_motion_planner/CommandPlanner"
+    planning_plugins = {
+        "planning_plugins": ["pilz_industrial_motion_planner/CommandPlanner"]
     }
 
     # run test
@@ -29,7 +29,7 @@ def generate_test_description():
         name="unittest_pilz_industrial_motion_planner",
         parameters=[
             test_config.to_dict(),
-            planning_plugin,
+            planning_plugins,
         ],
         output="screen",
     )

moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp

@@ -74,8 +74,8 @@ class CommandPlannerTest : public testing::Test
     ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model";
 
     // Load planner name from node parameters
-    ASSERT_TRUE(node_->has_parameter("planning_plugin")) << "Could not find parameter 'planning_plugin'";
-    node_->get_parameter<std::string>("planning_plugin", planner_plugin_name_);
+    ASSERT_TRUE(node_->has_parameter("planning_plugins")) << "Could not find parameter 'planning_plugins'";
+    node_->get_parameter<std::vector<std::string>>("planning_plugins", planner_plugin_names_);
 
     // Load the plugin
     try
@@ -92,7 +92,7 @@ class CommandPlannerTest : public testing::Test
     // Create planner
     try
     {
-      planner_instance_.reset(planner_plugin_loader_->createUnmanagedInstance(planner_plugin_name_));
+      planner_instance_.reset(planner_plugin_loader_->createUnmanagedInstance(planner_plugin_names_.at(0)));
       ASSERT_TRUE(planner_instance_->initialize(robot_model_, node_, ""))
           << "Initializing the planner instance failed.";
     }
@@ -104,7 +104,7 @@ class CommandPlannerTest : public testing::Test
 
   void TearDown() override
   {
-    planner_plugin_loader_->unloadLibraryForClass(planner_plugin_name_);
+    planner_plugin_loader_->unloadLibraryForClass(planner_plugin_names_.at(0));
     robot_model_.reset();
   }
 
@@ -114,7 +114,7 @@ class CommandPlannerTest : public testing::Test
   moveit::core::RobotModelConstPtr robot_model_;
   std::unique_ptr<robot_model_loader::RobotModelLoader> rm_loader_;
 
-  std::string planner_plugin_name_;
+  std::vector<std::string> planner_plugin_names_;
   std::unique_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader_;
   planning_interface::PlannerManagerPtr planner_instance_;
 };

moveit_planners/stomp/src/stomp_moveit_planner_plugin.cpp

@@ -118,12 +118,6 @@ class StompPlannerManager : public PlannerManager
       RCLCPP_ERROR(LOGGER, "Invalid joint group '%s'", req.group_name.c_str());
       return false;
     }
-
-    if (!req.reference_trajectories.empty())
-    {
-      RCLCPP_WARN(LOGGER, "Ignoring reference trajectories - not implemented!");
-    }
-
     return true;
   }
 

moveit_planners/test_configs/prbt_moveit_config/launch/demo.launch.py

@@ -81,12 +81,20 @@ def generate_launch_description():
         "default_planning_pipeline": "ompl",
         "planning_pipelines": ["pilz", "ompl"],
         "pilz": {
-            "planning_plugin": "pilz_industrial_motion_planner/CommandPlanner",
-            "request_adapters": "",
+            "planning_plugins": ["pilz_industrial_motion_planner/CommandPlanner"],
         },
         "ompl": {
-            "planning_plugin": "ompl_interface/OMPLPlanner",
-            "request_adapters": """default_planning_request_adapters/AddTimeOptimalParameterization default_planning_request_adapters/ValidateWorkspaceBounds default_planning_request_adapters/CheckStartStateBounds default_planning_request_adapters/CheckStartStateCollision default_planning_request_adapters/FixStartStatePathConstraints""",
+            "planning_plugins": ["ompl_interface/OMPLPlanner"],
+            "request_adapters": [
+                "default_planning_request_adapters/ResolveConstraintFrames",
+                "default_planning_request_adapters/ValidateWorkspaceBounds",
+                "default_planning_request_adapters/CheckStartStateBounds",
+                "default_planning_request_adapters/CheckStartStateCollision",
+            ],
+            "response_adapters": [
+                "default_planning_response_adapters/AddTimeOptimalParameterization",
+                "default_planning_response_adapters/ValidateSolution",
+            ],
         },
     }
     ompl_planning_yaml = load_yaml(

moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h

@@ -169,9 +169,8 @@ class BenchmarkExecutor
 
   void shiftConstraintsByOffset(moveit_msgs::msg::Constraints& constraints, const std::vector<double>& offset);
 
-  /// Check that the desired planner plugins and algorithms exist for the given group
-  bool plannerConfigurationsExist(const std::map<std::string, std::vector<std::string>>& planners,
-                                  const std::string& group_name);
+  /// Check that the desired planning pipelines exist
+  bool pipelinesExist(const std::map<std::string, std::vector<std::string>>& planners);
 
   /// Load the planning scene with the given name from the warehouse
   bool loadPlanningScene(const std::string& scene_name, moveit_msgs::msg::PlanningScene& scene_msg);

moveit_ros/benchmarks/src/BenchmarkExecutor.cpp

@@ -121,7 +121,7 @@ BenchmarkExecutor::~BenchmarkExecutor()
 
     const auto& pipeline = moveit_cpp_->getPlanningPipelines().at(planning_pipeline_name);
     // Verify the pipeline has successfully initialized a planner
-    if (!pipeline->getPlannerManager())
+    if (!pipeline)
     {
       RCLCPP_ERROR(getLogger(), "Failed to initialize planning pipeline '%s'", planning_pipeline_name.c_str());
       continue;
@@ -139,8 +139,7 @@ BenchmarkExecutor::~BenchmarkExecutor()
     for (const std::pair<const std::string, planning_pipeline::PlanningPipelinePtr>& entry :
          moveit_cpp_->getPlanningPipelines())
     {
-      RCLCPP_INFO_STREAM(getLogger(),
-                         "Pipeline: " << entry.first << ", Planner: " << entry.second->getPlannerPluginName());
+      RCLCPP_INFO_STREAM(getLogger(), entry.first);
     }
   }
   return true;
@@ -267,7 +266,7 @@ bool BenchmarkExecutor::initializeBenchmarks(const BenchmarkOptions& options,
                                              moveit_msgs::msg::PlanningScene& scene_msg,
                                              std::vector<BenchmarkRequest>& requests)
 {
-  if (!plannerConfigurationsExist(options.planning_pipelines, options.group_name))
+  if (!pipelinesExist(options.planning_pipelines))
   {
     return false;
   }
@@ -528,8 +527,7 @@ void BenchmarkExecutor::createRequestCombinations(const BenchmarkRequest& benchm
   }
 }
 
-bool BenchmarkExecutor::plannerConfigurationsExist(
-    const std::map<std::string, std::vector<std::string>>& pipeline_configurations, const std::string& group_name)
+bool BenchmarkExecutor::pipelinesExist(const std::map<std::string, std::vector<std::string>>& pipeline_configurations)
 {
   // Make sure planner plugins exist
   for (const std::pair<const std::string, std::vector<std::string>>& pipeline_config_entry : pipeline_configurations)
@@ -549,42 +547,6 @@ bool BenchmarkExecutor::plannerConfigurationsExist(
       return false;
     }
   }
-
-  // Make sure planners exist within those pipelines
-  auto planning_pipelines = moveit_cpp_->getPlanningPipelines();
-  for (const std::pair<const std::string, std::vector<std::string>>& entry : pipeline_configurations)
-  {
-    planning_interface::PlannerManagerPtr pm = planning_pipelines[entry.first]->getPlannerManager();
-    const planning_interface::PlannerConfigurationMap& config_map = pm->getPlannerConfigurations();
-
-    // if the planner is chomp or stomp skip this function and return true for checking planner configurations for the
-    // planning group otherwise an error occurs, because for OMPL a specific planning algorithm needs to be defined for
-    // a planning group, whereas with STOMP and CHOMP this is not necessary
-    if (pm->getDescription().compare("stomp") || pm->getDescription().compare("chomp"))
-      continue;
-
-    for (std::size_t i = 0; i < entry.second.size(); ++i)
-    {
-      bool planner_exists = false;
-      for (const std::pair<const std::string, planning_interface::PlannerConfigurationSettings>& config_entry :
-           config_map)
-      {
-        std::string planner_name = group_name + "[" + entry.second[i] + "]";
-        planner_exists = (config_entry.second.group == group_name && config_entry.second.name == planner_name);
-      }
-
-      if (!planner_exists)
-      {
-        RCLCPP_ERROR(getLogger(), "Planner '%s' does NOT exist for group '%s' in pipeline '%s'",
-                     entry.second[i].c_str(), group_name.c_str(), entry.first.c_str());
-        std::cout << "There are " << config_map.size() << " planner entries: " << '\n';
-        for (const auto& config_map_entry : config_map)
-          std::cout << config_map_entry.second.name << '\n';
-        return false;
-      }
-    }
-  }
-
   return true;
 }
 

moveit_ros/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp

@@ -60,7 +60,7 @@ bool MoveItPlanningPipeline::initialize(const rclcpp::Node::SharedPtr& node)
   node->declare_parameter<double>(PLAN_REQUEST_PARAM_NS + "planning_time", 1.0);
   node->declare_parameter<double>(PLAN_REQUEST_PARAM_NS + "max_velocity_scaling_factor", 1.0);
   node->declare_parameter<double>(PLAN_REQUEST_PARAM_NS + "max_acceleration_scaling_factor", 1.0);
-  node->declare_parameter<std::string>("ompl.planning_plugin", "ompl_interface/OMPLPlanner");
+  node->declare_parameter<std::vector<std::string>>("ompl.planning_plugins", { "ompl_interface/OMPLPlanner" });
 
   // Planning Scene options
   node->declare_parameter<std::string>(PLANNING_SCENE_MONITOR_NS + "name", UNDEFINED);

moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py

@@ -70,8 +70,18 @@ def generate_common_hybrid_launch_description():
     # The global planner uses the typical OMPL parameters
     planning_pipelines_config = {
         "ompl": {
-            "planning_plugin": "ompl_interface/OMPLPlanner",
-            "request_adapters": """default_planning_request_adapters/AddTimeOptimalParameterization default_planning_request_adapters/ValidateWorkspaceBounds default_planning_request_adapters/CheckStartStateBounds default_planning_request_adapters/CheckStartStateCollision""",
+            "planning_plugins": ["ompl_interface/OMPLPlanner"],
+            "request_adapters": [
+                "default_planning_request_adapters/ResolveConstraintFrames",
+                "default_planning_request_adapters/ValidateWorkspaceBounds",
+                "default_planning_request_adapters/CheckStartStateBounds",
+                "default_planning_request_adapters/CheckStartStateCollision",
+            ],
+            "response_adapters": [
+                "default_planning_response_adapters/AddTimeOptimalParameterization",
+                "default_planning_response_adapters/ValidateSolution",
+                "default_planning_response_adapters/DisplayMotionPath",
+            ],
         }
     }
     ompl_planning_yaml = load_yaml(