start_position -2.55 0.25 0.965
goal_position -0.52 0.55 -0.042
start_yaw 0.0
goal_yaw 0.0
