Revert "Enable parallel planning with PipelinePlanner (#450)"

This reverts commit 0e02fcae77.
This commit is contained in:
Robert Haschke 2024-03-10 18:31:19 +01:00
parent 56f75cc179
commit 5fd3ae6ff2
14 changed files with 191 additions and 301 deletions

View File

@ -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

View File

@ -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",

View File

@ -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

View File

@ -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

View File

@ -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)

View File

@ -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;

View File

@ -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);

View File

@ -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);

View File

@ -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;

View File

@ -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();
}

View File

@ -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

View File

@ -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)

View File

@ -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

View File

@ -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;
}();