Feature: allow zero-length paths

[Aposhian]

Feature request

Allow planners to submit zero-length paths as valid paths that are trivially concatenated during ComputePlanThroughPoses.

Feature description

Currently, planners communicate failure to create a plan by returning a zero-length path from createPlan. This precludes the possibility of providing a goal that is the same pose as the starting position. That may seem like a nonsensical case to handle, but it actually comes up in real cases of using NavigateThroughPoses/ComputePlanThroughPoses.

Suppose you have a sequence of poses that represent a known maneuver you want a robot to complete. With the current implementation, if you get the robot so close to the starting position that it is an equivalent pose as far as the planner is concerned, the the first path segment will be a zero-length path. At this point, the planner server ceases to compute paths for subsequent goal poses.

For this situation, the workaround would be to retry the sequence, but without the first pose, but I believe this should work out of the box, and this quirk only exists because zero-length paths were chosen as a proxy for an error condition.

Implementation considerations

Instead of returning a Path from GlobalPlanner::createPlan, createPlan could either return an

• std::error_code, while returning the Path by reference
• Result type, which has the Path, or an error condition

Then all nav2_planners would have to have their error-handling logic to communicate errors with this new mechanism. While taking some significant refactoring, the benefit would be that, if desired, planners could propagate the actual error condition, rather than having a single error code of "zero-length path".

[SteveMacenski]

That may seem like a nonsensical case to handle, but it actually comes up in real cases of using NavigateThroughPoses/ComputePlanThroughPoses.

That seems fair enough, maybe the best solution is just to have all the planners return single-lengthed paths in which contain just the point of interest from the start. That should get you past the 0-length test to be a 'meaningful' path and can continue on your merry way. That should be easy enough to modify the smac planners / navfn / theta* planners to do.

A true error in the planners typically throw an exception which is then caught in the planner server as a failure mode. That is already the case and most of the planners utilize that, which is more or less your exact suggestion. However since the planners are plugin interfaces in which anyone could possibly create a plugin to use, we need to design the main nav2 servers to be especially tolerant of bogus outputs, like 0-length paths. So either way, we'd need to check for invalid 0-lengthed paths if someone screws up and doesn't implement an error as an exception in their custom plugin (or they'll come back and complain to me that my software is broken 😆 ).

[Aposhian]

we need to design the main nav2 servers to be especially tolerant of bogus outputs, like 0-length paths.

Are you suggesting that planner_server should or should not interpret a 0-length path as an error? I don't see anything wrong with making a breaking change that requires plugins to correctly throw exceptions.

That seems fair enough, maybe the best solution is just to have all the planners return single-lengthed paths in which contain just the point of interest from the start. That should get you past the 0-length test to be a 'meaningful' path and can continue on your merry way. That should be easy enough to modify the smac planners / navfn / theta* planners to do.

To clarify, that would entail refactoring the nav2 planners to return a single-length path when the goal is the same pose as the start? That seems fine, since I'm guessing the controller won't mind. I would be happy to implement that.

[SteveMacenski]

planner_server should interpret 0-length paths as non-valid paths. But a single point path would be. So if you had the start and end goals at the same points (or within costmap resolution of each other), I think a valid return would be just that single point. If the planners were updated to do that, I think that would resolve all of your concerns. I think almost every planner has a preliminary pre-planning check for that exact criteria you could easily hijack and change to return 1 pose and fix that concern across the 2 smac planners, NavFn, and Theta*

It's not about a requirement, its about defensive programming. Someone, somewhere, at sometime is going to forget or mess that up and we need to be able to handle it. 0-length paths are clearly bogus and undrivable. There's no reason to make the code less sound to punish accidental mistakes.

[Aposhian]

That's fine. When I have time, I can make a PR for converting valid zero-length paths into trivial single-length paths.

My perspective on zero-length paths was that it means, "hey, there is nothing to drive," and it is ambiguous if that means you are already there, or that a plan could not be found. In the context of ComputePlanThroughPoses, where all of the mini-plans are concatenated together, a zero-length plan makes sense to me, especially if you gave a sequence of goal poses that are very close together at times relative to the costmap. In order to avoid that ambiguity, planners should throw exceptions or return an error code instead when something is truly wrong.

[SteveMacenski]

Any update?

[SteveMacenski]

PR open in #2488 which implements for the 3 2D planners (Theta*, Smac2D, and NavFn). It sets start == goal to having a single pose length of that pose. Then it also adds in a solution to another ticket adding in a new parameter for how to set the final pose of the plan (whether to use the requested final orientation or the path's approach orientation to the goal).