Modified move_base_straight_v2.py