Enable parallel planning with PipelinePlanner (#450)

* Refactor pipeline planner

Make code readable

Re-order plan functions

Make usable with parallel planning

Enable configuring multiple pipelines

Add callbacks

Cleanup and documentation

Add API to set parallel planning callbacks and deprecate functions

Pass pipeline map by reference

Small clang-tidy fix

Update core/src/solvers/pipeline_planner.cpp

Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>

Update core/src/solvers/pipeline_planner.cpp

Format

Refactor to avoid calling .at(0) twice

Use no default stopping criteria

Update fallbacks_move demo

* Cleanup + address deprecation warnings

* Enabling optionally using a property defined pipeline planner map

* Address review

* Disable humble CI for ros2 branch

* Add pipeline planner unittests + some checks

* Add short comment
This commit is contained in:
Sebastian Jahr 2023-10-04 15:20:10 +02:00 committed by Robert Haschke
parent 0ba9796fe8
commit 0e02fcae77
9 changed files with 303 additions and 169 deletions

View File

@ -19,7 +19,6 @@ jobs:
fail-fast: false
matrix:
env:
- IMAGE: humble-source
- IMAGE: rolling-source
NAME: ccov
TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage"

View File

@ -32,13 +32,16 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Robert Haschke
Desc: plan using MoveIt's PlanningPipeline
/* Authors: Robert Haschke, Sebastian Jahr
Description: Solver that uses a set of MoveIt PlanningPipelines to solve a given planning problem.
*/
#pragma once
#include <moveit/task_constructor/solvers/planner_interface.h>
#include <moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp>
#include <moveit/planning_pipeline_interfaces/solution_selection_functions.hpp>
#include <moveit/planning_pipeline_interfaces/stopping_criterion_functions.hpp>
#include <rclcpp/node.hpp>
#include <moveit/macros/class_forward.h>
@ -56,48 +59,107 @@ MOVEIT_CLASS_FORWARD(PipelinePlanner);
class PipelinePlanner : public PlannerInterface
{
public:
struct Specification
{
moveit::core::RobotModelConstPtr model;
std::string ns{ "ompl" };
std::string pipeline{ "ompl" };
std::string adapter_param{ "request_adapters" };
};
static planning_pipeline::PlanningPipelinePtr create(const rclcpp::Node::SharedPtr& node,
const moveit::core::RobotModelConstPtr& model) {
Specification spec;
spec.model = model;
return create(node, spec);
}
static planning_pipeline::PlanningPipelinePtr create(const rclcpp::Node::SharedPtr& node, const Specification& spec);
/**
*
* @param node used to load the parameters for the planning pipeline
/** 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 = "ompl");
PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name = "ompl",
const std::string& planner_id = "")
: PipelinePlanner(node, { { pipeline_name, planner_id } }) {}
PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline);
/** \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);
void setPlannerId(const std::string& planner) { setProperty("planner", planner); }
/** \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);
/** \brief Set stopping criterion function for parallel planning
* \param [in] stopping_criterion_callback New stopping criterion function that will be used
*/
void setStoppingCriterionFunction(
const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback);
/** \brief Set solution selection function for parallel planning
* \param [in] solution_selection_function New solution selection that will be used
*/
void setSolutionSelectionFunction(
const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function);
/** \brief 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
*/
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const core::JointModelGroup* joint_model_group, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
/** \brief Plan a trajectory from a planning scene 'from' to a 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
*/
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
const moveit::core::JointModelGroup* joint_model_group, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
protected:
std::string pipeline_name_;
planning_pipeline::PlanningPipelinePtr planner_;
/** \brief 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
*/
bool plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
const moveit::core::JointModelGroup* joint_model_group,
const moveit_msgs::msg::Constraints& goal_constraints, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints());
rclcpp::Node::SharedPtr node_;
/** \brief Map of 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

@ -53,160 +53,174 @@ namespace moveit {
namespace task_constructor {
namespace solvers {
struct PlannerCache
{
using PlannerID = std::tuple<std::string, std::string>;
using PlannerMap = std::map<PlannerID, std::weak_ptr<planning_pipeline::PlanningPipeline> >;
using ModelList = std::list<std::pair<std::weak_ptr<const moveit::core::RobotModel>, PlannerMap> >;
ModelList cache_;
using PipelineMap = std::unordered_map<std::string, std::string>;
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()));
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?");
return model_it->second.insert(std::make_pair(id, PlannerMap::mapped_type())).first->second;
}
};
planning_pipeline::PlanningPipelinePtr PipelinePlanner::create(const rclcpp::Node::SharedPtr& node,
const PipelinePlanner::Specification& spec) {
static PlannerCache cache;
static constexpr char const* PLUGIN_PARAMETER_NAME = "planning_plugin";
std::string pipeline_ns = spec.ns;
const std::string parameter_name = pipeline_ns + "." + PLUGIN_PARAMETER_NAME;
// fallback to old structure for pipeline parameters in MoveIt
if (!node->has_parameter(parameter_name)) {
node->declare_parameter(parameter_name, rclcpp::ParameterType::PARAMETER_STRING);
}
if (std::string parameter; !node->get_parameter(parameter_name, parameter)) {
RCLCPP_WARN(node->get_logger(), "Failed to find '%s.%s'. %s", pipeline_ns.c_str(), PLUGIN_PARAMETER_NAME,
"Attempting to load pipeline from old parameter structure. Please update your MoveIt config.");
pipeline_ns = "move_group";
}
PlannerCache::PlannerID id(pipeline_ns, spec.adapter_param);
std::weak_ptr<planning_pipeline::PlanningPipeline>& entry = cache.retrieve(spec.model, id);
planning_pipeline::PlanningPipelinePtr planner = entry.lock();
if (!planner) {
// create new entry
planner = std::make_shared<planning_pipeline::PlanningPipeline>(spec.model, node, pipeline_ns,
PLUGIN_PARAMETER_NAME, spec.adapter_param);
// store in cache
entry = planner;
}
return planner;
properties().declare<double>("goal_joint_tolerance", 1e-4, "tolerance for reaching joint goals");
properties().declare<double>("goal_position_tolerance", 1e-4, "tolerance for reaching position goals");
properties().declare<double>("goal_orientation_tolerance", 1e-4, "tolerance for reaching orientation goals");
// Declare properties that configure the planning pipeline
properties().declare<bool>("display_motion_plans", false,
"publish generated solutions on topic " +
planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC);
properties().declare<bool>("publish_planning_requests", false,
"publish motion planning requests on topic " +
planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC);
properties().declare("pipeline_id_planner_id_map", pipeline_id_planner_id_map,
"Set of pipelines and planners used for planning");
}
PipelinePlanner::PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name)
: pipeline_name_{ pipeline_name }, node_(node) {
auto& p = properties();
p.declare<std::string>("planner", "", "planner id");
p.declare<uint>("num_planning_attempts", 1u, "number of planning attempts");
p.declare<moveit_msgs::msg::WorkspaceParameters>("workspace_parameters", moveit_msgs::msg::WorkspaceParameters(),
"allowed workspace of mobile base?");
p.declare<double>("goal_joint_tolerance", 1e-4, "tolerance for reaching joint goals");
p.declare<double>("goal_position_tolerance", 1e-4, "tolerance for reaching position goals");
p.declare<double>("goal_orientation_tolerance", 1e-4, "tolerance for reaching orientation goals");
p.declare<bool>("display_motion_plans", false,
"publish generated solutions on topic " + planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC);
p.declare<bool>("publish_planning_requests", false,
"publish motion planning requests on topic " +
planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC);
bool PipelinePlanner::setPlannerId(const std::string& pipeline_name, const std::string& planner_id) {
// Only set ID if pipeline exists. It is not possible to create new pipelines with this command.
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", std::move(map));
return true;
}
PipelinePlanner::PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline)
: PipelinePlanner(rclcpp::Node::SharedPtr()) {
planner_ = planning_pipeline;
void PipelinePlanner::setStoppingCriterionFunction(
const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_function) {
stopping_criterion_callback_ = stopping_criterion_function;
}
void PipelinePlanner::setSolutionSelectionFunction(
const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function) {
solution_selection_function_ = solution_selection_function;
}
void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) {
if (!planner_) {
Specification spec;
spec.model = robot_model;
spec.pipeline = pipeline_name_;
spec.ns = pipeline_name_;
planner_ = create(node_, spec);
} else if (robot_model != planner_->getRobotModel()) {
throw std::runtime_error(
"The robot model of the planning pipeline isn't the same as the task's robot model -- "
"use Task::setRobotModel for setting the robot model when using custom planning pipeline");
// 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;
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(std::move(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");
// Configure all pipelines according to the configuration in properties
for (auto const& name_pipeline_pair : planning_pipelines_) {
name_pipeline_pair.second->displayComputedMotionPlans(properties().get<bool>("display_motion_plans"));
name_pipeline_pair.second->publishReceivedRequests(properties().get<bool>("publish_planning_requests"));
}
}
bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup* joint_model_group, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints) {
const auto& props = properties();
moveit_msgs::msg::MotionPlanRequest req;
initMotionPlanRequest(req, props, jmg, timeout);
req.goal_constraints.resize(1);
req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(to->getCurrentState(), jmg,
props.get<double>("goal_joint_tolerance"));
req.path_constraints = path_constraints;
::planning_interface::MotionPlanResponse res;
bool success = planner_->generatePlan(from, req, res);
result = res.trajectory;
return success;
// Construct goal constraints from the goal planning scene
const auto goal_constraints = kinematic_constraints::constructGoalConstraints(
to->getCurrentState(), joint_model_group, properties().get<double>("goal_joint_tolerance"));
return plan(from, joint_model_group, goal_constraints, timeout, result, path_constraints);
}
bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target_eigen,
const moveit::core::JointModelGroup* jmg, double timeout,
const moveit::core::JointModelGroup* joint_model_group, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints) {
const auto& props = properties();
moveit_msgs::msg::MotionPlanRequest req;
initMotionPlanRequest(req, props, jmg, timeout);
// Construct a Cartesian target pose from the given target transform and offset
geometry_msgs::msg::PoseStamped target;
target.header.frame_id = from->getPlanningFrame();
target.pose = tf2::toMsg(target_eigen * offset.inverse());
req.goal_constraints.resize(1);
req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(
link.getName(), target, props.get<double>("goal_position_tolerance"),
props.get<double>("goal_orientation_tolerance"));
req.path_constraints = path_constraints;
const auto goal_constraints = kinematic_constraints::constructGoalConstraints(
link.getName(), target, properties().get<double>("goal_position_tolerance"),
properties().get<double>("goal_orientation_tolerance"));
::planning_interface::MotionPlanResponse res;
bool success = planner_->generatePlan(from, req, res);
result = res.trajectory;
return success;
return plan(from, joint_model_group, goal_constraints, timeout, result, path_constraints);
}
bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
const moveit::core::JointModelGroup* joint_model_group,
const moveit_msgs::msg::Constraints& goal_constraints, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints) {
const auto& map = properties().get<PipelineMap>("pipeline_id_planner_id_map");
// 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;
return bool(result);
}
}
return false;
}
} // namespace solvers
} // namespace task_constructor

View File

@ -54,6 +54,7 @@ if (BUILD_TESTING)
mtc_add_gtest(test_move_to.cpp move_to.launch.py)
mtc_add_gtest(test_move_relative.cpp move_to.launch.py)
mtc_add_gtest(test_pipeline_planner.cpp)
# building these integration tests works without moveit config packages
ament_add_gtest_executable(pick_ur5 pick_ur5.cpp)

View File

@ -47,8 +47,7 @@ TEST(PA10, pick) {
t.setProperty("eef", std::string("la_tool_mount"));
t.setProperty("gripper", std::string("left_hand"));
auto pipeline = std::make_shared<solvers::PipelinePlanner>(node);
pipeline->setPlannerId("RRTConnectkConfigDefault");
auto pipeline = std::make_shared<solvers::PipelinePlanner>(node, "ompl", "RRTConnectkConfigDefault");
auto cartesian = std::make_shared<solvers::CartesianPath>();
Stage* initial_stage = nullptr;

View File

@ -40,8 +40,7 @@ TEST(PR2, pick) {
auto node = rclcpp::Node::make_shared("pr2");
// planner used for connect
auto pipeline = std::make_shared<solvers::PipelinePlanner>(node);
pipeline->setPlannerId("RRTConnectkConfigDefault");
auto pipeline = std::make_shared<solvers::PipelinePlanner>(node, "ompl", "RRTConnectkConfigDefault");
// connect to pick
stages::Connect::GroupPlannerVector planners = { { "left_arm", pipeline }, { "left_gripper", pipeline } };
auto connect = std::make_unique<stages::Connect>("connect", planners);

View File

@ -42,8 +42,7 @@ TEST(UR5, pick) {
auto node = rclcpp::Node::make_shared("ur5");
// planner used for connect
auto pipeline = std::make_shared<solvers::PipelinePlanner>(node);
pipeline->setPlannerId("RRTConnectkConfigDefault");
auto pipeline = std::make_shared<solvers::PipelinePlanner>(node, "ompl", "RRTConnectkConfigDefault");
// connect to pick
stages::Connect::GroupPlannerVector planners = { { "arm", pipeline }, { "gripper", pipeline } };
auto connect = std::make_unique<stages::Connect>("connect", planners);

View File

@ -0,0 +1,63 @@
#include "models.h"
#include <gtest/gtest.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/task_constructor/solvers/pipeline_planner.h>
using namespace moveit::task_constructor;
// Test fixture for PipelinePlanner
struct PipelinePlannerTest : public testing::Test
{
PipelinePlannerTest() {
node->declare_parameter<std::string>("STOMP.planning_plugin", "stomp_moveit/StompPlanner");
};
const rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("test_pipeline_planner");
const moveit::core::RobotModelPtr robot_model = getModel();
};
TEST_F(PipelinePlannerTest, testInitialization) {
// GIVEN a valid robot model, ROS node and PipelinePlanner
auto pipeline_planner = solvers::PipelinePlanner(node, "STOMP", "stomp");
// WHEN a PipelinePlanner instance is initialized
// THEN it does not throw
EXPECT_NO_THROW(pipeline_planner.init(robot_model));
}
TEST_F(PipelinePlannerTest, testWithoutPlanningPipelines) {
// GIVEN a PipelinePlanner instance without planning pipelines
std::unordered_map<std::string, std::string> empty_pipeline_id_planner_id_map;
auto pipeline_planner = solvers::PipelinePlanner(node, empty_pipeline_id_planner_id_map);
// WHEN a PipelinePlanner instance is initialized
// THEN it does not throw
EXPECT_THROW(pipeline_planner.init(robot_model), std::runtime_error);
}
TEST_F(PipelinePlannerTest, testValidPlan) {
// GIVEN an initialized PipelinePlanner
auto pipeline_planner = solvers::PipelinePlanner(node, "STOMP", "stomp");
pipeline_planner.init(robot_model);
// WHEN a solution for a valid request is requested
auto scene = std::make_shared<planning_scene::PlanningScene>(robot_model);
robot_trajectory::RobotTrajectoryPtr result =
std::make_shared<robot_trajectory::RobotTrajectory>(robot_model, robot_model->getJointModelGroup("group"));
// THEN it returns true
EXPECT_TRUE(pipeline_planner.plan(scene, scene, robot_model->getJointModelGroup("group"), 1.0, result));
}
TEST_F(PipelinePlannerTest, testInvalidPipelineID) {
// GIVEN a valid initialized PipelinePlanner instance
auto pipeline_planner = solvers::PipelinePlanner(node, "STOMP", "stomp");
pipeline_planner.init(robot_model);
// WHEN the planner ID for a non-existing planning pipeline is set
// THEN setPlannerID returns false
EXPECT_FALSE(pipeline_planner.setPlannerId("CHOMP", "stomp"));
}
int main(int argc, char** argv) {
testing::InitGoogleTest(&argc, argv);
rclcpp::init(argc, argv);
return RUN_ALL_TESTS();
}

View File

@ -31,14 +31,12 @@ int main(int argc, char** argv) {
cartesian->setJumpThreshold(2.0);
const auto ptp = [&node]() {
auto pp{ std::make_shared<solvers::PipelinePlanner>(node, "pilz_industrial_motion_planner") };
pp->setPlannerId("PTP");
auto pp{ std::make_shared<solvers::PipelinePlanner>(node, "pilz_industrial_motion_planner", "PTP") };
return pp;
}();
const auto rrtconnect = [&node]() {
auto pp{ std::make_shared<solvers::PipelinePlanner>(node, "ompl") };
pp->setPlannerId("RRTConnect");
auto pp{ std::make_shared<solvers::PipelinePlanner>(node, "ompl", "RRTConnectkConfigDefault") };
return pp;
}();