def get_tile_slots(): LM = 0.5 # half tile # tile_offset to = 0.20 # tile_curb tc = 0.05 positions = { 0: (+to, +tc), 1: (+tc, +to), 2: (-tc, +to), 3: (-to, +tc), 4: (-to, -tc), 5: (-tc, -to), 6: (+tc, -to), 7: (+to, -tc), } po = PlacedObject() for i, (x, y) in positions.items(): name = str(i) # if name in self.children: # continue sl = SignSlot() # theta = np.deg2rad(theta_deg) theta = 0 t = SE2Transform((-LM + x, -LM + y), theta) # noinspection PyTypeChecker po.set_object(name, sl, ground_truth=t) return po
def m1(): outdir = get_comptests_output_dir() gm = load_map("udem1") # dw = DuckietownWorld() # for map_name, tm in gym_maps.items(): # DW.root.set_object(map_name, tm) root = PlacedObject() world = PlacedObject() root.set_object("world", world) origin = SE2Transform([1, 10], np.deg2rad(10)) world.set_object("tile_map", gm, ground_truth=Constant[SE2Transform](origin)) # d = dw.as_json_dict() # print(json.dumps(d, indent=4)) # print(yaml.safe_dump(d, default_flow_style=False)) # G = get_meausurements_graph(root) fn = os.path.join(outdir, "out1.pdf") plot_measurement_graph(root, G, fn)
def create_lane_highlight(poses_sequence: SampledSequence, dw): def mapi(v): if isinstance(v, SE2Transform): return v.as_SE2() else: return v poses_sequence = poses_sequence.transform_values(mapi, np.ndarray) lane_pose_results = poses_sequence.transform_values( GetClosestLane(dw), object) visualization = PlacedObject() dw.set_object("visualization", visualization, ground_truth=SE2Transform.identity()) for i, (timestamp, name2pose) in enumerate(lane_pose_results): for name, lane_pose_result in name2pose.items(): assert isinstance(lane_pose_result, GetLanePoseResult) lane_segment = lane_pose_result.lane_segment rt = lane_pose_result.lane_segment_transform s = SampledSequence[Transform]([timestamp], [rt]) visualization.set_object("ls%s-%s-lane" % (i, name), lane_segment, ground_truth=s) p = SampledSequence[Transform]([timestamp], [lane_pose_result.center_point]) visualization.set_object("ls%s-%s-anchor" % (i, name), Anchor(), ground_truth=p) return lane_pose_results
def wb2(): root = PlacedObject() for map_name in list_maps(): tm = load_map(map_name) root.set_object(map_name, tm) d = root.as_json_dict() # print(json.dumps(d, indent=4)) # print(yaml.safe_dump(d, default_flow_style=False)) # print('------') r1 = Serializable.from_json_dict(d) d1 = r1.as_json_dict()
def wb1(): outdir = get_comptests_output_dir() root = PlacedObject() tile_map = create_map(H=3, W=3) world = PlacedObject() root.set_object('world', world) placement = Constant[SE2Transform](SE2Transform.identity()) world.set_object('map1', tile_map, ground_truth=placement) ego = PlacedObject() world_coordinates = Constant[SE2Transform](SE2Transform([0, 0], 0)) world.set_object('ego', ego, ground_truth=world_coordinates) d = root.as_json_dict() # print(json.dumps(DW.root.as_json_dict(), indent=4)) # print(yaml.safe_dump(d, default_flow_style=False)) # print('------') r1 = Serializable.from_json_dict(d) # print('read: %s' % r1) d1 = r1.as_json_dict()
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 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)