A system for controlling a vehicle navigating a roadway, including a perception module that generates sensor data and outputs a cost map and traffic data associated with traffic objects, a path planning module, and a vehicle control module; and a method for controlling a vehicle including collecting sensor data, generating a cost map, determining a vehicle path, and controlling the vehicle along the path.