Added navfn path length threshold

[DatSpace]

This PR simply adds a new parameter for Navfn ROS, that allows you to limit the max allowed distance of the generated path.

[DatSpace]

The goal is to be able to avoid going all around in a path. For example if you have a room with a doughnut like shape and the path is blocked somewhere, I dont want the robot to pathfind all the way around to the other side of the blocked path.

By setting a limit to the path (optionally) we can avoid that very simply.

With the way it is implemented, navfn will throw a warning and then it will try to re plan until it times out. After that it will go into recovery as expected, which will clear any possible obstacles in the path etc, making it a very good way to do this, I believe.