Parallel planning pipelines

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();