Trajectory planning and following in urban environments