I'd suggest using map::route() here to ask for a route around the vehicle, so you can re-use existing code.