Planning for Connected and Automated Vehicles