mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Make error code more informative (#529)
This commit is contained in:
parent
8a89a9762b
commit
4de8d2378c
@ -77,8 +77,13 @@ MoveItErrorCode JointInterpolationPlanner::plan(const planning_scene::PlanningSc
|
|||||||
|
|
||||||
// add first point
|
// add first point
|
||||||
result->addSuffixWayPoint(from->getCurrentState(), 0.0);
|
result->addSuffixWayPoint(from->getCurrentState(), 0.0);
|
||||||
if (from->isStateColliding(from_state, jmg->getName()) || !from_state.satisfiesBounds(jmg))
|
if (from->isStateColliding(from_state, jmg->getName())) {
|
||||||
return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Start state in collision or not within bounds");
|
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);
|
moveit::core::RobotState waypoint(from_state);
|
||||||
double delta = d < 1e-6 ? 1.0 : props.get<double>("max_step") / d;
|
double delta = d < 1e-6 ? 1.0 : props.get<double>("max_step") / d;
|
||||||
@ -86,15 +91,24 @@ MoveItErrorCode JointInterpolationPlanner::plan(const planning_scene::PlanningSc
|
|||||||
from_state.interpolate(to_state, t, waypoint);
|
from_state.interpolate(to_state, t, waypoint);
|
||||||
result->addSuffixWayPoint(waypoint, t);
|
result->addSuffixWayPoint(waypoint, t);
|
||||||
|
|
||||||
if (from->isStateColliding(waypoint, jmg->getName()) || !waypoint.satisfiesBounds(jmg))
|
if (from->isStateColliding(waypoint, jmg->getName())) {
|
||||||
return MoveItErrorCode(MoveItErrorCodes::FAILURE,
|
return MoveItErrorCode(MoveItErrorCodes::FAILURE, "One of the waypoint's state is in collision!");
|
||||||
"One of the waypoint's state is in collision or not within bounds");
|
}
|
||||||
|
|
||||||
|
if (!waypoint.satisfiesBounds(jmg)) {
|
||||||
|
return MoveItErrorCode(MoveItErrorCodes::FAILURE, "One of the waypoint's state is not within bounds!");
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// add goal point
|
// add goal point
|
||||||
result->addSuffixWayPoint(to_state, 1.0);
|
result->addSuffixWayPoint(to_state, 1.0);
|
||||||
if (from->isStateColliding(to_state, jmg->getName()) || !to_state.satisfiesBounds(jmg))
|
if (from->isStateColliding(to_state, jmg->getName())) {
|
||||||
return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Goal state in collision or not within bounds");
|
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<TimeParameterizationPtr>("time_parameterization");
|
auto timing = props.get<TimeParameterizationPtr>("time_parameterization");
|
||||||
timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
|
timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user