Do not skip waypoint if orientation is not aligned

[xiyuoh]

This PR is a follow-up to open-rmf/rmf_traffic#111 targeting issue open-rmf/rmf#556.

When a new path is provided to EasyFullControl from the traffic planner, follow_new_path would look for a connection between the path waypoints and the robot's current location in order to determine an overlapping starting point. EFC would then skip any unnecessary waypoints before the starting point to prevent the robot from backtracking. However, if the fleet config's skip_rotation_commands is set to False, these waypoints should not be skipped if they are responsible for in-place rotation.

In the proposed fix, if skip_rotation_commands flag is set to False, it would check for the orientation difference between the robot and potential starting point. It ensures in-place rotation waypoints are not skipped if the difference is too large (threshold is measured at 0.01, taken from another portion of the same code here).

For example, if the following path is provided, with the robot's current pose [18.8078, -10.3719, 0.0306] and skip_rotation_commands set to False:

[fleet_adapter-16] -- [wp_0] 18.7944, -10.3724, 0.0304936
[fleet_adapter-16] -- [wp_1] 18.7944, -10.3724, 1.58649
[fleet_adapter-16] -- [wp_2] 18.7395, -6.87398, 1.58649
[fleet_adapter-16] -- [wp_3] 18.7395, -6.87398, 1.49677
[fleet_adapter-16] -- [wp_4] 18.9498, -4.03908, 1.49677
[fleet_adapter-16] -- [wp_5] 18.9498, -4.03908, -0.0127633
[fleet_adapter-16] -- [wp_6] 13.1306, -3.96481, -0.0127633
[fleet_adapter-16] -- [wp_7] 13.1306, -3.96481, -0.296291
[fleet_adapter-16] -- [wp_8] 7.03417, -2.11099, -0.296291
[fleet_adapter-16] -- [wp_9] 7.03417, -2.11099, 1.14971
[fleet_adapter-16] -- [wp_10] 6.52584, -3.24596, 1.14971

Without the fix, the fleet adapter would skip waypoints wp_0 and wp_1, and request the robot to move directly from its current pose to wp_2.
With the fix applied, the new path would only skip waypoint wp_0 and commands the robot to move to wp_1 from its current pose, which is an in-place rotation, before moving on to wp_2.

Another minor change includes changing max_merge_lane_distance to max_merge_waypoint_distance here, since this condition block deals with waypoints in the same stack rather than the robot being on a lane.

[luca-della-vedova]

Thanks for the explanation! Does this depend on open-rmf/rmf_traffic#111 or can it be merged independently?

[xiyuoh]

This PR is independent, can be merged first. Thanks!