From 849acfdc1dbbaf6d2202f0346e273d3d515f5bf5 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 10 Nov 2022 13:57:58 +0100 Subject: [PATCH] Cleanup lookup of planning pipelines This attempts to cleanup the mess in configuring multiple planning pipelines, which exists since the very beginning and was changed back and forth since then, see #1096, #1114, #1522. This PR removes MoveItCpp::getPlanningPipelineNames(), which was obviously intended initially to provide a planning-group-based filter for all available planning pipelines: A pipeline was discarded for a group, if there were no `planner_configs` defined for that group on the parameter server. As pointed out in #1522, only OMPL actually explicitly declares planner_configs on the parameter server. To enable all other pipelines as well (and thus circumventing the original filter mechanism), #1522 introduced empty dummy planner_configs for all other planners as well (CHOMP + Pilz). This, obviously, renders the whole filter mechanism useless. Thus, here we just remove the function getPlanningPipelineNames(). --- .../include/moveit/moveit_cpp/moveit_cpp.h | 4 ---- .../include/moveit/moveit_cpp/planning_component.h | 1 - moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp | 12 ------------ .../planning/moveit_cpp/src/planning_component.cpp | 9 ++++----- 4 files changed, 4 insertions(+), 22 deletions(-) diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h index 03cdd36038a..c82a25468db 100644 --- a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h +++ b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -152,10 +152,6 @@ class MoveItCpp /** \brief Get all loaded planning pipeline instances mapped to their reference names */ const std::map& getPlanningPipelines() const; - /** \brief Get the names of all loaded planning pipelines. Specify group_name to filter the results by planning group - */ - std::set getPlanningPipelineNames(const std::string& group_name = "") const; - /** \brief Get the stored instance of the planning scene monitor */ const planning_scene_monitor::PlanningSceneMonitorPtr& getPlanningSceneMonitor() const; planning_scene_monitor::PlanningSceneMonitorPtr getPlanningSceneMonitorNonConst(); diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h index af61d194f2c..62d8ff2deea 100644 --- a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h +++ b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h @@ -216,7 +216,6 @@ class PlanningComponent const moveit::core::JointModelGroup* joint_model_group_; // Planning - std::set planning_pipeline_names_; // The start state used in the planning motion request moveit::core::RobotStatePtr considered_start_state_; std::vector current_goal_constraints_; diff --git a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp index 7599d0c49fb..d717c8e6415 100644 --- a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp +++ b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp @@ -213,18 +213,6 @@ const std::map& MoveItCpp:: return planning_pipelines_; } -std::set MoveItCpp::getPlanningPipelineNames(const std::string& group_name) const -{ - if (group_name.empty() || groups_pipelines_map_.count(group_name) == 0) - { - RCLCPP_ERROR(LOGGER, "No planning pipelines loaded for group '%s'. Check planning pipeline and controller setup.", - group_name.c_str()); - return {}; // empty - } - - return groups_pipelines_map_.at(group_name); -} - const planning_scene_monitor::PlanningSceneMonitorPtr& MoveItCpp::getPlanningSceneMonitor() const { return planning_scene_monitor_; diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index 5e1adf67aa5..d1b8f35c4f5 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -57,7 +57,6 @@ PlanningComponent::PlanningComponent(const std::string& group_name, const MoveIt RCLCPP_FATAL_STREAM(LOGGER, error); throw std::runtime_error(error); } - planning_pipeline_names_ = moveit_cpp_->getPlanningPipelineNames(group_name); plan_request_parameters_.load(node_); RCLCPP_DEBUG_STREAM( LOGGER, "Default plan request parameters loaded with --" @@ -79,7 +78,6 @@ PlanningComponent::PlanningComponent(const std::string& group_name, const rclcpp RCLCPP_FATAL_STREAM(LOGGER, error); throw std::runtime_error(error); } - planning_pipeline_names_ = moveit_cpp_->getPlanningPipelineNames(group_name); } const std::vector PlanningComponent::getNamedTargetStates() @@ -178,7 +176,9 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequest // Run planning attempt ::planning_interface::MotionPlanResponse res; - if (planning_pipeline_names_.find(parameters.planning_pipeline) == planning_pipeline_names_.end()) + const auto& pipelines = moveit_cpp_->getPlanningPipelines(); + auto it = pipelines.find(parameters.planning_pipeline); + if (it == pipelines.end()) { RCLCPP_ERROR(LOGGER, "No planning pipeline available for name '%s'", parameters.planning_pipeline.c_str()); plan_solution.error_code_ = moveit::core::MoveItErrorCode::FAILURE; @@ -188,8 +188,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequest } return plan_solution; } - const planning_pipeline::PlanningPipelinePtr pipeline = - moveit_cpp_->getPlanningPipelines().at(parameters.planning_pipeline); + const planning_pipeline::PlanningPipelinePtr pipeline = it->second; pipeline->generatePlan(planning_scene, req, res); plan_solution.error_code_ = res.error_code_;