@@ -53,28 +53,15 @@ namespace moveit {
5353namespace task_constructor {
5454namespace solvers {
5555
56- PipelinePlanner::PipelinePlanner (const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name,
57- const std::string& planner_id)
58- : PipelinePlanner(node, [&]() {
59- std::unordered_map<std::string, std::string> pipeline_id_planner_id_map;
60- pipeline_id_planner_id_map[pipeline_name] = planner_id;
61- return pipeline_id_planner_id_map;
62- }()) {}
56+ using PipelineMap = std::unordered_map<std::string, std::string>;
6357
6458PipelinePlanner::PipelinePlanner (
65- const rclcpp::Node::SharedPtr& node, const std::unordered_map<std::string, std::string>& pipeline_id_planner_id_map,
66- const std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr>& planning_pipelines,
59+ const rclcpp::Node::SharedPtr& node, const PipelineMap& pipeline_id_planner_id_map,
6760 const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback,
6861 const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function)
6962 : node_(node)
70- , pipeline_id_planner_id_map_(pipeline_id_planner_id_map)
7163 , stopping_criterion_callback_(stopping_criterion_callback)
7264 , solution_selection_function_(solution_selection_function) {
73- // If the pipeline name - pipeline map is passed as constructor argument, use it. Otherwise, it will be created in
74- // the init(..) function
75- if (!planning_pipelines.empty ()) {
76- planning_pipelines_ = planning_pipelines;
77- }
7865 // Declare properties of the MotionPlanRequest
7966 properties ().declare <uint>(" num_planning_attempts" 1u , " number of planning attempts"
8067 properties ().declare <moveit_msgs::msg::WorkspaceParameters>(
@@ -84,20 +71,22 @@ PipelinePlanner::PipelinePlanner(
8471 properties ().declare <double >(" goal_position_tolerance" 1e-4 , " tolerance for reaching position goals"
8572 properties ().declare <double >(" goal_orientation_tolerance" 1e-4 , " tolerance for reaching orientation goals"
8673 // Declare properties that configure the planning pipeline
87- properties ().declare <std::unordered_map<std::string, std::string>>(
88- " pipeline_id_planner_id_map"
89- " Set of pipelines and planners used for planning"
74+ properties ().declare (" pipeline_id_planner_id_map"
75+ " Set of pipelines and planners used for planning"
9076}
9177
9278bool PipelinePlanner::setPlannerId (const std::string& pipeline_name, const std::string& planner_id) {
9379 // Only set ID if pipeline exists. It is not possible to create new pipelines with this command.
94- if (pipeline_id_planner_id_map_.count (pipeline_name) == 0 ) {
80+ PipelineMap map =
properties ().
get <PipelineMap>(
" pipeline_id_planner_id_map" );
81+ auto it = map.find (pipeline_name);
82+ if (it == map.end ()) {
9583 RCLCPP_ERROR (node_->get_logger (),
9684 " PipelinePlanner does not have a pipeline called '%s'. Cannot set pipeline ID '%s'"
9785 pipeline_name.c_str (), planner_id.c_str ());
9886 return false ;
9987 }
100- pipeline_id_planner_id_map_[pipeline_name] = planner_id;
88+ it->second = planner_id;
89+ properties ().set (" pipeline_id_planner_id_map" std::move (map));
10190 return true ;
10291}
10392
@@ -111,31 +100,34 @@ void PipelinePlanner::setSolutionSelectionFunction(
111100}
112101
113102void PipelinePlanner::init (const core::RobotModelConstPtr& robot_model) {
114- // If no planning pipelines exist, create them based on the pipeline names provided in pipeline_id_planner_id_map_ .
115- // The assumption here is that all parameters required by the planning pipeline can be found in a namespace that
116- // equals the pipeline name.
103+ // Create planning pipelines once from pipeline_id_planner_id_map .
104+ // We assume that all parameters required by the pipeline can be found
105+ // in the namespace of the pipeline name.
117106 if (planning_pipelines_.empty ()) {
118- planning_pipelines_ = moveit::planning_pipeline_interfaces::createPlanningPipelineMap (
119- [&]() {
120- // Create pipeline name vector from the keys of pipeline_id_planner_id_map_
121- if (pipeline_id_planner_id_map_.empty ()) {
122- throw std::runtime_error (" Cannot initialize PipelinePlanner: No planning pipeline was provided and "
123- " pipeline_id_planner_id_map_ is empty!"
124- }
125-
126- std::vector<std::string> pipeline_names;
127- for (const auto & pipeline_name_planner_id_pair : pipeline_id_planner_id_map_) {
128- pipeline_names.push_back (pipeline_name_planner_id_pair.first );
129- }
130- return pipeline_names;
131- }(),
132- robot_model, node_);
133- }
107+ auto map =
properties ().
get <PipelineMap>(
" pipeline_id_planner_id_map" );
108+ // Create pipeline name vector from the keys of pipeline_id_planner_id_map_
109+ if (map.empty ()) {
110+ throw std::runtime_error (" Cannot initialize PipelinePlanner: pipeline_id_planner_id_map is empty!"
111+ }
134112
135- // Check if it is still empty
136- if (planning_pipelines_.empty ()) {
137- throw std::runtime_error (
138- " Cannot initialize PipelinePlanner: Could not create any valid entries for planning pipeline maps!"
113+ std::vector<std::string> pipeline_names;
114+ for (const auto & pipeline_name_planner_id_pair : map) {
115+ pipeline_names.push_back (pipeline_name_planner_id_pair.first );
116+ }
117+ planning_pipelines_ = moveit::planning_pipeline_interfaces::createPlanningPipelineMap (std::move (pipeline_names),
118+ robot_model, node_);
119+ // Check if it is still empty
120+ if (planning_pipelines_.empty ()) {
121+ throw std::runtime_error (" Failed to initialize PipelinePlanner: Could not create any valid pipeline"
122+ }
123+ } else {
124+ // Validate that all pipelines use the task's robot model
125+ for (const auto & pair : planning_pipelines_) {
126+ if (pair.second ->getRobotModel () != robot_model) {
127+ throw std::runtime_error (
128+ " The robot model of the planning pipeline isn't the same as the task's robot model -- "
129+ }
130+ }
139131 }
140132}
141133
@@ -172,28 +164,23 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning
172164 const moveit_msgs::msg::Constraints& goal_constraints, double timeout,
173165 robot_trajectory::RobotTrajectoryPtr& result,
174166 const moveit_msgs::msg::Constraints& path_constraints) {
167+ const auto & map =
properties ().
get <PipelineMap>(
" pipeline_id_planner_id_map" );
168+
175169 // Create a request for every planning pipeline that should run in parallel
176170 std::vector<moveit_msgs::msg::MotionPlanRequest> requests;
177- requests.reserve (pipeline_id_planner_id_map_ .size ());
171+ requests.reserve (map .size ());
178172
179- auto const property_pipeline_id_planner_id_map =
180- properties ().get <std::unordered_map<std::string, std::string>>(" pipeline_id_planner_id_map"
181- for (auto const & pipeline_id_planner_id_pair :
182- (!property_pipeline_id_planner_id_map.empty () ? property_pipeline_id_planner_id_map :
183- pipeline_id_planner_id_map_)) {
173+ for (const auto & [pipeline_id, planner_id] : map) {
184174 // Check that requested pipeline exists and skip it if it doesn't exist
185- if (planning_pipelines_.find (pipeline_id_planner_id_pair.first ) == planning_pipelines_.end ()) {
186- RCLCPP_WARN (
187- node_->get_logger (),
188- " Pipeline '%s' is not available of this PipelineSolver instance. Skipping a request for this pipeline."
189- pipeline_id_planner_id_pair.first .c_str ());
175+ if (planning_pipelines_.count (pipeline_id) == 0 ) {
176+ RCLCPP_WARN (node_->get_logger (), " Pipeline '%s' was not created. Skipping." c_str ());
190177 continue ;
191178 }
192179 // Create MotionPlanRequest for pipeline
193180 moveit_msgs::msg::MotionPlanRequest request;
194- request.pipeline_id = pipeline_id_planner_id_pair. first ;
181+ request.pipeline_id = pipeline_id ;
195182 request.group_name = joint_model_group->getName ();
196- request.planner_id = pipeline_id_planner_id_pair. second ;
183+ request.planner_id = planner_id ;
197184 request.allowed_planning_time = timeout;
198185 request.start_state .is_diff = true ; // we don't specify an extra start state
199186 request.num_planning_attempts = properties ().get <uint>(" num_planning_attempts"
0 commit comments