Skip to content

Commit b5721c3

Browse files
sjahrsjahr
committed
Enable configuring multiple pipelines
1 parent de2440d commit b5721c3

File tree

2 files changed

+72
-25
lines changed

2 files changed

+72
-25
lines changed

‎core/include/moveit/task_constructor/solvers/pipeline_planner.h‎

Lines changed: 13 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -73,21 +73,20 @@ class PipelinePlanner : public PlannerInterface
7373
return create(node, spec);
7474
}*/
7575

76-
/**
77-
*
78-
* @param node used to load the parameters for the planning pipeline
79-
*/
80-
PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_namespace = "ompl");
76+
// TODO: Deprecate
77+
PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name = "ompl",
78+
const std::string& planner_id = "");
8179

82-
// PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::vector<std::string>& pipeline_namespaces);
80+
PipelinePlanner(const rclcpp::Node::SharedPtr& node,
81+
const std::unordered_map<std::string, std::string>& pipeline_id_planner_id_map,
82+
const std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr> planning_pipelines =
83+
std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr>());
8384

84-
// PipelinePlanner(const std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr>&
85-
// planning_pipelines);
85+
// TODO: Deprecate
86+
void setPlannerId(const std::string& /*planner*/) { /* Do nothing */
87+
}
8688

87-
// PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline, const std::string& pipeline =
88-
// "");
89-
90-
void setPlannerId(const std::string& planner) { setProperty("planner", planner); }
89+
bool setPlannerId(const std::string& pipeline_name, const std::string& planner_id);
9190

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

@@ -109,9 +108,9 @@ class PipelinePlanner : public PlannerInterface
109108
robot_trajectory::RobotTrajectoryPtr& result,
110109
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints());
111110

112-
std::vector<std::string> pipeline_names_ = std::vector<std::string>(1);
113-
std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr> planning_pipelines_;
114111
rclcpp::Node::SharedPtr node_;
112+
std::unordered_map<std::string, std::string> pipeline_id_planner_id_map_;
113+
std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr> planning_pipelines_;
115114
};
116115
} // namespace solvers
117116
} // namespace task_constructor

‎core/src/solvers/pipeline_planner.cpp‎

Lines changed: 59 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -50,6 +50,10 @@
5050
#include <tf2_eigen/tf2_eigen.h>
5151
#endif
5252

53+
namespace {
54+
const std::pair<std::string, std::string> DEFAULT_REQUESTED_PIPELINE =
55+
std::pair<std::string, std::string>("ompl", "RRTConnect");
56+
}
5357
namespace moveit {
5458
namespace task_constructor {
5559
namespace solvers {
@@ -114,10 +118,25 @@ planning_pipeline::PlanningPipelinePtr PipelinePlanner::create(const rclcpp::Nod
114118
return planner;
115119
}*/
116120

117-
PipelinePlanner::PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name) : node_(node) {
118-
pipeline_names_.at(0) = pipeline_name;
119-
properties().declare<std::string>("planner", "", "planner id");
121+
PipelinePlanner::PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name,
122+
const std::string& planner_id)
123+
: PipelinePlanner(node, [&]() {
124+
std::unordered_map<std::string, std::string> pipeline_id_planner_id_map;
125+
pipeline_id_planner_id_map[pipeline_name] = planner_id;
126+
return pipeline_id_planner_id_map;
127+
}()) {}
120128

129+
PipelinePlanner::PipelinePlanner(
130+
const rclcpp::Node::SharedPtr& node, const std::unordered_map<std::string, std::string>& pipeline_id_planner_id_map,
131+
const std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr> planning_pipelines)
132+
: node_(node), pipeline_id_planner_id_map_(pipeline_id_planner_id_map) {
133+
for (auto pipeline_id_planner_id_pair : pipeline_id_planner_id_map_) {
134+
RCLCPP_WARN(node_->get_logger(), "'%s' : ''%s", pipeline_id_planner_id_pair.first.c_str(),
135+
pipeline_id_planner_id_pair.second.c_str());
136+
}
137+
if (!planning_pipelines.empty()) {
138+
planning_pipelines_ = planning_pipelines;
139+
}
121140
properties().declare<uint>("num_planning_attempts", 1u, "number of planning attempts");
122141
properties().declare<moveit_msgs::msg::WorkspaceParameters>(
123142
"workspace_parameters", moveit_msgs::msg::WorkspaceParameters(), "allowed workspace of mobile base?");
@@ -139,9 +158,26 @@ PipelinePlanner::PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std:
139158
// planning_pipelines_.at(0) = planning_pipeline;
140159
//}
141160

161+
bool PipelinePlanner::setPlannerId(const std::string& pipeline_name, const std::string& planner_id) {
162+
if (pipeline_id_planner_id_map_.count(pipeline_name) > 0) {
163+
pipeline_id_planner_id_map_[pipeline_name] = planner_id;
164+
}
165+
RCLCPP_WARN(node_->get_logger(), "PipelinePlanner does not have a pipeline called '%s'", pipeline_name.c_str());
166+
return false;
167+
}
168+
142169
void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) {
143-
planning_pipelines_ =
144-
moveit::planning_pipeline_interfaces::createPlanningPipelineMap(pipeline_names_, robot_model, node_);
170+
if (planning_pipelines_.empty()) {
171+
planning_pipelines_ = moveit::planning_pipeline_interfaces::createPlanningPipelineMap(
172+
[&]() {
173+
std::vector<std::string> pipeline_names;
174+
for (const auto& pipeline_name_planner_id_pair : pipeline_id_planner_id_map_) {
175+
pipeline_names.push_back(pipeline_name_planner_id_pair.first);
176+
}
177+
return pipeline_names;
178+
}(),
179+
robot_model, node_);
180+
}
145181

146182
for (auto const& name_pipeline_pair : planning_pipelines_) {
147183
name_pipeline_pair.second->displayComputedMotionPlans(properties().get<bool>("display_motion_plans"));
@@ -181,13 +217,22 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning
181217
robot_trajectory::RobotTrajectoryPtr& result,
182218
const moveit_msgs::msg::Constraints& path_constraints) {
183219
std::vector<moveit_msgs::msg::MotionPlanRequest> requests;
184-
requests.reserve(pipeline_names_.size());
220+
requests.reserve(pipeline_id_planner_id_map_.size());
185221

186-
for (auto const& pipeline_name : pipeline_names_) {
222+
for (auto const& pipeline_id_planner_id_pair : pipeline_id_planner_id_map_) {
223+
RCLCPP_WARN(node_->get_logger(), "'%s' : ''%s", pipeline_id_planner_id_pair.first.c_str(),
224+
pipeline_id_planner_id_pair.second.c_str());
225+
if (planning_pipelines_.find(pipeline_id_planner_id_pair.first) == planning_pipelines_.end()) {
226+
RCLCPP_WARN(
227+
node_->get_logger(),
228+
"Pipeline '%s' is not available of this PipelineSolver instance. Skipping a request for this pipeline.",
229+
pipeline_id_planner_id_pair.first.c_str());
230+
continue;
231+
}
187232
moveit_msgs::msg::MotionPlanRequest request;
188-
request.pipeline_id = pipeline_name;
233+
request.pipeline_id = pipeline_id_planner_id_pair.first;
189234
request.group_name = joint_model_group->getName();
190-
request.planner_id = properties().get<std::string>("planner");
235+
request.planner_id = pipeline_id_planner_id_pair.second;
191236
request.allowed_planning_time = timeout;
192237
request.start_state.is_diff = true; // we don't specify an extra start state
193238
request.num_planning_attempts = properties().get<uint>("num_planning_attempts");
@@ -204,8 +249,11 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning
204249
moveit::planning_pipeline_interfaces::planWithParallelPipelines(requests, planning_scene, planning_pipelines_);
205250

206251
// Just choose first result
207-
result = responses.at(0).trajectory;
208-
return bool(result);
252+
if (!responses.empty()) {
253+
result = responses.at(0).trajectory;
254+
return bool(result);
255+
}
256+
return false;
209257
}
210258
} // namespace solvers
211259
} // namespace task_constructor

0 commit comments

Comments
 (0)