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)
