If reverse movement is enabled, does not follow shortest path

[merose]

Still working on a simple way to reproduce, but using the demo launcher when publishing navigation goals poses from rviz the mock robot does not always follow the shortest path, sometimes turning around and backing toward the goal even if forward movement would be more efficient.

No problems found when limited to forward movement.