From 4de8d2378ccfa5c8117756fbe64c1898243671a2 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 7 Feb 2024 13:31:36 +0100 Subject: [PATCH] Make error code more informative (#529) --- core/src/solvers/joint_interpolation.cpp | 28 ++++++++++++++++++------ 1 file changed, 21 insertions(+), 7 deletions(-) diff --git a/core/src/solvers/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp index d910f91d9..993112f03 100644 --- a/core/src/solvers/joint_interpolation.cpp +++ b/core/src/solvers/joint_interpolation.cpp @@ -77,8 +77,13 @@ MoveItErrorCode JointInterpolationPlanner::plan(const planning_scene::PlanningSc // add first point result->addSuffixWayPoint(from->getCurrentState(), 0.0); - if (from->isStateColliding(from_state, jmg->getName()) || !from_state.satisfiesBounds(jmg)) - return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Start state in collision or not within bounds"); + if (from->isStateColliding(from_state, jmg->getName())) { + return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Start state is in collision!"); + } + + if (!from_state.satisfiesBounds(jmg)) { + return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Start state is not within bounds!"); + } moveit::core::RobotState waypoint(from_state); double delta = d < 1e-6 ? 1.0 : props.get("max_step") / d; @@ -86,15 +91,24 @@ MoveItErrorCode JointInterpolationPlanner::plan(const planning_scene::PlanningSc from_state.interpolate(to_state, t, waypoint); result->addSuffixWayPoint(waypoint, t); - if (from->isStateColliding(waypoint, jmg->getName()) || !waypoint.satisfiesBounds(jmg)) - return MoveItErrorCode(MoveItErrorCodes::FAILURE, - "One of the waypoint's state is in collision or not within bounds"); + if (from->isStateColliding(waypoint, jmg->getName())) { + return MoveItErrorCode(MoveItErrorCodes::FAILURE, "One of the waypoint's state is in collision!"); + } + + if (!waypoint.satisfiesBounds(jmg)) { + 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()) || !to_state.satisfiesBounds(jmg)) - return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Goal state in collision or not within bounds"); + if (from->isStateColliding(to_state, jmg->getName())) { + return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Goal state is in collision!"); + } + + if (!to_state.satisfiesBounds(jmg)) { + return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Goal state is not within bounds!"); + } auto timing = props.get("time_parameterization"); timing->computeTimeStamps(*result, props.get("max_velocity_scaling_factor"),