From c555bea0c6e8873631f511a999bf2dcfa04b07da Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 10 Mar 2024 18:31:19 +0100 Subject: [PATCH] Revert "Enable parallel planning with PipelinePlanner (#450)" This reverts commit 0e02fcae77c8162b954675685ccdc7307f6f82e7. --- .../solvers/pipeline_planner.h | 127 +++------ core/python/bindings/src/solvers.cpp | 8 +- core/python/test/test_mtc.py | 6 + core/src/solvers/pipeline_planner.cpp | 262 +++++++++--------- 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_move_relative.cpp | 3 +- core/test/test_pipeline_planner.cpp | 63 ----- demo/scripts/generate_pose.py | 3 +- demo/scripts/multi_planner.py | 6 +- demo/scripts/pickplace.py | 3 +- demo/src/fallbacks_move_to.cpp | 6 +- 14 files changed, 194 insertions(+), 303 deletions(-) delete mode 100644 core/test/test_pipeline_planner.cpp diff --git a/core/include/moveit/task_constructor/solvers/pipeline_planner.h b/core/include/moveit/task_constructor/solvers/pipeline_planner.h index a2bf1428a..ed0aaa65e 100644 --- a/core/include/moveit/task_constructor/solvers/pipeline_planner.h +++ b/core/include/moveit/task_constructor/solvers/pipeline_planner.h @@ -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 -#include -#include -#include +#include #include #include @@ -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. + 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 */ - 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 rclcpp::Node::SharedPtr& node, const std::string& pipeline = "ompl"); - /** \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); + PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline); - /** \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); + void setPlannerId(const std::string& planner) { setProperty("planner", planner); } + std::string getPlannerId() const override { return properties().get("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 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/python/bindings/src/solvers.cpp b/core/python/bindings/src/solvers.cpp index 7c6a12f49..8d64a2077 100644 --- a/core/python/bindings/src/solvers.cpp +++ b/core/python/bindings/src/solvers.cpp @@ -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("planner", "str: Planner ID") .property("num_planning_attempts", "int: Number of planning attempts") .property( "workspace_parameters", @@ -80,8 +82,8 @@ void export_solvers(py::module& m) { .property("goal_joint_tolerance", "float: Tolerance for reaching joint goals") .property("goal_position_tolerance", "float: Tolerance for reaching position goals") .property("goal_orientation_tolerance", "float: Tolerance for reaching orientation goals") - .def(py::init(), "node"_a, - "pipeline"_a = std::string("ompl"), "planner_id"_a = std::string("")); + .def(py::init(), "node"_a, + "pipeline"_a = std::string("ompl")); properties::class_( m, "JointInterpolationPlanner", diff --git a/core/python/test/test_mtc.py b/core/python/test/test_mtc.py index 8340d8cbd..6a55f0a8e 100644 --- a/core/python/test/test_mtc.py +++ b/core/python/test/test_mtc.py @@ -71,15 +71,21 @@ def test_assign_in_reference(self): 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 diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index e173e7ca7..d0d44e77a 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -53,168 +53,166 @@ namespace moveit { namespace task_constructor { namespace solvers { -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("pipeline_id_planner_id_map", pipeline_id_planner_id_map, - "Set of pipelines and planners used for planning"); -} +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); -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; + 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; } - it->second = planner_id; - properties().set("pipeline_id_planner_id_map", std::move(map)); - return true; + 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("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::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("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 -- "); - } - } + 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("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"); } 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("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("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("goal_position_tolerance"), - properties().get("goal_orientation_tolerance")); + 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; - 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("pipeline_id_planner_id_map"); - last_successful_planner_ = "Unknown"; - - // 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; - 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 diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index 5e4ce4b89..b8c06448e 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -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) diff --git a/core/test/pick_pa10.cpp b/core/test/pick_pa10.cpp index 4ee7b9336..367372400 100644 --- a/core/test/pick_pa10.cpp +++ b/core/test/pick_pa10.cpp @@ -47,7 +47,8 @@ TEST(PA10, pick) { t.setProperty("eef", std::string("la_tool_mount")); t.setProperty("gripper", std::string("left_hand")); - auto pipeline = std::make_shared(node, "ompl", "RRTConnectkConfigDefault"); + auto pipeline = std::make_shared(node); + pipeline->setPlannerId("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 c8a31c792..2554097b5 100644 --- a/core/test/pick_pr2.cpp +++ b/core/test/pick_pr2.cpp @@ -40,7 +40,8 @@ TEST(PR2, pick) { auto node = rclcpp::Node::make_shared("pr2"); // planner used for connect - auto pipeline = std::make_shared(node, "ompl", "RRTConnectkConfigDefault"); + auto pipeline = std::make_shared(node); + pipeline->setPlannerId("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 d268bdc15..87d17e87e 100644 --- a/core/test/pick_ur5.cpp +++ b/core/test/pick_ur5.cpp @@ -42,7 +42,8 @@ TEST(UR5, pick) { auto node = rclcpp::Node::make_shared("ur5"); // planner used for connect - auto pipeline = std::make_shared(node, "ompl", "RRTConnectkConfigDefault"); + auto pipeline = std::make_shared(node); + pipeline->setPlannerId("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_move_relative.cpp b/core/test/test_move_relative.cpp index 80a0ee810..f7216b13d 100644 --- a/core/test/test_move_relative.cpp +++ b/core/test/test_move_relative.cpp @@ -33,7 +33,8 @@ solvers::CartesianPathPtr create(const rclcpp::Node::Sha } template <> solvers::PipelinePlannerPtr create(const rclcpp::Node::SharedPtr& node) { - auto p = std::make_shared(node, "pilz_industrial_motion_planner", "LIN"); + auto p = std::make_shared(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; diff --git a/core/test/test_pipeline_planner.cpp b/core/test/test_pipeline_planner.cpp deleted file mode 100644 index 43df3153a..000000000 --- a/core/test/test_pipeline_planner.cpp +++ /dev/null @@ -1,63 +0,0 @@ -#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_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 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)); - 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(); -} diff --git a/demo/scripts/generate_pose.py b/demo/scripts/generate_pose.py index 2a20d84cc..319195d97 100755 --- a/demo/scripts/generate_pose.py +++ b/demo/scripts/generate_pose.py @@ -23,7 +23,8 @@ # 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 diff --git a/demo/scripts/multi_planner.py b/demo/scripts/multi_planner.py index 948dd96a3..b9f782f37 100755 --- a/demo/scripts/multi_planner.py +++ b/demo/scripts/multi_planner.py @@ -8,8 +8,10 @@ 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) diff --git a/demo/scripts/pickplace.py b/demo/scripts/pickplace.py index be32302e7..e8399c4ae 100755 --- a/demo/scripts/pickplace.py +++ b/demo/scripts/pickplace.py @@ -53,7 +53,8 @@ # [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 diff --git a/demo/src/fallbacks_move_to.cpp b/demo/src/fallbacks_move_to.cpp index 929dedd7b..781c02571 100644 --- a/demo/src/fallbacks_move_to.cpp +++ b/demo/src/fallbacks_move_to.cpp @@ -36,12 +36,14 @@ int main(int argc, char** argv) { auto cartesian = std::make_shared(); const auto ptp = [&node]() { - auto pp{ std::make_shared(node, "pilz_industrial_motion_planner", "PTP") }; + auto pp{ std::make_shared(node, "pilz_industrial_motion_planner") }; + pp->setPlannerId("PTP"); return pp; }(); const auto rrtconnect = [&node]() { - auto pp{ std::make_shared(node, "ompl", "RRTConnectkConfigDefault") }; + auto pp{ std::make_shared(node, "ompl") }; + pp->setPlannerId("RRTConnect"); return pp; }();