start_position -4.0 -0.25 0.15
goal_position 2.0 2.1 -0.25
vis_graph_status dev
