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 79ce7ed721d..c5c92aaf40e 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 1691fe69b36..ac03093280b 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 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; pipeline->generatePlan(planning_scene, req, res); plan_solution.error_code_ = res.error_code_;