Skip to content

Commit

Permalink
Cleanup lookup of planning pipelines
Browse files Browse the repository at this point in the history
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 moveit#1096, moveit#1114, moveit#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 moveit#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), moveit#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().
  • Loading branch information
rhaschke committed Nov 11, 2022
1 parent 1f0f4f4 commit 5a91b0b
Show file tree
Hide file tree
Showing 4 changed files with 4 additions and 22 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -152,10 +152,6 @@ class MoveItCpp
/** \brief Get all loaded planning pipeline instances mapped to their reference names */
const std::map<std::string, planning_pipeline::PlanningPipelinePtr>& getPlanningPipelines() const;

/** \brief Get the names of all loaded planning pipelines. Specify group_name to filter the results by planning group
*/
std::set<std::string> 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();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -216,7 +216,6 @@ class PlanningComponent
const moveit::core::JointModelGroup* joint_model_group_;

// Planning
std::set<std::string> planning_pipeline_names_;
// The start state used in the planning motion request
moveit::core::RobotStatePtr considered_start_state_;
std::vector<moveit_msgs::msg::Constraints> current_goal_constraints_;
Expand Down
12 changes: 0 additions & 12 deletions moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -213,18 +213,6 @@ const std::map<std::string, planning_pipeline::PlanningPipelinePtr>& MoveItCpp::
return planning_pipelines_;
}

std::set<std::string> 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_;
Expand Down
9 changes: 4 additions & 5 deletions moveit_ros/planning/moveit_cpp/src/planning_component.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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 --"
Expand All @@ -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<std::string> PlanningComponent::getNamedTargetStates()
Expand Down Expand Up @@ -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;
Expand All @@ -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_;
Expand Down

0 comments on commit 5a91b0b

Please sign in to comment.