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 47bafa8eb..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 = 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