def list_to_msg(type, array, add=None, override=None):
array = copy(array)
if add is not None:
assert len(array) == len(add)
array += add
if override is not None:
assert len(array) == len(override)
for i in range(len(array)):
if not np.isnan(override[i]):
array[i] = override[i]
if type == 'Pose':
msg = Pose()
msg.position.x = array[0]
msg.position.y = array[1]
msg.position.z = array[2]
if len(array) == 4:
msg.orientation = Quaternion(
*quaternion_from_euler(0, 0, radians(array[3])))
else:
msg.orientation.w = 1
elif type == 'GeoPoint':
msg = GeoPoint()
msg.latitude = array[0]
msg.longitude = array[1]
msg.altitude = array[2]
elif type == 'GeoPose':
msg = GeoPose()
msg.position.latitude = array[0]
msg.position.longitude = array[1]
msg.position.altitude = array[2]
if len(array) == 4:
msg.orientation = Quaternion(
*quaternion_from_euler(0, 0, radians(array[3])))
else:
msg.orientation.w = 1
else:
raise ValueError('Unknown type: {}'.format(type))
return msg
