qw, qx, qy, qz = quaternion_from_euler(waypoint[3], waypoint[4], waypoint[5])
pose_ = deepcopy(pose)
pose_.position.x = waypoint[0]
pose_.position.y = waypoint[1]
pose_.position.z = waypoint[2]
pose_.orientation.w = qw
pose_.orientation.x = qx
pose_.orientation.y = qy
pose_.orientation.z = qz
it should be qx, qy, qz,qw ?
because quaternion_from_euler return an array , q[3] should be w
it should be qx, qy, qz,qw ?
because quaternion_from_euler return an array , q[3] should be w