mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
Revert "Enable parallel planning with PipelinePlanner (#450)"
This reverts commit 0e02fcae77
.
This commit is contained in:
parent
56f75cc179
commit
5fd3ae6ff2
@ -32,16 +32,14 @@
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
|
||||
/* Authors: Robert Haschke, Sebastian Jahr
|
||||
Description: Solver that uses a set of MoveIt PlanningPipelines to solve a given planning problem.
|
||||
/* Authors: Robert Haschke
|
||||
Desc: plan using MoveIt's PlanningPipeline
|
||||
*/
|
||||
|
||||
#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 <moveit_msgs/msg/motion_plan_request.hpp>
|
||||
#include <rclcpp/node.hpp>
|
||||
#include <moveit/macros/class_forward.hpp>
|
||||
|
||||
@ -59,111 +57,52 @@ MOVEIT_CLASS_FORWARD(PipelinePlanner);
|
||||
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. Empty string means default.
|
||||
*/
|
||||
PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name = "ompl",
|
||||
const std::string& planner_id = "")
|
||||
: PipelinePlanner(node, { { pipeline_name, planner_id } }) {}
|
||||
struct Specification
|
||||
{
|
||||
moveit::core::RobotModelConstPtr model;
|
||||
std::string ns{ "ompl" };
|
||||
std::string pipeline{ "ompl" };
|
||||
std::string adapter_param{ "request_adapters" };
|
||||
};
|
||||
|
||||
/** \brief Constructor
|
||||
* \param [in] node ROS 2 node
|
||||
* \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 moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback = nullptr,
|
||||
const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function =
|
||||
&moveit::planning_pipeline_interfaces::getShortestSolution);
|
||||
static planning_pipeline::PlanningPipelinePtr create(const rclcpp::Node::SharedPtr& node,
|
||||
const moveit::core::RobotModelConstPtr& model) {
|
||||
Specification spec;
|
||||
spec.model = model;
|
||||
return create(node, spec);
|
||||
}
|
||||
|
||||
/** \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);
|
||||
static planning_pipeline::PlanningPipelinePtr create(const rclcpp::Node::SharedPtr& node, const Specification& spec);
|
||||
|
||||
/** \brief Set stopping criterion function for parallel planning
|
||||
* \param [in] stopping_criterion_callback New stopping criterion function that will be used
|
||||
/**
|
||||
*
|
||||
* @param node used to load the parameters for the planning pipeline
|
||||
*/
|
||||
void setStoppingCriterionFunction(
|
||||
const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback);
|
||||
PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline = "ompl");
|
||||
|
||||
/** \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);
|
||||
PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline);
|
||||
|
||||
void setPlannerId(const std::string& planner) { setProperty("planner", planner); }
|
||||
std::string getPlannerId() const override { return properties().get<std::string>("planner"); }
|
||||
|
||||
/** \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'
|
||||
* \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
|
||||
*/
|
||||
Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
|
||||
const core::JointModelGroup* joint_model_group, double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const core::JointModelGroup* jmg, 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 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
|
||||
* \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
|
||||
*/
|
||||
Result 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::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
|
||||
|
||||
std::string getPlannerId() const override { return last_successful_planner_; }
|
||||
|
||||
protected:
|
||||
/** \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] 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
|
||||
*/
|
||||
Result 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());
|
||||
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit_msgs::msg::MotionPlanRequest& req,
|
||||
robot_trajectory::RobotTrajectoryPtr& result);
|
||||
|
||||
std::string pipeline_name_;
|
||||
planning_pipeline::PlanningPipelinePtr planner_;
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
|
||||
std::string last_successful_planner_;
|
||||
|
||||
/** \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
|
||||
|
@ -70,9 +70,11 @@ void export_solvers(py::module& m) {
|
||||
from moveit.task_constructor import core
|
||||
|
||||
# Create and configure a planner instance
|
||||
pipelinePlanner = core.PipelinePlanner(node, 'ompl', 'PRMkConfigDefault')
|
||||
pipelinePlanner = core.PipelinePlanner()
|
||||
pipelinePlanner.planner = 'PRMkConfigDefault'
|
||||
pipelinePlanner.num_planning_attempts = 10
|
||||
)")
|
||||
.property<std::string>("planner", "str: Planner ID")
|
||||
.property<uint>("num_planning_attempts", "int: Number of planning attempts")
|
||||
.property<moveit_msgs::msg::WorkspaceParameters>(
|
||||
"workspace_parameters",
|
||||
@ -80,8 +82,8 @@ void export_solvers(py::module& m) {
|
||||
.property<double>("goal_joint_tolerance", "float: Tolerance for reaching joint goals")
|
||||
.property<double>("goal_position_tolerance", "float: Tolerance for reaching position goals")
|
||||
.property<double>("goal_orientation_tolerance", "float: Tolerance for reaching orientation goals")
|
||||
.def(py::init<const rclcpp::Node::SharedPtr&, const std::string&, const std::string&>(), "node"_a,
|
||||
"pipeline"_a = std::string("ompl"), "planner_id"_a = std::string(""));
|
||||
.def(py::init<const rclcpp::Node::SharedPtr&, const std::string&>(), "node"_a,
|
||||
"pipeline"_a = std::string("ompl"));
|
||||
|
||||
properties::class_<JointInterpolationPlanner, PlannerInterface>(
|
||||
m, "JointInterpolationPlanner",
|
||||
|
@ -71,15 +71,21 @@ class TestPropertyMap(unittest.TestCase):
|
||||
|
||||
props["planner"] = "planner"
|
||||
self.assertEqual(props["planner"], "planner")
|
||||
self.assertEqual(planner.planner, "planner")
|
||||
|
||||
props["double"] = 3.14
|
||||
a = props
|
||||
props["double"] = 2.71
|
||||
self.assertEqual(a["double"], 2.71)
|
||||
|
||||
planner.planner = "other"
|
||||
self.assertEqual(props["planner"], "other")
|
||||
self.assertEqual(planner.planner, "other")
|
||||
|
||||
del planner
|
||||
# We can still access props, because actual destruction of planner is delayed
|
||||
self.assertEqual(props["goal_joint_tolerance"], 2.71)
|
||||
self.assertEqual(props["planner"], "other")
|
||||
|
||||
def test_iter(self):
|
||||
# assign values so we can iterate over them
|
||||
|
@ -53,169 +53,166 @@ namespace moveit {
|
||||
namespace task_constructor {
|
||||
namespace solvers {
|
||||
|
||||
using PipelineMap = std::unordered_map<std::string, std::string>;
|
||||
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 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)
|
||||
, stopping_criterion_callback_(stopping_criterion_callback)
|
||||
, solution_selection_function_(solution_selection_function) {
|
||||
// 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?");
|
||||
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()));
|
||||
|
||||
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("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.
|
||||
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;
|
||||
return model_it->second.insert(std::make_pair(id, PlannerMap::mapped_type())).first->second;
|
||||
}
|
||||
it->second = planner_id;
|
||||
properties().set("pipeline_id_planner_id_map", map);
|
||||
return true;
|
||||
};
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
void PipelinePlanner::setStoppingCriterionFunction(
|
||||
const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_function) {
|
||||
stopping_criterion_callback_ = stopping_criterion_function;
|
||||
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);
|
||||
}
|
||||
void PipelinePlanner::setSolutionSelectionFunction(
|
||||
const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function) {
|
||||
solution_selection_function_ = solution_selection_function;
|
||||
|
||||
PipelinePlanner::PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline)
|
||||
: PipelinePlanner(rclcpp::Node::SharedPtr()) {
|
||||
planner_ = planning_pipeline;
|
||||
}
|
||||
|
||||
void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) {
|
||||
// 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()) {
|
||||
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!");
|
||||
}
|
||||
|
||||
std::vector<std::string> pipeline_names;
|
||||
pipeline_names.reserve(map.size());
|
||||
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(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 -- ");
|
||||
}
|
||||
}
|
||||
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");
|
||||
}
|
||||
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
|
||||
|
||||
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");
|
||||
}
|
||||
|
||||
PlannerInterface::Result PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
|
||||
const planning_scene::PlanningSceneConstPtr& to,
|
||||
const moveit::core::JointModelGroup* joint_model_group, double timeout,
|
||||
const moveit::core::JointModelGroup* jmg, 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);
|
||||
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;
|
||||
|
||||
return plan(from, req, result);
|
||||
}
|
||||
|
||||
PlannerInterface::Result 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* joint_model_group, double timeout,
|
||||
const moveit::core::JointModelGroup* jmg, 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
|
||||
const auto& props = properties();
|
||||
moveit_msgs::msg::MotionPlanRequest req;
|
||||
initMotionPlanRequest(req, props, jmg, timeout);
|
||||
|
||||
geometry_msgs::msg::PoseStamped target;
|
||||
target.header.frame_id = from->getPlanningFrame();
|
||||
target.pose = tf2::toMsg(target_eigen * offset.inverse());
|
||||
|
||||
const auto goal_constraints = kinematic_constraints::constructGoalConstraints(
|
||||
link.getName(), target, properties().get<double>("goal_position_tolerance"),
|
||||
properties().get<double>("goal_orientation_tolerance"));
|
||||
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;
|
||||
|
||||
return plan(from, joint_model_group, goal_constraints, timeout, result, path_constraints);
|
||||
return plan(from, req, result);
|
||||
}
|
||||
|
||||
PlannerInterface::Result 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) {
|
||||
const auto& map = properties().get<PipelineMap>("pipeline_id_planner_id_map");
|
||||
last_successful_planner_ = "Unknown";
|
||||
|
||||
// Create a request for every planning pipeline that should run in parallel
|
||||
std::vector<moveit_msgs::msg::MotionPlanRequest> requests;
|
||||
requests.reserve(map.size());
|
||||
|
||||
for (const auto& [pipeline_id, planner_id] : map) {
|
||||
// Check that requested pipeline exists and skip it if it doesn't exist
|
||||
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;
|
||||
request.group_name = joint_model_group->getName();
|
||||
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");
|
||||
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;
|
||||
last_successful_planner_ = solution.planner_id;
|
||||
return { true, "" };
|
||||
}
|
||||
return { false, solution.error_code.message };
|
||||
}
|
||||
return { false, "No solutions generated from Pipeline Planner" };
|
||||
PlannerInterface::Result PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
|
||||
const moveit_msgs::msg::MotionPlanRequest& req,
|
||||
robot_trajectory::RobotTrajectoryPtr& result) {
|
||||
::planning_interface::MotionPlanResponse res;
|
||||
bool success = planner_->generatePlan(from, req, res);
|
||||
result = res.trajectory;
|
||||
return { success, success ? std::string() : moveit::core::error_code_to_string(res.error_code.val) };
|
||||
}
|
||||
|
||||
} // namespace solvers
|
||||
} // namespace task_constructor
|
||||
} // namespace moveit
|
||||
|
@ -56,7 +56,6 @@ if (BUILD_TESTING)
|
||||
|
||||
mtc_add_gtest(test_move_to.cpp test.launch.py)
|
||||
mtc_add_gtest(test_move_relative.cpp test.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,7 +47,8 @@ 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, "ompl", "RRTConnectkConfigDefault");
|
||||
auto pipeline = std::make_shared<solvers::PipelinePlanner>(node);
|
||||
pipeline->setPlannerId("RRTConnectkConfigDefault");
|
||||
auto cartesian = std::make_shared<solvers::CartesianPath>();
|
||||
|
||||
Stage* initial_stage = nullptr;
|
||||
|
@ -40,7 +40,8 @@ TEST(PR2, pick) {
|
||||
|
||||
auto node = rclcpp::Node::make_shared("pr2");
|
||||
// planner used for connect
|
||||
auto pipeline = std::make_shared<solvers::PipelinePlanner>(node, "ompl", "RRTConnectkConfigDefault");
|
||||
auto pipeline = std::make_shared<solvers::PipelinePlanner>(node);
|
||||
pipeline->setPlannerId("RRTConnectkConfigDefault");
|
||||
// connect to pick
|
||||
stages::Connect::GroupPlannerVector planners = { { "left_arm", pipeline }, { "left_gripper", pipeline } };
|
||||
auto connect = std::make_unique<stages::Connect>("connect", planners);
|
||||
|
@ -42,7 +42,8 @@ TEST(UR5, pick) {
|
||||
|
||||
auto node = rclcpp::Node::make_shared("ur5");
|
||||
// planner used for connect
|
||||
auto pipeline = std::make_shared<solvers::PipelinePlanner>(node, "ompl", "RRTConnectkConfigDefault");
|
||||
auto pipeline = std::make_shared<solvers::PipelinePlanner>(node);
|
||||
pipeline->setPlannerId("RRTConnectkConfigDefault");
|
||||
// connect to pick
|
||||
stages::Connect::GroupPlannerVector planners = { { "arm", pipeline }, { "gripper", pipeline } };
|
||||
auto connect = std::make_unique<stages::Connect>("connect", planners);
|
||||
|
@ -33,7 +33,8 @@ solvers::CartesianPathPtr create<solvers::CartesianPath>(const rclcpp::Node::Sha
|
||||
}
|
||||
template <>
|
||||
solvers::PipelinePlannerPtr create<solvers::PipelinePlanner>(const rclcpp::Node::SharedPtr& node) {
|
||||
auto p = std::make_shared<solvers::PipelinePlanner>(node, "pilz_industrial_motion_planner", "LIN");
|
||||
auto p = std::make_shared<solvers::PipelinePlanner>(node, "pilz_industrial_motion_planner");
|
||||
p->setPlannerId("LIN");
|
||||
p->setProperty("max_velocity_scaling_factor", 0.1);
|
||||
p->setProperty("max_acceleration_scaling_factor", 0.1);
|
||||
return p;
|
||||
|
@ -1,63 +0,0 @@
|
||||
#include "models.h"
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <moveit/robot_model/robot_model.hpp>
|
||||
|
||||
#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::vector<std::string>>("STOMP.planning_plugins", { "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));
|
||||
EXPECT_EQ(pipeline_planner.getPlannerId(), "stomp");
|
||||
}
|
||||
|
||||
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();
|
||||
}
|
@ -23,7 +23,8 @@ task.add(currentState)
|
||||
|
||||
# Create a planner instance that is used to connect
|
||||
# the current state to the grasp approach pose
|
||||
pipelinePlanner = core.PipelinePlanner(node, "ompl", "RRTConnectkConfigDefault")
|
||||
pipelinePlanner = core.PipelinePlanner(node)
|
||||
pipelinePlanner.planner = "RRTConnectkConfigDefault"
|
||||
planners = [(group, pipelinePlanner)]
|
||||
|
||||
# Connect the two stages
|
||||
|
@ -8,8 +8,10 @@ import time
|
||||
rclcpp.init()
|
||||
node = rclcpp.Node("mtc_tutorial")
|
||||
|
||||
ompl_planner = core.PipelinePlanner(node, "ompl", "RRTConnectkConfigDefault")
|
||||
pilz_planner = core.PipelinePlanner(node, "pilz_industrial_motion_planner", "PTP")
|
||||
ompl_planner = core.PipelinePlanner(node, "ompl")
|
||||
ompl_planner.planner = "RRTConnectkConfigDefault"
|
||||
pilz_planner = core.PipelinePlanner(node, "pilz_industrial_motion_planner")
|
||||
pilz_planner.planner = "PTP"
|
||||
multiPlanner = core.MultiPlanner()
|
||||
multiPlanner.add(pilz_planner, ompl_planner)
|
||||
|
||||
|
@ -53,7 +53,8 @@ task.add(stages.CurrentState("current"))
|
||||
# [initAndConfigConnect]
|
||||
# Create a planner instance that is used to connect
|
||||
# the current state to the grasp approach pose
|
||||
pipeline = core.PipelinePlanner(node, "ompl", "RRTConnectkConfigDefault")
|
||||
pipeline = core.PipelinePlanner(node)
|
||||
pipeline.planner = "RRTConnectkConfigDefault"
|
||||
planners = [(arm, pipeline)]
|
||||
|
||||
# Connect the two stages
|
||||
|
@ -36,12 +36,14 @@ int main(int argc, char** argv) {
|
||||
auto cartesian = std::make_shared<solvers::CartesianPath>();
|
||||
|
||||
const auto ptp = [&node]() {
|
||||
auto pp{ std::make_shared<solvers::PipelinePlanner>(node, "pilz_industrial_motion_planner", "PTP") };
|
||||
auto pp{ std::make_shared<solvers::PipelinePlanner>(node, "pilz_industrial_motion_planner") };
|
||||
pp->setPlannerId("PTP");
|
||||
return pp;
|
||||
}();
|
||||
|
||||
const auto rrtconnect = [&node]() {
|
||||
auto pp{ std::make_shared<solvers::PipelinePlanner>(node, "ompl", "RRTConnectkConfigDefault") };
|
||||
auto pp{ std::make_shared<solvers::PipelinePlanner>(node, "ompl") };
|
||||
pp->setPlannerId("RRTConnect");
|
||||
return pp;
|
||||
}();
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user