44 namespace planning_pipeline_interfaces
55 const std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr>& planning_pipelines)
58 auto it = planning_pipelines.find(motion_plan_request.pipeline_id);
59 if (it == planning_pipelines.end())
61 RCLCPP_ERROR(
getLogger(),
"No planning pipeline available for name '%s'", motion_plan_request.pipeline_id.c_str());
62 motion_plan_response.
error_code = moveit::core::MoveItErrorCode::FAILURE;
63 return motion_plan_response;
65 const planning_pipeline::PlanningPipelinePtr pipeline = it->second;
66 if (!pipeline->generatePlan(
planning_scene, motion_plan_request, motion_plan_response))
68 motion_plan_response.
error_code = moveit::core::MoveItErrorCode::FAILURE;
70 return motion_plan_response;
74 const std::vector<::planning_interface::MotionPlanRequest>& motion_plan_requests,
76 const std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr>& planning_pipelines,
82 std::vector<std::thread> planning_threads;
83 planning_threads.reserve(motion_plan_requests.size());
87 const auto hardware_concurrency = std::thread::hardware_concurrency();
88 if (motion_plan_requests.size() > hardware_concurrency && hardware_concurrency != 0)
91 "More parallel planning problems defined ('%ld') than possible to solve concurrently with the "
93 motion_plan_requests.size(), hardware_concurrency);
97 for (
const auto& request : motion_plan_requests)
99 auto planning_thread = std::thread([&]() {
106 catch (
const std::exception& e)
108 RCLCPP_ERROR(
getLogger(),
"Planning pipeline '%s' threw exception '%s'", request.pipeline_id.c_str(), e.what());
110 plan_solution.error_code = moveit::core::MoveItErrorCode::FAILURE;
112 plan_solution.planner_id = request.planner_id;
113 plan_responses_container.pushBack(plan_solution);
115 if (stopping_criterion_callback !=
nullptr)
117 if (stopping_criterion_callback(plan_responses_container, motion_plan_requests))
120 RCLCPP_INFO(
getLogger(),
"Stopping criterion met: Terminating planning pipelines that are still active");
121 for (
const auto& request : motion_plan_requests)
131 catch (
const std::out_of_range&)
133 RCLCPP_WARN(
getLogger(),
"Cannot terminate pipeline '%s' because no pipeline with that name exists",
134 request.pipeline_id.c_str());
140 planning_threads.push_back(std::move(planning_thread));
144 for (
auto& planning_thread : planning_threads)
146 if (planning_thread.joinable())
148 planning_thread.join();
153 if (solution_selection_function)
155 std::vector<::planning_interface::MotionPlanResponse> solutions;
156 solutions.reserve(1);
157 solutions.push_back(solution_selection_function(plan_responses_container.getSolutions()));
162 return plan_responses_container.getSolutions();
165 std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr>
167 const moveit::core::RobotModelConstPtr& robot_model,
const rclcpp::Node::SharedPtr& node,
168 const std::string& parameter_namespace)
170 std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr> planning_pipelines;
171 for (
const auto& planning_pipeline_name : pipeline_names)
174 if (planning_pipelines.count(planning_pipeline_name) > 0)
176 RCLCPP_WARN(
getLogger(),
"Skipping duplicate entry for planning pipeline '%s'.", planning_pipeline_name.c_str());
181 planning_pipeline::PlanningPipelinePtr pipeline = std::make_shared<planning_pipeline::PlanningPipeline>(
182 robot_model, node, parameter_namespace + planning_pipeline_name);
186 RCLCPP_ERROR(
getLogger(),
"Failed to initialize planning pipeline '%s'.", planning_pipeline_name.c_str());
190 planning_pipelines[planning_pipeline_name] = pipeline;
193 return planning_pipelines;
A container to thread-safely store multiple MotionPlanResponses.
::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.
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 ¶meter_namespace=std::string())
Utility function to create a map of named planning pipelines.
rclcpp::Logger getLogger()
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.
const std::vector<::planning_interface::MotionPlanResponse > 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 ...
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.
Main namespace for MoveIt.
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest
This namespace includes the central class for representing planning contexts.
Response to a planning query.
moveit::core::MoveItErrorCode error_code