Skip to content

Commit

Permalink
Cleanup and documentation
Browse files Browse the repository at this point in the history
  • Loading branch information
@sjahr
sjahr committed Apr 20, 2023
1 parent 7dd2743 commit 132235f
Show file tree
Hide file tree
Showing 2 changed files with 89 additions and 90 deletions.
83 changes: 64 additions & 19 deletions core/include/moveit/task_constructor/solvers/pipeline_planner.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,13 +32,14 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

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

#pragma once

#include <moveit/task_constructor/solvers/planner_interface.h>
#include <moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp>
#include <rclcpp/node.hpp>
#include <moveit/macros/class_forward.h>

Expand All @@ -56,27 +57,17 @@ MOVEIT_CLASS_FORWARD(PipelinePlanner);
class PipelinePlanner : public PlannerInterface
{
public:
/*
struct Specification
{
moveit::core::RobotModelConstPtr model;
std::string ns{ "ompl" };
std::string pipeline{ "ompl" };
std::string adapter_param{ "request_adapters" };
};*/

/*
static planning_pipeline::PlanningPipelinePtr
createPlanningPipelines(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModelConstPtr& model) {
Specification spec;
spec.model = model;
return create(node, spec);
}*/

// TODO: Deprecate
PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name = "ompl",
const std::string& planner_id = "");

/** \brief Constructor
* \param [in] node ROS 2 node handle
* \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
*/
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 =
Expand All @@ -86,30 +77,84 @@ class PipelinePlanner : public PlannerInterface
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
* \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 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 init(const moveit::core::RobotModelConstPtr& robot_model) override;

/**
* \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
* \param [in] timeout ?
* \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& from, const planning_scene::PlanningSceneConstPtr& to,
const core::JointModelGroup* joint_model_group, double timeout,
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
* \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
* \param [in] target Target pose
* \param [in] joint_model_group Group of joints for which this trajectory is created
* \param [in] timeout ?
* \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& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
const moveit::core::JointModelGroup* joint_model_group, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
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 automatically the best (user-defined)
* solution.
* \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] timeout ?
* \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,
const moveit_msgs::msg::Constraints& goal_constraints, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints());

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(..) */
std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr> planning_pipelines_;
};
} // namespace solvers
Expand Down
96 changes: 25 additions & 71 deletions core/src/solvers/pipeline_planner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,66 +60,6 @@ namespace moveit {
namespace task_constructor {
namespace solvers {

/*
static constexpr char const* PLUGIN_PARAMETER_NAME = "planning_plugin";
struct PlannerCache
{
using PlannerID = std::tuple<std::string, std::string>;
using PlannerMap = std::map<PlannerID, std::weak_ptr<planning_pipeline::PlanningPipeline>>;
using ModelList = std::list<std::pair<std::weak_ptr<const moveit::core::RobotModel>, PlannerMap>>;
ModelList cache_;
PlannerMap::mapped_type& retrieve(const moveit::core::RobotModelConstPtr& model, const PlannerID& planner_id) {
// find model in cache_ and remove expired entries while doing so
ModelList::iterator model_it = cache_.begin();
while (model_it != cache_.end()) {
if (model_it->first.expired()) {
model_it = cache_.erase(model_it);
continue;
}
if (model_it->first.lock() == model)
break;
++model_it;
}
if (model_it == cache_.end()) // if not found, create a new PlannerMap for this model
model_it = cache_.insert(cache_.begin(), std::make_pair(model, PlannerMap()));
return model_it->second.insert(std::make_pair(planner_id, PlannerMap::mapped_type())).first->second;
}
};*/

/*
planning_pipeline::PlanningPipelinePtr PipelinePlanner::create(const rclcpp::Node::SharedPtr& node,
const PipelinePlanner::Specification& specification) {
static PlannerCache cache;
std::string pipeline_ns = specification.ns;
const std::string parameter_name = pipeline_ns + "." + PLUGIN_PARAMETER_NAME;
// fallback to old structure for pipeline parameters in MoveIt
if (!node->has_parameter(parameter_name)) {
node->declare_parameter(parameter_name, rclcpp::ParameterType::PARAMETER_STRING);
}
if (std::string parameter; !node->get_parameter(parameter_name, parameter)) {
RCLCPP_WARN(node->get_logger(), "Failed to find '%s.%s'. %s", pipeline_ns.c_str(), PLUGIN_PARAMETER_NAME,
"Attempting to load pipeline from old parameter structure. Please update your MoveIt config.");
pipeline_ns = "move_group";
}
PlannerCache::PlannerID id(pipeline_ns, specification.adapter_param);
std::weak_ptr<planning_pipeline::PlanningPipeline>& entry = cache.retrieve(specification.model, id);
planning_pipeline::PlanningPipelinePtr planner = entry.lock();
if (!planner) {
// create new entry
planner = std::make_shared<planning_pipeline::PlanningPipeline>(
specification.model, node, pipeline_ns, PLUGIN_PARAMETER_NAME, specification.adapter_param);
// store in cache
entry = planner;
}
return planner;
}*/

PipelinePlanner::PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name,
const std::string& planner_id)
: PipelinePlanner(node, [&]() {
Expand All @@ -132,21 +72,20 @@ 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)
: node_(node), pipeline_id_planner_id_map_(pipeline_id_planner_id_map) {
for (auto pipeline_id_planner_id_pair : pipeline_id_planner_id_map_) {
RCLCPP_WARN(node_->get_logger(), "'%s' : ''%s", pipeline_id_planner_id_pair.first.c_str(),
pipeline_id_planner_id_pair.second.c_str());
}
// 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>(
"workspace_parameters", moveit_msgs::msg::WorkspaceParameters(), "allowed workspace of mobile base?");

properties().declare<double>("goal_joint_tolerance", 1e-4, "tolerance for reaching joint goals");
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<bool>("display_motion_plans", false,
"publish generated solutions on topic " +
planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC);
Expand All @@ -161,17 +100,23 @@ PipelinePlanner::PipelinePlanner(
//}

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) {
pipeline_id_planner_id_map_[pipeline_name] = planner_id;
}
RCLCPP_WARN(node_->get_logger(), "PipelinePlanner does not have a pipeline called '%s'", pipeline_name.c_str());
RCLCPP_WARN(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;
}

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.
if (planning_pipelines_.empty()) {
planning_pipelines_ = moveit::planning_pipeline_interfaces::createPlanningPipelineMap(
[&]() {
// Create pipeline name vector from the keys of pipeline_id_planner_id_map_
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);
Expand All @@ -181,6 +126,7 @@ void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) {
robot_model, node_);
}

// Configure all piplines according to the configuration in properties
for (auto const& name_pipeline_pair : planning_pipelines_) {
name_pipeline_pair.second->displayComputedMotionPlans(properties().get<bool>("display_motion_plans"));
name_pipeline_pair.second->publishReceivedRequests(properties().get<bool>("publish_planning_requests"));
Expand All @@ -192,6 +138,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::JointModelGroup* joint_model_group, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints) {
// Construct goal constraints from the goal planning scene
const auto goal_constraints = kinematic_constraints::constructGoalConstraints(
to->getCurrentState(), joint_model_group, properties().get<double>("goal_joint_tolerance"));
return plan(from, joint_model_group, goal_constraints, timeout, result, path_constraints);
Expand All @@ -202,6 +149,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, co
const moveit::core::JointModelGroup* joint_model_group, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints) {
// Construct a Cartesian target pose from the given target transform and offset
geometry_msgs::msg::PoseStamped target;
target.header.frame_id = from->getPlanningFrame();
target.pose = tf2::toMsg(target_eigen * offset.inverse());
Expand All @@ -218,19 +166,20 @@ 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) {
// 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());

for (auto const& pipeline_id_planner_id_pair : pipeline_id_planner_id_map_) {
RCLCPP_WARN(node_->get_logger(), "'%s' : ''%s", pipeline_id_planner_id_pair.first.c_str(),
pipeline_id_planner_id_pair.second.c_str());
// 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());
continue;
}
// Create MotionPlanRequest for pipeline
moveit_msgs::msg::MotionPlanRequest request;
request.pipeline_id = pipeline_id_planner_id_pair.first;
request.group_name = joint_model_group->getName();
Expand All @@ -247,11 +196,16 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning
requests.push_back(request);
}

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

// Just choose first result
if (!responses.empty()) {
// If solutions exist and the first one is successful
if (!responses.empty() && responses.at(0)) {
// Choose the first solution trajectory as response
result = responses.at(0).trajectory;
return bool(result);
}
Expand Down

0 comments on commit 132235f

Please sign in to comment.