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; }