Skip to content

Commit

Permalink
Enable parallel planning with PipelinePlanner moveit#450
Browse files Browse the repository at this point in the history 
commit 8e2c560
Author: Sebastian Jahr <sebastian.jahr@picknik.ai>
Date:   Mon Apr 17 09:49:51 2023 +0200

    Use no default stopping criteria

commit d095f7f
Author: Sebastian Jahr <sebastian.jahr@picknik.ai>
Date:   Fri Apr 14 12:20:44 2023 +0200

    Refactor to avoid calling .at(0) twice

commit 4238dc3
Author: Sebastian Jahr <sebastian.jahr@picknik.ai>
Date:   Fri Apr 14 12:10:45 2023 +0200

    Format

commit 2c74526
Author: Sebastian Jahr <sebastian.jahr@tuta.io>
Date:   Fri Apr 14 10:39:26 2023 +0200

    Update core/src/solvers/pipeline_planner.cpp

commit f06a93c
Author: Sebastian Jahr <sebastian.jahr@tuta.io>
Date:   Fri Apr 14 10:34:02 2023 +0200

    Update core/src/solvers/pipeline_planner.cpp

    Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>

commit 168cbd5
Author: Sebastian Jahr <sebastian.jahr@picknik.ai>
Date:   Tue Apr 11 11:14:19 2023 +0200

    Small clang-tidy fix

commit c0ea5cd
Author: Sebastian Jahr <sebastian.jahr@picknik.ai>
Date:   Thu Apr 6 10:34:20 2023 +0200

    Pass pipeline map by reference

commit 9be2ab3
Author: Sebastian Jahr <sebastian.jahr@picknik.ai>
Date:   Fri Mar 31 17:55:58 2023 +0200

    Add API to set parallel planning callbacks and deprecate functions

commit 132235f
Author: Sebastian Jahr <sebastian.jahr@picknik.ai>
Date:   Thu Mar 30 20:18:50 2023 +0200

    Cleanup and documentation

commit 7dd2743
Author: Sebastian Jahr <sebastian.jahr@picknik.ai>
Date:   Thu Mar 30 17:12:44 2023 +0200

    Add callbacks

commit f050adb
Author: Sebastian Jahr <sebastian.jahr@picknik.ai>
Date:   Thu Mar 30 16:00:25 2023 +0200

    Enable configuring multiple pipelines

commit b96992c
Author: Sebastian Jahr <sebastian.jahr@picknik.ai>
Date:   Thu Mar 30 10:29:47 2023 +0200

    Make usable with parallel planning

commit 2a7234d
Author: Sebastian Jahr <sebastian.jahr@picknik.ai>
Date:   Thu Mar 30 09:47:38 2023 +0200

    Re-order plan functions

commit 6679c98
Author: Sebastian Jahr <sebastian.jahr@picknik.ai>
Date:   Tue Mar 28 15:28:02 2023 +0200

    Make code readable
  • Loading branch information
@sjahr
sjahr committed May 3, 2023
1 parent d7d8c58 commit 609fc20
Show file tree
Hide file tree
Showing 2 changed files with 258 additions and 159 deletions.
149 changes: 118 additions & 31 deletions core/include/moveit/task_constructor/solvers/pipeline_planner.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,13 +32,16 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Authors: Robert Haschke
Desc: plan using MoveIt's PlanningPipeline
/* Authors: Robert Haschke, Sebastian Jahr
Description: Solver that uses a set of MoveIt PlanningPipelines to solve a given planning problem.
*/

#pragma once

#include <moveit/task_constructor/solvers/planner_interface.h>
#include <moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp>
#include <moveit/planning_pipeline_interfaces/solution_selection_functions.hpp>
#include <moveit/planning_pipeline_interfaces/stopping_criterion_functions.hpp>
#include <rclcpp/node.hpp>
#include <moveit/macros/class_forward.h>

Expand All @@ -56,48 +59,132 @@ MOVEIT_CLASS_FORWARD(PipelinePlanner);
class PipelinePlanner : public PlannerInterface
{
public:
struct Specification
{
moveit::core::RobotModelConstPtr model;
std::string ns{ "ompl" };
std::string pipeline{ "ompl" };
std::string adapter_param{ "request_adapters" };
};

static planning_pipeline::PlanningPipelinePtr create(const rclcpp::Node::SharedPtr& node,
const moveit::core::RobotModelConstPtr& model) {
Specification spec;
spec.model = model;
return create(node, spec);
/** Simple Constructor to use only a single pipeline
* \param [in] node ROS 2 node
* \param [in] pipeline_name Name of the planning pipeline to be used. This is also the assumed namespace where the
* parameters of this pipeline can be found \param [in] planner_id Planner id to be used for planning
*/
PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name = "ompl",
const std::string& planner_id = "");

[[deprecated("Deprecated: Use new constructor implementations.")]] // clang-format off
PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& /*planning_pipeline*/){};

/** \brief Constructor
* \param [in] node ROS 2 node
* \param [in] pipeline_id_planner_id_map A map containing pairs of planning pipeline name and planner plugin name
* for the planning requests
* \param [in] planning_pipelines Optional: A map with the pipeline names and initialized corresponding planning
* pipelines
* \param [in] stopping_criterion_callback Callback function to stop parallel planning pipeline according to a user defined criteria
* \param [in] solution_selection_function Callback function to choose the best solution when multiple pipelines are used
*/
PipelinePlanner(const rclcpp::Node::SharedPtr& node,
const std::unordered_map<std::string, std::string>& pipeline_id_planner_id_map,
const std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr>& planning_pipelines =
std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr>(),
const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback = nullptr,
const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function =
&moveit::planning_pipeline_interfaces::getShortestSolution);

[[deprecated("Replaced with setPlannerId(pipeline_name, planner_id)")]] // clang-format off
void setPlannerId(const std::string& /*planner*/) { /* Do nothing */
}

static planning_pipeline::PlanningPipelinePtr create(const rclcpp::Node::SharedPtr& node, const Specification& spec);

/**
*
* @param node used to load the parameters for the planning pipeline
/** \brief Set the planner id for a specific planning pipeline for the planning requests
* \param [in] pipeline_name Name of the planning pipeline for which the planner id is set
* \param [in] planner_id Name of the planner ID that should be used by the planning pipeline
* \return true if the pipeline exists and the corresponding ID is set successfully
*/
bool setPlannerId(const std::string& pipeline_name, const std::string& planner_id);

/** \brief Set stopping criterion function for parallel planning
* \param [in] stopping_criterion_callback New stopping criterion function that will be used
*/
void setStoppingCriterionFunction(const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback);

/** \brief Set solution selection function for parallel planning
* \param [in] solution_selection_function New solution selection that will be used
*/
void setSolutionSelectionFunction(const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function);

/** \brief This function is called when an MTC task that uses this solver is initialized. If no pipelines are
* configured when this function is invoked, the planning pipelines named in the 'pipeline_id_planner_id_map' are
* initialized with the given robot model.
* \param [in] robot_model A robot model that is used to initialize the
* planning pipelines of this solver
*/
PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline = "ompl");

PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline);

void setPlannerId(const std::string& planner) { setProperty("planner", planner); }

void init(const moveit::core::RobotModelConstPtr& robot_model) override;

/**
* \brief Plan a trajectory from a planning scene 'from' to scene 'to'
* \param [in] from Start planning scene
* \param [in] to Goal planning scene (used to create goal constraints)
* \param [in] joint_model_group Group of joints for which this trajectory is created
* \param [in] timeout ?
* \param [in] result Reference to the location where the created trajectory is stored if planning is successful
* \param [in] path_constraints Path contraints for the planning problem
* \return true If the solver found a successful solution for the given planning problem
*/
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const core::JointModelGroup* joint_model_group, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;

/** \brief Plan a trajectory from a planning scene 'from' to a target pose with an offset
* \param [in] from Start planning scene
* \param [in] link Link for which a target pose is given
* \param [in] offset Offset to be applied to a given target pose
* \param [in] target Target pose
* \param [in] joint_model_group Group of joints for which this trajectory is created
* \param [in] timeout ?
* \param [in] result Reference to the location where the created trajectory is stored if planning is successful
* \param [in] path_constraints Path contraints for the planning problem
* \return true If the solver found a successful solution for the given planning problem
*/
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
const moveit::core::JointModelGroup* joint_model_group, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;

protected:
std::string pipeline_name_;
planning_pipeline::PlanningPipelinePtr planner_;
/** \brief Function that actually uses the planning pipelines to solve the given planning problem. It is called by
* the public plan function after the goal constraints are generated. This function uses a predefined number of
* planning pipelines in parallel to solve the planning problem and choose the automatically the best (user-defined)
* solution.
* \param [in] planning_scene Scene for which the planning should be solved
* \param [in] joint_model_group
* Group of joints for which this trajectory is created
* \param [in] goal_constraints Set of constraints that need to
* be satisfied by the solution
* \param [in] timeout ?
* \param [in] result Reference to the location where the created
* trajectory is stored if planning is successful
* \param [in] path_constraints Path contraints for the planning
* problem
* \return true If the solver found a successful solution for the given planning problem
*/
bool plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
const moveit::core::JointModelGroup* joint_model_group,
const moveit_msgs::msg::Constraints& goal_constraints, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints());

rclcpp::Node::SharedPtr node_;

/** \brief Map of pipeline names (ids) and their corresponding planner ids. The planning problem is solved for every
* pipeline-planner pair in this map. If no pipelines are passed via constructor argument, the pipeline names are
* used to initialize a set of planning pipelines. */
std::unordered_map<std::string, std::string> pipeline_id_planner_id_map_;

/** \brief Map of pipelines names and planning pipelines. This map is used to quickly search for a requested motion
* planning pipeline when during plan(..) */
std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr> planning_pipelines_;

moveit::planning_pipeline_interfaces::StoppingCriterionFunction stopping_criterion_callback_;
moveit::planning_pipeline_interfaces::SolutionSelectionFunction solution_selection_function_;

};
} // namespace solvers
} // namespace task_constructor
Expand Down
Loading

0 comments on commit 609fc20

Please sign in to comment.