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.