Determine slotcar target heading by proximity to the commanded heading in its trajectory

[mrushyendra]

Modifies compute_change_in_rotation to choose a heading based on proximity to the commanded heading
in the slotcar's trajectory, instead of choosing a heading based off of the amount of rotation required.

[luca-della-vedova]

I gave it a quick try on the office scenario and I could still find some cases in which the robots have the unintended spinning but it does happen very seldom so it's a tricky issue to debug.
You might need to set the reversible tag to false in the fleet adapter launch since it seems the behavior mostly happens with non reversible robots, i.e add:

<arg name="reversible" value="false"/>

[mrushyendra]

(Reposting from our meeting discussion just for log purposes):

After logging several different examples, it seems like the slotcar itself largely rotates correctly - i.e. to match the direction of dpos, taking into account the waypoint pose and travelling in the shortest direction possible (clockwise/anti-clockwise). However, sometimes the commanded pose itself may be such that it requires some spinning. This mostly seems to occur when:

1. The robot starts travelling to a new waypoint, but some negotiation happens, causing its goal to be reset to a previous waypoint.
2. The distance to this previous waypoint dpos is very small, but the angle between its current pose and the direction vector calculated from dpos may be large, so the robot spins in place to get to this new waypoint which is very close by.

I think we discussed that this would need some changes elsewhere to potentially fix.