Do sidesteps in path planning

[schmidma]

This

leads to this:

We should look ahead in the path and do side-steps...
Otherwise we miss the opportunity to track the obstacle because we cannot see it and also show our widest radius towards the obstacle...

[rmburg]

fixed with #1854