moveit2
The MoveIt Motion Planning Framework for ROS 2.
Classes | Typedefs | Functions
moveit::planning_pipeline_interfaces Namespace Reference

Classes

class  PlanResponsesContainer
 A container to thread-safely store multiple MotionPlanResponses. More...
 

Typedefs

typedef std::function< bool(const PlanResponsesContainer &plan_responses_container, const std::vector<::planning_interface::MotionPlanRequest > &plan_requests)> StoppingCriterionFunction
 A stopping criterion callback function for the parallel planning API of planning component. More...
 
typedef std::function<::planning_interface::MotionPlanResponse(const std::vector<::planning_interface::MotionPlanResponse > &solutions)> SolutionSelectionFunction
 A solution callback function type for the parallel planning API of planning component. More...
 

Functions

 MOVEIT_CLASS_FORWARD (PlanResponsesContainer)
 
::planning_interface::MotionPlanResponse planWithSinglePipeline (const ::planning_interface::MotionPlanRequest &motion_plan_request, const ::planning_scene::PlanningSceneConstPtr &planning_scene, const std::unordered_map< std::string, planning_pipeline::PlanningPipelinePtr > &planning_pipelines)
 Function to calculate the MotionPlanResponse for a given MotionPlanRequest and a PlanningScene. More...
 
const std::vector<::planning_interface::MotionPlanResponseplanWithParallelPipelines (const std::vector<::planning_interface::MotionPlanRequest > &motion_plan_requests, const ::planning_scene::PlanningSceneConstPtr &planning_scene, const std::unordered_map< std::string, planning_pipeline::PlanningPipelinePtr > &planning_pipelines, const StoppingCriterionFunction &stopping_criterion_callback=nullptr, const SolutionSelectionFunction &solution_selection_function=nullptr)
 Function to solve multiple planning problems in parallel threads with multiple planning pipelines at the same time. More...
 
std::unordered_map< std::string, planning_pipeline::PlanningPipelinePtr > createPlanningPipelineMap (const std::vector< std::string > &pipeline_names, const moveit::core::RobotModelConstPtr &robot_model, const rclcpp::Node::SharedPtr &node, const std::string &parameter_namespace=std::string())
 Utility function to create a map of named planning pipelines. More...
 
::planning_interface::MotionPlanResponse getShortestSolution (const std::vector<::planning_interface::MotionPlanResponse > &solutions)
 Function that returns the shortest solution out of a vector of solutions based on robot_trajectory::pathLength(...) More...
 
bool stopAtFirstSolution (const PlanResponsesContainer &plan_responses_container, const std::vector<::planning_interface::MotionPlanRequest > &plan_requests)
 A callback function that can be used as a parallel planning stop criterion. It stops parallel planning as soon as any planner finds a solution. More...
 
rclcpp::Logger getLogger ()
 

Typedef Documentation

◆ SolutionSelectionFunction

A solution callback function type for the parallel planning API of planning component.

Parameters
[in]solutionsMotion plan responses to choose from
Returns
Selected motion plan response

Definition at line 65 of file planning_pipeline_interfaces.hpp.

◆ StoppingCriterionFunction

typedef std::function<bool(const PlanResponsesContainer& plan_responses_container, const std::vector<::planning_interface::MotionPlanRequest>& plan_requests)> moveit::planning_pipeline_interfaces::StoppingCriterionFunction

A stopping criterion callback function for the parallel planning API of planning component.

Parameters
[in]plan_responses_containerContainer with responses to be taken into account for the stopping decision
[in]plan_requestsMotion plan requests for the parallel planner
Returns
True to stop parallel planning and abort planning pipelines that haven't terminated by now

Definition at line 57 of file planning_pipeline_interfaces.hpp.

Function Documentation

◆ createPlanningPipelineMap()

std::unordered_map< std::string, planning_pipeline::PlanningPipelinePtr > moveit::planning_pipeline_interfaces::createPlanningPipelineMap ( const std::vector< std::string > &  pipeline_names,
const moveit::core::RobotModelConstPtr &  robot_model,
const rclcpp::Node::SharedPtr &  node,
const std::string &  parameter_namespace = std::string() 
)

Utility function to create a map of named planning pipelines.

Parameters
[in]pipeline_namesVector of planning pipeline names to be used. Each name is also the namespace from which the pipeline parameters are loaded
[in]robot_modelRobot model used to initialize the pipelines
[in]nodeNode used to load parameters
[in]parameter_namespaceOptional prefix for the pipeline parameter namespace. Empty by default, so only the pipeline name is used as namespace
Returns
Map of PlanningPipelinePtr's associated with a name for faster look-up

Definition at line 166 of file planning_pipeline_interfaces.cpp.

Here is the call graph for this function:

◆ getLogger()

rclcpp::Logger moveit::planning_pipeline_interfaces::getLogger ( )

Definition at line 47 of file planning_pipeline_interfaces.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ getShortestSolution()

planning_interface::MotionPlanResponse moveit::planning_pipeline_interfaces::getShortestSolution ( const std::vector<::planning_interface::MotionPlanResponse > &  solutions)

Function that returns the shortest solution out of a vector of solutions based on robot_trajectory::pathLength(...)

Parameters
[in]solutionsVector of solutions to chose the shortest one from
Returns
Shortest solution, trajectory of the returned MotionPlanResponse is a nullptr if no solution is found!

Definition at line 44 of file solution_selection_functions.cpp.

Here is the caller graph for this function:

◆ MOVEIT_CLASS_FORWARD()

moveit::planning_pipeline_interfaces::MOVEIT_CLASS_FORWARD ( PlanResponsesContainer  )

◆ planWithParallelPipelines()

const std::vector<::planning_interface::MotionPlanResponse > moveit::planning_pipeline_interfaces::planWithParallelPipelines ( const std::vector<::planning_interface::MotionPlanRequest > &  motion_plan_requests,
const ::planning_scene::PlanningSceneConstPtr &  planning_scene,
const std::unordered_map< std::string, planning_pipeline::PlanningPipelinePtr > &  planning_pipelines,
const StoppingCriterionFunction stopping_criterion_callback = nullptr,
const SolutionSelectionFunction solution_selection_function = nullptr 
)

Function to solve multiple planning problems in parallel threads with multiple planning pipelines at the same time.

Parameters
[in]motion_plan_requestMotion planning problems to be solved
[in]planning_scenePlanning scene for which the given planning problem needs to be solved
[in]planning_pipelinesPipelines available to solve the problems, if a requested pipeline is not provided the MotionPlanResponse will be FAILURE
[in]stopping_criterion_callbackIf this function returns true, the planning pipelines that are still running will be terminated and the existing solutions will be evaluated. If no callback is provided, all planning pipelines terminate after the max. planning time defined in the MotionPlanningRequest is reached.
[in]solution_selection_functionFunction to select a specific solution out of all available solution. If no function is provided, all solutions are returned.
Returns
If a solution_selection_function is provided a vector containing the selected response is returned, otherwise the vector contains all solutions produced.

Definition at line 73 of file planning_pipeline_interfaces.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ planWithSinglePipeline()

planning_interface::MotionPlanResponse moveit::planning_pipeline_interfaces::planWithSinglePipeline ( const ::planning_interface::MotionPlanRequest motion_plan_request,
const ::planning_scene::PlanningSceneConstPtr &  planning_scene,
const std::unordered_map< std::string, planning_pipeline::PlanningPipelinePtr > &  planning_pipelines 
)

Function to calculate the MotionPlanResponse for a given MotionPlanRequest and a PlanningScene.

Parameters
[in]motion_plan_requestMotion planning problem to be solved
[in]planning_scenePlanning scene for which the given planning problem needs to be solved
[in]planning_pipelinesPipelines available to solve the problem, if the requested pipeline is not provided the MotionPlanResponse will be FAILURE
Returns
MotionPlanResponse for the given planning problem

Definition at line 53 of file planning_pipeline_interfaces.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ stopAtFirstSolution()

bool moveit::planning_pipeline_interfaces::stopAtFirstSolution ( const PlanResponsesContainer plan_responses_container,
const std::vector<::planning_interface::MotionPlanRequest > &  plan_requests 
)

A callback function that can be used as a parallel planning stop criterion. It stops parallel planning as soon as any planner finds a solution.

Definition at line 43 of file stopping_criterion_function.cpp.

Here is the call graph for this function: