Feature request

Rotate to goal heading when reaching goal

Feature description

When reaching the goal, the rotation shim controller could take back control of the robot to rotate to the goal heading.

Implementation considerations

The current implementation does not take it into account. It is the responsibility of the primary controller to reach the correct heading.

Here is a first idea of the implementation: 94d3a9d
Note: I could not test it yet, probably it is not fully working as is 😄