Replanning without skipping a rotation target

[lidor51]

Replanning might skip rotation targets and cause to undesired behavior of robot on the field.

The suggested behavior is if skipping a rotation target, set the new path to begin with "fast as possible" rotation target to the interpolated rotation target based on current robot location (interpolation between prev and next rotation targets).