Pathfinding finished, tested only on TRUCKS.