def list2orientation(xyzw):
assert len(xyzw) == 4, "length of inputList should be 4 in order to fit x,y,z,w "
orientation = Pose().orientation
orientation.x = xyzw[0]
orientation.y = xyzw[1]
orientation.z = xyzw[2]
orientation.w = xyzw[3]
return orientation
def auto_encode(data):
t = type(data)
if t in ts.matrix_types:
m = data if t in ts.symengine_matrix_types else data.to_sym_matrix()
if m.shape == (4, 4):
return encode_pose(m)
elif m.shape == (3, 3):
return encode_rotation(m)
elif m.shape == (3, 1):
return encode_point(m)
elif m.shape == (4, 1):
return encode_vector(m) if m[3] == 0 else encode_point(m)
else:
raise Exception(
'No encoding known for matrices of shape {}'.format(m.shape))
elif t == list or t == tuple:
if len(data) == 0:
raise Exception('Can not encode empty list.')
s_t = type(data[0])
if s_t is float or s_t is int or s_t is list or s_t is tuple:
return auto_encode(se.Matrix(data))
raise Exception('Can not encode list with inner type {}'.format(s_t))
elif t == pb.Transform:
out = PoseMsg()
out.position.x = data.origin.x
out.position.y = data.origin.y
out.position.z = data.origin.z
out.orientation.x = data.rotation.x
out.orientation.y = data.rotation.y
out.orientation.z = data.rotation.z
out.orientation.w = data.rotation.w
return out
elif t == pb.Quaternion:
out = QuaternionMsg()
out.x = data.x
out.y = data.y
out.z = data.z
out.w = data.w
return out
elif t == pb.Vector3:
out = PointMsg()
out.x = data.x
out.y = data.y
out.z = data.z
return out
else:
raise Exception('Can not encode data of type {}'.format(t))
