From 2ffacf821fb1834cc8021a9548f3ed5b614a038b Mon Sep 17 00:00:00 2001 From: Abishalini Date: Tue, 16 Jan 2024 01:06:47 -0700 Subject: [PATCH 1/7] Use tl expected --- core/CMakeLists.txt | 2 ++ .../task_constructor/solvers/cartesian_path.h | 4 +-- .../solvers/joint_interpolation.h | 16 ++++++---- .../task_constructor/solvers/multi_planner.h | 16 ++++++---- .../solvers/pipeline_planner.h | 6 ++-- .../solvers/planner_interface.h | 19 +++++------ core/package.xml | 1 + core/src/solvers/cartesian_path.cpp | 20 ++++++------ core/src/solvers/joint_interpolation.cpp | 20 ++++++------ core/src/solvers/multi_planner.cpp | 20 ++++++------ core/src/solvers/pipeline_planner.cpp | 32 ++++++++++--------- core/src/stages/connect.cpp | 5 ++- core/src/stages/move_relative.cpp | 11 +++++-- core/src/stages/move_to.cpp | 12 +++++-- 14 files changed, 108 insertions(+), 76 deletions(-) diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index d10d2f5ba..7d21c5da6 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(moveit_task_constructor_msgs REQUIRED) find_package(rclcpp REQUIRED) find_package(rviz_marker_tools REQUIRED) find_package(tf2_eigen REQUIRED) +find_package(tl_expected REQUIRED) find_package(visualization_msgs REQUIRED) set(CMAKE_CXX_STANDARD 17) @@ -44,5 +45,6 @@ ament_export_dependencies(moveit_task_constructor_msgs) ament_export_dependencies(rclcpp) ament_export_dependencies(rviz_marker_tools) ament_export_dependencies(tf2_eigen) +ament_export_dependencies(tl_expected) ament_export_dependencies(visualization_msgs) ament_package() diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h index 8dfa3ea2b..6296945ab 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; - bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, + tl::expected 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; - bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, + tl::expected 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 66df1e510..c440cda40 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; - bool 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; + tl::expected + 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; - 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 moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; + tl::expected + 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 std::string("JointInterpolationPlanner"); } }; diff --git a/core/include/moveit/task_constructor/solvers/multi_planner.h b/core/include/moveit/task_constructor/solvers/multi_planner.h index 2ddb47f51..d5b816a74 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 + 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; - 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 moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; + tl::expected + 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; }; } // namespace solvers } // namespace task_constructor diff --git a/core/include/moveit/task_constructor/solvers/pipeline_planner.h b/core/include/moveit/task_constructor/solvers/pipeline_planner.h index f024da201..e23b5f4dc 100644 --- a/core/include/moveit/task_constructor/solvers/pipeline_planner.h +++ b/core/include/moveit/task_constructor/solvers/pipeline_planner.h @@ -126,7 +126,7 @@ 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 */ - bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, + tl::expected 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; @@ -142,7 +142,7 @@ 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 */ - bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, + tl::expected 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, @@ -170,7 +170,7 @@ class PipelinePlanner : public PlannerInterface * problem * \return true If the solver found a successful solution for the given planning problem */ - bool plan(const planning_scene::PlanningSceneConstPtr& planning_scene, + tl::expected 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, diff --git a/core/include/moveit/task_constructor/solvers/planner_interface.h b/core/include/moveit/task_constructor/solvers/planner_interface.h index ff9948414..6f4648139 100644 --- a/core/include/moveit/task_constructor/solvers/planner_interface.h +++ b/core/include/moveit/task_constructor/solvers/planner_interface.h @@ -42,6 +42,7 @@ #include #include #include +#include namespace planning_scene { MOVEIT_CLASS_FORWARD(PlanningScene); @@ -88,17 +89,17 @@ class PlannerInterface virtual void init(const moveit::core::RobotModelConstPtr& robot_model) = 0; /// plan trajectory between to robot states - virtual bool 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 tl::expected + 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 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 moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0; + virtual tl::expected + 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/package.xml b/core/package.xml index 3cb44239b..7a0d7c389 100644 --- a/core/package.xml +++ b/core/package.xml @@ -22,6 +22,7 @@ rclcpp rviz_marker_tools tf2_eigen + tl_expected visualization_msgs ament_cmake_gmock diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index 1b0b9b7f0..ab2738b42 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -63,10 +63,11 @@ CartesianPath::CartesianPath() { void CartesianPath::init(const core::RobotModelConstPtr& /*robot_model*/) {} -bool 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) { +tl::expected 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) { RCLCPP_WARN_STREAM(LOGGER, "no unique tip for joint model group: " << jmg->getName()); @@ -78,11 +79,12 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, std::min(timeout, properties().get("timeout")), result, path_constraints); } -bool 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) { +tl::expected 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(); diff --git a/core/src/solvers/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp index a0184b652..0e0e34bea 100644 --- a/core/src/solvers/joint_interpolation.cpp +++ b/core/src/solvers/joint_interpolation.cpp @@ -59,11 +59,10 @@ JointInterpolationPlanner::JointInterpolationPlanner() { void JointInterpolationPlanner::init(const core::RobotModelConstPtr& /*robot_model*/) {} -bool 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*/) { +tl::expected 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 @@ -116,11 +115,12 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr return true; } -bool 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) { +tl::expected +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); diff --git a/core/src/solvers/multi_planner.cpp b/core/src/solvers/multi_planner.cpp index cfb90ab5b..e335483d3 100644 --- a/core/src/solvers/multi_planner.cpp +++ b/core/src/solvers/multi_planner.cpp @@ -49,10 +49,11 @@ void MultiPlanner::init(const core::RobotModelConstPtr& robot_model) { p->init(robot_model); } -bool 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) { +tl::expected 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(); @@ -71,11 +72,12 @@ bool MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, return false; } -bool 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) { +tl::expected 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(); diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index d658951b6..1dd44de6c 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -140,22 +140,24 @@ void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) { } } -bool 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) { +tl::expected 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); } -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* joint_model_group, double timeout, - robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::msg::Constraints& path_constraints) { +tl::expected 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) { last_successful_planner_.clear(); // Construct a Cartesian target pose from the given target transform and offset @@ -170,11 +172,11 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, co 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) { +tl::expected 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()); diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index d4591f5c6..181873204 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -160,7 +160,10 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { intermediate_scenes.push_back(end); robot_trajectory::RobotTrajectoryPtr trajectory; - success = pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints); + const auto planner_solution_maybe = pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints); + if (planner_solution_maybe.has_value()) { + success = planner_solution_maybe.value(); + } trajectory_planner_vector.push_back(PlannerIdTrajectoryPair({ pair.second->getPlannerId(), trajectory })); if (!success) diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 62450fa77..c6de2d290 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -198,7 +198,11 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning if (getJointStateFromOffset(direction, dir, jmg, scene->getCurrentStateNonConst())) { // plan to joint-space target - success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); + const auto planner_solution_maybe = + planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); + if (planner_solution_maybe.has_value()) { + success = planner_solution_maybe.value(); + } solution.setPlannerId(planner_->getPlannerId()); } else { // Cartesian targets require an IK reference frame @@ -286,8 +290,11 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning // offset from link to ik_frame const Eigen::Isometry3d& offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world; - success = + const auto planner_solution_maybe = planner_->plan(state.scene(), *link, offset, target_eigen, jmg, timeout, robot_trajectory, path_constraints); + if (planner_solution_maybe.has_value()) { + success = planner_solution_maybe.value(); + } solution.setPlannerId(planner_->getPlannerId()); if (robot_trajectory) { // the following requires a robot_trajectory returned from planning diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 1a44a197a..dd8efd4fa 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -208,7 +208,11 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP if (getJointStateGoal(goal, jmg, scene->getCurrentStateNonConst())) { // plan to joint-space target - success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); + const auto planner_solution_maybe = + planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); + if (planner_solution_maybe.has_value()) { + success = planner_solution_maybe.value(); + } solution.setPlannerId(planner_->getPlannerId()); } else { // Cartesian goal // Where to go? @@ -241,7 +245,11 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP Eigen::Isometry3d offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world; // plan to Cartesian target - success = planner_->plan(state.scene(), *link, offset, target, jmg, timeout, robot_trajectory, path_constraints); + const auto planner_solution_maybe = + planner_->plan(state.scene(), *link, offset, target, jmg, timeout, robot_trajectory, path_constraints); + if (planner_solution_maybe.has_value()) { + success = planner_solution_maybe.value(); + } solution.setPlannerId(planner_->getPlannerId()); } From 823899fabd7fca2423898edb1f019098454b7c41 Mon Sep 17 00:00:00 2001 From: Abishalini Date: Thu, 18 Jan 2024 10:57:43 -0700 Subject: [PATCH 2/7] Propagate errors from planners --- core/src/solvers/cartesian_path.cpp | 7 +++++-- core/src/solvers/joint_interpolation.cpp | 10 +++++----- core/src/solvers/pipeline_planner.cpp | 10 ++++++++-- core/src/stages/connect.cpp | 7 ++++++- core/src/stages/move_relative.cpp | 12 +++++++++++- core/src/stages/move_to.cpp | 9 ++++++++- 6 files changed, 43 insertions(+), 12 deletions(-) diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index ab2738b42..a90921759 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -71,7 +71,7 @@ tl::expected CartesianPath::plan(const planning_scene::Planni const moveit::core::LinkModel* link = jmg->getOnlyOneEndEffectorTip(); if (!link) { RCLCPP_WARN_STREAM(LOGGER, "no unique tip for joint model group: " << jmg->getName()); - return false; + return tl::make_unexpected("no unique tip for joint model group: " + jmg->getName()); } // reach pose of forward kinematics @@ -116,7 +116,10 @@ tl::expected CartesianPath::plan(const planning_scene::Planni timing->computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), props.get("max_acceleration_scaling_factor")); - return achieved_fraction >= props.get("min_fraction"); + if (achieved_fraction >= props.get("min_fraction")) { + return tl::make_unexpected("Min fraction not met. Achieved fraction : " + std::to_string(achieved_fraction)); + } + return true; } } // namespace solvers } // namespace task_constructor diff --git a/core/src/solvers/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp index 0e0e34bea..e6344ddb5 100644 --- a/core/src/solvers/joint_interpolation.cpp +++ b/core/src/solvers/joint_interpolation.cpp @@ -77,7 +77,7 @@ tl::expected JointInterpolationPlanner::plan( // add first point result->addSuffixWayPoint(from->getCurrentState(), 0.0); if (from->isStateColliding(from_state, jmg->getName()) || !from_state.satisfiesBounds(jmg)) - return false; + return tl::make_unexpected("Start state in collision or not within bounds"); moveit::core::RobotState waypoint(from_state); double delta = d < 1e-6 ? 1.0 : props.get("max_step") / d; @@ -86,13 +86,13 @@ tl::expected JointInterpolationPlanner::plan( result->addSuffixWayPoint(waypoint, t); if (from->isStateColliding(waypoint, jmg->getName()) || !waypoint.satisfiesBounds(jmg)) - return false; + return tl::make_unexpected("One of the waypoint's state is in collision or not within bounds"); } // add goal point result->addSuffixWayPoint(to_state, 1.0); if (from->isStateColliding(to_state, jmg->getName()) || !to_state.satisfiesBounds(jmg)) - return false; + return tl::make_unexpected("Goal state in collision or not within bounds"); auto timing = props.get("time_parameterization"); timing->computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), @@ -140,12 +140,12 @@ JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& fro // TODO(v4hn): planners need a way to add feedback to failing plans // in case of an invalid solution feedback should include unwanted collisions or violated constraints RCLCPP_WARN(LOGGER, "IK failed for pose target"); - return false; + return tl::make_unexpected("IK failed for pose target"); } to->getCurrentStateNonConst().update(); if (std::chrono::steady_clock::now() >= deadline) - return false; + return tl::make_unexpected("Timed out"); return plan(from, to, jmg, timeout, result, path_constraints); } diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index 1dd44de6c..8955cae2e 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -224,10 +224,16 @@ tl::expected PipelinePlanner::plan(const planning_scene::Plan // Choose the first solution trajectory as response result = solution.trajectory; last_successful_planner_ = solution.planner_id; - return bool(result); + if (!bool(result)) // If the plan was not a success + { + return tl::make_unexpected(solution.error_code.source + " returned error code " + + moveit::core::errorCodeToString(solution.error_code) + + ". Reason : " + solution.error_code.message); // Maybe print the error code too + } + return true; } } - return false; + return tl::make_unexpected("No solutions generated from Pipeline Planner"); } std::string PipelinePlanner::getPlannerId() const { return last_successful_planner_.empty() ? std::string("Unknown") : last_successful_planner_; diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 181873204..530cb07c2 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -148,6 +148,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { intermediate_scenes.push_back(start); bool success = false; + std::string error_message = ""; std::vector positions; for (const GroupPlannerVector::value_type& pair : planner_) { // set intermediate goal state @@ -164,6 +165,10 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { if (planner_solution_maybe.has_value()) { success = planner_solution_maybe.value(); } + std::string error_message = ""; + if (!success) { + error_message += planner_solution_maybe.error(); + } trajectory_planner_vector.push_back(PlannerIdTrajectoryPair({ pair.second->getPlannerId(), trajectory })); if (!success) @@ -179,7 +184,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { if (!solution) // success == false or merging failed: store sequentially solution = makeSequential(trajectory_planner_vector, intermediate_scenes, from, to); if (!success) // error during sequential planning - solution->markAsFailure(); + solution->markAsFailure("Hardcode an error message"); connect(from, to, solution); } diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index c6de2d290..e734e0b1f 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -195,6 +195,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning robot_trajectory::RobotTrajectoryPtr robot_trajectory; bool success = false; + std::string error_message = ""; if (getJointStateFromOffset(direction, dir, jmg, scene->getCurrentStateNonConst())) { // plan to joint-space target @@ -203,6 +204,9 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning if (planner_solution_maybe.has_value()) { success = planner_solution_maybe.value(); } + if (!success) { + error_message = planner_solution_maybe.error(); + } solution.setPlannerId(planner_->getPlannerId()); } else { // Cartesian targets require an IK reference frame @@ -295,6 +299,9 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning if (planner_solution_maybe.has_value()) { success = planner_solution_maybe.value(); } + if (!success) { + error_message = planner_solution_maybe.error(); + } solution.setPlannerId(planner_->getPlannerId()); if (robot_trajectory) { // the following requires a robot_trajectory returned from planning @@ -338,8 +345,11 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning robot_trajectory->reverse(); solution.setTrajectory(robot_trajectory); - if (!success) + if (!success && solution.comment().empty()) { + solution.markAsFailure(error_message); + } else if (!success) { solution.markAsFailure(); + } return true; } return false; diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index dd8efd4fa..f172295a3 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -205,6 +205,7 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP const auto& path_constraints = props.get("path_constraints"); robot_trajectory::RobotTrajectoryPtr robot_trajectory; bool success = false; + std::string error_message = ""; if (getJointStateGoal(goal, jmg, scene->getCurrentStateNonConst())) { // plan to joint-space target @@ -213,6 +214,9 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP if (planner_solution_maybe.has_value()) { success = planner_solution_maybe.value(); } + if (!success) { + error_message = planner_solution_maybe.error(); + } solution.setPlannerId(planner_->getPlannerId()); } else { // Cartesian goal // Where to go? @@ -250,6 +254,9 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP if (planner_solution_maybe.has_value()) { success = planner_solution_maybe.value(); } + if (!success) { + error_message = planner_solution_maybe.error(); + } solution.setPlannerId(planner_->getPlannerId()); } @@ -266,7 +273,7 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP solution.setTrajectory(robot_trajectory); if (!success) - solution.markAsFailure(); + solution.markAsFailure(error_message); return true; } From c5bf96f5f710f44c7847ec55781099c5b8418b61 Mon Sep 17 00:00:00 2001 From: Abishalini Date: Thu, 18 Jan 2024 12:55:29 -0700 Subject: [PATCH 3/7] Fix bug in move relative --- core/src/solvers/cartesian_path.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index a90921759..9cc959dd8 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -116,7 +116,7 @@ tl::expected CartesianPath::plan(const planning_scene::Planni timing->computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), props.get("max_acceleration_scaling_factor")); - if (achieved_fraction >= props.get("min_fraction")) { + if (achieved_fraction < props.get("min_fraction")) { return tl::make_unexpected("Min fraction not met. Achieved fraction : " + std::to_string(achieved_fraction)); } return true; From c8a75cb2acc3e191a61e4963aeb9f38fc08aa79d Mon Sep 17 00:00:00 2001 From: Abishalini Date: Tue, 23 Jan 2024 09:02:10 -0700 Subject: [PATCH 4/7] tl::expected -> MoveItErrorCode --- .../task_constructor/solvers/cartesian_path.h | 4 +- .../solvers/joint_interpolation.h | 4 +- .../task_constructor/solvers/multi_planner.h | 4 +- .../solvers/pipeline_planner.h | 6 +-- .../solvers/planner_interface.h | 9 ++-- core/src/solvers/cartesian_path.cpp | 28 ++++++------- core/src/solvers/joint_interpolation.cpp | 37 +++++++++-------- core/src/solvers/multi_planner.cpp | 33 ++++++++------- core/src/solvers/pipeline_planner.cpp | 41 +++++++++---------- core/src/stages/connect.cpp | 17 ++++---- core/src/stages/move_relative.cpp | 16 ++++---- core/src/stages/move_to.cpp | 16 ++++---- 12 files changed, 108 insertions(+), 107 deletions(-) diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h index 6296945ab..3002da9dc 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; - tl::expected 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; - tl::expected 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 c440cda40..91050128a 100644 --- a/core/include/moveit/task_constructor/solvers/joint_interpolation.h +++ b/core/include/moveit/task_constructor/solvers/joint_interpolation.h @@ -58,12 +58,12 @@ class JointInterpolationPlanner : public PlannerInterface void init(const moveit::core::RobotModelConstPtr& robot_model) override; - tl::expected + 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; - tl::expected + 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, diff --git a/core/include/moveit/task_constructor/solvers/multi_planner.h b/core/include/moveit/task_constructor/solvers/multi_planner.h index d5b816a74..168691a5e 100644 --- a/core/include/moveit/task_constructor/solvers/multi_planner.h +++ b/core/include/moveit/task_constructor/solvers/multi_planner.h @@ -63,12 +63,12 @@ class MultiPlanner : public PlannerInterface, public std::vector + 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; - tl::expected + 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, diff --git a/core/include/moveit/task_constructor/solvers/pipeline_planner.h b/core/include/moveit/task_constructor/solvers/pipeline_planner.h index e23b5f4dc..b9c17bb75 100644 --- a/core/include/moveit/task_constructor/solvers/pipeline_planner.h +++ b/core/include/moveit/task_constructor/solvers/pipeline_planner.h @@ -126,7 +126,7 @@ 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 */ - tl::expected plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, + 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; @@ -142,7 +142,7 @@ 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 */ - tl::expected 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* joint_model_group, double timeout, robot_trajectory::RobotTrajectoryPtr& result, @@ -170,7 +170,7 @@ class PipelinePlanner : public PlannerInterface * problem * \return true If the solver found a successful solution for the given planning problem */ - tl::expected plan(const planning_scene::PlanningSceneConstPtr& planning_scene, + 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, diff --git a/core/include/moveit/task_constructor/solvers/planner_interface.h b/core/include/moveit/task_constructor/solvers/planner_interface.h index 6f4648139..1b699b8bf 100644 --- a/core/include/moveit/task_constructor/solvers/planner_interface.h +++ b/core/include/moveit/task_constructor/solvers/planner_interface.h @@ -41,8 +41,8 @@ #include #include #include +#include #include -#include namespace planning_scene { MOVEIT_CLASS_FORWARD(PlanningScene); @@ -65,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 { @@ -89,13 +92,13 @@ class PlannerInterface virtual void init(const moveit::core::RobotModelConstPtr& robot_model) = 0; /// plan trajectory between to robot states - virtual tl::expected + 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 tl::expected + 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, diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index 9cc959dd8..330314197 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -63,15 +63,15 @@ CartesianPath::CartesianPath() { void CartesianPath::init(const core::RobotModelConstPtr& /*robot_model*/) {} -tl::expected 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) { RCLCPP_WARN_STREAM(LOGGER, "no unique tip for joint model group: " << jmg->getName()); - return tl::make_unexpected("no unique tip for joint model group: " + jmg->getName()); + return MoveItErrorCode(MoveItErrorCodes::FAILURE, "no unique tip for joint model group: " + jmg->getName()); } // reach pose of forward kinematics @@ -79,12 +79,11 @@ tl::expected CartesianPath::plan(const planning_scene::Planni std::min(timeout, properties().get("timeout")), result, path_constraints); } -tl::expected 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(); @@ -117,9 +116,10 @@ tl::expected CartesianPath::plan(const planning_scene::Planni props.get("max_acceleration_scaling_factor")); if (achieved_fraction < props.get("min_fraction")) { - return tl::make_unexpected("Min fraction not met. Achieved fraction : " + std::to_string(achieved_fraction)); + return MoveItErrorCode(MoveItErrorCodes::FAILURE, + "Min fraction not met. Achieved fraction : " + std::to_string(achieved_fraction)); } - return true; + return MoveItErrorCode(MoveItErrorCodes::SUCCESS); } } // namespace solvers } // namespace task_constructor diff --git a/core/src/solvers/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp index e6344ddb5..6186b745f 100644 --- a/core/src/solvers/joint_interpolation.cpp +++ b/core/src/solvers/joint_interpolation.cpp @@ -59,10 +59,11 @@ JointInterpolationPlanner::JointInterpolationPlanner() { void JointInterpolationPlanner::init(const core::RobotModelConstPtr& /*robot_model*/) {} -tl::expected 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 @@ -77,7 +78,7 @@ tl::expected JointInterpolationPlanner::plan( // add first point result->addSuffixWayPoint(from->getCurrentState(), 0.0); if (from->isStateColliding(from_state, jmg->getName()) || !from_state.satisfiesBounds(jmg)) - return tl::make_unexpected("Start state in collision or not within bounds"); + return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Start state in collision or not within bounds"); moveit::core::RobotState waypoint(from_state); double delta = d < 1e-6 ? 1.0 : props.get("max_step") / d; @@ -86,13 +87,14 @@ tl::expected JointInterpolationPlanner::plan( result->addSuffixWayPoint(waypoint, t); if (from->isStateColliding(waypoint, jmg->getName()) || !waypoint.satisfiesBounds(jmg)) - return tl::make_unexpected("One of the waypoint's state is in collision or not within bounds"); + return MoveItErrorCode(MoveItErrorCodes::FAILURE, + "One of the waypoint's state is in collision or not within bounds"); } // add goal point result->addSuffixWayPoint(to_state, 1.0); if (from->isStateColliding(to_state, jmg->getName()) || !to_state.satisfiesBounds(jmg)) - return tl::make_unexpected("Goal state in collision or not within bounds"); + return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Goal state in collision or not within bounds"); auto timing = props.get("time_parameterization"); timing->computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), @@ -112,15 +114,15 @@ tl::expected JointInterpolationPlanner::plan( } } - return true; + return MoveItErrorCode(MoveItErrorCodes::SUCCESS); } -tl::expected -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); @@ -140,12 +142,13 @@ JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& fro // TODO(v4hn): planners need a way to add feedback to failing plans // in case of an invalid solution feedback should include unwanted collisions or violated constraints RCLCPP_WARN(LOGGER, "IK failed for pose target"); - return tl::make_unexpected("IK failed for pose target"); + return MoveItErrorCode(MoveItErrorCodes::FAILURE, "IK failed for pose target."); } to->getCurrentStateNonConst().update(); - if (std::chrono::steady_clock::now() >= deadline) - return tl::make_unexpected("Timed out"); + if (std::chrono::steady_clock::now() >= deadline) { + return MoveItErrorCode(MoveItErrorCodes::FAILURE, "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 e335483d3..62da6420d 100644 --- a/core/src/solvers/multi_planner.cpp +++ b/core/src/solvers/multi_planner.cpp @@ -49,51 +49,50 @@ void MultiPlanner::init(const core::RobotModelConstPtr& robot_model) { p->init(robot_model); } -tl::expected 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(); for (const auto& p : *this) { if (remaining_time < 0) - return false; // timeout + return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Timeout"); // timeout if (result) result->clear(); if (p->plan(from, to, jmg, remaining_time, result, path_constraints)) - return true; + return MoveItErrorCode(MoveItErrorCodes::SUCCESS); auto now = std::chrono::steady_clock::now(); remaining_time -= std::chrono::duration(now - start_time).count(); start_time = now; } - return false; + return MoveItErrorCode(MoveItErrorCodes::FAILURE); } -tl::expected 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(); for (const auto& p : *this) { if (remaining_time < 0) - return false; // timeout + return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Timeout"); // timeout if (result) result->clear(); if (p->plan(from, link, offset, target, jmg, remaining_time, result, path_constraints)) - return true; + return MoveItErrorCode(MoveItErrorCodes::SUCCESS); auto now = std::chrono::steady_clock::now(); remaining_time -= std::chrono::duration(now - start_time).count(); start_time = now; } - return false; + return MoveItErrorCode(MoveItErrorCodes::FAILURE); } } // namespace solvers } // namespace task_constructor diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index 8955cae2e..766c4f9ea 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -140,24 +140,23 @@ void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) { } } -tl::expected 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); } -tl::expected 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) { last_successful_planner_.clear(); // Construct a Cartesian target pose from the given target transform and offset @@ -172,11 +171,11 @@ tl::expected PipelinePlanner::plan(const planning_scene::Plan return plan(from, joint_model_group, goal_constraints, timeout, result, path_constraints); } -tl::expected 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) { // Create a request for every planning pipeline that should run in parallel std::vector requests; requests.reserve(pipeline_id_planner_id_map_.size()); @@ -226,14 +225,12 @@ tl::expected PipelinePlanner::plan(const planning_scene::Plan last_successful_planner_ = solution.planner_id; if (!bool(result)) // If the plan was not a success { - return tl::make_unexpected(solution.error_code.source + " returned error code " + - moveit::core::errorCodeToString(solution.error_code) + - ". Reason : " + solution.error_code.message); // Maybe print the error code too + return MoveItErrorCode(solution.error_code.val, solution.error_code.message, solution.error_code.source); } - return true; + return MoveItErrorCode(MoveItErrorCodes::SUCCESS); } } - return tl::make_unexpected("No solutions generated from Pipeline Planner"); + return MoveItErrorCode(MoveItErrorCodes::FAILURE, "No solutions generated from Pipeline Planner"); } std::string PipelinePlanner::getPlannerId() const { return last_successful_planner_.empty() ? std::string("Unknown") : last_successful_planner_; diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 530cb07c2..d37cf970a 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -161,18 +161,17 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { intermediate_scenes.push_back(end); robot_trajectory::RobotTrajectoryPtr trajectory; - const auto planner_solution_maybe = pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints); - if (planner_solution_maybe.has_value()) { - success = planner_solution_maybe.value(); - } - std::string error_message = ""; - if (!success) { - error_message += planner_solution_maybe.error(); + const auto planner_solution_status = pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints); + if (bool(planner_solution_status)) { + success = true; } + trajectory_planner_vector.push_back(PlannerIdTrajectoryPair({ pair.second->getPlannerId(), trajectory })); - if (!success) + if (!success) { + error_message = planner_solution_status.message; break; + } // continue from reached state start = end; @@ -184,7 +183,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { if (!solution) // success == false or merging failed: store sequentially solution = makeSequential(trajectory_planner_vector, intermediate_scenes, from, to); if (!success) // error during sequential planning - solution->markAsFailure("Hardcode an error message"); + solution->markAsFailure(error_message); connect(from, to, solution); } diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index e734e0b1f..4b10d2dd0 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -199,13 +199,13 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning if (getJointStateFromOffset(direction, dir, jmg, scene->getCurrentStateNonConst())) { // plan to joint-space target - const auto planner_solution_maybe = + const auto planner_solution_status = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); - if (planner_solution_maybe.has_value()) { - success = planner_solution_maybe.value(); + if (bool(planner_solution_status)) { + success = true; } if (!success) { - error_message = planner_solution_maybe.error(); + error_message = planner_solution_status.message; } solution.setPlannerId(planner_->getPlannerId()); } else { @@ -294,13 +294,13 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning // offset from link to ik_frame const Eigen::Isometry3d& offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world; - const auto planner_solution_maybe = + const auto planner_solution_status = planner_->plan(state.scene(), *link, offset, target_eigen, jmg, timeout, robot_trajectory, path_constraints); - if (planner_solution_maybe.has_value()) { - success = planner_solution_maybe.value(); + if (bool(planner_solution_status)) { + success = true; } if (!success) { - error_message = planner_solution_maybe.error(); + error_message = planner_solution_status.message; } solution.setPlannerId(planner_->getPlannerId()); diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index f172295a3..99218dc42 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -209,13 +209,13 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP if (getJointStateGoal(goal, jmg, scene->getCurrentStateNonConst())) { // plan to joint-space target - const auto planner_solution_maybe = + const auto planner_solution_status = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); - if (planner_solution_maybe.has_value()) { - success = planner_solution_maybe.value(); + if (bool(planner_solution_status)) { + success = true; } if (!success) { - error_message = planner_solution_maybe.error(); + error_message = planner_solution_status.message; } solution.setPlannerId(planner_->getPlannerId()); } else { // Cartesian goal @@ -249,13 +249,13 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP Eigen::Isometry3d offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world; // plan to Cartesian target - const auto planner_solution_maybe = + const auto planner_solution_status = planner_->plan(state.scene(), *link, offset, target, jmg, timeout, robot_trajectory, path_constraints); - if (planner_solution_maybe.has_value()) { - success = planner_solution_maybe.value(); + if (bool(planner_solution_status)) { + success = true; } if (!success) { - error_message = planner_solution_maybe.error(); + error_message = planner_solution_status.message; } solution.setPlannerId(planner_->getPlannerId()); } From 32cd2b80c0318163ecbc4fc7873ea9c8efa26d60 Mon Sep 17 00:00:00 2001 From: Abishalini Date: Wed, 24 Jan 2024 10:06:48 -0700 Subject: [PATCH 5/7] Remove tl::expected --- core/CMakeLists.txt | 2 -- core/package.xml | 1 - 2 files changed, 3 deletions(-) diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 7d21c5da6..d10d2f5ba 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -14,7 +14,6 @@ find_package(moveit_task_constructor_msgs REQUIRED) find_package(rclcpp REQUIRED) find_package(rviz_marker_tools REQUIRED) find_package(tf2_eigen REQUIRED) -find_package(tl_expected REQUIRED) find_package(visualization_msgs REQUIRED) set(CMAKE_CXX_STANDARD 17) @@ -45,6 +44,5 @@ ament_export_dependencies(moveit_task_constructor_msgs) ament_export_dependencies(rclcpp) ament_export_dependencies(rviz_marker_tools) ament_export_dependencies(tf2_eigen) -ament_export_dependencies(tl_expected) ament_export_dependencies(visualization_msgs) ament_package() diff --git a/core/package.xml b/core/package.xml index 7a0d7c389..3cb44239b 100644 --- a/core/package.xml +++ b/core/package.xml @@ -22,7 +22,6 @@ rclcpp rviz_marker_tools tf2_eigen - tl_expected visualization_msgs ament_cmake_gmock From 0d7b86e218ac2474a8d9c9a72bbd835f580e7930 Mon Sep 17 00:00:00 2001 From: Abishalini Date: Wed, 24 Jan 2024 10:10:06 -0700 Subject: [PATCH 6/7] Modify return type in comments --- .../moveit/task_constructor/solvers/pipeline_planner.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/core/include/moveit/task_constructor/solvers/pipeline_planner.h b/core/include/moveit/task_constructor/solvers/pipeline_planner.h index b9c17bb75..d08977283 100644 --- a/core/include/moveit/task_constructor/solvers/pipeline_planner.h +++ b/core/include/moveit/task_constructor/solvers/pipeline_planner.h @@ -124,7 +124,7 @@ 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 planning_scene::PlanningSceneConstPtr& to, const core::JointModelGroup* joint_model_group, double timeout, @@ -140,7 +140,7 @@ 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, From 5a2607e35805b2ae426255b3abb36cb026dc7cc8 Mon Sep 17 00:00:00 2001 From: Abishalini Date: Wed, 24 Jan 2024 10:20:46 -0700 Subject: [PATCH 7/7] Remove TODO --- core/src/solvers/joint_interpolation.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/core/src/solvers/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp index 6186b745f..d910f91d9 100644 --- a/core/src/solvers/joint_interpolation.cpp +++ b/core/src/solvers/joint_interpolation.cpp @@ -139,8 +139,6 @@ MoveItErrorCode JointInterpolationPlanner::plan(const planning_scene::PlanningSc } }; if (!to->getCurrentStateNonConst().setFromIK(jmg, target * offset.inverse(), link.getName(), timeout, is_valid)) { - // TODO(v4hn): planners need a way to add feedback to failing plans - // in case of an invalid solution feedback should include unwanted collisions or violated constraints RCLCPP_WARN(LOGGER, "IK failed for pose target"); return MoveItErrorCode(MoveItErrorCodes::FAILURE, "IK failed for pose target."); }