From f050adb4e9658651c01a507a34e506fdecd09a39 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 30 Mar 2023 16:00:25 +0200 Subject: [PATCH] Enable configuring multiple pipelines --- .../solvers/pipeline_planner.h | 27 ++++--- core/src/solvers/pipeline_planner.cpp | 70 ++++++++++++++++--- 2 files changed, 72 insertions(+), 25 deletions(-) diff --git a/core/include/moveit/task_constructor/solvers/pipeline_planner.h b/core/include/moveit/task_constructor/solvers/pipeline_planner.h index 23648e37a..3e7a2221c 100644 --- a/core/include/moveit/task_constructor/solvers/pipeline_planner.h +++ b/core/include/moveit/task_constructor/solvers/pipeline_planner.h @@ -73,21 +73,20 @@ class PipelinePlanner : public PlannerInterface return create(node, spec); }*/ - /** - * - * @param node used to load the parameters for the planning pipeline - */ - PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_namespace = "ompl"); + // TODO: Deprecate + PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name = "ompl", + const std::string& planner_id = ""); - // PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::vector& pipeline_namespaces); + PipelinePlanner(const rclcpp::Node::SharedPtr& node, + const std::unordered_map& pipeline_id_planner_id_map, + const std::unordered_map planning_pipelines = + std::unordered_map()); - // PipelinePlanner(const std::unordered_map& - // planning_pipelines); + // TODO: Deprecate + void setPlannerId(const std::string& /*planner*/) { /* Do nothing */ + } - // PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline, const std::string& pipeline = - // ""); - - void setPlannerId(const std::string& planner) { setProperty("planner", planner); } + bool setPlannerId(const std::string& pipeline_name, const std::string& planner_id); void init(const moveit::core::RobotModelConstPtr& robot_model) override; @@ -109,9 +108,9 @@ class PipelinePlanner : public PlannerInterface robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()); - std::vector pipeline_names_ = std::vector(1); - std::unordered_map planning_pipelines_; rclcpp::Node::SharedPtr node_; + std::unordered_map pipeline_id_planner_id_map_; + std::unordered_map planning_pipelines_; }; } // namespace solvers } // namespace task_constructor diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index e684769c4..9e0c67310 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -50,6 +50,10 @@ #include #endif +namespace { +const std::pair DEFAULT_REQUESTED_PIPELINE = + std::pair("ompl", "RRTConnect"); +} namespace moveit { namespace task_constructor { namespace solvers { @@ -114,10 +118,25 @@ planning_pipeline::PlanningPipelinePtr PipelinePlanner::create(const rclcpp::Nod return planner; }*/ -PipelinePlanner::PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name) : node_(node) { - pipeline_names_.at(0) = pipeline_name; - properties().declare("planner", "", "planner id"); +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) + : node_(node), pipeline_id_planner_id_map_(pipeline_id_planner_id_map) { + for (auto pipeline_id_planner_id_pair : pipeline_id_planner_id_map_) { + RCLCPP_WARN(node_->get_logger(), "'%s' : ''%s", pipeline_id_planner_id_pair.first.c_str(), + pipeline_id_planner_id_pair.second.c_str()); + } + if (!planning_pipelines.empty()) { + planning_pipelines_ = planning_pipelines; + } properties().declare("num_planning_attempts", 1u, "number of planning attempts"); properties().declare( "workspace_parameters", moveit_msgs::msg::WorkspaceParameters(), "allowed workspace of mobile base?"); @@ -139,9 +158,26 @@ PipelinePlanner::PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std: // planning_pipelines_.at(0) = planning_pipeline; //} +bool PipelinePlanner::setPlannerId(const std::string& pipeline_name, const std::string& planner_id) { + if (pipeline_id_planner_id_map_.count(pipeline_name) > 0) { + pipeline_id_planner_id_map_[pipeline_name] = planner_id; + } + RCLCPP_WARN(node_->get_logger(), "PipelinePlanner does not have a pipeline called '%s'", pipeline_name.c_str()); + return false; +} + void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) { - planning_pipelines_ = - moveit::planning_pipeline_interfaces::createPlanningPipelineMap(pipeline_names_, robot_model, node_); + if (planning_pipelines_.empty()) { + planning_pipelines_ = moveit::planning_pipeline_interfaces::createPlanningPipelineMap( + [&]() { + 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_); + } for (auto const& name_pipeline_pair : planning_pipelines_) { name_pipeline_pair.second->displayComputedMotionPlans(properties().get("display_motion_plans")); @@ -181,13 +217,22 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints) { std::vector requests; - requests.reserve(pipeline_names_.size()); + requests.reserve(pipeline_id_planner_id_map_.size()); - for (auto const& pipeline_name : pipeline_names_) { + for (auto const& pipeline_id_planner_id_pair : pipeline_id_planner_id_map_) { + RCLCPP_WARN(node_->get_logger(), "'%s' : ''%s", pipeline_id_planner_id_pair.first.c_str(), + pipeline_id_planner_id_pair.second.c_str()); + 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; + } moveit_msgs::msg::MotionPlanRequest request; - request.pipeline_id = pipeline_name; + request.pipeline_id = pipeline_id_planner_id_pair.first; request.group_name = joint_model_group->getName(); - request.planner_id = properties().get("planner"); + 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"); @@ -204,8 +249,11 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning moveit::planning_pipeline_interfaces::planWithParallelPipelines(requests, planning_scene, planning_pipelines_); // Just choose first result - result = responses.at(0).trajectory; - return bool(result); + if (!responses.empty()) { + result = responses.at(0).trajectory; + return bool(result); + } + return false; } } // namespace solvers } // namespace task_constructor