def get_marker_array_from_map(m): known_tile_types, known_sign_types = get_list_of_supported_types() marker_array = MarkerArray() marker_id = 0 # Add all tiles records = list(iterate_by_class(m, dw.Tile)) for record in records: tile = record.object matrix = record.transform_sequence.asmatrix2d().m translation, angle, scale = geo.translation_angle_scale_from_E2(matrix) if tile.kind not in known_tile_types: rospy.logerr("Unknown tile type: %s", tile.kind) marker = get_tile_marker(translation[0], translation[1], angle, tile.kind, marker_id, scale) rospy.loginfo("Created marker for %s", tile.kind) marker_array.markers.append(marker) marker_id += 1 marker_id = 0 # Add all signs records = list(iterate_by_class(m, dw.Sign)) for record in records: sign = record.object matrix = record.transform_sequence.asmatrix2d().m translation, angle, scale = geo.translation_angle_scale_from_E2(matrix) if sign.get_name_texture() not in known_sign_types: rospy.logerr("Unknown sign type: %s", sign.get_name_texture()) marker = get_sign_marker(translation[0], translation[1], angle, sign.get_name_texture(), marker_id, scale) rospy.loginfo("Created marker for %s", sign.get_name_texture()) marker_array.markers.append(marker) marker_id += 1 marker_id = 0 # Add all apriltags records = list(iterate_by_class(m, dw.tags_db.FloorTag)) for record in records: matrix = record.transform_sequence.asmatrix2d().m translation, angle, scale = geo.translation_angle_scale_from_E2(matrix) marker = get_apriltag_marker(translation[0], translation[1], angle, "apriltag", marker_id, scale) rospy.loginfo("Created marker for Apriltag-%s", record.fqn[0]) marker_array.markers.append(marker) marker_id += 1 return marker_array
def get_lane_poses(dw, q, tol=0.000001): from duckietown_world.geo.measurements_utils import iterate_by_class, IterateByTestResult from .lane_segment import LaneSegment from duckietown_world import TileCoords for it in iterate_by_class(dw, Tile): assert isinstance(it, IterateByTestResult), it assert isinstance(it.object, Tile), it.object tile = it.object tile_fqn = it.fqn tile_transform = it.transform_sequence for _ in tile_transform.transforms: if isinstance(_, TileCoords): tile_coords = _ break else: msg = 'Could not find tile coords in %s' % tile_transform assert False, msg # print('tile_transform: %s' % tile_transform.asmatrix2d().m) tile_relative_pose = relative_pose(tile_transform.asmatrix2d().m, q) p = translation_from_O3(tile_relative_pose) # print('tile_relative_pose: %s' % tile_relative_pose) if not tile.get_footprint().contains(p): continue nresults = 0 for it2 in iterate_by_class(tile, LaneSegment): lane_segment = it2.object lane_segment_fqn = tile_fqn + it2.fqn assert isinstance(lane_segment, LaneSegment), lane_segment lane_segment_wrt_tile = it2.transform_sequence.asmatrix2d() lane_segment_relative_pose = relative_pose(lane_segment_wrt_tile.m, tile_relative_pose) lane_segment_transform = TransformSequence( tile_transform.transforms + it2.transform_sequence.transforms) lane_pose = lane_segment.lane_pose_from_SE2( lane_segment_relative_pose, tol=tol) M = lane_segment_transform.asmatrix2d().m center_point = lane_pose.center_point.as_SE2() center_point_abs = np.dot(M, center_point) center_point_abs_t = Matrix2D(center_point_abs) if lane_pose.along_inside and lane_pose.inside and lane_pose.correct_direction: yield GetLanePoseResult( tile=tile, tile_fqn=tile_fqn, tile_transform=tile_transform, tile_relative_pose=Matrix2D(tile_relative_pose), lane_segment=lane_segment, lane_segment_relative_pose=Matrix2D( lane_segment_relative_pose), lane_pose=lane_pose, lane_segment_fqn=lane_segment_fqn, lane_segment_transform=lane_segment_transform, tile_coords=tile_coords, center_point=center_point_abs_t) nresults += 1
def get_tile_at_point(dw: DuckietownMap, q: SE2value) -> TileCoords: if not isinstance(q, np.ndarray): raise TypeError(type(q)) l = list(iterate_by_class(dw, Tile)) if not l: msg = "No tiles" raise ZValueError(msg, dw=dw) distances = {} for it in l: assert isinstance(it, IterateByTestResult), it assert isinstance(it.object, Tile), it.object tile = it.object tile_transform = it.transform_sequence for _ in tile_transform.transforms: if isinstance(_, TileCoords): tile_coords = _ break else: msg = "Could not find tile coords in %s" % tile_transform assert False, msg # print('tile_transform: %s' % tile_transform.asmatrix2d().m) tile_relative_pose = relative_pose(tile_transform.asmatrix2d().m, q) p = translation_from_O3(tile_relative_pose) footprint = tile.get_footprint() d = footprint.distance(p) distances[it.fqn] = d # print(f'tile_relative_pose: {tile_coords} {p} {d}') # if d > 0.001: if tile.get_footprint().contains(p, 0.001): return tile_coords raise NotInMap(q=q, distances=distances)
def get_marker_array_from_map(m): known_types = get_list_of_supported_types() marker_array = MarkerArray() marker_id = 0 # Add all tiles records = list(iterate_by_class(m, dw.Tile)) for record in records: # only first 15 tile = record.object matrix = record.transform_sequence.asmatrix2d().m translation, angle, scale = geo.translation_angle_scale_from_E2(matrix) if tile.kind not in known_types: rospy.logerr("Unknown tile type: %s", tile.kind) marker = get_tile_marker(translation[0], translation[1], angle, tile.kind, marker_id, scale) rospy.loginfo("Created marker for %s", tile.kind) marker_array.markers.append(marker) marker_id += 1 return marker_array
def get_skeleton_graph(po): """ Returns a graph with the lane segments of the map """ # Get all the LaneSegments root = PlacedObject() class MeetingPoint(object): def __init__(self): self.point = None self.incoming = set() self.outcoming = set() def __repr__(self): return 'MP(%d %d | %s, %s)' % (len( self.incoming), len( self.outcoming), self.incoming, self.outcoming) def discretize(tran): def D(x): return np.round(x, decimals=2) p, theta = geo.translation_angle_from_SE2(tran.as_SE2()) return D(p[0]), D(p[1]), D(np.cos(theta)), D(np.sin(theta)) meeting_points = defaultdict(MeetingPoint) for i, it in enumerate(iterate_by_class(po, LaneSegment)): lane_segment = it.object # lane_segment_fqn = it.fqn assert isinstance(lane_segment, LaneSegment), lane_segment absolute_pose = it.transform_sequence.asmatrix2d() lane_segment_transformed = transform_lane_segment( lane_segment, absolute_pose) identity = SE2Transform.identity() name = 'ls%03d' % i root.set_object(name, lane_segment_transformed, ground_truth=identity) p0 = discretize(lane_segment_transformed.control_points[0]) p1 = discretize(lane_segment_transformed.control_points[-1]) meeting_points[p0].point = lane_segment_transformed.control_points[0] meeting_points[p0].outcoming.add(name) meeting_points[p1].point = lane_segment_transformed.control_points[-1] meeting_points[p1].incoming.add(name) for k, mp in meeting_points.items(): if (len(mp.incoming) == 0) or (len(mp.outcoming) == 0): msg = 'Completeness assumption violated at point %s: %s' % (k, mp) raise Exception(msg) # compress the lanes which are contiguous aliases = {} created = {} def resolve_alias(x): return x if x not in aliases else resolve_alias(aliases[x]) for k, mp in list(meeting_points.items()): # continue if not (len(mp.incoming) == 1 and len(mp.outcoming) == 1): continue # not necessary anymore meeting_points.pop(k) lin_name = list(mp.incoming)[0] lout_name = list(mp.outcoming)[0] lin_name = resolve_alias(lin_name) lout_name = resolve_alias(lout_name) # print(' -> %s and %s meet at %s' % (lin_name, lout_name, mp)) # print('%s and %s meet at %s' % (lin_name, lout_name, k)) def get(it): if it in root.children: return root.children[it] else: return created[it] lin = get(lin_name) lout = get(lout_name) # name = 'alias%s' % (len(aliases)) # name = '%s-%s' % (lin_name, lout_name) name = 'L%d' % (len(created)) width = lin.width control_points = lin.control_points + lout.control_points[1:] ls = LaneSegment(width=width, control_points=control_points) created[name] = ls aliases[lin_name] = name aliases[lout_name] = name # print('new alias %s' % name) # # print('created: %s' % list(created)) # print('aliases: %s' % aliases) root2 = PlacedObject() for k, v in created.items(): if not k in aliases: root2.set_object(k, v, ground_truth=SE2Transform.identity()) for k, v in root.children.items(): if not k in aliases: root2.set_object(k, v, ground_truth=SE2Transform.identity()) import networkx as nx G = nx.MultiDiGraph() k2name = {} for i, (k, mp) in enumerate(meeting_points.items()): node_name = 'P%d' % i k2name[k] = node_name G.add_node(node_name, point=mp.point) ls2start = {} ls2end = {} for i, (k, mp) in enumerate(meeting_points.items()): node_name = k2name[k] for l in mp.incoming: ls2end[resolve_alias(l)] = node_name for l in mp.outcoming: ls2start[resolve_alias(l)] = node_name # print(ls2start) # print(ls2end) for l in ls2start: n1 = ls2start[l] n2 = ls2end[l] G.add_edge(n1, n2, lane=l) return SkeletonGraphResult(root=root, root2=root2, G=G)
def construct_map(yaml_data, tile_size): dm = DuckietownMap(tile_size) tiles = yaml_data['tiles'] assert len(tiles) > 0 assert len(tiles[0]) > 0 # Create the grid A = len(tiles) B = len(tiles[0]) tm = TileMap(H=B, W=A) templates = load_tile_types() for a, row in enumerate(tiles): if len(row) != B: msg = "each row of tiles must have the same length" raise ValueError(msg) # For each tile in this row for b, tile in enumerate(row): tile = tile.strip() if tile == 'empty': continue DEFAULT_ORIENT = 'E' # = no rotation if '/' in tile: kind, orient = tile.split('/') kind = kind.strip() orient = orient.strip() drivable = True elif '4' in tile: kind = '4way' # angle = 2 orient = DEFAULT_ORIENT drivable = True else: kind = tile # angle = 0 orient = DEFAULT_ORIENT drivable = False tile = Tile(kind=kind, drivable=drivable) if kind in templates: tile.set_object(kind, templates[kind], ground_truth=SE2Transform.identity()) else: pass # msg = 'Could not find %r in %s' % (kind, templates) # logger.debug(msg) tm.add_tile(b, (A - 1) - a, orient, tile) def go(obj_name, desc): obj = get_object(desc) transform = get_transform(desc, tm, tile_size) dm.set_object(obj_name, obj, ground_truth=transform) objects = yaml_data.get('objects', []) if isinstance(objects, list): for obj_idx, desc in enumerate(objects): kind = desc['kind'] obj_name = 'ob%02d-%s' % (obj_idx, kind) go(obj_name, desc) elif isinstance(objects, dict): for obj_name, desc in objects.items(): go(obj_name, desc) else: raise ValueError(objects) for it in list(iterate_by_class(tm, Tile)): ob = it.object if 'slots' in ob.children: slots = ob.children['slots'] for k, v in list(slots.children.items()): if not v.children: slots.remove_object(k) if not slots.children: ob.remove_object('slots') dm.set_object('tilemap', tm, ground_truth=Scale2D(tile_size)) return dm
def get_skeleton_graph(po: DuckietownMap) -> SkeletonGraphResult: """ Returns a graph with the lane segments of the map """ root = PlacedObject() meeting_points: Dict[str, MeetingPoint] = defaultdict(MeetingPoint) for i, it in enumerate(iterate_by_class(po, LaneSegment)): lane_segment = cast(LaneSegment, it.object) assert isinstance(lane_segment, LaneSegment), lane_segment absolute_pose = it.transform_sequence.asmatrix2d() lane_segment_transformed = transform_lane_segment(lane_segment, absolute_pose) identity = SE2Transform.identity() name = "ls%03d" % i root.set_object(name, lane_segment_transformed, ground_truth=identity) p0 = discretize(lane_segment_transformed.control_points[0]) p1 = discretize(lane_segment_transformed.control_points[-1]) if not p0 in meeting_points: meeting_points[p0] = MeetingPoint( set(), set(), set(), lane_segment_transformed.control_points[0], None, None, ) if not p1 in meeting_points: meeting_points[p1] = MeetingPoint( set(), set(), set(), lane_segment_transformed.control_points[-1], None, None, ) # meeting_points[p0].point = lane_segment_transformed.control_points[0] meeting_points[p0].outcoming.add(name) # meeting_points[p1].point = lane_segment_transformed.control_points[-1] meeting_points[p1].incoming.add(name) meeting_points[p0].connects_to.add(p1) tile_coords = [_ for _ in it.transform_sequence.transforms if isinstance(_, TileCoords)] if not tile_coords: raise ZException(p0=p0, p1=p1, transforms=it.transform_sequence.transforms) tile_coord = tile_coords[0] ij = tile_coord.i, tile_coord.j meeting_points[p0].into_tile = ij meeting_points[p1].from_tile = ij for k, mp in meeting_points.items(): if (len(mp.incoming) == 0) or (len(mp.outcoming) == 0): msg = "Completeness assumption violated at point %s: %s" % (k, mp) raise Exception(msg) G0 = graph_for_meeting_points(meeting_points) # compress the lanes which are contiguous aliases = {} created = {} def resolve_alias(x): return x if x not in aliases else resolve_alias(aliases[x]) for k, mp in list(meeting_points.items()): # continue if not (len(mp.incoming) == 1 and len(mp.outcoming) == 1): continue # not necessary anymore meeting_points.pop(k) lin_name = list(mp.incoming)[0] lout_name = list(mp.outcoming)[0] lin_name = resolve_alias(lin_name) lout_name = resolve_alias(lout_name) # print(' -> %s and %s meet at %s' % (lin_name, lout_name, mp)) # print('%s and %s meet at %s' % (lin_name, lout_name, k)) def get(it): if it in root.children: return root.children[it] else: return created[it] lin = get(lin_name) lout = get(lout_name) # name = 'alias%s' % (len(aliases)) # name = '%s-%s' % (lin_name, lout_name) name = "L%d" % (len(created)) width = lin.width control_points = lin.control_points + lout.control_points[1:] ls = LaneSegment(width=width, control_points=control_points) created[name] = ls aliases[lin_name] = name aliases[lout_name] = name # print('new alias %s' % name) # # print('created: %s' % list(created)) # print('aliases: %s' % aliases) root2 = PlacedObject() for k, v in created.items(): if not k in aliases: root2.set_object(k, v, ground_truth=SE2Transform.identity()) for k, v in root.children.items(): if not k in aliases: root2.set_object(k, v, ground_truth=SE2Transform.identity()) G = nx.MultiDiGraph() k2name = {} for i, (k, mp) in enumerate(meeting_points.items()): node_name = "P%d" % i k2name[k] = node_name G.add_node(node_name, point=mp.point) ls2start = {} ls2end = {} for i, (k, mp) in enumerate(meeting_points.items()): node_name = k2name[k] for l in mp.incoming: ls2end[resolve_alias(l)] = node_name for l in mp.outcoming: ls2start[resolve_alias(l)] = node_name # print(ls2start) # print(ls2end) for l in ls2start: n1 = ls2start[l] n2 = ls2end[l] G.add_edge(n1, n2, lane=l) return SkeletonGraphResult(root=root, root2=root2, G=G, G0=G0)