[Planning Pipeline Refactoring] #2 Enable chaining 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_plugin:
+  - 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

@@ -165,7 +165,7 @@ class PlannerManager
                           const std::string& parameter_namespace);
 
   /// Get \brief a short string that identifies the planning interface
-  virtual std::string getDescription() const;
+  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_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h

@@ -170,8 +170,7 @@ 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);
+  bool plannerConfigurationsExist(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

@@ -114,7 +114,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;
@@ -132,8 +132,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;
@@ -260,7 +259,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 (!plannerConfigurationsExist(options.planning_pipelines))
   {
     return false;
   }
@@ -522,7 +521,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)
+    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)
@@ -542,42 +541,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/move_group/src/default_capabilities/query_planners_service_capability.cpp

@@ -73,94 +73,98 @@ void MoveGroupQueryPlannersService::initialize()
 bool MoveGroupQueryPlannersService::queryInterface(
     const std::shared_ptr<rmw_request_id_t>& /* unused */,
     const std::shared_ptr<moveit_msgs::srv::QueryPlannerInterfaces::Request>& /*req*/,
-    const std::shared_ptr<moveit_msgs::srv::QueryPlannerInterfaces::Response>& res)
+    const std::shared_ptr<moveit_msgs::srv::QueryPlannerInterfaces::Response>& /* unused */)
 {
-  for (const auto& planning_pipelines : context_->moveit_cpp_->getPlanningPipelines())
-  {
-    const auto& pipeline_id = planning_pipelines.first;
-    const auto& planning_pipeline = planning_pipelines.second;
-    const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline->getPlannerManager();
-    if (planner_interface)
-    {
-      std::vector<std::string> algs;
-      planner_interface->getPlanningAlgorithms(algs);
-      moveit_msgs::msg::PlannerInterfaceDescription pi_desc;
-      pi_desc.name = planner_interface->getDescription();
-      pi_desc.pipeline_id = pipeline_id;
-      planner_interface->getPlanningAlgorithms(pi_desc.planner_ids);
-      res->planner_interfaces.push_back(pi_desc);
-    }
-  }
-  return true;
+  // TODO(sjahr): This is currently not working. Need to decide whether to fix or remove this.
+  // for (const auto& planning_pipelines : context_->moveit_cpp_->getPlanningPipelines())
+  // {
+  //   const auto& pipeline_id = planning_pipelines.first;
+  //   const auto& planning_pipeline = planning_pipelines.second;
+  //   const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline->getPlannerManager();
+  //   if (planner_interface)
+  //   {
+  //     std::vector<std::string> algs;
+  //     planner_interface->getPlanningAlgorithms(algs);
+  //     moveit_msgs::msg::PlannerInterfaceDescription pi_desc;
+  //     pi_desc.name = planner_interface->getDescription();
+  //     pi_desc.pipeline_id = pipeline_id;
+  //     planner_interface->getPlanningAlgorithms(pi_desc.planner_ids);
+  //     res->planner_interfaces.push_back(pi_desc);
+  //   }
+  // }
+  return false;
 }
 
-bool MoveGroupQueryPlannersService::getParams(const std::shared_ptr<rmw_request_id_t>& /* unused */,
-                                              const std::shared_ptr<moveit_msgs::srv::GetPlannerParams::Request>& req,
-                                              const std::shared_ptr<moveit_msgs::srv::GetPlannerParams::Response>& res)
+bool MoveGroupQueryPlannersService::getParams(
+    const std::shared_ptr<rmw_request_id_t>& /* unused */,
+    const std::shared_ptr<moveit_msgs::srv::GetPlannerParams::Request>& /* unused */,
+    const std::shared_ptr<moveit_msgs::srv::GetPlannerParams::Response>& /* unused */)
 {
-  const planning_pipeline::PlanningPipelinePtr planning_pipeline = resolvePlanningPipeline(req->pipeline_id);
-  if (!planning_pipeline)
-    return false;
-
-  const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline->getPlannerManager();
-  if (planner_interface)
-  {
-    std::map<std::string, std::string> config;
-
-    const planning_interface::PlannerConfigurationMap& configs = planner_interface->getPlannerConfigurations();
-
-    planning_interface::PlannerConfigurationMap::const_iterator it =
-        configs.find(req->planner_config);  // fetch default params first
-    if (it != configs.end())
-      config.insert(it->second.config.begin(), it->second.config.end());
-
-    if (!req->group.empty())
-    {  // merge in group-specific params
-      it = configs.find(req->group + "[" + req->planner_config + "]");
-      if (it != configs.end())
-        config.insert(it->second.config.begin(), it->second.config.end());
-    }
-
-    for (const auto& key_value_pair : config)
-    {
-      res->params.keys.push_back(key_value_pair.first);
-      res->params.values.push_back(key_value_pair.second);
-    }
-  }
-  return true;
+  // TODO(sjahr): This is currently not working. Need to decide whether to fix or remove this.
+  // const planning_pipeline::PlanningPipelinePtr planning_pipeline = resolvePlanningPipeline(req->pipeline_id);
+  // if (!planning_pipeline)
+  //   return false;
+  //
+  // const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline->getPlannerManager();
+  // if (planner_interface)
+  //{
+  //  std::map<std::string, std::string> config;
+  //
+  //  const planning_interface::PlannerConfigurationMap& configs = planner_interface->getPlannerConfigurations();
+  //
+  //  planning_interface::PlannerConfigurationMap::const_iterator it =
+  //      configs.find(req->planner_config);  // fetch default params first
+  //  if (it != configs.end())
+  //    config.insert(it->second.config.begin(), it->second.config.end());
+  //
+  //  if (!req->group.empty())
+  //  {  // merge in group-specific params
+  //    it = configs.find(req->group + "[" + req->planner_config + "]");
+  //    if (it != configs.end())
+  //      config.insert(it->second.config.begin(), it->second.config.end());
+  //  }
+  //
+  //  for (const auto& key_value_pair : config)
+  //  {
+  //    res->params.keys.push_back(key_value_pair.first);
+  //    res->params.values.push_back(key_value_pair.second);
+  //  }
+  //}
+  return false;
 }
 
 bool MoveGroupQueryPlannersService::setParams(
     const std::shared_ptr<rmw_request_id_t>& /* unused */,
-    const std::shared_ptr<moveit_msgs::srv::SetPlannerParams::Request>& req,
+    const std::shared_ptr<moveit_msgs::srv::SetPlannerParams::Request>& /* unused */,
     const std::shared_ptr<moveit_msgs::srv::SetPlannerParams::Response>& /*res*/)
 {
-  if (req->params.keys.size() != req->params.values.size())
-    return false;
-
-  const planning_pipeline::PlanningPipelinePtr planning_pipeline = resolvePlanningPipeline(req->pipeline_id);
-  if (!planning_pipeline)
-    return false;
-
-  const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline->getPlannerManager();
-
-  if (planner_interface)
-  {
-    planning_interface::PlannerConfigurationMap configs = planner_interface->getPlannerConfigurations();
-    const std::string config_name =
-        req->group.empty() ? req->planner_config : req->group + "[" + req->planner_config + "]";
-
-    planning_interface::PlannerConfigurationSettings& config = configs[config_name];
-    config.group = req->group;
-    config.name = config_name;
-    if (req->replace)
-      config.config.clear();
-    for (unsigned int i = 0, end = req->params.keys.size(); i < end; ++i)
-      config.config[req->params.keys[i]] = req->params.values[i];
-
-    planner_interface->setPlannerConfigurations(configs);
-  }
-  return true;
+  // TODO(sjahr): This is currently not working. Need to decide whether to fix or remove this.
+  // if (req->params.keys.size() != req->params.values.size())
+  //   return false;
+  //
+  // const planning_pipeline::PlanningPipelinePtr planning_pipeline = resolvePlanningPipeline(req->pipeline_id);
+  // if (!planning_pipeline)
+  //  return false;
+  //
+  // const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline->getPlannerManager();
+  //
+  // if (planner_interface)
+  //{
+  //  planning_interface::PlannerConfigurationMap configs = planner_interface->getPlannerConfigurations();
+  //  const std::string config_name =
+  //      req->group.empty() ? req->planner_config : req->group + "[" + req->planner_config + "]";
+  //
+  //  planning_interface::PlannerConfigurationSettings& config = configs[config_name];
+  //  config.group = req->group;
+  //  config.name = config_name;
+  //  if (req->replace)
+  //    config.config.clear();
+  //  for (unsigned int i = 0, end = req->params.keys.size(); i < end; ++i)
+  //    config.config[req->params.keys[i]] = req->params.values[i];
+  //
+  //  planner_interface->setPlannerConfigurations(configs);
+  //}
+  return false;
 }
 }  // namespace move_group
 

moveit_ros/move_group/src/move_group_context.cpp

@@ -82,18 +82,17 @@ move_group::MoveGroupContext::~MoveGroupContext()
 
 bool move_group::MoveGroupContext::status() const
 {
-  const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline_->getPlannerManager();
-  if (planner_interface)
+  if (planning_pipeline_)
   {
     RCLCPP_INFO_STREAM(moveit::getLogger(),
-                       "MoveGroup context using planning plugin " << planning_pipeline_->getPlannerPluginName());
+                       "MoveGroup context using pipeline " << planning_pipeline_->getName().c_str());
     RCLCPP_INFO_STREAM(moveit::getLogger(), "MoveGroup context initialization complete");
     return true;
   }
   else
   {
     RCLCPP_WARN_STREAM(moveit::getLogger(),
-                       "MoveGroup running was unable to load " << planning_pipeline_->getPlannerPluginName());
+                       "MoveGroup running was unable to load pipeline " << planning_pipeline_->getName().c_str());
     return false;
   }
 }