start_position 5.129236737787561 3.9076041286150187 0.7232657436869832
goal_position 5.920021065855787 4.286522588066688 0.12493551262399635
start_yaw 0.3342064122667578
goal_yaw 0.4932848137363388
