Parallel planning pipelines (#1420)

* Add setTrajectoryConstraints() to PlanningComponent

* Add planning time to PlanningComponent::PlanSolution

* Replace PlanSolution with MotionPlanResponse

* Address review

* Add MultiPipelinePlanRequestParameters

Add plan(const MultiPipelinePlanRequestParameters& parameters)

Add mutex to avoid segfaults

Add optional stop_criterion_callback and solution_selection_callback

Remove stop_criterion_callback

Make default solution_selection_callback = nullptr

Remove parameter handling copy&paste code in favor of a template

Add TODO to refactor pushBack() method into insert()

Fix selection criteria and add RCLCPP_INFO output

Changes due to rebase and formatting

Fix race condition and segfault when no solution is found

Satisfy clang tidy

Remove mutex and thread safety TODOs

Add stopping functionality to parallel planning

Remove unnecessary TODOs

* Fix unused plan solution with failure

* Add sanity check for number of parallel planning problems

* Check stopping criterion when new solution is generated + make thread safe

* Add terminatePlanningPipeline() to MoveItCpp interface

* Format!

* Bug fixes

* Move getShortestSolution callback into own function

* No east const

* Remove PlanSolutions and make planner_id accessible

* Make solution executable

* Rename update_last_solution to store_solution

* Alphabetize includes and include plan_solutions.hpp instead of .h

* Address review

* Add missing header

* Apply suggestions from code review

Co-authored-by: AndyZe <andyz@utexas.edu>

Co-authored-by: AndyZe <andyz@utexas.edu>

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

@@ -37,23 +37,39 @@
 #pragma once
 
 #include <moveit/robot_trajectory/robot_trajectory.h>
+#include <moveit/utils/moveit_error_code.h>
 #include <moveit_msgs/msg/move_it_error_codes.hpp>
 #include <moveit_msgs/msg/motion_plan_response.hpp>
 #include <moveit_msgs/msg/motion_plan_detailed_response.hpp>
 
 namespace planning_interface
 {
+/// \brief Response to a planning query
 struct MotionPlanResponse
 {
-  MotionPlanResponse() : planning_time_(0.0)
+  /** \brief Constructor */
+  MotionPlanResponse() : trajectory_(nullptr), planning_time_(0.0), error_code_(moveit::core::MoveItErrorCode::FAILURE)
   {
   }
 
+  /** \brief Construct a ROS message from struct data */
   void getMessage(moveit_msgs::msg::MotionPlanResponse& msg) const;
 
+  // Trajectory generated by the queried planner
   robot_trajectory::RobotTrajectoryPtr trajectory_;
+  // Time to plan the respond to the planning query
   double planning_time_;
-  moveit_msgs::msg::MoveItErrorCodes error_code_;
+  // Result status of the query
+  moveit::core::MoveItErrorCode error_code_;
+  /// The full starting state used for planning
+  moveit_msgs::msg::RobotState start_state_;
+  std::string planner_id_;
+
+  // \brief Enable checking of query success or failure, for example if(response) ...
+  explicit operator bool() const
+  {
+    return bool(error_code_);
+  }
 };
 
 struct MotionPlanDetailedResponse
@@ -64,6 +80,7 @@ struct MotionPlanDetailedResponse
   std::vector<std::string> description_;
   std::vector<double> processing_time_;
   moveit_msgs::msg::MoveItErrorCodes error_code_;
+  std::string planner_id_;
 };
 
 }  // namespace planning_interface

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

@@ -132,17 +132,17 @@ moveit_msgs::msg::MotionPlanResponse MoveItPlanningPipeline::plan(
 
   // Plan motion
   auto plan_solution = planning_components->plan(plan_params);
-  if (plan_solution.error_code != moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
+  if (!bool(plan_solution.error_code_))
   {
-    response.error_code = plan_solution.error_code;
+    response.error_code = plan_solution.error_code_;
     return response;
   }
 
   // Transform solution into MotionPlanResponse and publish it
-  response.trajectory_start = plan_solution.start_state;
+  response.trajectory_start = plan_solution.start_state_;
   response.group_name = motion_plan_req.group_name;
-  plan_solution.trajectory->getRobotTrajectoryMsg(response.trajectory);
-  response.error_code = plan_solution.error_code;
+  plan_solution.trajectory_->getRobotTrajectoryMsg(response.trajectory);
+  response.error_code = plan_solution.error_code_;
 
   return response;
 }

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

@@ -172,6 +172,9 @@ class MoveItCpp
                                                      const robot_trajectory::RobotTrajectoryPtr& robot_trajectory,
                                                      bool blocking = true);
 
+  /** \brief Utility to terminate all active planning pipelines */
+  bool terminatePlanningPipeline(const std::string& pipeline_name);
+
 private:
   //  Core properties and instances
   rclcpp::Node::SharedPtr node_;

moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/plan_solutions.hpp

@@ -0,0 +1,105 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2019, PickNik Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of PickNik Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Sebastian Jahr
+   Desc: A safe data structure for MotionPlanResponses and free functions to analyze them */
+
+#pragma once
+
+#include <algorithm>
+#include <moveit/planning_interface/planning_response.h>
+#include <thread>
+
+namespace moveit_cpp
+{
+MOVEIT_CLASS_FORWARD(PlanSolutions);  // Defines PlanningComponentPtr, ConstPtr, WeakPtr... etc
+
+/** \brief A container to thread-safely store multiple MotionPlanResponses for later usage */
+class PlanSolutions
+{
+public:
+  PlanSolutions(const size_t expected_size = 0)
+  {
+    solutions_.reserve(expected_size);
+  }
+
+  /** \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 created a sorted container e.g. according to a user specified criteria
+   */
+  void pushBack(planning_interface::MotionPlanResponse plan_solution)
+  {
+    std::lock_guard<std::mutex> lock_guard(solutions_mutex_);
+    solutions_.push_back(plan_solution);
+  }
+
+  /** \brief Get solutions */
+  const std::vector<planning_interface::MotionPlanResponse>& getSolutions() const
+  {
+    return solutions_;
+  }
+
+private:
+  std::vector<planning_interface::MotionPlanResponse> solutions_;
+  std::mutex solutions_mutex_;
+};
+
+/** \brief Function that returns the shortest solution out of a vector of solutions based on robot_trajectory::path_length(...)
+ *  \param [in] solutions Vector of solutions to chose the shortest one from
+ *  \return Shortest solution, trajectory_ of the returned MotionPlanResponse is a nullptr if no solution is found!
+ */
+planning_interface::MotionPlanResponse
+getShortestSolution(const std::vector<planning_interface::MotionPlanResponse>& solutions)
+{
+  // Find trajectory with minimal path
+  auto const shortest_trajectory = std::min_element(solutions.begin(), solutions.end(),
+                                                    [](const planning_interface::MotionPlanResponse& solution_a,
+                                                       const planning_interface::MotionPlanResponse& solution_b) {
+                                                      // If both solutions were successful, check which path is shorter
+                                                      if (solution_a && solution_b)
+                                                      {
+                                                        return robot_trajectory::path_length(*solution_a.trajectory_) <
+                                                               robot_trajectory::path_length(*solution_b.trajectory_);
+                                                      }
+                                                      // If only solution a is successful, return a
+                                                      else if (solution_a)
+                                                      {
+                                                        return true;
+                                                      }
+                                                      // Else return solution b, either because it is successful or not
+                                                      return false;
+                                                    });
+  return *shortest_trajectory;
+}
+}  // namespace moveit_cpp

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

@@ -37,42 +37,22 @@
 
 #pragma once
 
-#include <rclcpp/rclcpp.hpp>
-#include <moveit/moveit_cpp/moveit_cpp.h>
-#include <moveit/robot_state/robot_state.h>
 #include <geometry_msgs/msg/pose_stamped.h>
+#include <moveit/moveit_cpp/moveit_cpp.h>
+#include <moveit/moveit_cpp/plan_solutions.hpp>
+#include <moveit/planning_interface/planning_response.h>
 #include <moveit/robot_state/conversions.h>
+#include <moveit/robot_state/robot_state.h>
 #include <moveit/utils/moveit_error_code.h>
+#include <rclcpp/rclcpp.hpp>
 
 namespace moveit_cpp
 {
 MOVEIT_CLASS_FORWARD(PlanningComponent);  // Defines PlanningComponentPtr, ConstPtr, WeakPtr... etc
-
 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
-  {
-    /// The full starting state used for planning
-    moveit_msgs::msg::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;
-
-    /// Reason why the plan failed
-    moveit::core::MoveItErrorCode error_code;
-
-    explicit operator bool() const
-    {
-      return bool(error_code);
-    }
-  };
-
   /// Planner parameters provided with the MotionPlanRequest
   struct PlanRequestParameters
   {
@@ -83,18 +63,67 @@ class PlanningComponent
     double max_velocity_scaling_factor;
     double max_acceleration_scaling_factor;
 
-    void load(const rclcpp::Node::SharedPtr& node)
+    template <typename T>
+    void declareOrGetParam(const rclcpp::Node::SharedPtr& node, const std::string& param_name, T& output_value,
+                           T default_value)
+    {
+      // Try to get parameter or use default
+      if (!node->get_parameter_or(param_name, output_value, default_value))
+      {
+        RCLCPP_WARN(node->get_logger(),
+                    "Parameter \'%s\' not found in config use default value instead, check parameter type and "
+                    "namespace in YAML file",
+                    (param_name).c_str());
+      }
+    }
+
+    void load(const rclcpp::Node::SharedPtr& node, const std::string& param_namespace = "")
     {
+      // Set namespace
       std::string ns = "plan_request_params.";
-      node->get_parameter_or(ns + "planner_id", planner_id, std::string(""));
-      node->get_parameter_or(ns + "planning_pipeline", planning_pipeline, std::string(""));
-      node->get_parameter_or(ns + "planning_time", planning_time, 1.0);
-      node->get_parameter_or(ns + "planning_attempts", planning_attempts, 5);
-      node->get_parameter_or(ns + "max_velocity_scaling_factor", max_velocity_scaling_factor, 1.0);
-      node->get_parameter_or(ns + "max_acceleration_scaling_factor", max_acceleration_scaling_factor, 1.0);
+      if (!param_namespace.empty())
+      {
+        ns = param_namespace + ".plan_request_params.";
+      }
+
+      // Declare parameters
+      declareOrGetParam<std::string>(node, ns + "planner_id", planner_id, std::string(""));
+      declareOrGetParam<std::string>(node, ns + "planning_pipeline", planning_pipeline, std::string(""));
+      declareOrGetParam<double>(node, ns + "planning_time", planning_time, 1.0);
+      declareOrGetParam<int>(node, ns + "planning_attempts", planning_attempts, 5);
+      declareOrGetParam<double>(node, ns + "max_velocity_scaling_factor", max_velocity_scaling_factor, 1.0);
+      declareOrGetParam<double>(node, ns + "max_acceleration_scaling_factor", max_acceleration_scaling_factor, 1.0);
     }
   };
 
+  /// Planner parameters provided with the MotionPlanRequest
+  struct MultiPipelinePlanRequestParameters
+  {
+    MultiPipelinePlanRequestParameters(const rclcpp::Node::SharedPtr& node,
+                                       const std::vector<std::string>& planning_pipeline_names)
+    {
+      multi_plan_request_parameters.reserve(planning_pipeline_names.size());
+
+      for (const auto& planning_pipeline_name : planning_pipeline_names)
+      {
+        PlanRequestParameters parameters;
+        parameters.load(node, 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(
+      const std::vector<planning_interface::MotionPlanResponse>& solutions)>
+      SolutionCallbackFunction;
+  /** \brief A stopping criterion callback function for the parallel planning API of planning component */
+  typedef std::function<bool(const PlanSolutions& solutions,
+                             const MultiPipelinePlanRequestParameters& plan_request_parameters)>
+      StoppingCriterionFunction;
+
   /** \brief Constructor */
   PlanningComponent(const std::string& group_name, const rclcpp::Node::SharedPtr& node);
   PlanningComponent(const std::string& group_name, const MoveItCppPtr& moveit_cpp);
@@ -154,19 +183,29 @@ class PlanningComponent
   /** \brief Set the path constraints generated from a moveit msg Constraints */
   bool setPathConstraints(const moveit_msgs::msg::Constraints& path_constraints);
 
+  /** \brief Set the trajectory constraints generated from a moveit msg Constraints */
+  bool setTrajectoryConstraints(const moveit_msgs::msg::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,
+       SolutionCallbackFunction solution_selection_callback = &getShortestSolution,
+       StoppingCriterionFunction stopping_criterion_callback = nullptr);
 
   /** \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();
+  const planning_interface::MotionPlanResponse& getLastMotionPlanResponse();
 
 private:
   // Core properties and instances
@@ -182,10 +221,11 @@ class PlanningComponent
   moveit::core::RobotStatePtr considered_start_state_;
   std::vector<moveit_msgs::msg::Constraints> current_goal_constraints_;
   moveit_msgs::msg::Constraints current_path_constraints_;
+  moveit_msgs::msg::TrajectoryConstraints current_trajectory_constraints_;
   PlanRequestParameters plan_request_parameters_;
   moveit_msgs::msg::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

@@ -276,6 +276,25 @@ MoveItCpp::execute(const std::string& group_name, const robot_trajectory::RobotT
   return moveit_controller_manager::ExecutionStatus::RUNNING;
 }
 
+bool MoveItCpp::terminatePlanningPipeline(const std::string& pipeline_name)
+{
+  try
+  {
+    const auto& planning_pipeline = planning_pipelines_.at(pipeline_name);
+    if (planning_pipeline->isActive())
+    {
+      planning_pipeline->terminate();
+    }
+    return true;
+  }
+  catch (const std::out_of_range& oor)
+  {
+    RCLCPP_ERROR(LOGGER, "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 planning_scene_monitor_->getTFClient();