Skip to content

Commit f57e659

Browse files
committed
Enabling optionally using a property defined pipeline planner map
1 parent 8118309 commit f57e659

File tree

1 file changed

+8
-1
lines changed

1 file changed

+8
-1
lines changed

core/src/solvers/pipeline_planner.cpp

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -90,6 +90,9 @@ PipelinePlanner::PipelinePlanner(
9090
properties().declare<bool>("publish_planning_requests", false,
9191
"publish motion planning requests on topic " +
9292
planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC);
93+
properties().declare<std::unordered_map<std::string, std::string>>(
94+
"pipeline_id_planner_id_map", std::unordered_map<std::string, std::string>(),
95+
"Set of pipelines and planners used for planning");
9396
}
9497

9598
bool PipelinePlanner::setPlannerId(const std::string& pipeline_name, const std::string& planner_id) {
@@ -173,7 +176,11 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning
173176
std::vector<moveit_msgs::msg::MotionPlanRequest> requests;
174177
requests.reserve(pipeline_id_planner_id_map_.size());
175178

176-
for (auto const& pipeline_id_planner_id_pair : pipeline_id_planner_id_map_) {
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_)) {
177184
// Check that requested pipeline exists and skip it if it doesn't exist
178185
if (planning_pipelines_.find(pipeline_id_planner_id_pair.first) == planning_pipelines_.end()) {
179186
RCLCPP_WARN(

0 commit comments

Comments
 (0)