Fixed bug so that robot keeps driving even if the first few points of the trajectory are occupied
Reinstated cmd_vel_costs