fix: account for entity's actual position

This change avoids the snapping of entities to the route's first waypoint, whenever they are on the same lane, but on a different position.

It also makes the necessary modifications to take into account that the received waypoints are already in the correct frame. The LegacyRouteAlignment user setting was added temporarily to maintain legacy behavior if that is needed. It will only be kept during a transition period.