Skip to content

Commit b6d49f2

Browse files
sjahrsjahr
committed
Cleanup and documentation
1 parent cc3b71c commit b6d49f2

File tree

2 files changed

+89
-90
lines changed

2 files changed

+89
-90
lines changed

‎core/include/moveit/task_constructor/solvers/pipeline_planner.h‎

Lines changed: 64 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -32,13 +32,14 @@
3232
* POSSIBILITY OF SUCH DAMAGE.
3333
*********************************************************************/
3434

35-
/* Authors: Robert Haschke
36-
Desc: plan using MoveIt's PlanningPipeline
35+
/* Authors: Robert Haschke, Sebastian Jahr
36+
Description: Solver that uses a set of MoveIt PlanningPipelines to solve a given planning problem.
3737
*/
3838

3939
#pragma once
4040

4141
#include <moveit/task_constructor/solvers/planner_interface.h>
42+
#include <moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp>
4243
#include <rclcpp/node.hpp>
4344
#include <moveit/macros/class_forward.h>
4445

@@ -56,27 +57,17 @@ MOVEIT_CLASS_FORWARD(PipelinePlanner);
5657
class PipelinePlanner : public PlannerInterface
5758
{
5859
public:
59-
/*
60-
struct Specification
61-
{
62-
moveit::core::RobotModelConstPtr model;
63-
std::string ns{ "ompl" };
64-
std::string pipeline{ "ompl" };
65-
std::string adapter_param{ "request_adapters" };
66-
};*/
67-
68-
/*
69-
static planning_pipeline::PlanningPipelinePtr
70-
createPlanningPipelines(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModelConstPtr& model) {
71-
Specification spec;
72-
spec.model = model;
73-
return create(node, spec);
74-
}*/
75-
7660
// TODO: Deprecate
7761
PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name = "ompl",
7862
const std::string& planner_id = "");
7963

64+
/** \brief Constructor
65+
* \param [in] node ROS 2 node handle
66+
* \param [in] pipeline_id_planner_id_map A map containing pairs of planning pipeline name and planner plugin name
67+
* for the planning requests
68+
* \param [in] planning_pipelines Optional: A map with the pipeline names and initialized corresponding planning
69+
* pipelines
70+
*/
8071
PipelinePlanner(const rclcpp::Node::SharedPtr& node,
8172
const std::unordered_map<std::string, std::string>& pipeline_id_planner_id_map,
8273
const std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr> planning_pipelines =
@@ -86,30 +77,84 @@ class PipelinePlanner : public PlannerInterface
8677
void setPlannerId(const std::string& /*planner*/) { /* Do nothing */
8778
}
8879

80+
/** \brief Set the planner id for a specific planning pipeline for the planning requests
81+
* \param [in] pipeline_name Name of the planning pipeline for which the planner id is set
82+
* \param [in] planner_id Name of the planner ID that should be used by the planning pipeline
83+
* \return true if the pipeline exists and the corresponding ID is set successfully
84+
*/
8985
bool setPlannerId(const std::string& pipeline_name, const std::string& planner_id);
9086

87+
/** \brief This function is called when an MTC task that uses this solver is initialized. If no pipelines are
88+
* configured when this function is invoked, the planning pipelines named in the 'pipeline_id_planner_id_map' are
89+
* initialized with the given robot model. \param [in] robot_model A robot model that is used to initialize the
90+
* planning pipelines of this solver
91+
*/
9192
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
9293

94+
/**
95+
* \brief Plan a trajectory from a planning scene 'from' to scene 'to'
96+
* \param [in] from Start planning scene
97+
* \param [in] to Goal planning scene (used to create goal constraints)
98+
* \param [in] joint_model_group Group of joints for which this trajectory is created
99+
* \param [in] timeout ?
100+
* \param [in] result Reference to the location where the created trajectory is stored if planning is successful
101+
* \param [in] path_constraints Path contraints for the planning problem
102+
* \return true If the solver found a successful solution for the given planning problem
103+
*/
93104
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
94105
const core::JointModelGroup* joint_model_group, double timeout,
95106
robot_trajectory::RobotTrajectoryPtr& result,
96107
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
97108

109+
/** \brief Plan a trajectory from a planning scene 'from' to a target pose with an offset
110+
* \param [in] from Start planning scene
111+
* \param [in] link Link for which a target pose is given
112+
* \param [in] offset Offset to be applied to a given target pose
113+
* \param [in] target Target pose
114+
* \param [in] joint_model_group Group of joints for which this trajectory is created
115+
* \param [in] timeout ?
116+
* \param [in] result Reference to the location where the created trajectory is stored if planning is successful
117+
* \param [in] path_constraints Path contraints for the planning problem
118+
* \return true If the solver found a successful solution for the given planning problem
119+
*/
98120
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
99121
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
100122
const moveit::core::JointModelGroup* joint_model_group, double timeout,
101123
robot_trajectory::RobotTrajectoryPtr& result,
102124
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
103125

104126
protected:
127+
/** \brief Function that actually uses the planning pipelines to solve the given planning problem. It is called by
128+
* the public plan function after the goal constraints are generated. This function uses a predefined number of
129+
* planning pipelines in parallel to solve the planning problem and choose the automatically the best (user-defined)
130+
* solution.
131+
* \param [in] planning_scene Scene for which the planning should be solved
132+
* \param [in] joint_model_group
133+
* Group of joints for which this trajectory is created
134+
* \param [in] goal_constraints Set of constraints that need to
135+
* be satisfied by the solution
136+
* \param [in] timeout ?
137+
* \param [in] result Reference to the location where the created
138+
* trajectory is stored if planning is successful
139+
* \param [in] path_constraints Path contraints for the planning
140+
* problem
141+
* \return true If the solver found a successful solution for the given planning problem
142+
*/
105143
bool plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
106144
const moveit::core::JointModelGroup* joint_model_group,
107145
const moveit_msgs::msg::Constraints& goal_constraints, double timeout,
108146
robot_trajectory::RobotTrajectoryPtr& result,
109147
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints());
110148

111149
rclcpp::Node::SharedPtr node_;
150+
151+
/** \brief Map of pipeline names (ids) and their corresponding planner ids. The planning problem is solved for every
152+
* pipeline-planner pair in this map. If no pipelines are passed via constructor argument, the pipeline names are
153+
* used to initialize a set of planning pipelines. */
112154
std::unordered_map<std::string, std::string> pipeline_id_planner_id_map_;
155+
156+
/** \brief Map of pipelines names and planning pipelines. This map is used to quickly search for a requested motion
157+
* planning pipeline when during plan(..) */
113158
std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr> planning_pipelines_;
114159
};
115160
} // namespace solvers

‎core/src/solvers/pipeline_planner.cpp‎

Lines changed: 25 additions & 71 deletions
Original file line numberDiff line numberDiff line change
@@ -60,66 +60,6 @@ namespace moveit {
6060
namespace task_constructor {
6161
namespace solvers {
6262

63-
/*
64-
static constexpr char const* PLUGIN_PARAMETER_NAME = "planning_plugin";
65-
66-
struct PlannerCache
67-
{
68-
using PlannerID = std::tuple<std::string, std::string>;
69-
using PlannerMap = std::map<PlannerID, std::weak_ptr<planning_pipeline::PlanningPipeline>>;
70-
using ModelList = std::list<std::pair<std::weak_ptr<const moveit::core::RobotModel>, PlannerMap>>;
71-
ModelList cache_;
72-
73-
PlannerMap::mapped_type& retrieve(const moveit::core::RobotModelConstPtr& model, const PlannerID& planner_id) {
74-
// find model in cache_ and remove expired entries while doing so
75-
ModelList::iterator model_it = cache_.begin();
76-
while (model_it != cache_.end()) {
77-
if (model_it->first.expired()) {
78-
model_it = cache_.erase(model_it);
79-
continue;
80-
}
81-
if (model_it->first.lock() == model)
82-
break;
83-
++model_it;
84-
}
85-
if (model_it == cache_.end()) // if not found, create a new PlannerMap for this model
86-
model_it = cache_.insert(cache_.begin(), std::make_pair(model, PlannerMap()));
87-
88-
return model_it->second.insert(std::make_pair(planner_id, PlannerMap::mapped_type())).first->second;
89-
}
90-
};*/
91-
92-
/*
93-
planning_pipeline::PlanningPipelinePtr PipelinePlanner::create(const rclcpp::Node::SharedPtr& node,
94-
const PipelinePlanner::Specification& specification) {
95-
static PlannerCache cache;
96-
97-
std::string pipeline_ns = specification.ns;
98-
const std::string parameter_name = pipeline_ns + "." + PLUGIN_PARAMETER_NAME;
99-
// fallback to old structure for pipeline parameters in MoveIt
100-
if (!node->has_parameter(parameter_name)) {
101-
node->declare_parameter(parameter_name, rclcpp::ParameterType::PARAMETER_STRING);
102-
}
103-
if (std::string parameter; !node->get_parameter(parameter_name, parameter)) {
104-
RCLCPP_WARN(node->get_logger(), "Failed to find '%s.%s'. %s", pipeline_ns.c_str(), PLUGIN_PARAMETER_NAME,
105-
"Attempting to load pipeline from old parameter structure. Please update your MoveIt config.");
106-
pipeline_ns = "move_group";
107-
}
108-
109-
PlannerCache::PlannerID id(pipeline_ns, specification.adapter_param);
110-
111-
std::weak_ptr<planning_pipeline::PlanningPipeline>& entry = cache.retrieve(specification.model, id);
112-
planning_pipeline::PlanningPipelinePtr planner = entry.lock();
113-
if (!planner) {
114-
// create new entry
115-
planner = std::make_shared<planning_pipeline::PlanningPipeline>(
116-
specification.model, node, pipeline_ns, PLUGIN_PARAMETER_NAME, specification.adapter_param);
117-
// store in cache
118-
entry = planner;
119-
}
120-
return planner;
121-
}*/
122-
12363
PipelinePlanner::PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name,
12464
const std::string& planner_id)
12565
: PipelinePlanner(node, [&]() {
@@ -132,21 +72,20 @@ PipelinePlanner::PipelinePlanner(
13272
const rclcpp::Node::SharedPtr& node, const std::unordered_map<std::string, std::string>& pipeline_id_planner_id_map,
13373
const std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr> planning_pipelines)
13474
: node_(node), pipeline_id_planner_id_map_(pipeline_id_planner_id_map) {
135-
for (auto pipeline_id_planner_id_pair : pipeline_id_planner_id_map_) {
136-
RCLCPP_WARN(node_->get_logger(), "'%s' : ''%s", pipeline_id_planner_id_pair.first.c_str(),
137-
pipeline_id_planner_id_pair.second.c_str());
138-
}
75+
// If the pipeline name - pipeline map is passed as constructor argument, use it. Otherwise, it will be created in
76+
// the init(..) function
13977
if (!planning_pipelines.empty()) {
14078
planning_pipelines_ = planning_pipelines;
14179
}
80+
// Declare properties of the MotionPlanRequest
14281
properties().declare<uint>("num_planning_attempts", 1u, "number of planning attempts");
14382
properties().declare<moveit_msgs::msg::WorkspaceParameters>(
14483
"workspace_parameters", moveit_msgs::msg::WorkspaceParameters(), "allowed workspace of mobile base?");
14584

14685
properties().declare<double>("goal_joint_tolerance", 1e-4, "tolerance for reaching joint goals");
14786
properties().declare<double>("goal_position_tolerance", 1e-4, "tolerance for reaching position goals");
14887
properties().declare<double>("goal_orientation_tolerance", 1e-4, "tolerance for reaching orientation goals");
149-
88+
// Declare properties that configure the planning pipeline
15089
properties().declare<bool>("display_motion_plans", false,
15190
"publish generated solutions on topic " +
15291
planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC);
@@ -161,17 +100,23 @@ PipelinePlanner::PipelinePlanner(
161100
//}
162101

163102
bool PipelinePlanner::setPlannerId(const std::string& pipeline_name, const std::string& planner_id) {
103+
// Only set ID if pipeline exists. It is not possible to create new pipelines with this command.
164104
if (pipeline_id_planner_id_map_.count(pipeline_name) > 0) {
165105
pipeline_id_planner_id_map_[pipeline_name] = planner_id;
166106
}
167-
RCLCPP_WARN(node_->get_logger(), "PipelinePlanner does not have a pipeline called '%s'", pipeline_name.c_str());
107+
RCLCPP_WARN(node_->get_logger(), "PipelinePlanner does not have a pipeline called '%s'. Cannot set pipeline ID '%s'",
108+
pipeline_name.c_str(), planner_id.c_str());
168109
return false;
169110
}
170111

171112
void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) {
113+
// If no planning pipelines exist, create them based on the pipeline names provided in pipeline_id_planner_id_map_.
114+
// The assumption here is that all parameters required by the planning pipeline can be found in a namespace that
115+
// equals the pipeline name.
172116
if (planning_pipelines_.empty()) {
173117
planning_pipelines_ = moveit::planning_pipeline_interfaces::createPlanningPipelineMap(
174118
[&]() {
119+
// Create pipeline name vector from the keys of pipeline_id_planner_id_map_
175120
std::vector<std::string> pipeline_names;
176121
for (const auto& pipeline_name_planner_id_pair : pipeline_id_planner_id_map_) {
177122
pipeline_names.push_back(pipeline_name_planner_id_pair.first);
@@ -181,6 +126,7 @@ void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) {
181126
robot_model, node_);
182127
}
183128

129+
// Configure all piplines according to the configuration in properties
184130
for (auto const& name_pipeline_pair : planning_pipelines_) {
185131
name_pipeline_pair.second->displayComputedMotionPlans(properties().get<bool>("display_motion_plans"));
186132
name_pipeline_pair.second->publishReceivedRequests(properties().get<bool>("publish_planning_requests"));
@@ -192,6 +138,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
192138
const moveit::core::JointModelGroup* joint_model_group, double timeout,
193139
robot_trajectory::RobotTrajectoryPtr& result,
194140
const moveit_msgs::msg::Constraints& path_constraints) {
141+
// Construct goal constraints from the goal planning scene
195142
const auto goal_constraints = kinematic_constraints::constructGoalConstraints(
196143
to->getCurrentState(), joint_model_group, properties().get<double>("goal_joint_tolerance"));
197144
return plan(from, joint_model_group, goal_constraints, timeout, result, path_constraints);
@@ -202,6 +149,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, co
202149
const moveit::core::JointModelGroup* joint_model_group, double timeout,
203150
robot_trajectory::RobotTrajectoryPtr& result,
204151
const moveit_msgs::msg::Constraints& path_constraints) {
152+
// Construct a Cartesian target pose from the given target transform and offset
205153
geometry_msgs::msg::PoseStamped target;
206154
target.header.frame_id = from->getPlanningFrame();
207155
target.pose = tf2::toMsg(target_eigen * offset.inverse());
@@ -218,19 +166,20 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning
218166
const moveit_msgs::msg::Constraints& goal_constraints, double timeout,
219167
robot_trajectory::RobotTrajectoryPtr& result,
220168
const moveit_msgs::msg::Constraints& path_constraints) {
169+
// Create a request for every planning pipeline that should run in parallel
221170
std::vector<moveit_msgs::msg::MotionPlanRequest> requests;
222171
requests.reserve(pipeline_id_planner_id_map_.size());
223172

224173
for (auto const& pipeline_id_planner_id_pair : pipeline_id_planner_id_map_) {
225-
RCLCPP_WARN(node_->get_logger(), "'%s' : ''%s", pipeline_id_planner_id_pair.first.c_str(),
226-
pipeline_id_planner_id_pair.second.c_str());
174+
// Check that requested pipeline exists and skip it if it doesn't exist
227175
if (planning_pipelines_.find(pipeline_id_planner_id_pair.first) == planning_pipelines_.end()) {
228176
RCLCPP_WARN(
229177
node_->get_logger(),
230178
"Pipeline '%s' is not available of this PipelineSolver instance. Skipping a request for this pipeline.",
231179
pipeline_id_planner_id_pair.first.c_str());
232180
continue;
233181
}
182+
// Create MotionPlanRequest for pipeline
234183
moveit_msgs::msg::MotionPlanRequest request;
235184
request.pipeline_id = pipeline_id_planner_id_pair.first;
236185
request.group_name = joint_model_group->getName();
@@ -247,11 +196,16 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning
247196
requests.push_back(request);
248197
}
249198

199+
// Run planning pipelines in parallel to create a vector of responses. If a solution selection function is provided,
200+
// planWithParallelPipelines will return a vector with the single best solution
250201
std::vector<::planning_interface::MotionPlanResponse> responses =
251-
moveit::planning_pipeline_interfaces::planWithParallelPipelines(requests, planning_scene, planning_pipelines_, &moveit::planning_pipeline_interfaces::stopAtFirstSolution, &moveit::planning_pipeline_interfaces::getShortestSolution);
202+
moveit::planning_pipeline_interfaces::planWithParallelPipelines(
203+
requests, planning_scene, planning_pipelines_, &moveit::planning_pipeline_interfaces::stopAtFirstSolution,
204+
&moveit::planning_pipeline_interfaces::getShortestSolution);
252205

253-
// Just choose first result
254-
if (!responses.empty()) {
206+
// If solutions exist and the first one is successful
207+
if (!responses.empty() && responses.at(0)) {
208+
// Choose the first solution trajectory as response
255209
result = responses.at(0).trajectory;
256210
return bool(result);
257211
}

0 commit comments

Comments
 (0)