mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
Enable parallel planning with PipelinePlanner (#450)
* Refactor pipeline planner Make code readable Re-order plan functions Make usable with parallel planning Enable configuring multiple pipelines Add callbacks Cleanup and documentation Add API to set parallel planning callbacks and deprecate functions Pass pipeline map by reference Small clang-tidy fix Update core/src/solvers/pipeline_planner.cpp Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Update core/src/solvers/pipeline_planner.cpp Format Refactor to avoid calling .at(0) twice Use no default stopping criteria Update fallbacks_move demo * Cleanup + address deprecation warnings * Enabling optionally using a property defined pipeline planner map * Address review * Disable humble CI for ros2 branch * Add pipeline planner unittests + some checks * Add short comment
This commit is contained in:
parent
0ba9796fe8
commit
f4cd7d5b85
1
.github/workflows/ci.yaml
vendored
1
.github/workflows/ci.yaml
vendored
@ -19,7 +19,6 @@ jobs:
|
||||
fail-fast: false
|
||||
matrix:
|
||||
env:
|
||||
- IMAGE: humble-source
|
||||
- IMAGE: rolling-source
|
||||
NAME: ccov
|
||||
TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage"
|
||||
|
@ -32,13 +32,16 @@
|
||||
* 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 <moveit/planning_pipeline_interfaces/solution_selection_functions.hpp>
|
||||
#include <moveit/planning_pipeline_interfaces/stopping_criterion_functions.hpp>
|
||||
#include <rclcpp/node.hpp>
|
||||
#include <moveit/macros/class_forward.h>
|
||||
|
||||
@ -56,48 +59,131 @@ 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" };
|
||||
};
|
||||
/** 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
|
||||
*/
|
||||
PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name = "ompl",
|
||||
const std::string& planner_id = "");
|
||||
|
||||
static planning_pipeline::PlanningPipelinePtr create(const rclcpp::Node::SharedPtr& node,
|
||||
const moveit::core::RobotModelConstPtr& model) {
|
||||
Specification spec;
|
||||
spec.model = model;
|
||||
return create(node, spec);
|
||||
[[deprecated("Deprecated: Use new constructor implementations.")]] // clang-format off
|
||||
PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& /*planning_pipeline*/){};
|
||||
|
||||
/** \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
|
||||
*/
|
||||
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 */
|
||||
}
|
||||
|
||||
static planning_pipeline::PlanningPipelinePtr create(const rclcpp::Node::SharedPtr& node, const Specification& spec);
|
||||
|
||||
/**
|
||||
*
|
||||
* @param node used to load the parameters for the planning pipeline
|
||||
/** \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
|
||||
*/
|
||||
PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline = "ompl");
|
||||
bool setPlannerId(const std::string& pipeline_name, const std::string& planner_id);
|
||||
|
||||
PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline);
|
||||
/** \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 setPlannerId(const std::string& planner) { setProperty("planner", planner); }
|
||||
/** \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 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 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
|
||||
*/
|
||||
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
|
||||
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||
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 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
|
||||
*/
|
||||
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
|
||||
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
|
||||
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||
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:
|
||||
std::string pipeline_name_;
|
||||
planning_pipeline::PlanningPipelinePtr planner_;
|
||||
/** \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.
|
||||
* \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 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
|
||||
*/
|
||||
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_;
|
||||
|
||||
moveit::planning_pipeline_interfaces::StoppingCriterionFunction stopping_criterion_callback_;
|
||||
moveit::planning_pipeline_interfaces::SolutionSelectionFunction solution_selection_function_;
|
||||
|
||||
};
|
||||
} // namespace solvers
|
||||
} // namespace task_constructor
|
||||
|
@ -53,160 +53,187 @@ namespace moveit {
|
||||
namespace task_constructor {
|
||||
namespace solvers {
|
||||
|
||||
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_;
|
||||
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;
|
||||
}()) {}
|
||||
|
||||
PlannerMap::mapped_type& retrieve(const moveit::core::RobotModelConstPtr& model, const PlannerID& 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(id, PlannerMap::mapped_type())).first->second;
|
||||
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 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>(
|
||||
"workspace_parameters", moveit_msgs::msg::WorkspaceParameters(), "allowed workspace of mobile base?");
|
||||
|
||||
planning_pipeline::PlanningPipelinePtr PipelinePlanner::create(const rclcpp::Node::SharedPtr& node,
|
||||
const PipelinePlanner::Specification& spec) {
|
||||
static PlannerCache cache;
|
||||
|
||||
static constexpr char const* PLUGIN_PARAMETER_NAME = "planning_plugin";
|
||||
|
||||
std::string pipeline_ns = spec.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, spec.adapter_param);
|
||||
|
||||
std::weak_ptr<planning_pipeline::PlanningPipeline>& entry = cache.retrieve(spec.model, id);
|
||||
planning_pipeline::PlanningPipelinePtr planner = entry.lock();
|
||||
if (!planner) {
|
||||
// create new entry
|
||||
planner = std::make_shared<planning_pipeline::PlanningPipeline>(spec.model, node, pipeline_ns,
|
||||
PLUGIN_PARAMETER_NAME, spec.adapter_param);
|
||||
// store in cache
|
||||
entry = planner;
|
||||
}
|
||||
return planner;
|
||||
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);
|
||||
properties().declare<bool>("publish_planning_requests", false,
|
||||
"publish motion planning requests on topic " +
|
||||
planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC);
|
||||
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");
|
||||
}
|
||||
|
||||
PipelinePlanner::PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name)
|
||||
: pipeline_name_{ pipeline_name }, node_(node) {
|
||||
auto& p = properties();
|
||||
p.declare<std::string>("planner", "", "planner id");
|
||||
|
||||
p.declare<uint>("num_planning_attempts", 1u, "number of planning attempts");
|
||||
p.declare<moveit_msgs::msg::WorkspaceParameters>("workspace_parameters", moveit_msgs::msg::WorkspaceParameters(),
|
||||
"allowed workspace of mobile base?");
|
||||
|
||||
p.declare<double>("goal_joint_tolerance", 1e-4, "tolerance for reaching joint goals");
|
||||
p.declare<double>("goal_position_tolerance", 1e-4, "tolerance for reaching position goals");
|
||||
p.declare<double>("goal_orientation_tolerance", 1e-4, "tolerance for reaching orientation goals");
|
||||
|
||||
p.declare<bool>("display_motion_plans", false,
|
||||
"publish generated solutions on topic " + planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC);
|
||||
p.declare<bool>("publish_planning_requests", false,
|
||||
"publish motion planning requests on topic " +
|
||||
planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC);
|
||||
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) {
|
||||
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;
|
||||
return true;
|
||||
}
|
||||
|
||||
PipelinePlanner::PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline)
|
||||
: PipelinePlanner(rclcpp::Node::SharedPtr()) {
|
||||
planner_ = planning_pipeline;
|
||||
void PipelinePlanner::setStoppingCriterionFunction(
|
||||
const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_function) {
|
||||
stopping_criterion_callback_ = stopping_criterion_function;
|
||||
}
|
||||
void PipelinePlanner::setSolutionSelectionFunction(
|
||||
const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function) {
|
||||
solution_selection_function_ = solution_selection_function;
|
||||
}
|
||||
|
||||
void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) {
|
||||
if (!planner_) {
|
||||
Specification spec;
|
||||
spec.model = robot_model;
|
||||
spec.pipeline = pipeline_name_;
|
||||
spec.ns = pipeline_name_;
|
||||
planner_ = create(node_, spec);
|
||||
} else if (robot_model != planner_->getRobotModel()) {
|
||||
throw std::runtime_error(
|
||||
"The robot model of the planning pipeline isn't the same as the task's robot model -- "
|
||||
"use Task::setRobotModel for setting the robot model when using custom planning pipeline");
|
||||
// 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_
|
||||
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_);
|
||||
}
|
||||
planner_->displayComputedMotionPlans(properties().get<bool>("display_motion_plans"));
|
||||
planner_->publishReceivedRequests(properties().get<bool>("publish_planning_requests"));
|
||||
}
|
||||
|
||||
void initMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& req, const PropertyMap& p,
|
||||
const moveit::core::JointModelGroup* jmg, double timeout) {
|
||||
req.group_name = jmg->getName();
|
||||
req.planner_id = p.get<std::string>("planner");
|
||||
req.allowed_planning_time = std::min(timeout, p.get<double>("timeout"));
|
||||
req.start_state.is_diff = true; // we don't specify an extra start state
|
||||
// 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!");
|
||||
}
|
||||
|
||||
req.num_planning_attempts = p.get<uint>("num_planning_attempts");
|
||||
req.max_velocity_scaling_factor = p.get<double>("max_velocity_scaling_factor");
|
||||
req.max_acceleration_scaling_factor = p.get<double>("max_acceleration_scaling_factor");
|
||||
req.workspace_parameters = p.get<moveit_msgs::msg::WorkspaceParameters>("workspace_parameters");
|
||||
// Configure all pipelines 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"));
|
||||
}
|
||||
}
|
||||
|
||||
bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
|
||||
const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg,
|
||||
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const planning_scene::PlanningSceneConstPtr& to,
|
||||
const moveit::core::JointModelGroup* joint_model_group, double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::msg::Constraints& path_constraints) {
|
||||
const auto& props = properties();
|
||||
moveit_msgs::msg::MotionPlanRequest req;
|
||||
initMotionPlanRequest(req, props, jmg, timeout);
|
||||
|
||||
req.goal_constraints.resize(1);
|
||||
req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(to->getCurrentState(), jmg,
|
||||
props.get<double>("goal_joint_tolerance"));
|
||||
req.path_constraints = path_constraints;
|
||||
|
||||
::planning_interface::MotionPlanResponse res;
|
||||
bool success = planner_->generatePlan(from, req, res);
|
||||
result = res.trajectory;
|
||||
return success;
|
||||
// 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);
|
||||
}
|
||||
|
||||
bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
|
||||
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target_eigen,
|
||||
const moveit::core::JointModelGroup* jmg, double timeout,
|
||||
const moveit::core::JointModelGroup* joint_model_group, double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::msg::Constraints& path_constraints) {
|
||||
const auto& props = properties();
|
||||
moveit_msgs::msg::MotionPlanRequest req;
|
||||
initMotionPlanRequest(req, props, jmg, timeout);
|
||||
|
||||
// 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());
|
||||
|
||||
req.goal_constraints.resize(1);
|
||||
req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(
|
||||
link.getName(), target, props.get<double>("goal_position_tolerance"),
|
||||
props.get<double>("goal_orientation_tolerance"));
|
||||
req.path_constraints = path_constraints;
|
||||
const auto goal_constraints = kinematic_constraints::constructGoalConstraints(
|
||||
link.getName(), target, properties().get<double>("goal_position_tolerance"),
|
||||
properties().get<double>("goal_orientation_tolerance"));
|
||||
|
||||
::planning_interface::MotionPlanResponse res;
|
||||
bool success = planner_->generatePlan(from, req, res);
|
||||
result = res.trajectory;
|
||||
return success;
|
||||
return plan(from, joint_model_group, goal_constraints, timeout, result, path_constraints);
|
||||
}
|
||||
|
||||
bool PipelinePlanner::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) {
|
||||
// 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());
|
||||
|
||||
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_)) {
|
||||
// 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();
|
||||
request.planner_id = pipeline_id_planner_id_pair.second;
|
||||
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");
|
||||
request.max_velocity_scaling_factor = properties().get<double>("max_velocity_scaling_factor");
|
||||
request.max_acceleration_scaling_factor = properties().get<double>("max_acceleration_scaling_factor");
|
||||
request.workspace_parameters = properties().get<moveit_msgs::msg::WorkspaceParameters>("workspace_parameters");
|
||||
request.goal_constraints.resize(1);
|
||||
request.goal_constraints.at(0) = goal_constraints;
|
||||
request.path_constraints = path_constraints;
|
||||
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_, stopping_criterion_callback_, solution_selection_function_);
|
||||
|
||||
// If solutions exist and the first one is successful
|
||||
if (!responses.empty()) {
|
||||
auto const solution = responses.at(0);
|
||||
if (solution) {
|
||||
// Choose the first solution trajectory as response
|
||||
result = solution.trajectory;
|
||||
return bool(result);
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
} // namespace solvers
|
||||
} // namespace task_constructor
|
||||
|
@ -54,6 +54,7 @@ if (BUILD_TESTING)
|
||||
|
||||
mtc_add_gtest(test_move_to.cpp move_to.launch.py)
|
||||
mtc_add_gtest(test_move_relative.cpp move_to.launch.py)
|
||||
mtc_add_gtest(test_pipeline_planner.cpp)
|
||||
|
||||
# building these integration tests works without moveit config packages
|
||||
ament_add_gtest_executable(pick_ur5 pick_ur5.cpp)
|
||||
|
@ -47,8 +47,7 @@ TEST(PA10, pick) {
|
||||
t.setProperty("eef", std::string("la_tool_mount"));
|
||||
t.setProperty("gripper", std::string("left_hand"));
|
||||
|
||||
auto pipeline = std::make_shared<solvers::PipelinePlanner>(node);
|
||||
pipeline->setPlannerId("RRTConnectkConfigDefault");
|
||||
auto pipeline = std::make_shared<solvers::PipelinePlanner>(node, "ompl", "RRTConnectkConfigDefault");
|
||||
auto cartesian = std::make_shared<solvers::CartesianPath>();
|
||||
|
||||
Stage* initial_stage = nullptr;
|
||||
|
@ -40,8 +40,7 @@ TEST(PR2, pick) {
|
||||
|
||||
auto node = rclcpp::Node::make_shared("pr2");
|
||||
// planner used for connect
|
||||
auto pipeline = std::make_shared<solvers::PipelinePlanner>(node);
|
||||
pipeline->setPlannerId("RRTConnectkConfigDefault");
|
||||
auto pipeline = std::make_shared<solvers::PipelinePlanner>(node, "ompl", "RRTConnectkConfigDefault");
|
||||
// connect to pick
|
||||
stages::Connect::GroupPlannerVector planners = { { "left_arm", pipeline }, { "left_gripper", pipeline } };
|
||||
auto connect = std::make_unique<stages::Connect>("connect", planners);
|
||||
|
@ -42,8 +42,7 @@ TEST(UR5, pick) {
|
||||
|
||||
auto node = rclcpp::Node::make_shared("ur5");
|
||||
// planner used for connect
|
||||
auto pipeline = std::make_shared<solvers::PipelinePlanner>(node);
|
||||
pipeline->setPlannerId("RRTConnectkConfigDefault");
|
||||
auto pipeline = std::make_shared<solvers::PipelinePlanner>(node, "ompl", "RRTConnectkConfigDefault");
|
||||
// connect to pick
|
||||
stages::Connect::GroupPlannerVector planners = { { "arm", pipeline }, { "gripper", pipeline } };
|
||||
auto connect = std::make_unique<stages::Connect>("connect", planners);
|
||||
|
63
core/test/test_pipeline_planner.cpp
Normal file
63
core/test/test_pipeline_planner.cpp
Normal file
@ -0,0 +1,63 @@
|
||||
#include "models.h"
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <moveit/robot_model/robot_model.h>
|
||||
|
||||
#include <moveit/task_constructor/solvers/pipeline_planner.h>
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
|
||||
// Test fixture for PipelinePlanner
|
||||
struct PipelinePlannerTest : public testing::Test
|
||||
{
|
||||
PipelinePlannerTest() {
|
||||
node->declare_parameter<std::string>("STOMP.planning_plugin", "stomp_moveit/StompPlanner");
|
||||
};
|
||||
const rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("test_pipeline_planner");
|
||||
const moveit::core::RobotModelPtr robot_model = getModel();
|
||||
};
|
||||
|
||||
TEST_F(PipelinePlannerTest, testInitialization) {
|
||||
// GIVEN a valid robot model, ROS node and PipelinePlanner
|
||||
auto pipeline_planner = solvers::PipelinePlanner(node, "STOMP", "stomp");
|
||||
// WHEN a PipelinePlanner instance is initialized
|
||||
// THEN it does not throw
|
||||
EXPECT_NO_THROW(pipeline_planner.init(robot_model));
|
||||
}
|
||||
|
||||
TEST_F(PipelinePlannerTest, testWithoutPlanningPipelines) {
|
||||
// GIVEN a PipelinePlanner instance without planning pipelines
|
||||
std::unordered_map<std::string, std::string> empty_pipeline_id_planner_id_map;
|
||||
auto pipeline_planner = solvers::PipelinePlanner(node, empty_pipeline_id_planner_id_map);
|
||||
// WHEN a PipelinePlanner instance is initialized
|
||||
// THEN it does not throw
|
||||
EXPECT_THROW(pipeline_planner.init(robot_model), std::runtime_error);
|
||||
}
|
||||
|
||||
TEST_F(PipelinePlannerTest, testValidPlan) {
|
||||
// GIVEN an initialized PipelinePlanner
|
||||
auto pipeline_planner = solvers::PipelinePlanner(node, "STOMP", "stomp");
|
||||
pipeline_planner.init(robot_model);
|
||||
// WHEN a solution for a valid request is requested
|
||||
auto scene = std::make_shared<planning_scene::PlanningScene>(robot_model);
|
||||
robot_trajectory::RobotTrajectoryPtr result =
|
||||
std::make_shared<robot_trajectory::RobotTrajectory>(robot_model, robot_model->getJointModelGroup("group"));
|
||||
// THEN it returns true
|
||||
EXPECT_TRUE(pipeline_planner.plan(scene, scene, robot_model->getJointModelGroup("group"), 1.0, result));
|
||||
}
|
||||
|
||||
TEST_F(PipelinePlannerTest, testInvalidPipelineID) {
|
||||
// GIVEN a valid initialized PipelinePlanner instance
|
||||
auto pipeline_planner = solvers::PipelinePlanner(node, "STOMP", "stomp");
|
||||
pipeline_planner.init(robot_model);
|
||||
// WHEN the planner ID for a non-existing planning pipeline is set
|
||||
// THEN setPlannerID returns false
|
||||
EXPECT_FALSE(pipeline_planner.setPlannerId("CHOMP", "stomp"));
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
@ -31,14 +31,12 @@ int main(int argc, char** argv) {
|
||||
cartesian->setJumpThreshold(2.0);
|
||||
|
||||
const auto ptp = [&node]() {
|
||||
auto pp{ std::make_shared<solvers::PipelinePlanner>(node, "pilz_industrial_motion_planner") };
|
||||
pp->setPlannerId("PTP");
|
||||
auto pp{ std::make_shared<solvers::PipelinePlanner>(node, "pilz_industrial_motion_planner", "PTP") };
|
||||
return pp;
|
||||
}();
|
||||
|
||||
const auto rrtconnect = [&node]() {
|
||||
auto pp{ std::make_shared<solvers::PipelinePlanner>(node, "ompl") };
|
||||
pp->setPlannerId("RRTConnect");
|
||||
auto pp{ std::make_shared<solvers::PipelinePlanner>(node, "ompl", "RRTConnectkConfigDefault") };
|
||||
return pp;
|
||||
}();
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user