Merging vehicle_refactor code from the robot