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_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 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 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))