Skip to content

Commit

Permalink
Merge branch 'cleanup-moveit#450'
Browse files Browse the repository at this point in the history
  • Loading branch information
@rhaschke
rhaschke committed Mar 6, 2024
2 parents 21787b3 + d0ac68e commit 8eb93dc
Show file tree
Hide file tree
Showing 2 changed files with 79 additions and 116 deletions.
96 changes: 36 additions & 60 deletions core/include/moveit/task_constructor/solvers/pipeline_planner.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,63 +61,52 @@ class PipelinePlanner : public PlannerInterface
public:
/** Simple Constructor to use only a single pipeline
* \param [in] node ROS 2 node
* \param [in] pipeline_name Name of the planning pipeline to be used. This is also the assumed namespace where the
* parameters of this pipeline can be found \param [in] planner_id Planner id to be used for planning
* \param [in] pipeline_name Name of the planning pipeline to be used.
* This is also the assumed namespace where the parameters of this pipeline can be found
* \param [in] planner_id Planner id to be used for planning. Empty string means default.
*/
PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name = "ompl",
const std::string& planner_id = "");

[[deprecated("Deprecated: Use new constructor implementations.")]] // clang-format off
PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& /*planning_pipeline*/){};
const std::string& planner_id = "")
: PipelinePlanner(node, { { pipeline_name, planner_id } }) {}

/** \brief Constructor
* \param [in] node ROS 2 node
* \param [in] pipeline_id_planner_id_map A map containing pairs of planning pipeline name and planner plugin name
* for the planning requests
* \param [in] planning_pipelines Optional: A map with the pipeline names and initialized corresponding planning
* pipelines
* \param [in] stopping_criterion_callback Callback function to stop parallel planning pipeline according to a user defined criteria
* \param [in] solution_selection_function Callback function to choose the best solution when multiple pipelines are used
* \param [in] pipeline_id_planner_id_map map containing pairs of pipeline and plugin names to be used for planning
* \param [in] stopping_criterion_callback callback to decide when to stop parallel planning
* \param [in] solution_selection_function callback to choose the best solution from multiple ran pipelines
*/
PipelinePlanner(const rclcpp::Node::SharedPtr& node,
const std::unordered_map<std::string, std::string>& pipeline_id_planner_id_map,
const std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr>& planning_pipelines =
std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr>(),
const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback = nullptr,
const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function =
&moveit::planning_pipeline_interfaces::getShortestSolution);

[[deprecated("Replaced with setPlannerId(pipeline_name, planner_id)")]] // clang-format off
void setPlannerId(const std::string& /*planner*/) { /* Do nothing */
}

/** \brief Set the planner id for a specific planning pipeline for the planning requests
* \param [in] pipeline_name Name of the planning pipeline for which the planner id is set
* \param [in] planner_id Name of the planner ID that should be used by the planning pipeline
PipelinePlanner(
const rclcpp::Node::SharedPtr& node,
const std::unordered_map<std::string, std::string>& pipeline_id_planner_id_map,
const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback = nullptr,
const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function =
&moveit::planning_pipeline_interfaces::getShortestSolution);

/** \brief Set the planner id for a specific planning pipeline
* \param [in] pipeline_name Name of the to-be-used planning pipeline
* \param [in] planner_id Name of the to-be-used planner ID
* \return true if the pipeline exists and the corresponding ID is set successfully
*/
bool setPlannerId(const std::string& pipeline_name, const std::string& planner_id);

/** \brief Set stopping criterion function for parallel planning
* \param [in] stopping_criterion_callback New stopping criterion function that will be used
*/
void setStoppingCriterionFunction(const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback);
*/
void setStoppingCriterionFunction(
const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback);

/** \brief Set solution selection function for parallel planning
* \param [in] solution_selection_function New solution selection that will be used
*/
void setSolutionSelectionFunction(const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function);

/** \brief This function is called when an MTC task that uses this solver is initialized. If no pipelines are
* configured when this function is invoked, the planning pipelines named in the 'pipeline_id_planner_id_map' are
* initialized with the given robot model.
* \param [in] robot_model A robot model that is used to initialize the
* planning pipelines of this solver
*/
void setSolutionSelectionFunction(
const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function);

/** \brief If not yet done, initialize pipelines from pipeline_id_planner_id_map
* \param [in] robot_model robot model used to initialize the planning pipelines of this solver
*/
void init(const moveit::core::RobotModelConstPtr& robot_model) override;

/**
* \brief Plan a trajectory from a planning scene 'from' to scene 'to'
/** \brief Plan a trajectory from a planning scene 'from' to scene 'to'
* \param [in] from Start planning scene
* \param [in] to Goal planning scene (used to create goal constraints)
* \param [in] joint_model_group Group of joints for which this trajectory is created
Expand All @@ -131,7 +120,7 @@ class PipelinePlanner : public PlannerInterface
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;

/** \brief Plan a trajectory from a planning scene 'from' to a target pose with an offset
/** \brief Plan a trajectory from a planning scene 'from' to a Cartesian target pose with an offset
* \param [in] from Start planning scene
* \param [in] link Link for which a target pose is given
* \param [in] offset Offset to be applied to a given target pose
Expand All @@ -149,20 +138,14 @@ class PipelinePlanner : public PlannerInterface
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;

protected:
/** \brief Function that actually uses the planning pipelines to solve the given planning problem. It is called by
* the public plan function after the goal constraints are generated. This function uses a predefined number of
* planning pipelines in parallel to solve the planning problem and choose the best (user-defined) solution.
/** \brief Actual plan() implementation, targeting the given goal_constraints.
* \param [in] planning_scene Scene for which the planning should be solved
* \param [in] joint_model_group
* Group of joints for which this trajectory is created
* \param [in] goal_constraints Set of constraints that need to
* be satisfied by the solution
* \param [in] joint_model_group Group of joints for which this trajectory is created
* \param [in] goal_constraints Set of constraints that need to be satisfied by the solution
* \param [in] timeout Maximum planning timeout for an individual stage that is using the pipeline planner in seconds
* \param [in] result Reference to the location where the created
* trajectory is stored if planning is successful
* \param [in] path_constraints Path contraints for the planning
* problem
* \return true If the solver found a successful solution for the given planning problem
* \param [in] result Reference to the location where the created trajectory is stored if planning is successful
* \param [in] path_constraints Path contraints for the planning problem
* \return true if the solver found a successful solution for the given planning problem
*/
bool plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
const moveit::core::JointModelGroup* joint_model_group,
Expand All @@ -172,18 +155,11 @@ class PipelinePlanner : public PlannerInterface

rclcpp::Node::SharedPtr node_;

/** \brief Map of pipeline names (ids) and their corresponding planner ids. The planning problem is solved for every
* pipeline-planner pair in this map. If no pipelines are passed via constructor argument, the pipeline names are
* used to initialize a set of planning pipelines. */
std::unordered_map<std::string, std::string> pipeline_id_planner_id_map_;

/** \brief Map of pipelines names and planning pipelines. This map is used to quickly search for a requested motion
* planning pipeline when during plan(..) */
/** \brief Map of instantiated (and named) planning pipelines. */
std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr> planning_pipelines_;

moveit::planning_pipeline_interfaces::StoppingCriterionFunction stopping_criterion_callback_;
moveit::planning_pipeline_interfaces::SolutionSelectionFunction solution_selection_function_;

};
} // namespace solvers
} // namespace task_constructor
Expand Down
99 changes: 43 additions & 56 deletions core/src/solvers/pipeline_planner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,28 +53,15 @@ namespace moveit {
namespace task_constructor {
namespace solvers {

PipelinePlanner::PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name,
const std::string& planner_id)
: PipelinePlanner(node, [&]() {
std::unordered_map<std::string, std::string> pipeline_id_planner_id_map;
pipeline_id_planner_id_map[pipeline_name] = planner_id;
return pipeline_id_planner_id_map;
}()) {}
using PipelineMap = std::unordered_map<std::string, std::string>;

PipelinePlanner::PipelinePlanner(
const rclcpp::Node::SharedPtr& node, const std::unordered_map<std::string, std::string>& pipeline_id_planner_id_map,
const std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr>& planning_pipelines,
const rclcpp::Node::SharedPtr& node, const PipelineMap& pipeline_id_planner_id_map,
const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback,
const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function)
: node_(node)
, pipeline_id_planner_id_map_(pipeline_id_planner_id_map)
, stopping_criterion_callback_(stopping_criterion_callback)
, solution_selection_function_(solution_selection_function) {
// If the pipeline name - pipeline map is passed as constructor argument, use it. Otherwise, it will be created in
// the init(..) function
if (!planning_pipelines.empty()) {
planning_pipelines_ = planning_pipelines;
}
// Declare properties of the MotionPlanRequest
properties().declare<uint>("num_planning_attempts", 1u, "number of planning attempts");
properties().declare<moveit_msgs::msg::WorkspaceParameters>(
Expand All @@ -84,20 +71,22 @@ PipelinePlanner::PipelinePlanner(
properties().declare<double>("goal_position_tolerance", 1e-4, "tolerance for reaching position goals");
properties().declare<double>("goal_orientation_tolerance", 1e-4, "tolerance for reaching orientation goals");
// Declare properties that configure the planning pipeline
properties().declare<std::unordered_map<std::string, std::string>>(
"pipeline_id_planner_id_map", std::unordered_map<std::string, std::string>(),
"Set of pipelines and planners used for planning");
properties().declare("pipeline_id_planner_id_map", pipeline_id_planner_id_map,
"Set of pipelines and planners used for planning");
}

bool PipelinePlanner::setPlannerId(const std::string& pipeline_name, const std::string& planner_id) {
// Only set ID if pipeline exists. It is not possible to create new pipelines with this command.
if (pipeline_id_planner_id_map_.count(pipeline_name) == 0) {
PipelineMap map = properties().get<PipelineMap>("pipeline_id_planner_id_map");
auto it = map.find(pipeline_name);
if (it == map.end()) {
RCLCPP_ERROR(node_->get_logger(),
"PipelinePlanner does not have a pipeline called '%s'. Cannot set pipeline ID '%s'",
pipeline_name.c_str(), planner_id.c_str());
return false;
}
pipeline_id_planner_id_map_[pipeline_name] = planner_id;
it->second = planner_id;
properties().set("pipeline_id_planner_id_map", std::move(map));
return true;
}

Expand All @@ -111,31 +100,34 @@ void PipelinePlanner::setSolutionSelectionFunction(
}

void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) {
// If no planning pipelines exist, create them based on the pipeline names provided in pipeline_id_planner_id_map_.
// The assumption here is that all parameters required by the planning pipeline can be found in a namespace that
// equals the pipeline name.
// Create planning pipelines once from pipeline_id_planner_id_map.
// We assume that all parameters required by the pipeline can be found
// in the namespace of the pipeline name.
if (planning_pipelines_.empty()) {
planning_pipelines_ = moveit::planning_pipeline_interfaces::createPlanningPipelineMap(
[&]() {
// Create pipeline name vector from the keys of pipeline_id_planner_id_map_
if (pipeline_id_planner_id_map_.empty()) {
throw std::runtime_error("Cannot initialize PipelinePlanner: No planning pipeline was provided and "
"pipeline_id_planner_id_map_ is empty!");
}

std::vector<std::string> pipeline_names;
for (const auto& pipeline_name_planner_id_pair : pipeline_id_planner_id_map_) {
pipeline_names.push_back(pipeline_name_planner_id_pair.first);
}
return pipeline_names;
}(),
robot_model, node_);
}
auto map = properties().get<PipelineMap>("pipeline_id_planner_id_map");
// Create pipeline name vector from the keys of pipeline_id_planner_id_map_
if (map.empty()) {
throw std::runtime_error("Cannot initialize PipelinePlanner: pipeline_id_planner_id_map is empty!");
}

// Check if it is still empty
if (planning_pipelines_.empty()) {
throw std::runtime_error(
"Cannot initialize PipelinePlanner: Could not create any valid entries for planning pipeline maps!");
std::vector<std::string> pipeline_names;
for (const auto& pipeline_name_planner_id_pair : map) {
pipeline_names.push_back(pipeline_name_planner_id_pair.first);
}
planning_pipelines_ = moveit::planning_pipeline_interfaces::createPlanningPipelineMap(std::move(pipeline_names),
robot_model, node_);
// Check if it is still empty
if (planning_pipelines_.empty()) {
throw std::runtime_error("Failed to initialize PipelinePlanner: Could not create any valid pipeline");
}
} else {
// Validate that all pipelines use the task's robot model
for (const auto& pair : planning_pipelines_) {
if (pair.second->getRobotModel() != robot_model) {
throw std::runtime_error(
"The robot model of the planning pipeline isn't the same as the task's robot model -- ");
}
}
}
}

Expand Down Expand Up @@ -172,28 +164,23 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning
const moveit_msgs::msg::Constraints& goal_constraints, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints) {
const auto& map = properties().get<PipelineMap>("pipeline_id_planner_id_map");

// Create a request for every planning pipeline that should run in parallel
std::vector<moveit_msgs::msg::MotionPlanRequest> requests;
requests.reserve(pipeline_id_planner_id_map_.size());
requests.reserve(map.size());

auto const property_pipeline_id_planner_id_map =
properties().get<std::unordered_map<std::string, std::string>>("pipeline_id_planner_id_map");
for (auto const& pipeline_id_planner_id_pair :
(!property_pipeline_id_planner_id_map.empty() ? property_pipeline_id_planner_id_map :
pipeline_id_planner_id_map_)) {
for (const auto& [pipeline_id, planner_id] : map) {
// Check that requested pipeline exists and skip it if it doesn't exist
if (planning_pipelines_.find(pipeline_id_planner_id_pair.first) == planning_pipelines_.end()) {
RCLCPP_WARN(
node_->get_logger(),
"Pipeline '%s' is not available of this PipelineSolver instance. Skipping a request for this pipeline.",
pipeline_id_planner_id_pair.first.c_str());
if (planning_pipelines_.count(pipeline_id) == 0) {
RCLCPP_WARN(node_->get_logger(), "Pipeline '%s' was not created. Skipping.", pipeline_id.c_str());
continue;
}
// Create MotionPlanRequest for pipeline
moveit_msgs::msg::MotionPlanRequest request;
request.pipeline_id = pipeline_id_planner_id_pair.first;
request.pipeline_id = pipeline_id;
request.group_name = joint_model_group->getName();
request.planner_id = pipeline_id_planner_id_pair.second;
request.planner_id = planner_id;
request.allowed_planning_time = timeout;
request.start_state.is_diff = true; // we don't specify an extra start state
request.num_planning_attempts = properties().get<uint>("num_planning_attempts");
Expand Down

0 comments on commit 8eb93dc

Please sign in to comment.