From f6d0b3bea359710ba22b4539e36ded56dd3fcf73 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 18 Mar 2024 12:39:22 +0100 Subject: [PATCH 1/5] Use MoveItErrorCode and revert some changes for discussion --- .../task_constructor/solvers/cartesian_path.h | 4 +- .../solvers/joint_interpolation.h | 16 +++-- .../task_constructor/solvers/multi_planner.h | 16 +++-- .../solvers/pipeline_planner.h | 67 ++++++++++++------- .../solvers/planner_interface.h | 30 ++++----- core/src/solvers/cartesian_path.cpp | 25 +++---- core/src/solvers/joint_interpolation.cpp | 41 ++++++------ core/src/solvers/multi_planner.cpp | 46 ++++++------- core/src/solvers/pipeline_planner.cpp | 64 +++++++++++------- core/src/stages/connect.cpp | 3 +- core/src/stages/move_relative.cpp | 2 +- core/src/stages/move_to.cpp | 2 +- 12 files changed, 169 insertions(+), 147 deletions(-) diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h index dcd46ba2a..c92023856 100644 --- a/core/include/moveit/task_constructor/solvers/cartesian_path.h +++ b/core/include/moveit/task_constructor/solvers/cartesian_path.h @@ -63,11 +63,11 @@ class CartesianPath : public PlannerInterface void init(const moveit::core::RobotModelConstPtr& robot_model) override; - Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, + MoveItErrorCode plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; - Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, + MoveItErrorCode 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 moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; diff --git a/core/include/moveit/task_constructor/solvers/joint_interpolation.h b/core/include/moveit/task_constructor/solvers/joint_interpolation.h index 0adc6979c..38e3e6228 100644 --- a/core/include/moveit/task_constructor/solvers/joint_interpolation.h +++ b/core/include/moveit/task_constructor/solvers/joint_interpolation.h @@ -58,14 +58,16 @@ class JointInterpolationPlanner : public PlannerInterface void init(const moveit::core::RobotModelConstPtr& robot_model) override; - Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, - const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; + MoveItErrorCode + plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, + const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; - Result 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 moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; + MoveItErrorCode + 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 moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; std::string getPlannerId() const override { return "JointInterpolationPlanner"; } }; diff --git a/core/include/moveit/task_constructor/solvers/multi_planner.h b/core/include/moveit/task_constructor/solvers/multi_planner.h index 6f27badb2..168691a5e 100644 --- a/core/include/moveit/task_constructor/solvers/multi_planner.h +++ b/core/include/moveit/task_constructor/solvers/multi_planner.h @@ -63,14 +63,16 @@ class MultiPlanner : public PlannerInterface, public std::vector& 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); @@ -101,7 +104,9 @@ class PipelinePlanner : public PlannerInterface void setSolutionSelectionFunction( const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function); - /** \brief If not yet done, initialize pipelines from pipeline_id_planner_id_map + /** \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 robot model used to initialize the planning pipelines of this solver */ void init(const moveit::core::RobotModelConstPtr& robot_model) override; @@ -113,14 +118,15 @@ class PipelinePlanner : public PlannerInterface * \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 + * \return MoveItErrorCode - Return MoveItErrorCodes::SUCCESS if solver found a successful solution. Otherwise, + * return an appropriate error type and message explaining why it failed. */ - 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 moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; + MoveItErrorCode + plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, + const core::JointModelGroup* joint_model_group, double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; - /** \brief Plan a trajectory from a planning scene 'from' to a Cartesian target pose with an offset + /** \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 @@ -129,18 +135,26 @@ class PipelinePlanner : public PlannerInterface * \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 + * \return MoveItErrorCode - Return MoveItErrorCodes::SUCCESS if solver found a successful solution. Otherwise, + * return an appropriate error type and message explaining why it failed. + */ + MoveItErrorCode + 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_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; + + /** + * \brief Get planner name + * \return Name of the last successful planner */ - 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_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. + /** \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 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 @@ -149,17 +163,18 @@ class PipelinePlanner : public PlannerInterface * \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()); + MoveItErrorCode 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_; std::string last_successful_planner_; - /** \brief Map of instantiated (and named) planning pipelines. */ + /** \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_; diff --git a/core/include/moveit/task_constructor/solvers/planner_interface.h b/core/include/moveit/task_constructor/solvers/planner_interface.h index 33f9e627e..1b699b8bf 100644 --- a/core/include/moveit/task_constructor/solvers/planner_interface.h +++ b/core/include/moveit/task_constructor/solvers/planner_interface.h @@ -41,6 +41,7 @@ #include #include #include +#include #include namespace planning_scene { @@ -64,6 +65,9 @@ namespace moveit { namespace task_constructor { namespace solvers { +using MoveItErrorCode = moveit::core::MoveItErrorCode; +using MoveItErrorCodes = moveit_msgs::msg::MoveItErrorCodes; + MOVEIT_CLASS_FORWARD(PlannerInterface); class PlannerInterface { @@ -71,14 +75,6 @@ class PlannerInterface PropertyMap properties_; public: - struct Result - { - bool success; - std::string message; - - operator bool() const { return success; } - }; - PlannerInterface(); virtual ~PlannerInterface() {} @@ -96,17 +92,17 @@ class PlannerInterface virtual void init(const moveit::core::RobotModelConstPtr& robot_model) = 0; /// plan trajectory between to robot states - virtual Result plan(const planning_scene::PlanningSceneConstPtr& from, - const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg, - double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0; + virtual MoveItErrorCode + plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, + const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0; /// plan trajectory from current robot state to Cartesian target, such that pose(link)*offset == target - virtual Result 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 moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0; + virtual MoveItErrorCode + 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 moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0; // get name of the planner virtual std::string getPlannerId() const = 0; diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index dd02dcab0..fc0a3e41a 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -63,11 +63,11 @@ CartesianPath::CartesianPath() { void CartesianPath::init(const core::RobotModelConstPtr& /*robot_model*/) {} -PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, - const planning_scene::PlanningSceneConstPtr& to, - const moveit::core::JointModelGroup* jmg, double timeout, - robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::msg::Constraints& path_constraints) { +MoveItErrorCode CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, + const planning_scene::PlanningSceneConstPtr& to, + const moveit::core::JointModelGroup* jmg, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::msg::Constraints& path_constraints) { const moveit::core::LinkModel* link = jmg->getOnlyOneEndEffectorTip(); if (!link) return { false, "no unique tip for joint model group: " + jmg->getName() }; @@ -77,11 +77,11 @@ PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningScene std::min(timeout, properties().get("timeout")), result, path_constraints); } -PlannerInterface::Result CartesianPath::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 moveit_msgs::msg::Constraints& path_constraints) { +MoveItErrorCode CartesianPath::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 moveit_msgs::msg::Constraints& path_constraints) { const auto& props = properties(); planning_scene::PlanningScenePtr sandbox_scene = from->diff(); @@ -114,9 +114,10 @@ PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningScene props.get("max_acceleration_scaling_factor")); if (achieved_fraction < props.get("min_fraction")) { - return { false, "min_fraction not met. Achieved: " + std::to_string(achieved_fraction) }; + return MoveItErrorCode(MoveItErrorCodes::FAILURE, + "Min fraction not met. Achieved fraction : " + std::to_string(achieved_fraction)); } - return { true, "achieved fraction: " + std::to_string(achieved_fraction) }; + return MoveItErrorCode(MoveItErrorCodes::SUCCESS, "achieved fraction: " + std::to_string(achieved_fraction)); } } // namespace solvers } // namespace task_constructor diff --git a/core/src/solvers/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp index 9fda1daf5..1fbe81f22 100644 --- a/core/src/solvers/joint_interpolation.cpp +++ b/core/src/solvers/joint_interpolation.cpp @@ -59,11 +59,11 @@ JointInterpolationPlanner::JointInterpolationPlanner() { void JointInterpolationPlanner::init(const core::RobotModelConstPtr& /*robot_model*/) {} -PlannerInterface::Result JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, - const planning_scene::PlanningSceneConstPtr& to, - const moveit::core::JointModelGroup* jmg, double /*timeout*/, - robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::msg::Constraints& /*path_constraints*/) { +MoveItErrorCode JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, + const planning_scene::PlanningSceneConstPtr& to, + const moveit::core::JointModelGroup* jmg, double /*timeout*/, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::msg::Constraints& /*path_constraints*/) { const auto& props = properties(); // Get maximum joint distance @@ -78,10 +78,10 @@ PlannerInterface::Result JointInterpolationPlanner::plan(const planning_scene::P // add first point result->addSuffixWayPoint(from->getCurrentState(), 0.0); if (from->isStateColliding(from_state, jmg->getName())) - return { false, "Start state is in collision!" }; + return MoveItErrorCode(MoveItErrorCodes::START_STATE_IN_COLLISION, "Start state is in collision!"); if (!from_state.satisfiesBounds(jmg)) - return { false, "Start state is out of bounds!" }; + return MoveItErrorCode(MoveItErrorCodes::START_STATE_INVALID, "Start state is not within bounds!"); moveit::core::RobotState waypoint(from_state); double delta = d < 1e-6 ? 1.0 : props.get("max_step") / d; @@ -90,19 +90,19 @@ PlannerInterface::Result JointInterpolationPlanner::plan(const planning_scene::P result->addSuffixWayPoint(waypoint, t); if (from->isStateColliding(waypoint, jmg->getName())) - return { false, "Waypoint is in collision!" }; + return MoveItErrorCode(MoveItErrorCodes::FAILURE, "One of the waypoint's state is in collision!"); if (!waypoint.satisfiesBounds(jmg)) - return { false, "Waypoint is out of bounds!" }; + return MoveItErrorCode(MoveItErrorCodes::FAILURE, "One of the waypoint's state is not within bounds!"); } // add goal point result->addSuffixWayPoint(to_state, 1.0); if (from->isStateColliding(to_state, jmg->getName())) - return { false, "Goal state is in collision!" }; + return MoveItErrorCode(MoveItErrorCodes::GOAL_IN_COLLISION, "Goal state is in collision!"); if (!to_state.satisfiesBounds(jmg)) - return { false, "Goal state is out of bounds!" }; + return MoveItErrorCode(MoveItErrorCodes::GOAL_STATE_INVALID, "Goal state is out of bounds!"); auto timing = props.get("time_parameterization"); timing->computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), @@ -122,16 +122,15 @@ PlannerInterface::Result JointInterpolationPlanner::plan(const planning_scene::P } } - return { true, "" }; + return MoveItErrorCode(MoveItErrorCodes::SUCCESS); } -PlannerInterface::Result JointInterpolationPlanner::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 moveit_msgs::msg::Constraints& path_constraints) { +MoveItErrorCode JointInterpolationPlanner::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 moveit_msgs::msg::Constraints& path_constraints) { timeout = std::min(timeout, properties().get("timeout")); const auto deadline = std::chrono::steady_clock::now() + std::chrono::duration>(timeout); @@ -148,11 +147,11 @@ PlannerInterface::Result JointInterpolationPlanner::plan(const planning_scene::P } }; if (!to->getCurrentStateNonConst().setFromIK(jmg, target * offset.inverse(), link.getName(), timeout, is_valid)) - return { false, "IK failed for pose target." }; + return MoveItErrorCode(MoveItErrorCodes::NO_IK_SOLUTION, "IK failed for pose target."); to->getCurrentStateNonConst().update(); if (std::chrono::steady_clock::now() >= deadline) - return { false, "timeout" }; + return MoveItErrorCode(MoveItErrorCodes::TIMED_OUT, "Timed out."); return plan(from, to, jmg, timeout, result, path_constraints); } diff --git a/core/src/solvers/multi_planner.cpp b/core/src/solvers/multi_planner.cpp index 59b48d9af..398a0cc4a 100644 --- a/core/src/solvers/multi_planner.cpp +++ b/core/src/solvers/multi_planner.cpp @@ -49,58 +49,54 @@ void MultiPlanner::init(const core::RobotModelConstPtr& robot_model) { p->init(robot_model); } -PlannerInterface::Result MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, - const planning_scene::PlanningSceneConstPtr& to, - const moveit::core::JointModelGroup* jmg, double timeout, - robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::msg::Constraints& path_constraints) { +MoveItErrorCode MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, + const planning_scene::PlanningSceneConstPtr& to, + const moveit::core::JointModelGroup* jmg, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::msg::Constraints& path_constraints) { double remaining_time = std::min(timeout, properties().get("timeout")); auto start_time = std::chrono::steady_clock::now(); - std::string comment = "No planner specified"; + MoveItErrorCodes error_code = MoveItErrorCode(MoveItErrorCodes::FAILURE, "No planner specified"); for (const auto& p : *this) { if (remaining_time < 0) - return { false, "timeout" }; + return MoveItErrorCode(MoveItErrorCodes::TIMED_OUT, "Timeout"); if (result) result->clear(); - auto r = p->plan(from, to, jmg, remaining_time, result, path_constraints); - if (r) - return r; - else - comment = r.message; + error_code = p->plan(from, to, jmg, remaining_time, result, path_constraints); + if (error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + return error_code; auto now = std::chrono::steady_clock::now(); remaining_time -= std::chrono::duration(now - start_time).count(); start_time = now; } - return { false, comment }; + return error_code; } -PlannerInterface::Result MultiPlanner::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 moveit_msgs::msg::Constraints& path_constraints) { +MoveItErrorCode MultiPlanner::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 moveit_msgs::msg::Constraints& path_constraints) { double remaining_time = std::min(timeout, properties().get("timeout")); auto start_time = std::chrono::steady_clock::now(); - std::string comment = "No planner specified"; + MoveItErrorCodes error_code = MoveItErrorCode(MoveItErrorCodes::FAILURE, "No planner specified"); for (const auto& p : *this) { if (remaining_time < 0) return { false, "timeout" }; if (result) result->clear(); - auto r = p->plan(from, link, offset, target, jmg, remaining_time, result, path_constraints); - if (r) - return r; - else - comment = r.message; + error_code = p->plan(from, link, offset, target, jmg, remaining_time, result, path_constraints); + if (error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + return error_code; auto now = std::chrono::steady_clock::now(); remaining_time -= std::chrono::duration(now - start_time).count(); start_time = now; } - return { false, comment }; + return error_code; } } // namespace solvers } // namespace task_constructor diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index e173e7ca7..c188c0858 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -54,14 +54,22 @@ 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 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) + , last_successful_planner_("") , 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; + } // Declare properties of the MotionPlanRequest properties().declare("num_planning_attempts", 1u, "number of planning attempts"); properties().declare( @@ -77,7 +85,7 @@ 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. - PipelineMap map = properties().get("pipeline_id_planner_id_map"); + PipelineMap& map = properties().get("pipeline_id_planner_id_map"); auto it = map.find(pipeline_name); if (it == map.end()) { RCLCPP_ERROR(node_->get_logger(), @@ -100,9 +108,9 @@ void PipelinePlanner::setSolutionSelectionFunction( } 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 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()) { auto map = properties().get("pipeline_id_planner_id_map"); // Create pipeline name vector from the keys of pipeline_id_planner_id_map_ @@ -116,9 +124,11 @@ void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) { } 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"); + throw std::runtime_error( + "Cannot initialize PipelinePlanner: Could not create any valid entries for planning pipeline maps!"); } } else { // Validate that all pipelines use the task's robot model @@ -131,23 +141,23 @@ void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) { } } -PlannerInterface::Result PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, - 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) { +MoveItErrorCode PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, + 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) { // 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); } -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, - robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::msg::Constraints& path_constraints) { +MoveItErrorCode 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, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::msg::Constraints& path_constraints) { // Construct a Cartesian target pose from the given target transform and offset geometry_msgs::msg::PoseStamped target; target.header.frame_id = from->getPlanningFrame(); @@ -160,11 +170,11 @@ PlannerInterface::Result PipelinePlanner::plan(const planning_scene::PlanningSce return plan(from, joint_model_group, goal_constraints, timeout, result, path_constraints); } -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) { +MoveItErrorCode 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"; @@ -174,8 +184,11 @@ PlannerInterface::Result PipelinePlanner::plan(const planning_scene::PlanningSce 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()); + if (planning_pipelines_.find(pipeline_id) == 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.c_str()); continue; } // Create MotionPlanRequest for pipeline @@ -208,11 +221,10 @@ PlannerInterface::Result PipelinePlanner::plan(const planning_scene::PlanningSce // Choose the first solution trajectory as response result = solution.trajectory; last_successful_planner_ = solution.planner_id; - return { true, "" }; + return MoveItErrorCode(solution.error_code.val, solution.error_code.message, solution.error_code.source); } - return { false, solution.error_code.message }; } - return { false, "No solutions generated from Pipeline Planner" }; + return MoveItErrorCode(MoveItErrorCodes::FAILURE, "No solutions generated from Pipeline Planner"); } } // namespace solvers diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index be88a7538..6ec793459 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -163,7 +163,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { intermediate_scenes.push_back(end); robot_trajectory::RobotTrajectoryPtr trajectory; - auto result = pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints); + const auto result = pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints); success = bool(result); sub_trajectories.push_back({ pair.second->getPlannerId(), trajectory }); @@ -261,7 +261,6 @@ SubTrajectoryPtr Connect::merge(const std::vector& sub_ properties().get("path_constraints"))) return SubTrajectoryPtr(); - return std::make_shared(trajectory); return std::make_shared(trajectory, 0.0, std::string(""), planner_ids); } } // namespace stages diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index af078b4b8..cbe9fe47b 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -199,7 +199,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning if (getJointStateFromOffset(direction, dir, jmg, scene->getCurrentStateNonConst())) { // plan to joint-space target - auto result = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); + const auto result = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); success = bool(result); if (!success) comment = result.message; diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 0a713c18a..e8ddd23d2 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -209,7 +209,7 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP if (getJointStateGoal(goal, jmg, scene->getCurrentStateNonConst())) { // plan to joint-space target - auto result = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); + const auto result = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); success = bool(result); if (!success) comment = result.message; From da874df27335ed04f54497e350fdc906ea3303fe Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 18 Mar 2024 18:55:38 +0100 Subject: [PATCH 2/5] Update core/src/solvers/pipeline_planner.cpp --- core/src/solvers/pipeline_planner.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index c188c0858..36d6410ab 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -127,8 +127,7 @@ void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) { // 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!"); + 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 From e11f89c7707b88d658f13a580860ff4625794d97 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 25 Mar 2024 10:55:42 +0100 Subject: [PATCH 3/5] Update core/src/solvers/pipeline_planner.cpp --- core/src/solvers/pipeline_planner.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index 36d6410ab..7507f072e 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -220,8 +220,8 @@ MoveItErrorCode PipelinePlanner::plan(const planning_scene::PlanningSceneConstPt // Choose the first solution trajectory as response result = solution.trajectory; last_successful_planner_ = solution.planner_id; - return MoveItErrorCode(solution.error_code.val, solution.error_code.message, solution.error_code.source); } + return MoveItErrorCode(solution.error_code.val, solution.error_code.message, solution.error_code.source); } return MoveItErrorCode(MoveItErrorCodes::FAILURE, "No solutions generated from Pipeline Planner"); } From 6039ef886904a751b9fc610733c65b7f28f13280 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 25 Mar 2024 11:36:20 +0100 Subject: [PATCH 4/5] Remove planning pipelines argument --- .../moveit/task_constructor/solvers/pipeline_planner.h | 3 --- core/src/solvers/pipeline_planner.cpp | 6 ------ 2 files changed, 9 deletions(-) diff --git a/core/include/moveit/task_constructor/solvers/pipeline_planner.h b/core/include/moveit/task_constructor/solvers/pipeline_planner.h index ef08bc91c..f10452fab 100644 --- a/core/include/moveit/task_constructor/solvers/pipeline_planner.h +++ b/core/include/moveit/task_constructor/solvers/pipeline_planner.h @@ -72,15 +72,12 @@ class PipelinePlanner : public PlannerInterface /** \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] planning_pipelines Optional: A map with the names and initialized corresponding planning pipelines * \param [in] stopping_criterion_callback Optional: Callback to decide when to stop parallel planning * \param [in] solution_selection_function Optional: 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 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); diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index 7507f072e..ca8e7cc8b 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -58,18 +58,12 @@ using PipelineMap = std::unordered_map; PipelinePlanner::PipelinePlanner( const rclcpp::Node::SharedPtr& node, const PipelineMap& 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) , last_successful_planner_("") , 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; - } // Declare properties of the MotionPlanRequest properties().declare("num_planning_attempts", 1u, "number of planning attempts"); properties().declare( From e397b52bceb5ccf1e3c65f7b948316ee3fd27a1a Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 25 Mar 2024 11:37:43 +0100 Subject: [PATCH 5/5] Update core/src/solvers/pipeline_planner.cpp --- core/src/solvers/pipeline_planner.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index ca8e7cc8b..e676daa8d 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -54,7 +54,6 @@ namespace task_constructor { namespace solvers { using PipelineMap = std::unordered_map; -; PipelinePlanner::PipelinePlanner( const rclcpp::Node::SharedPtr& node, const PipelineMap& pipeline_id_planner_id_map,