Lane change manoeuver sometimes doesn't work

[divitsharma]

During a lane change, a vehicle will sometimes jump two lanes at once or not go through with the lane change at all. This is likely due to syncing problems of the reference path between the Planner and SV.

[divitsharma]

One issue that could be the cause of this:
On the planning frame that changes the reference path, plan_maneuver for lane swerve or cut-in could return an invalid trajectory. When this happens SV ignores the result from the planner, so it is never notified of the reference path change, and now the planner and SV are following different reference paths. So in the next tick planner writes a trajectory based on a different reference.

A possible solution is adding a 'frame' variable to the shared array, like sim frame position that can be used to compute the reference path when the SV notices it's changed. Unlike new_frenet_frame we have currently this won't be a per-tick variable.