Route Following Plugin "return to shortest path logic" triggers as a result of controller drift

[kjrush]

Types of Issue

• [x] Anomaly report (something appears to not work correctly)
• [ ] Enhancement request (describe the enhancement being requested)
• [ ] Other (please ensure the description clarifies why the issue doesn’t fall into either of the above categories)

Descriptive summary

Route Following Plugin contains logic to detect when the vehicle has left the shortest path through the route and to plan lane changes as necessary to return the vehicle to the a newly-computed shortest path when control is returned to the RFP. This logic is triggering unintentionally as a result of control drift causing the vehicle's reference point to drift into an adjacent lane during RFP execution. This causes the vehicle to plan a lane change from the adjacent lane back onto a new shortest path, which immediately begins to use the adjacent lane's centerline as control input, causing undesired steering oscillation.

As a related but somewhat independent issue, this check has difficulty determining the vehicle's proper lanelet when multiple lanelet's overlap (such as at divergences or convergences) and can trigger this behavior in such a scenario as well.

Carma version where this issue was discovered

Expected behavior

Actual behavior

CARMA System 4.1 as used in IHP testing.