MoveTo/MoveRelative: report failure on invalid trajectories (#107)

This commit is contained in:
Robert Haschke 2019-10-22 12:06:11 +02:00 committed by GitHub
parent 6764b2a9ca
commit 1ba4bc4508
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 5 additions and 4 deletions

View File

@ -290,8 +290,9 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
solution.setTrajectory(robot_trajectory);
if (!success)
solution.markAsFailure();
return true;
}
return true;
return false;
}
void MoveRelative::computeForward(const InterfaceState& from) {

View File

@ -246,12 +246,12 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
if (!success)
solution.markAsFailure();
return true;
}
return true;
return false;
}
// -1 TODO: move these as default implementation to PropagateEitherWay?
// Essentially, here compute() is a class-specific worker function
void MoveTo::computeForward(const InterfaceState& from) {
planning_scene::PlanningScenePtr to;
SubTrajectory trajectory;