start_position -2.000 -2.000 0.000
goal_position 2.000 2.000 0.000
vis_graph_status test