From 0e02fcae77c8162b954675685ccdc7307f6f82e7 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 4 Oct 2023 15:20:10 +0200 Subject: [PATCH] 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 --- .github/workflows/ci.yaml | 1 - .../solvers/pipeline_planner.h | 122 ++++++-- core/src/solvers/pipeline_planner.cpp | 272 +++++++++--------- core/test/CMakeLists.txt | 1 + core/test/pick_pa10.cpp | 3 +- core/test/pick_pr2.cpp | 3 +- core/test/pick_ur5.cpp | 3 +- core/test/test_pipeline_planner.cpp | 63 ++++ demo/src/fallbacks_move_to.cpp | 6 +- 9 files changed, 304 insertions(+), 170 deletions(-) create mode 100644 core/test/test_pipeline_planner.cpp diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 8ce5174e8..b5552d85e 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -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" diff --git a/core/include/moveit/task_constructor/solvers/pipeline_planner.h b/core/include/moveit/task_constructor/solvers/pipeline_planner.h index 68f10cd8e..ddf4568c6 100644 --- a/core/include/moveit/task_constructor/solvers/pipeline_planner.h +++ b/core/include/moveit/task_constructor/solvers/pipeline_planner.h @@ -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 +#include +#include +#include #include #include @@ -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 } }) {} + + /** \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& 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 + * \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); - PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline); + /** \brief Set stopping criterion function for parallel planning + * \param [in] stopping_criterion_callback New stopping criterion function that will be used + */ + void setStoppingCriterionFunction( + const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback); - void setPlannerId(const std::string& planner) { setProperty("planner", planner); } + /** \brief Set solution selection function for parallel planning + * \param [in] solution_selection_function New solution selection that will be used + */ + void setSolutionSelectionFunction( + const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function); + /** \brief 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 planning_pipelines_; + + moveit::planning_pipeline_interfaces::StoppingCriterionFunction stopping_criterion_callback_; + moveit::planning_pipeline_interfaces::SolutionSelectionFunction solution_selection_function_; }; } // namespace solvers } // namespace task_constructor diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index bd469022e..a98f1c697 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -53,160 +53,174 @@ namespace moveit { namespace task_constructor { namespace solvers { -struct PlannerCache -{ - using PlannerID = std::tuple; - using PlannerMap = std::map >; - using ModelList = std::list, PlannerMap> >; - ModelList cache_; - - PlannerMap::mapped_type& retrieve(const moveit::core::RobotModelConstPtr& model, const PlannerID& id) { - // find model in cache_ and remove expired entries while doing so - ModelList::iterator model_it = cache_.begin(); - while (model_it != cache_.end()) { - if (model_it->first.expired()) { - model_it = cache_.erase(model_it); - continue; - } - if (model_it->first.lock() == model) - break; - ++model_it; - } - if (model_it == cache_.end()) // if not found, create a new PlannerMap for this model - model_it = cache_.insert(cache_.begin(), std::make_pair(model, PlannerMap())); - - return model_it->second.insert(std::make_pair(id, PlannerMap::mapped_type())).first->second; - } -}; - -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); +using PipelineMap = std::unordered_map; + +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("num_planning_attempts", 1u, "number of planning attempts"); + properties().declare( + "workspace_parameters", moveit_msgs::msg::WorkspaceParameters(), "allowed workspace of mobile base?"); + + properties().declare("goal_joint_tolerance", 1e-4, "tolerance for reaching joint goals"); + properties().declare("goal_position_tolerance", 1e-4, "tolerance for reaching position goals"); + properties().declare("goal_orientation_tolerance", 1e-4, "tolerance for reaching orientation goals"); + // Declare properties that configure the planning pipeline + properties().declare("display_motion_plans", false, + "publish generated solutions on topic " + + planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC); + properties().declare("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"); +} - std::weak_ptr& entry = cache.retrieve(spec.model, id); - planning_pipeline::PlanningPipelinePtr planner = entry.lock(); - if (!planner) { - // create new entry - planner = std::make_shared(spec.model, node, pipeline_ns, - PLUGIN_PARAMETER_NAME, spec.adapter_param); - // store in cache - entry = planner; +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("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 planner; + it->second = planner_id; + properties().set("pipeline_id_planner_id_map", std::move(map)); + return true; } -PipelinePlanner::PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name) - : pipeline_name_{ pipeline_name }, node_(node) { - auto& p = properties(); - p.declare("planner", "", "planner id"); - - p.declare("num_planning_attempts", 1u, "number of planning attempts"); - p.declare("workspace_parameters", moveit_msgs::msg::WorkspaceParameters(), - "allowed workspace of mobile base?"); - - p.declare("goal_joint_tolerance", 1e-4, "tolerance for reaching joint goals"); - p.declare("goal_position_tolerance", 1e-4, "tolerance for reaching position goals"); - p.declare("goal_orientation_tolerance", 1e-4, "tolerance for reaching orientation goals"); - - p.declare("display_motion_plans", false, - "publish generated solutions on topic " + planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC); - p.declare("publish_planning_requests", false, - "publish motion planning requests on topic " + - planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC); +void PipelinePlanner::setStoppingCriterionFunction( + const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_function) { + stopping_criterion_callback_ = stopping_criterion_function; } - -PipelinePlanner::PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline) - : PipelinePlanner(rclcpp::Node::SharedPtr()) { - planner_ = planning_pipeline; +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("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 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("display_motion_plans")); - planner_->publishReceivedRequests(properties().get("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("planner"); - req.allowed_planning_time = std::min(timeout, p.get("timeout")); - req.start_state.is_diff = true; // we don't specify an extra start state - - req.num_planning_attempts = p.get("num_planning_attempts"); - req.max_velocity_scaling_factor = p.get("max_velocity_scaling_factor"); - req.max_acceleration_scaling_factor = p.get("max_acceleration_scaling_factor"); - req.workspace_parameters = p.get("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("display_motion_plans")); + name_pipeline_pair.second->publishReceivedRequests(properties().get("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("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("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("goal_position_tolerance"), - props.get("goal_orientation_tolerance")); - req.path_constraints = path_constraints; + const auto goal_constraints = kinematic_constraints::constructGoalConstraints( + link.getName(), target, properties().get("goal_position_tolerance"), + properties().get("goal_orientation_tolerance")); + + 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("pipeline_id_planner_id_map"); - ::planning_interface::MotionPlanResponse res; - bool success = planner_->generatePlan(from, req, res); - result = res.trajectory; - return success; + // Create a request for every planning pipeline that should run in parallel + std::vector 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("num_planning_attempts"); + request.max_velocity_scaling_factor = properties().get("max_velocity_scaling_factor"); + request.max_acceleration_scaling_factor = properties().get("max_acceleration_scaling_factor"); + request.workspace_parameters = properties().get("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 diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index 1d8ab42f0..69fe81160 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -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) diff --git a/core/test/pick_pa10.cpp b/core/test/pick_pa10.cpp index 367372400..4ee7b9336 100644 --- a/core/test/pick_pa10.cpp +++ b/core/test/pick_pa10.cpp @@ -47,8 +47,7 @@ TEST(PA10, pick) { t.setProperty("eef", std::string("la_tool_mount")); t.setProperty("gripper", std::string("left_hand")); - auto pipeline = std::make_shared(node); - pipeline->setPlannerId("RRTConnectkConfigDefault"); + auto pipeline = std::make_shared(node, "ompl", "RRTConnectkConfigDefault"); auto cartesian = std::make_shared(); Stage* initial_stage = nullptr; diff --git a/core/test/pick_pr2.cpp b/core/test/pick_pr2.cpp index 2554097b5..c8a31c792 100644 --- a/core/test/pick_pr2.cpp +++ b/core/test/pick_pr2.cpp @@ -40,8 +40,7 @@ TEST(PR2, pick) { auto node = rclcpp::Node::make_shared("pr2"); // planner used for connect - auto pipeline = std::make_shared(node); - pipeline->setPlannerId("RRTConnectkConfigDefault"); + auto pipeline = std::make_shared(node, "ompl", "RRTConnectkConfigDefault"); // connect to pick stages::Connect::GroupPlannerVector planners = { { "left_arm", pipeline }, { "left_gripper", pipeline } }; auto connect = std::make_unique("connect", planners); diff --git a/core/test/pick_ur5.cpp b/core/test/pick_ur5.cpp index 87d17e87e..d268bdc15 100644 --- a/core/test/pick_ur5.cpp +++ b/core/test/pick_ur5.cpp @@ -42,8 +42,7 @@ TEST(UR5, pick) { auto node = rclcpp::Node::make_shared("ur5"); // planner used for connect - auto pipeline = std::make_shared(node); - pipeline->setPlannerId("RRTConnectkConfigDefault"); + auto pipeline = std::make_shared(node, "ompl", "RRTConnectkConfigDefault"); // connect to pick stages::Connect::GroupPlannerVector planners = { { "arm", pipeline }, { "gripper", pipeline } }; auto connect = std::make_unique("connect", planners); diff --git a/core/test/test_pipeline_planner.cpp b/core/test/test_pipeline_planner.cpp new file mode 100644 index 000000000..347d0b572 --- /dev/null +++ b/core/test/test_pipeline_planner.cpp @@ -0,0 +1,63 @@ +#include "models.h" + +#include +#include + +#include + +using namespace moveit::task_constructor; + +// Test fixture for PipelinePlanner +struct PipelinePlannerTest : public testing::Test +{ + PipelinePlannerTest() { + node->declare_parameter("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 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(robot_model); + robot_trajectory::RobotTrajectoryPtr result = + std::make_shared(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(); +} diff --git a/demo/src/fallbacks_move_to.cpp b/demo/src/fallbacks_move_to.cpp index 7bcdccef7..a95ef42ab 100644 --- a/demo/src/fallbacks_move_to.cpp +++ b/demo/src/fallbacks_move_to.cpp @@ -31,14 +31,12 @@ int main(int argc, char** argv) { cartesian->setJumpThreshold(2.0); const auto ptp = [&node]() { - auto pp{ std::make_shared(node, "pilz_industrial_motion_planner") }; - pp->setPlannerId("PTP"); + auto pp{ std::make_shared(node, "pilz_industrial_motion_planner", "PTP") }; return pp; }(); const auto rrtconnect = [&node]() { - auto pp{ std::make_shared(node, "ompl") }; - pp->setPlannerId("RRTConnect"); + auto pp{ std::make_shared(node, "ompl", "RRTConnectkConfigDefault") }; return pp; }();