From 28682976d67b43537e1d5f2ff051c7b8fe7d1a95 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Tue, 28 Mar 2023 15:28:02 +0200 Subject: [PATCH 1/7] 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 --- .../solvers/pipeline_planner.h | 149 ++++++++-- core/src/solvers/pipeline_planner.cpp | 268 +++++++++--------- demo/src/fallbacks_move_to.cpp | 6 +- 3 files changed, 260 insertions(+), 163 deletions(-) diff --git a/core/include/moveit/task_constructor/solvers/pipeline_planner.h b/core/include/moveit/task_constructor/solvers/pipeline_planner.h index 68f10cd8e..6609957ed 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,132 @@ 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); + /** 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 + */ + PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name = "ompl", + const std::string& planner_id = ""); + + [[deprecated("Deprecated: Use new constructor implementations.")]] // clang-format off + PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& /*planning_pipeline*/){}; + + /** \brief Constructor + * \param [in] node ROS 2 node + * \param [in] pipeline_id_planner_id_map A map containing pairs of planning pipeline name and planner plugin name + * for the planning requests + * \param [in] planning_pipelines Optional: A map with the pipeline names and initialized corresponding planning + * pipelines + * \param [in] stopping_criterion_callback Callback function to stop parallel planning pipeline according to a user defined criteria + * \param [in] solution_selection_function Callback function to choose the best solution when multiple pipelines are used + */ + PipelinePlanner(const rclcpp::Node::SharedPtr& node, + const std::unordered_map& pipeline_id_planner_id_map, + const std::unordered_map& planning_pipelines = + std::unordered_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); + + [[deprecated("Replaced with setPlannerId(pipeline_name, planner_id)")]] // clang-format off + void setPlannerId(const std::string& /*planner*/) { /* Do nothing */ } - static planning_pipeline::PlanningPipelinePtr create(const rclcpp::Node::SharedPtr& node, const Specification& spec); - - /** - * - * @param node used to load the parameters for the planning pipeline + /** \brief Set the planner id for a specific planning pipeline for the planning requests + * \param [in] pipeline_name Name of the planning pipeline for which the planner id is set + * \param [in] planner_id Name of the planner ID that should be used by the planning pipeline + * \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 This function is called when an MTC task that uses this solver is initialized. If no pipelines are + * configured when this function is invoked, the planning pipelines named in the 'pipeline_id_planner_id_map' are + * initialized with the given robot model. + * \param [in] robot_model A robot model that is used to initialize the + * planning pipelines of this solver */ - PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline = "ompl"); - - PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline); - - void setPlannerId(const std::string& planner) { setProperty("planner", planner); } - 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 ? + * \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 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 ? + * \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 Function that actually uses the planning pipelines to solve the given planning problem. It is called by + * the public plan function after the goal constraints are generated. This function uses a predefined number of + * planning pipelines in parallel to solve the planning problem and choose the automatically the best (user-defined) + * solution. + * \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 ? + * \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 pipeline names (ids) and their corresponding planner ids. The planning problem is solved for every + * pipeline-planner pair in this map. If no pipelines are passed via constructor argument, the pipeline names are + * used to initialize a set of planning pipelines. */ + std::unordered_map pipeline_id_planner_id_map_; + + /** \brief Map of pipelines names and planning pipelines. This map is used to quickly search for a requested motion + * planning pipeline when during plan(..) */ + 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..acfc9e453 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -49,164 +49,176 @@ #include #endif +namespace { +const std::pair DEFAULT_REQUESTED_PIPELINE = + std::pair("ompl", "RRTConnect"); +} 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); +PipelinePlanner::PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name, + const std::string& planner_id) + : PipelinePlanner(node, [&]() { + std::unordered_map pipeline_id_planner_id_map; + pipeline_id_planner_id_map[pipeline_name] = planner_id; + return pipeline_id_planner_id_map; + }()) {} + +PipelinePlanner::PipelinePlanner( + const rclcpp::Node::SharedPtr& node, const std::unordered_map& pipeline_id_planner_id_map, + const std::unordered_map& planning_pipelines, + const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback, + const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function) + : node_(node) + , pipeline_id_planner_id_map_(pipeline_id_planner_id_map) + , stopping_criterion_callback_(stopping_criterion_callback) + , solution_selection_function_(solution_selection_function) { + // If the pipeline name - pipeline map is passed as constructor argument, use it. Otherwise, it will be created in + // the init(..) function + if (!planning_pipelines.empty()) { + planning_pipelines_ = planning_pipelines; } - 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); + // 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); +} - 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. + if (pipeline_id_planner_id_map_.count(pipeline_name) > 0) { + pipeline_id_planner_id_map_[pipeline_name] = planner_id; } - return planner; + 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; } -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"); + // If no planning pipelines exist, create them based on the pipeline names provided in pipeline_id_planner_id_map_. + // The assumption here is that all parameters required by the planning pipeline can be found in a namespace that + // equals the pipeline name. + if (planning_pipelines_.empty()) { + planning_pipelines_ = moveit::planning_pipeline_interfaces::createPlanningPipelineMap( + [&]() { + // Create pipeline name vector from the keys of pipeline_id_planner_id_map_ + std::vector pipeline_names; + for (const auto& pipeline_name_planner_id_pair : pipeline_id_planner_id_map_) { + pipeline_names.push_back(pipeline_name_planner_id_pair.first); + } + return pipeline_names; + }(), + robot_model, node_); } - 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")); - ::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) { + // Create a request for every planning pipeline that should run in parallel + std::vector requests; + requests.reserve(pipeline_id_planner_id_map_.size()); + + for (auto const& pipeline_id_planner_id_pair : pipeline_id_planner_id_map_) { + // Check that requested pipeline exists and skip it if it doesn't exist + if (planning_pipelines_.find(pipeline_id_planner_id_pair.first) == planning_pipelines_.end()) { + RCLCPP_WARN( + node_->get_logger(), + "Pipeline '%s' is not available of this PipelineSolver instance. Skipping a request for this pipeline.", + pipeline_id_planner_id_pair.first.c_str()); + continue; + } + // Create MotionPlanRequest for pipeline + moveit_msgs::msg::MotionPlanRequest request; + request.pipeline_id = pipeline_id_planner_id_pair.first; + request.group_name = joint_model_group->getName(); + request.planner_id = pipeline_id_planner_id_pair.second; + 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/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; }(); From 8118309dda4a9cedce1b7e7c579d04f3ca714905 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 4 Sep 2023 12:24:59 +0200 Subject: [PATCH 2/7] Cleanup + address deprecation warnings --- core/src/solvers/pipeline_planner.cpp | 4 ---- core/test/pick_pa10.cpp | 3 +-- core/test/pick_pr2.cpp | 3 +-- core/test/pick_ur5.cpp | 3 +-- 4 files changed, 3 insertions(+), 10 deletions(-) diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index acfc9e453..9ad0303c0 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -49,10 +49,6 @@ #include #endif -namespace { -const std::pair DEFAULT_REQUESTED_PIPELINE = - std::pair("ompl", "RRTConnect"); -} namespace moveit { namespace task_constructor { namespace solvers { 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); From f57e65932e9f61741db99176800879de3291d218 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 4 Sep 2023 12:40:54 +0200 Subject: [PATCH 3/7] Enabling optionally using a property defined pipeline planner map --- core/src/solvers/pipeline_planner.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index 9ad0303c0..53d08cb48 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -90,6 +90,9 @@ PipelinePlanner::PipelinePlanner( 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", std::unordered_map(), + "Set of pipelines and planners used for planning"); } bool PipelinePlanner::setPlannerId(const std::string& pipeline_name, const std::string& planner_id) { @@ -173,7 +176,11 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning std::vector requests; requests.reserve(pipeline_id_planner_id_map_.size()); - for (auto const& pipeline_id_planner_id_pair : pipeline_id_planner_id_map_) { + auto const property_pipeline_id_planner_id_map = + properties().get>("pipeline_id_planner_id_map"); + for (auto const& pipeline_id_planner_id_pair : + (!property_pipeline_id_planner_id_map.empty() ? property_pipeline_id_planner_id_map : + pipeline_id_planner_id_map_)) { // Check that requested pipeline exists and skip it if it doesn't exist if (planning_pipelines_.find(pipeline_id_planner_id_pair.first) == planning_pipelines_.end()) { RCLCPP_WARN( From 322d7a4f0fce8258c9dd7f313cf8e1574f156bf2 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Tue, 26 Sep 2023 10:17:22 +0200 Subject: [PATCH 4/7] Address review --- .../task_constructor/solvers/pipeline_planner.h | 9 ++++----- core/src/solvers/pipeline_planner.cpp | 13 +++++++------ 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/core/include/moveit/task_constructor/solvers/pipeline_planner.h b/core/include/moveit/task_constructor/solvers/pipeline_planner.h index 6609957ed..4415123c5 100644 --- a/core/include/moveit/task_constructor/solvers/pipeline_planner.h +++ b/core/include/moveit/task_constructor/solvers/pipeline_planner.h @@ -121,7 +121,7 @@ class PipelinePlanner : public PlannerInterface * \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 ? + * \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 @@ -137,7 +137,7 @@ class PipelinePlanner : public PlannerInterface * \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 ? + * \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 @@ -151,14 +151,13 @@ class PipelinePlanner : public PlannerInterface protected: /** \brief Function that actually uses the planning pipelines to solve the given planning problem. It is called by * the public plan function after the goal constraints are generated. This function uses a predefined number of - * planning pipelines in parallel to solve the planning problem and choose the automatically the best (user-defined) - * solution. + * planning pipelines in parallel to solve the planning problem and choose the best (user-defined) solution. * \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 ? + * \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 diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index 53d08cb48..35ac6ac4d 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -97,13 +97,14 @@ PipelinePlanner::PipelinePlanner( 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. - if (pipeline_id_planner_id_map_.count(pipeline_name) > 0) { - pipeline_id_planner_id_map_[pipeline_name] = planner_id; + if (pipeline_id_planner_id_map_.count(pipeline_name) == 0) { + 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; } - 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; + pipeline_id_planner_id_map_[pipeline_name] = planner_id; + return true; } void PipelinePlanner::setStoppingCriterionFunction( From dd4dfeb09e892aeb637306ae696666fac2b0719b Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 27 Sep 2023 17:41:39 +0200 Subject: [PATCH 5/7] Disable humble CI for ros2 branch --- .github/workflows/ci.yaml | 1 - 1 file changed, 1 deletion(-) 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" From 7f0fa91e5a79fce99e1f483eff5d2d801da80f5b Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Tue, 3 Oct 2023 18:23:57 +0200 Subject: [PATCH 6/7] Add pipeline planner unittests + some checks --- core/src/solvers/pipeline_planner.cpp | 11 +++++ core/test/CMakeLists.txt | 1 + core/test/test_pipeline_planner.cpp | 62 +++++++++++++++++++++++++++ 3 files changed, 74 insertions(+) create mode 100644 core/test/test_pipeline_planner.cpp diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index 35ac6ac4d..702cd44cd 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -124,6 +124,11 @@ void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) { planning_pipelines_ = moveit::planning_pipeline_interfaces::createPlanningPipelineMap( [&]() { // Create pipeline name vector from the keys of pipeline_id_planner_id_map_ + if (pipeline_id_planner_id_map_.empty()) { + throw std::runtime_error("Cannot initialize PipelinePlanner: No planning pipeline was provided and " + "pipeline_id_planner_id_map_ is empty!"); + } + std::vector pipeline_names; for (const auto& pipeline_name_planner_id_pair : pipeline_id_planner_id_map_) { pipeline_names.push_back(pipeline_name_planner_id_pair.first); @@ -133,6 +138,12 @@ void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) { robot_model, node_); } + // Check if it is still empty + if (planning_pipelines_.empty()) { + throw std::runtime_error( + "Cannot initialize PipelinePlanner: Could not create any valid entries for planning pipeline maps!"); + } + // 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")); 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/test_pipeline_planner.cpp b/core/test/test_pipeline_planner.cpp new file mode 100644 index 000000000..00914e776 --- /dev/null +++ b/core/test/test_pipeline_planner.cpp @@ -0,0 +1,62 @@ +#include "models.h" + +#include +#include + +#include + +using namespace moveit::task_constructor; + +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(); +} From 8d8006e10b58db93a8f4e232ddcbac8524f97726 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 4 Oct 2023 14:18:21 +0200 Subject: [PATCH 7/7] Add short comment --- core/test/test_pipeline_planner.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/core/test/test_pipeline_planner.cpp b/core/test/test_pipeline_planner.cpp index 00914e776..347d0b572 100644 --- a/core/test/test_pipeline_planner.cpp +++ b/core/test/test_pipeline_planner.cpp @@ -7,6 +7,7 @@ using namespace moveit::task_constructor; +// Test fixture for PipelinePlanner struct PipelinePlannerTest : public testing::Test { PipelinePlannerTest() {