Allow planning with multiple pipelines in parallel with moveit_cpp (#…

…3244)

This is a backport of:
- moveit/moveit2#1420
- moveit/moveit2#1710

Co-authored-by: Robert Haschke <rhaschke@users.noreply.github.com>

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

@@ -37,23 +37,59 @@
 #pragma once
 
 #include <moveit/robot_trajectory/robot_trajectory.h>
-#include <moveit_msgs/MoveItErrorCodes.h>
+#include <moveit/utils/moveit_error_code.h>
 #include <moveit_msgs/MotionPlanResponse.h>
 #include <moveit_msgs/MotionPlanDetailedResponse.h>
 
 namespace planning_interface
 {
 struct MotionPlanResponse
 {
-  MotionPlanResponse() : planning_time_(0.0)
+  robot_trajectory::RobotTrajectoryPtr trajectory_;
+  double planning_time_;
+  moveit::core::MoveItErrorCode error_code_;
+  moveit_msgs::RobotState start_state_;
+  std::string planner_id_;
+
+  [[deprecated("Use trajectory_ instead.")]] const robot_trajectory::RobotTrajectoryPtr& trajectory;
+  [[deprecated("Use planning_time_ instead.")]] const double& planning_time;
+  [[deprecated("Use error_code_ instead.")]] const moveit::core::MoveItErrorCode& error_code;
+  [[deprecated("Use start_state_ instead.")]] const moveit_msgs::RobotState& start_state;
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+  MotionPlanResponse()
+    : planning_time_(0.0)
+    , trajectory(trajectory_)
+    , planning_time(planning_time_)
+    , error_code(error_code_)
+    , start_state(start_state_)
   {
   }
+#pragma GCC diagnostic pop
+
+  MotionPlanResponse(const MotionPlanResponse& response) : MotionPlanResponse()
+  {
+    *this = response;
+  }
+
+  MotionPlanResponse& operator=(const MotionPlanResponse& response)
+  {
+    trajectory_ = response.trajectory_;
+    planning_time_ = response.planning_time_;
+    error_code_ = response.error_code_;
+    start_state_ = response.start_state_;
+    planner_id_ = response.planner_id_;
+    return *this;
+  }
 
   void getMessage(moveit_msgs::MotionPlanResponse& msg) const;
 
-  robot_trajectory::RobotTrajectoryPtr trajectory_;
-  double planning_time_;
-  moveit_msgs::MoveItErrorCodes error_code_;
+  // Enable checking of query success or failure, for example if(response) ...
+  explicit operator bool() const
+  {
+    return bool(error_code_);
+  }
 };
 
 struct MotionPlanDetailedResponse
@@ -63,7 +99,15 @@ struct MotionPlanDetailedResponse
   std::vector<robot_trajectory::RobotTrajectoryPtr> trajectory_;
   std::vector<std::string> description_;
   std::vector<double> processing_time_;
-  moveit_msgs::MoveItErrorCodes error_code_;
+  moveit::core::MoveItErrorCode error_code_;
+  moveit_msgs::RobotState start_state_;
+  std::string planner_id_;
+
+  // Enable checking of query success or failure, for example if(response) ...
+  explicit operator bool() const
+  {
+    return bool(error_code_);
+  }
 };
 
 }  // namespace planning_interface

moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h

@@ -300,4 +300,12 @@ class RobotTrajectory
   std::deque<moveit::core::RobotStatePtr> waypoints_;
   std::deque<double> duration_from_previous_;
 };
+
+/// \brief Calculate the path length of a given trajectory based on the
+/// accumulated robot state distances.
+/// The distance between two robot states is calculated based on the sum of
+/// active joint distances between the two states (L1 norm).
+/// \param[in] trajectory Given robot trajectory
+/// \return Length of the robot trajectory [rad]
+[[nodiscard]] double path_length(RobotTrajectory const& trajectory);
 }  // namespace robot_trajectory

moveit_core/robot_trajectory/src/robot_trajectory.cpp

@@ -504,4 +504,15 @@ bool RobotTrajectory::getStateAtDurationFromStart(const double request_duration,
   return true;
 }
 
+double path_length(RobotTrajectory const& trajectory)
+{
+  auto trajectory_length = 0.0;
+  for (std::size_t index = 1; index < trajectory.getWayPointCount(); ++index)
+  {
+    auto const& first = trajectory.getWayPoint(index - 1);
+    auto const& second = trajectory.getWayPoint(index);
+    trajectory_length += first.distance(second);
+  }
+  return trajectory_length;
+}
 }  // end of namespace robot_trajectory

moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h

@@ -142,10 +142,6 @@ class MoveItCpp
   /** \brief Get all loaded planning pipeline instances mapped to their reference names */
   const std::map<std::string, planning_pipeline::PlanningPipelinePtr>& getPlanningPipelines() const;
 
-  /** \brief Get the names of all loaded planning pipelines. Specify group_name to filter the results by planning group
-   */
-  std::set<std::string> getPlanningPipelineNames(const std::string& group_name = "") const;
-
   /** \brief Get the stored instance of the planning scene monitor */
   const planning_scene_monitor::PlanningSceneMonitorPtr& getPlanningSceneMonitor() const;
   planning_scene_monitor::PlanningSceneMonitorPtr getPlanningSceneMonitorNonConst();
@@ -161,6 +157,9 @@ class MoveItCpp
   bool execute(const std::string& group_name, const robot_trajectory::RobotTrajectoryPtr& robot_trajectory,
                bool blocking = true);
 
+  /** \brief Utility to terminate a given planning pipeline */
+  bool terminatePlanningPipeline(std::string const& pipeline_name);
+
 protected:
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
   std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
@@ -173,7 +172,6 @@ class MoveItCpp
 
   // Planning
   std::map<std::string, planning_pipeline::PlanningPipelinePtr> planning_pipelines_;
-  std::map<std::string, std::set<std::string>> groups_pipelines_map_;
   std::map<std::string, std::set<std::string>> groups_algorithms_map_;
 
   // Execution

moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h

@@ -41,34 +41,50 @@
 #include <moveit/moveit_cpp/moveit_cpp.h>
 #include <moveit_msgs/MoveItErrorCodes.h>
 #include <moveit/utils/moveit_error_code.h>
+#include <moveit/planning_interface/planning_response.h>
+#include <mutex>
 
 namespace moveit_cpp
 {
 MOVEIT_CLASS_FORWARD(PlanningComponent);  // Defines PlanningComponentPtr, ConstPtr, WeakPtr... etc
 
+/// \brief A function to choose the solution with the shortest path from a vector of solutions
+planning_interface::MotionPlanResponse
+getShortestSolution(std::vector<planning_interface::MotionPlanResponse> const& solutions);
+
 class PlanningComponent
 {
 public:
-  MOVEIT_STRUCT_FORWARD(PlanSolution);
-
   using MoveItErrorCode [[deprecated("Use moveit::core::MoveItErrorCode")]] = moveit::core::MoveItErrorCode;
 
-  /// The representation of a plan solution
-  struct PlanSolution
+  class PlanSolutions
   {
-    /// The full starting state used for planning
-    moveit_msgs::RobotState start_state;
-
-    /// The trajectory of the robot (may not contain joints that are the same as for the start_state_)
-    robot_trajectory::RobotTrajectoryPtr trajectory;
+  public:
+    /// \brief Constructor
+    PlanSolutions(const size_t expected_size = 0)
+    {
+      solutions_.reserve(expected_size);
+    }
 
-    /// Reason why the plan failed
-    moveit::core::MoveItErrorCode error_code;
+    /// \brief Thread safe method to add PlanSolutions to this data structure
+    /// TODO(sjahr): Refactor this method to an insert method similar to
+    /// https://github.com/ompl/ompl/blob/main/src/ompl/base/src/ProblemDefinition.cpp#L54-L161. This way, it is
+    /// possible to create a sorted container e.g. according to a user specified criteria
+    void pushBack(const planning_interface::MotionPlanResponse& plan_solution)
+    {
+      std::lock_guard<std::mutex> lock_guard(solutions_mutex_);
+      solutions_.push_back(plan_solution);
+    }
 
-    explicit operator bool() const
+    /// \brief Get solutions
+    std::vector<planning_interface::MotionPlanResponse> const& getSolutions() const
     {
-      return bool(error_code);
+      return solutions_;
     }
+
+  private:
+    std::vector<planning_interface::MotionPlanResponse> solutions_;
+    std::mutex solutions_mutex_;
   };
 
   /// Planner parameters provided with the MotionPlanRequest
@@ -81,9 +97,14 @@ class PlanningComponent
     double max_velocity_scaling_factor;
     double max_acceleration_scaling_factor;
 
-    void load(const ros::NodeHandle& nh)
+    void load(const ros::NodeHandle& nh, const std::string& param_namespace = "")
     {
       std::string ns = "plan_request_params/";
+      if (!param_namespace.empty())
+      {
+        ns = param_namespace + "/plan_request_params/";
+      }
+
       nh.param(ns + "planner_id", planner_id, std::string(""));
       nh.param(ns + "planning_pipeline", planning_pipeline, std::string(""));
       nh.param(ns + "planning_time", planning_time, 1.0);
@@ -93,6 +114,34 @@ class PlanningComponent
     }
   };
 
+  /// Planner parameters provided with the MotionPlanRequest
+  struct MultiPipelinePlanRequestParameters
+  {
+    MultiPipelinePlanRequestParameters(const ros::NodeHandle& nh,
+                                       const std::vector<std::string>& planning_pipeline_names)
+    {
+      multi_plan_request_parameters.reserve(planning_pipeline_names.size());
+
+      for (auto const& planning_pipeline_name : planning_pipeline_names)
+      {
+        PlanRequestParameters parameters;
+        parameters.load(nh, planning_pipeline_name);
+        multi_plan_request_parameters.push_back(parameters);
+      }
+    }
+    // Plan request parameters for the individual planning pipelines which run concurrently
+    std::vector<PlanRequestParameters> multi_plan_request_parameters;
+  };
+
+  /// \brief A solution callback function type for the parallel planning API of planning component
+  typedef std::function<planning_interface::MotionPlanResponse(
+      std::vector<planning_interface::MotionPlanResponse> const& solutions)>
+      SolutionCallbackFunction;
+  /// \brief A stopping criterion callback function for the parallel planning API of planning component
+  typedef std::function<bool(PlanSolutions const& solutions,
+                             MultiPipelinePlanRequestParameters const& plan_request_parameters)>
+      StoppingCriterionFunction;
+
   /** \brief Constructor */
   PlanningComponent(const std::string& group_name, const ros::NodeHandle& nh);
   PlanningComponent(const std::string& group_name, const MoveItCppPtr& moveit_cpp);
@@ -152,19 +201,29 @@ class PlanningComponent
   /** \brief Set the path constraints used for planning */
   bool setPathConstraints(const moveit_msgs::Constraints& path_constraints);
 
+  /** \brief Set the trajectory constraints generated from a moveit msg Constraints */
+  bool setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints& trajectory_constraints);
+
   /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using
    * default parameters. */
-  PlanSolution plan();
+  planning_interface::MotionPlanResponse plan();
+  /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using the
+   * provided PlanRequestParameters. */
+  planning_interface::MotionPlanResponse plan(const PlanRequestParameters& parameters, const bool store_solution = true);
+
   /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using the
    * provided PlanRequestParameters. */
-  PlanSolution plan(const PlanRequestParameters& parameters);
+  planning_interface::MotionPlanResponse
+  plan(const MultiPipelinePlanRequestParameters& parameters,
+       const SolutionCallbackFunction& solution_selection_callback = &getShortestSolution,
+       const StoppingCriterionFunction& stopping_criterion_callback = StoppingCriterionFunction());
 
   /** \brief Execute the latest computed solution trajectory computed by plan(). By default this function terminates
    * after the execution is complete. The execution can be run in background by setting blocking to false. */
   bool execute(bool blocking = true);
 
   /** \brief Return the last plan solution*/
-  const PlanSolutionPtr getLastPlanSolution();
+  planning_interface::MotionPlanResponse const& getLastMotionPlanResponse();
 
 private:
   // Core properties and instances
@@ -175,15 +234,15 @@ class PlanningComponent
   const moveit::core::JointModelGroup* joint_model_group_;
 
   // Planning
-  std::set<std::string> planning_pipeline_names_;
   // The start state used in the planning motion request
   moveit::core::RobotStatePtr considered_start_state_;
   std::vector<moveit_msgs::Constraints> current_goal_constraints_;
+  moveit_msgs::TrajectoryConstraints current_trajectory_constraints_;
   moveit_msgs::Constraints current_path_constraints_;
   PlanRequestParameters plan_request_parameters_;
   moveit_msgs::WorkspaceParameters workspace_parameters_;
   bool workspace_parameters_set_ = false;
-  PlanSolutionPtr last_plan_solution_;
+  planning_interface::MotionPlanResponse last_plan_solution_;
 
   // common properties for goals
   // TODO(henningkayser): support goal tolerances

moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp

@@ -159,23 +159,6 @@ bool MoveItCpp::loadPlanningPipelines(const PlanningPipelineOptions& options)
     return false;
   }
 
-  // Retrieve group/pipeline mapping for faster lookup
-  std::vector<std::string> group_names = robot_model_->getJointModelGroupNames();
-  for (const auto& pipeline_entry : planning_pipelines_)
-  {
-    for (const auto& group_name : group_names)
-    {
-      const auto& pipeline = pipeline_entry.second;
-      for (const auto& planner_configuration : pipeline->getPlannerManager()->getPlannerConfigurations())
-      {
-        if (planner_configuration.second.group == group_name)
-        {
-          groups_pipelines_map_[group_name].insert(pipeline_entry.first);
-        }
-      }
-    }
-  }
-
   return true;
 }
 
@@ -216,19 +199,6 @@ const std::map<std::string, planning_pipeline::PlanningPipelinePtr>& MoveItCpp::
   return planning_pipelines_;
 }
 
-std::set<std::string> MoveItCpp::getPlanningPipelineNames(const std::string& group_name) const
-{
-  if (group_name.empty() || groups_pipelines_map_.count(group_name) == 0)
-  {
-    ROS_ERROR_NAMED(LOGNAME,
-                    "No planning pipelines loaded for group '%s'. Check planning pipeline and controller setup.",
-                    group_name.c_str());
-    return {};  // empty
-  }
-
-  return groups_pipelines_map_.at(group_name);
-}
-
 const planning_scene_monitor::PlanningSceneMonitorPtr& MoveItCpp::getPlanningSceneMonitor() const
 {
   return planning_scene_monitor_;
@@ -279,6 +249,25 @@ bool MoveItCpp::execute(const std::string& group_name, const robot_trajectory::R
   return true;
 }
 
+bool MoveItCpp::terminatePlanningPipeline(std::string const& pipeline_name)
+{
+  try
+  {
+    auto const& planning_pipeline = planning_pipelines_.at(pipeline_name);
+    if (planning_pipeline->isActive())
+    {
+      planning_pipeline->terminate();
+    }
+    return true;
+  }
+  catch (const std::out_of_range& oor)
+  {
+    ROS_ERROR_NAMED(LOGNAME, "Cannot terminate pipeline '%s' because no pipeline with that name exists",
+                    pipeline_name.c_str());
+    return false;
+  }
+}
+
 const std::shared_ptr<tf2_ros::Buffer>& MoveItCpp::getTFBuffer() const
 {
   return tf_buffer_;