Testing stage of move_base_straight_all.py