def test_from_two_vecs(self):
v1 = Vector3(100, 0, 0)
v2 = Vector3(1, 1, 0)
qt = Quaternion.from_two_vectors(v1, v2)
angle = qt.angular_distance(quaternion=Quaternion())
self.assertAlmostEqual(angle * 180.0 / math.pi, 45.0)
v3 = v1 * qt
v3.normalize()
v2.normalize()
self.assertTrue(numpy.isclose(v2.data(), v3.data()).all())
def test_euler(self):
angles = (0.1, 0.2, 0.3)
qt_ex = Quaternion.from_euler_extrinsic(*angles)
qt_in = Quaternion.from_euler_intrinsic(
angles=reversed(angles),
ordering=[Axis.AXIS_Z, Axis.AXIS_Y, Axis.AXIS_X])
self.assertTrue(numpy.isclose(qt_ex.coeffs(), qt_in.coeffs()).all())
euler = qt_ex.to_euler()
for i in range(3):
self.assertAlmostEqual(angles[i], euler[i])
def from_msg(cls, ros_msg):
# type: (genpy.Message) -> Transform
if isinstance(ros_msg, RosTransform):
return Transform(translation=Vector3.from_msg(ros_msg.translation),
rotation=Quaternion.from_msg(ros_msg.rotation))
elif isinstance(ros_msg, RosPose):
return Transform(translation=Vector3.from_msg(ros_msg.position),
rotation=Quaternion.from_msg(ros_msg.orientation))
else:
raise NotImplementedError(
'Unknown message type to convert to math6D.Transform: {}'.
format(type(ros_msg)))
def test_transform_matrix(self):
t = Transform(Vector3(1, 2, 3),
Quaternion.from_euler_extrinsic(0.1, 0.2, 0.3))
m = t.matrix()
t2 = Transform.from_matrix(m)
m2 = t2.matrix()
self.assertTrue(numpy.isclose(m, m2).all())
def test_from_xy(self):
random_qt = Quaternion.from_euler_extrinsic(0.2, 0.3, 0.9)
x_vec = Vector3(1, 0, 0) * random_qt
y_vec = Vector3(0, 1, 0) * random_qt
z_vec = Vector3(0, 0, 1) * random_qt
m = numpy.identity(3)
m[:, 0] = x_vec.data()
m[:, 1] = y_vec.data()
m[:, 2] = z_vec.data()
m = numpy.matrix(m)
new_q = Quaternion.from_matrix(m)
self.assertTrue(
numpy.isclose(new_q.coeffs(), random_qt.coeffs()).all())
def from_matrix(cls, matrix):
# type: (numpy.matrix) -> Transform
# Check matrix properties
if matrix.shape != (4, 4):
raise ValueError(
'Invalid matrix shape: Input must be a 4x4 matrix')
if not numpy.isclose(numpy.linalg.det(matrix), 1.0):
raise ValueError('Matrix determinant must be +1.0')
q = Quaternion.from_matrix(matrix[0:3, 0:3])
t = Vector3(matrix[0:3, 3].flatten())
return Transform(t, q)
def __init__(self, *args, **kwargs):
self.__rotation = None
self.__translation = None
if len(args) == 0 and len(kwargs) == 0:
self.__rotation = Quaternion()
self.__translation = Vector3()
elif len(args) == 1:
_arg = args[0]
if isinstance(_arg, genpy.Message):
t = Transform.from_msg(_arg)
self.__rotation = t.rotation
self.__translation = t.translation
else:
raise NotImplementedError(
'Expected Message type, got: {}'.format(type(_arg)))
elif len(args) + len(kwargs) == 2:
if len(args) == 2:
_tr = args[0]
_rot = args[1]
else:
_tr = kwargs[
self.
TRANSLATION_KW] if self.TRANSLATION_KW in kwargs else Vector3(
)
_rot = kwargs[
self.
ROTATION_KW] if self.ROTATION_KW in kwargs else Quaternion(
)
if isinstance(_tr, RosVector3):
self.__translation = Vector3().from_msg(_tr)
elif isinstance(_tr, Vector3):
self.__translation = _tr
else:
self.__translation = Vector3(_tr)
if isinstance(_rot, RosQuaternion):
self.__rotation = Quaternion().from_msg(_rot)
elif isinstance(_rot, Quaternion):
self.__rotation = _rot
else:
self.__rotation = Quaternion(_rot)
else:
raise NotImplementedError(
'Unexpected input args of len: {}'.format(
len(args) + len(kwargs)))
class Transform(MsgConversion):
"""
Isometric Transform
"""
TRANSLATION_KW = 'translation'
ROTATION_KW = 'rotation'
def __init__(self, *args, **kwargs):
self.__rotation = None
self.__translation = None
if len(args) == 0 and len(kwargs) == 0:
self.__rotation = Quaternion()
self.__translation = Vector3()
elif len(args) == 1:
_arg = args[0]
if isinstance(_arg, genpy.Message):
t = Transform.from_msg(_arg)
self.__rotation = t.rotation
self.__translation = t.translation
else:
raise NotImplementedError(
'Expected Message type, got: {}'.format(type(_arg)))
elif len(args) + len(kwargs) == 2:
if len(args) == 2:
_tr = args[0]
_rot = args[1]
else:
_tr = kwargs[
self.
TRANSLATION_KW] if self.TRANSLATION_KW in kwargs else Vector3(
)
_rot = kwargs[
self.
ROTATION_KW] if self.ROTATION_KW in kwargs else Quaternion(
)
if isinstance(_tr, RosVector3):
self.__translation = Vector3().from_msg(_tr)
elif isinstance(_tr, Vector3):
self.__translation = _tr
else:
self.__translation = Vector3(_tr)
if isinstance(_rot, RosQuaternion):
self.__rotation = Quaternion().from_msg(_rot)
elif isinstance(_rot, Quaternion):
self.__rotation = _rot
else:
self.__rotation = Quaternion(_rot)
else:
raise NotImplementedError(
'Unexpected input args of len: {}'.format(
len(args) + len(kwargs)))
#
# Properties
#
@property
def translation(self):
# type: () -> Vector3
return self.__translation
@property
def rotation(self):
# type: () -> Quaternion
return self.__rotation
@translation.setter
def translation(self, translation):
# type: (Vector3) -> None
self.__translation = translation
@rotation.setter
def rotation(self, rotation):
# type: (Quaternion) -> None
self.__rotation = rotation
#
# Transform Methods
#
def invert(self):
self.__rotation.invert()
self.__translation = -(self.__rotation * self.__translation)
def inverse(self):
# type: () -> Transform
t = copy.deepcopy(self)
t.invert()
return t
def matrix(self):
# type: () -> numpy.matrix
m = numpy.identity(4, dtype=numpy.float64)
m[:3, :3] = self.rotation.matrix()
m[:3, 3] = self.translation.data()
return numpy.asmatrix(m)
def dist_squared(self, other):
"""Return the square of the metric distance, as the unweighted sum of
linear and angular distance, to the 'other' transform. Note
that the units and scale among linear and angular
representations matters heavily.
"""
return self.translation.dist_squared(
other.translation) + self.rotation.angular_distance(
other.rotation)**2
def dist(self, other):
"""Return the metric distance, as unweighted combined linear and
angular distance, to the 'other' transform. Note that the
units and scale among linear and angular representations
matters heavily.
"""
return numpy.sqrt(self.dist_squared(other))
#
# Static Methods
#
@classmethod
def identity(cls):
# type: () -> Transform
return Transform()
@classmethod
def from_matrix(cls, matrix):
# type: (numpy.matrix) -> Transform
# Check matrix properties
if matrix.shape != (4, 4):
raise ValueError(
'Invalid matrix shape: Input must be a 4x4 matrix')
if not numpy.isclose(numpy.linalg.det(matrix), 1.0):
raise ValueError('Matrix determinant must be +1.0')
q = Quaternion.from_matrix(matrix[0:3, 0:3])
t = Vector3(matrix[0:3, 3].flatten())
return Transform(t, q)
#
# Operators
#
def __mul__(self, other):
if isinstance(other, Transform):
t = self.translation + (self.rotation * other.translation)
q = self.rotation * other.rotation
return Transform(t, q)
elif isinstance(other, Vector3):
v = numpy.ones(4)
v[:3] = other.data()
return Vector3(numpy.dot(self.matrix(), v)[:3])
else:
raise NotImplementedError('Cannot multiply type of: {}'.format(
type(other)))
#
# Printing
#
def __repr__(self):
return 'T[tran={}, rot={}]'.format(self.translation, self.rotation)
def __str__(self):
return self.__repr__()
#
# ROS compatibility
#
@classmethod
def from_msg(cls, ros_msg):
# type: (genpy.Message) -> Transform
if isinstance(ros_msg, RosTransform):
return Transform(translation=Vector3.from_msg(ros_msg.translation),
rotation=Quaternion.from_msg(ros_msg.rotation))
elif isinstance(ros_msg, RosPose):
return Transform(translation=Vector3.from_msg(ros_msg.position),
rotation=Quaternion.from_msg(ros_msg.orientation))
else:
raise NotImplementedError(
'Unknown message type to convert to math6D.Transform: {}'.
format(type(ros_msg)))
def to_msg(self, message_type=RosTransform):
# type: (type) -> typing.Union[RosTransform, RosPose]
if message_type == RosTransform:
return RosTransform(
translation=self.__translation.to_msg(RosVector3),
rotation=self.__rotation.to_msg(RosQuaternion))
elif message_type == RosPose:
return RosPose(position=self.__translation.to_msg(RosPoint),
orientation=self.__rotation.to_msg(RosQuaternion))
else:
raise NotImplementedError(
'Unknown target msg type: {}'.format(message_type))
def test_quaternion_inverse(self):
qt = Quaternion(1, 0, 0, 0)
qt_inv = qt.inverse()
self.assertTrue(numpy.isclose(qt.coeffs(), qt_inv.coeffs()).all())
def generate_cartographer_map(cartographer_configuration_directory,
world_sdf_path,
resolution=0.02,
map_origin=(-30, -10),
map_size=(64, 64),
submap_locations=((30, 22, 64, 32), (46, 22, 64,
32))):
assert os.path.isdir(cartographer_configuration_directory)
assert os.path.isfile(world_sdf_path)
temp_png_f = tempfile.NamedTemporaryFile()
temp_pb_f = tempfile.NamedTemporaryFile()
cmd = [
'rosrun',
'cartographer_ros',
'sdf_to_pbstream',
'--configuration_directory',
cartographer_configuration_directory,
'--pbstream',
temp_pb_f.name,
'--map_png',
temp_png_f.name,
'--world_sdf',
world_sdf_path,
'--map_origin', # bottom left corner position
'{},{}'.format(*map_origin),
'--map_size',
'{},{}'.format(*map_size)
]
for s in submap_locations:
cmd += ['--submap_location', ','.join([str(_s) for _s in s])]
str_cmd = ' '.join(cmd)
subprocess.check_call(str_cmd, shell=True, stderr=subprocess.STDOUT)
temp_pb_f.seek(0)
pb_bytes = temp_pb_f.read()
temp_png_f.seek(0)
im_bytes = temp_png_f.read()
xml_file = open(world_sdf_path, 'r')
sdf_xml = xml_file.read()
parser = etree.XMLParser(remove_blank_text=True)
original_xml_element = etree.XML(sdf_xml, parser=parser)
world = original_xml_element.find('world')
#
# Extract the zones
#
zones = []
for model in world.findall('model'):
links = model.findall('link')
for link in links:
layer = (link.attrib['layer']
if 'layer' in link.attrib else '').lower()
zone_type = get_zone_type(layer)
if zone_type != Zone.UNKNOWN:
collision = link.find('visual')
geometry = collision.find('geometry')
polyline = geometry.find('polyline')
points = [[float(t) for t in point.text.split(' ')]
for point in polyline.findall('point')]
zones.append(
Zone(name=link.attrib['name']
if 'name' in link.attrib else layer,
zone_type=zone_type,
polygon=PolygonMsg(points=[
Point32Msg(point[0], point[1], 0)
for point in points
])))
#
# Extract the path nodes
#
nodes = []
for model in world.findall('model'):
for node in model.findall('node'):
position = node.find('position')
node_id = node.attrib['id']
points = [float(t) for t in position.text.split(' ')]
nodes.append(Node(id=node_id, x=points[0], y=points[1]))
#
# Extract the paths
#
paths = []
for model in world.findall('model'):
for path in model.findall('path'):
node_ids = [ni.text for ni in path.findall('node')]
paths.append(Path(
name='',
nodes=node_ids,
))
#
# Extract the markers
#
markers = []
for model in world.findall('model'):
links = model.findall('link')
for link in links:
layer = (link.attrib['layer']
if 'layer' in link.attrib else '').lower()
if layer == 'marker':
pose = link.find('pose')
values = [float(f) for f in pose.text.split(' ')]
if len(values) != 6:
raise Exception('Invalid Pose: {}'.format(values))
qt = Quaternion.from_euler_extrinsic(*values[3:])
markers.append(
Marker(name=link.attrib['name']
if 'name' in link.attrib else layer,
marker_type=0,
pose=PoseMsg(position=PointMsg(
values[0], values[1], values[2]),
orientation=QuaternionMsg(x=qt.x,
y=qt.y,
z=qt.z,
w=qt.w))))
#
# Save the map
#
add_map_srv = rospy.ServiceProxy(name='/map_manager/add_map',
service_class=AddMap)
add_map_srv.wait_for_service(timeout=10)
add_response = add_map_srv.call(
AddMapRequest(map=Map(info=MapInfo(
name='sim_map',
meta_data=MapMetaData(
resolution=resolution,
width=map_size[0] / resolution,
height=map_size[1] / resolution,
origin=PoseMsg(
position=PointMsg(x=map_origin[0], y=map_origin[1])))),
markers=markers,
zones=zones,
paths=paths,
nodes=nodes,
default_zone=Zone.EXCLUSION_ZONE),
occupancy_grid=CompressedImage(format='png',
data=im_bytes),
map_data=pb_bytes)) # type: AddMapResponse
if not add_response.success:
raise Exception('Failed to save map: {}'.format(add_response.message))