Ackermann drive plugin gets stuck when in place turns are introduced.

[luca-della-vedova]

Bug report

Required information:

Build from source of RMF on Ubuntu 20.04 and ROS Galactic

Description of the bug

When an ackermann drive vehicle is required to turn in place for any reason the plugin stops working altogether and the vehicle gets stuck in place.
This means that every time a vehicle ends a trajectory in a corner waypoint, it will likely be stuck there indefinitely if the corner is sharp enough.
I'm not too familiar with the ackermann code but my guess is that this is an edge case where the vehicle is in an area inside the arc between the waypoints that are being interpolated to make a smooth bend.

Steps to reproduce the bug

Create a navgraph with sharp turns (i.e. a square), make the vehicle execute two consecutive trajectories, where the first one ends in a corner and the second one starts from the same corner.

Expected behavior

The vehicle would execute a somewhat sharp turn to proceed to its first waypoint.

Actual behavior

The vehicle turns a bit, gets stuck and never actually executes its trajectory.

[luca-della-vedova]

A temporary fix is available in #54, a full implementation will be done as part of open-rmf/rmf_traffic#42