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.
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
*********************************************************************/
|
*********************************************************************/
|
||||||
|
|
||||||
/* Authors: Robert Haschke, Sebastian Jahr
|
/* Authors: Robert Haschke
|
||||||
Description: Solver that uses a set of MoveIt PlanningPipelines to solve a given planning problem.
|
Desc: plan using MoveIt's PlanningPipeline
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <moveit/task_constructor/solvers/planner_interface.h>
|
#include <moveit/task_constructor/solvers/planner_interface.h>
|
||||||
#include <moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp>
|
#include <moveit_msgs/msg/motion_plan_request.hpp>
|
||||||
#include <moveit/planning_pipeline_interfaces/solution_selection_functions.hpp>
|
|
||||||
#include <moveit/planning_pipeline_interfaces/stopping_criterion_functions.hpp>
|
|
||||||
#include <rclcpp/node.hpp>
|
#include <rclcpp/node.hpp>
|
||||||
#include <moveit/macros/class_forward.hpp>
|
#include <moveit/macros/class_forward.hpp>
|
||||||
|
|
||||||
@ -59,111 +57,52 @@ MOVEIT_CLASS_FORWARD(PipelinePlanner);
|
|||||||
class PipelinePlanner : public PlannerInterface
|
class PipelinePlanner : public PlannerInterface
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
/** Simple Constructor to use only a single pipeline
|
struct Specification
|
||||||
* \param [in] node ROS 2 node
|
{
|
||||||
* \param [in] pipeline_name Name of the planning pipeline to be used.
|
moveit::core::RobotModelConstPtr model;
|
||||||
* This is also the assumed namespace where the parameters of this pipeline can be found
|
std::string ns{ "ompl" };
|
||||||
* \param [in] planner_id Planner id to be used for planning. Empty string means default.
|
std::string pipeline{ "ompl" };
|
||||||
*/
|
std::string adapter_param{ "request_adapters" };
|
||||||
PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name = "ompl",
|
};
|
||||||
const std::string& planner_id = "")
|
|
||||||
: PipelinePlanner(node, { { pipeline_name, planner_id } }) {}
|
|
||||||
|
|
||||||
/** \brief Constructor
|
static planning_pipeline::PlanningPipelinePtr create(const rclcpp::Node::SharedPtr& node,
|
||||||
* \param [in] node ROS 2 node
|
const moveit::core::RobotModelConstPtr& model) {
|
||||||
* \param [in] pipeline_id_planner_id_map map containing pairs of pipeline and plugin names to be used for planning
|
Specification spec;
|
||||||
* \param [in] stopping_criterion_callback callback to decide when to stop parallel planning
|
spec.model = model;
|
||||||
* \param [in] solution_selection_function callback to choose the best solution from multiple ran pipelines
|
return create(node, spec);
|
||||||
*/
|
}
|
||||||
PipelinePlanner(
|
|
||||||
const rclcpp::Node::SharedPtr& node,
|
|
||||||
const std::unordered_map<std::string, std::string>& pipeline_id_planner_id_map,
|
|
||||||
const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback = nullptr,
|
|
||||||
const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function =
|
|
||||||
&moveit::planning_pipeline_interfaces::getShortestSolution);
|
|
||||||
|
|
||||||
/** \brief Set the planner id for a specific planning pipeline
|
static planning_pipeline::PlanningPipelinePtr create(const rclcpp::Node::SharedPtr& node, const Specification& spec);
|
||||||
* \param [in] pipeline_name Name of the to-be-used planning pipeline
|
|
||||||
* \param [in] planner_id Name of the to-be-used planner ID
|
|
||||||
* \return true if the pipeline exists and the corresponding ID is set successfully
|
|
||||||
*/
|
|
||||||
bool setPlannerId(const std::string& pipeline_name, const std::string& planner_id);
|
|
||||||
|
|
||||||
/** \brief Set stopping criterion function for parallel planning
|
/**
|
||||||
* \param [in] stopping_criterion_callback New stopping criterion function that will be used
|
*
|
||||||
|
* @param node used to load the parameters for the planning pipeline
|
||||||
*/
|
*/
|
||||||
void setStoppingCriterionFunction(
|
PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline = "ompl");
|
||||||
const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback);
|
|
||||||
|
|
||||||
/** \brief Set solution selection function for parallel planning
|
PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline);
|
||||||
* \param [in] solution_selection_function New solution selection that will be used
|
|
||||||
*/
|
void setPlannerId(const std::string& planner) { setProperty("planner", planner); }
|
||||||
void setSolutionSelectionFunction(
|
std::string getPlannerId() const override { return properties().get<std::string>("planner"); }
|
||||||
const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function);
|
|
||||||
|
|
||||||
/** \brief If not yet done, initialize pipelines from pipeline_id_planner_id_map
|
|
||||||
* \param [in] robot_model robot model used to initialize the planning pipelines of this solver
|
|
||||||
*/
|
|
||||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
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,
|
Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
|
||||||
const core::JointModelGroup* joint_model_group, double timeout,
|
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
robot_trajectory::RobotTrajectoryPtr& result,
|
|
||||||
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
|
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,
|
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
|
||||||
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
|
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
|
||||||
const moveit::core::JointModelGroup* joint_model_group, double timeout,
|
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
robot_trajectory::RobotTrajectoryPtr& result,
|
|
||||||
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
|
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
|
||||||
|
|
||||||
std::string getPlannerId() const override { return last_successful_planner_; }
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
/** \brief Actual plan() implementation, targeting the given goal_constraints.
|
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit_msgs::msg::MotionPlanRequest& req,
|
||||||
* \param [in] planning_scene Scene for which the planning should be solved
|
robot_trajectory::RobotTrajectoryPtr& result);
|
||||||
* \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());
|
|
||||||
|
|
||||||
|
std::string pipeline_name_;
|
||||||
|
planning_pipeline::PlanningPipelinePtr planner_;
|
||||||
rclcpp::Node::SharedPtr node_;
|
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 solvers
|
||||||
} // namespace task_constructor
|
} // namespace task_constructor
|
||||||
|
@ -70,9 +70,11 @@ void export_solvers(py::module& m) {
|
|||||||
from moveit.task_constructor import core
|
from moveit.task_constructor import core
|
||||||
|
|
||||||
# Create and configure a planner instance
|
# Create and configure a planner instance
|
||||||
pipelinePlanner = core.PipelinePlanner(node, 'ompl', 'PRMkConfigDefault')
|
pipelinePlanner = core.PipelinePlanner()
|
||||||
|
pipelinePlanner.planner = 'PRMkConfigDefault'
|
||||||
pipelinePlanner.num_planning_attempts = 10
|
pipelinePlanner.num_planning_attempts = 10
|
||||||
)")
|
)")
|
||||||
|
.property<std::string>("planner", "str: Planner ID")
|
||||||
.property<uint>("num_planning_attempts", "int: Number of planning attempts")
|
.property<uint>("num_planning_attempts", "int: Number of planning attempts")
|
||||||
.property<moveit_msgs::msg::WorkspaceParameters>(
|
.property<moveit_msgs::msg::WorkspaceParameters>(
|
||||||
"workspace_parameters",
|
"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_joint_tolerance", "float: Tolerance for reaching joint goals")
|
||||||
.property<double>("goal_position_tolerance", "float: Tolerance for reaching position goals")
|
.property<double>("goal_position_tolerance", "float: Tolerance for reaching position goals")
|
||||||
.property<double>("goal_orientation_tolerance", "float: Tolerance for reaching orientation 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,
|
.def(py::init<const rclcpp::Node::SharedPtr&, const std::string&>(), "node"_a,
|
||||||
"pipeline"_a = std::string("ompl"), "planner_id"_a = std::string(""));
|
"pipeline"_a = std::string("ompl"));
|
||||||
|
|
||||||
properties::class_<JointInterpolationPlanner, PlannerInterface>(
|
properties::class_<JointInterpolationPlanner, PlannerInterface>(
|
||||||
m, "JointInterpolationPlanner",
|
m, "JointInterpolationPlanner",
|
||||||
|
@ -71,15 +71,21 @@ class TestPropertyMap(unittest.TestCase):
|
|||||||
|
|
||||||
props["planner"] = "planner"
|
props["planner"] = "planner"
|
||||||
self.assertEqual(props["planner"], "planner")
|
self.assertEqual(props["planner"], "planner")
|
||||||
|
self.assertEqual(planner.planner, "planner")
|
||||||
|
|
||||||
props["double"] = 3.14
|
props["double"] = 3.14
|
||||||
a = props
|
a = props
|
||||||
props["double"] = 2.71
|
props["double"] = 2.71
|
||||||
self.assertEqual(a["double"], 2.71)
|
self.assertEqual(a["double"], 2.71)
|
||||||
|
|
||||||
|
planner.planner = "other"
|
||||||
|
self.assertEqual(props["planner"], "other")
|
||||||
|
self.assertEqual(planner.planner, "other")
|
||||||
|
|
||||||
del planner
|
del planner
|
||||||
# We can still access props, because actual destruction of planner is delayed
|
# We can still access props, because actual destruction of planner is delayed
|
||||||
self.assertEqual(props["goal_joint_tolerance"], 2.71)
|
self.assertEqual(props["goal_joint_tolerance"], 2.71)
|
||||||
|
self.assertEqual(props["planner"], "other")
|
||||||
|
|
||||||
def test_iter(self):
|
def test_iter(self):
|
||||||
# assign values so we can iterate over them
|
# assign values so we can iterate over them
|
||||||
|
@ -53,169 +53,166 @@ namespace moveit {
|
|||||||
namespace task_constructor {
|
namespace task_constructor {
|
||||||
namespace solvers {
|
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(
|
PlannerMap::mapped_type& retrieve(const moveit::core::RobotModelConstPtr& model, const PlannerID& id) {
|
||||||
const rclcpp::Node::SharedPtr& node, const PipelineMap& pipeline_id_planner_id_map,
|
// find model in cache_ and remove expired entries while doing so
|
||||||
const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback,
|
ModelList::iterator model_it = cache_.begin();
|
||||||
const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function)
|
while (model_it != cache_.end()) {
|
||||||
: node_(node)
|
if (model_it->first.expired()) {
|
||||||
, stopping_criterion_callback_(stopping_criterion_callback)
|
model_it = cache_.erase(model_it);
|
||||||
, solution_selection_function_(solution_selection_function) {
|
continue;
|
||||||
// Declare properties of the MotionPlanRequest
|
}
|
||||||
properties().declare<uint>("num_planning_attempts", 1u, "number of planning attempts");
|
if (model_it->first.lock() == model)
|
||||||
properties().declare<moveit_msgs::msg::WorkspaceParameters>(
|
break;
|
||||||
"workspace_parameters", moveit_msgs::msg::WorkspaceParameters(), "allowed workspace of mobile base?");
|
++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");
|
return model_it->second.insert(std::make_pair(id, PlannerMap::mapped_type())).first->second;
|
||||||
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;
|
|
||||||
}
|
}
|
||||||
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(
|
PipelinePlanner::PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name)
|
||||||
const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_function) {
|
: pipeline_name_{ pipeline_name }, node_(node) {
|
||||||
stopping_criterion_callback_ = stopping_criterion_function;
|
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) {
|
PipelinePlanner::PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline)
|
||||||
solution_selection_function_ = solution_selection_function;
|
: PipelinePlanner(rclcpp::Node::SharedPtr()) {
|
||||||
|
planner_ = planning_pipeline;
|
||||||
}
|
}
|
||||||
|
|
||||||
void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) {
|
void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) {
|
||||||
// Create planning pipelines once from pipeline_id_planner_id_map.
|
if (!planner_) {
|
||||||
// We assume that all parameters required by the pipeline can be found
|
Specification spec;
|
||||||
// in the namespace of the pipeline name.
|
spec.model = robot_model;
|
||||||
if (planning_pipelines_.empty()) {
|
spec.pipeline = pipeline_name_;
|
||||||
auto map = properties().get<PipelineMap>("pipeline_id_planner_id_map");
|
spec.ns = pipeline_name_;
|
||||||
// Create pipeline name vector from the keys of pipeline_id_planner_id_map_
|
planner_ = create(node_, spec);
|
||||||
if (map.empty()) {
|
} else if (robot_model != planner_->getRobotModel()) {
|
||||||
throw std::runtime_error("Cannot initialize PipelinePlanner: pipeline_id_planner_id_map is empty!");
|
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");
|
||||||
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 -- ");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
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,
|
PlannerInterface::Result PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
|
||||||
const planning_scene::PlanningSceneConstPtr& to,
|
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,
|
robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
const moveit_msgs::msg::Constraints& path_constraints) {
|
const moveit_msgs::msg::Constraints& path_constraints) {
|
||||||
// Construct goal constraints from the goal planning scene
|
const auto& props = properties();
|
||||||
const auto goal_constraints = kinematic_constraints::constructGoalConstraints(
|
moveit_msgs::msg::MotionPlanRequest req;
|
||||||
to->getCurrentState(), joint_model_group, properties().get<double>("goal_joint_tolerance"));
|
initMotionPlanRequest(req, props, jmg, timeout);
|
||||||
return plan(from, joint_model_group, goal_constraints, timeout, result, path_constraints);
|
|
||||||
|
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,
|
PlannerInterface::Result PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
|
||||||
const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset,
|
const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset,
|
||||||
const Eigen::Isometry3d& target_eigen,
|
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,
|
robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
const moveit_msgs::msg::Constraints& path_constraints) {
|
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;
|
geometry_msgs::msg::PoseStamped target;
|
||||||
target.header.frame_id = from->getPlanningFrame();
|
target.header.frame_id = from->getPlanningFrame();
|
||||||
target.pose = tf2::toMsg(target_eigen * offset.inverse());
|
target.pose = tf2::toMsg(target_eigen * offset.inverse());
|
||||||
|
|
||||||
const auto goal_constraints = kinematic_constraints::constructGoalConstraints(
|
req.goal_constraints.resize(1);
|
||||||
link.getName(), target, properties().get<double>("goal_position_tolerance"),
|
req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(
|
||||||
properties().get<double>("goal_orientation_tolerance"));
|
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,
|
PlannerInterface::Result PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
|
||||||
const moveit::core::JointModelGroup* joint_model_group,
|
const moveit_msgs::msg::MotionPlanRequest& req,
|
||||||
const moveit_msgs::msg::Constraints& goal_constraints, double timeout,
|
robot_trajectory::RobotTrajectoryPtr& result) {
|
||||||
robot_trajectory::RobotTrajectoryPtr& result,
|
::planning_interface::MotionPlanResponse res;
|
||||||
const moveit_msgs::msg::Constraints& path_constraints) {
|
bool success = planner_->generatePlan(from, req, res);
|
||||||
const auto& map = properties().get<PipelineMap>("pipeline_id_planner_id_map");
|
result = res.trajectory;
|
||||||
last_successful_planner_ = "Unknown";
|
return { success, success ? std::string() : moveit::core::error_code_to_string(res.error_code.val) };
|
||||||
|
|
||||||
// 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" };
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace solvers
|
} // namespace solvers
|
||||||
} // namespace task_constructor
|
} // namespace task_constructor
|
||||||
} // namespace moveit
|
} // namespace moveit
|
||||||
|
@ -56,7 +56,6 @@ if (BUILD_TESTING)
|
|||||||
|
|
||||||
mtc_add_gtest(test_move_to.cpp test.launch.py)
|
mtc_add_gtest(test_move_to.cpp test.launch.py)
|
||||||
mtc_add_gtest(test_move_relative.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
|
# building these integration tests works without moveit config packages
|
||||||
ament_add_gtest_executable(pick_ur5 pick_ur5.cpp)
|
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("eef", std::string("la_tool_mount"));
|
||||||
t.setProperty("gripper", std::string("left_hand"));
|
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>();
|
auto cartesian = std::make_shared<solvers::CartesianPath>();
|
||||||
|
|
||||||
Stage* initial_stage = nullptr;
|
Stage* initial_stage = nullptr;
|
||||||
|
@ -40,7 +40,8 @@ TEST(PR2, pick) {
|
|||||||
|
|
||||||
auto node = rclcpp::Node::make_shared("pr2");
|
auto node = rclcpp::Node::make_shared("pr2");
|
||||||
// planner used for connect
|
// 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
|
// connect to pick
|
||||||
stages::Connect::GroupPlannerVector planners = { { "left_arm", pipeline }, { "left_gripper", pipeline } };
|
stages::Connect::GroupPlannerVector planners = { { "left_arm", pipeline }, { "left_gripper", pipeline } };
|
||||||
auto connect = std::make_unique<stages::Connect>("connect", planners);
|
auto connect = std::make_unique<stages::Connect>("connect", planners);
|
||||||
|
@ -42,7 +42,8 @@ TEST(UR5, pick) {
|
|||||||
|
|
||||||
auto node = rclcpp::Node::make_shared("ur5");
|
auto node = rclcpp::Node::make_shared("ur5");
|
||||||
// planner used for connect
|
// 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
|
// connect to pick
|
||||||
stages::Connect::GroupPlannerVector planners = { { "arm", pipeline }, { "gripper", pipeline } };
|
stages::Connect::GroupPlannerVector planners = { { "arm", pipeline }, { "gripper", pipeline } };
|
||||||
auto connect = std::make_unique<stages::Connect>("connect", planners);
|
auto connect = std::make_unique<stages::Connect>("connect", planners);
|
||||||
|
@ -33,7 +33,8 @@ solvers::CartesianPathPtr create<solvers::CartesianPath>(const rclcpp::Node::Sha
|
|||||||
}
|
}
|
||||||
template <>
|
template <>
|
||||||
solvers::PipelinePlannerPtr create<solvers::PipelinePlanner>(const rclcpp::Node::SharedPtr& node) {
|
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_velocity_scaling_factor", 0.1);
|
||||||
p->setProperty("max_acceleration_scaling_factor", 0.1);
|
p->setProperty("max_acceleration_scaling_factor", 0.1);
|
||||||
return p;
|
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
|
# Create a planner instance that is used to connect
|
||||||
# the current state to the grasp approach pose
|
# the current state to the grasp approach pose
|
||||||
pipelinePlanner = core.PipelinePlanner(node, "ompl", "RRTConnectkConfigDefault")
|
pipelinePlanner = core.PipelinePlanner(node)
|
||||||
|
pipelinePlanner.planner = "RRTConnectkConfigDefault"
|
||||||
planners = [(group, pipelinePlanner)]
|
planners = [(group, pipelinePlanner)]
|
||||||
|
|
||||||
# Connect the two stages
|
# Connect the two stages
|
||||||
|
@ -8,8 +8,10 @@ import time
|
|||||||
rclcpp.init()
|
rclcpp.init()
|
||||||
node = rclcpp.Node("mtc_tutorial")
|
node = rclcpp.Node("mtc_tutorial")
|
||||||
|
|
||||||
ompl_planner = core.PipelinePlanner(node, "ompl", "RRTConnectkConfigDefault")
|
ompl_planner = core.PipelinePlanner(node, "ompl")
|
||||||
pilz_planner = core.PipelinePlanner(node, "pilz_industrial_motion_planner", "PTP")
|
ompl_planner.planner = "RRTConnectkConfigDefault"
|
||||||
|
pilz_planner = core.PipelinePlanner(node, "pilz_industrial_motion_planner")
|
||||||
|
pilz_planner.planner = "PTP"
|
||||||
multiPlanner = core.MultiPlanner()
|
multiPlanner = core.MultiPlanner()
|
||||||
multiPlanner.add(pilz_planner, ompl_planner)
|
multiPlanner.add(pilz_planner, ompl_planner)
|
||||||
|
|
||||||
|
@ -53,7 +53,8 @@ task.add(stages.CurrentState("current"))
|
|||||||
# [initAndConfigConnect]
|
# [initAndConfigConnect]
|
||||||
# Create a planner instance that is used to connect
|
# Create a planner instance that is used to connect
|
||||||
# the current state to the grasp approach pose
|
# the current state to the grasp approach pose
|
||||||
pipeline = core.PipelinePlanner(node, "ompl", "RRTConnectkConfigDefault")
|
pipeline = core.PipelinePlanner(node)
|
||||||
|
pipeline.planner = "RRTConnectkConfigDefault"
|
||||||
planners = [(arm, pipeline)]
|
planners = [(arm, pipeline)]
|
||||||
|
|
||||||
# Connect the two stages
|
# Connect the two stages
|
||||||
|
@ -36,12 +36,14 @@ int main(int argc, char** argv) {
|
|||||||
auto cartesian = std::make_shared<solvers::CartesianPath>();
|
auto cartesian = std::make_shared<solvers::CartesianPath>();
|
||||||
|
|
||||||
const auto ptp = [&node]() {
|
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;
|
return pp;
|
||||||
}();
|
}();
|
||||||
|
|
||||||
const auto rrtconnect = [&node]() {
|
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;
|
return pp;
|
||||||
}();
|
}();
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user