def get_flattened_measurement_graph(po, include_root_to_self=False):
G = get_meausurements_graph(po)
G2 = nx.DiGraph()
root_name = ()
for name in G.nodes():
if name == root_name:
continue
path = nx.shortest_path(G, (), name)
transforms = []
for i in range(len(path) - 1):
a = path[i]
b = path[i + 1]
edges = G.get_edge_data(a, b)
k = list(edges)[0]
v = edges[k]
sr = v['attr_dict']['sr'].transform
transforms.append(sr)
from duckietown_world import TransformSequence
if any(isinstance(_, Sequence) for _ in transforms):
res = VariableTransformSequence(transforms)
else:
res = TransformSequence(transforms)
G2.add_edge(root_name, name, transform_sequence=res)
if include_root_to_self:
from duckietown_world import SE2Transform
transform_sequence = SE2Transform.identity()
G2.add_edge(root_name, root_name, transform_sequence=transform_sequence)
return G2
def __init__(self, children=None, spatial_relations=None):
children = children or {}
spatial_relations = spatial_relations or {}
self.children = children
for k, v in list(spatial_relations.items()):
from .transforms import Transform
if isinstance(v, Transform):
if k in self.children:
sr = GroundTruth(a=(), b=(k, ), transform=v)
spatial_relations[k] = sr
else:
msg = 'What is the "%s" referring to?' % k
raise ValueError(msg)
self.spatial_relations = spatial_relations
if not spatial_relations:
for child in self.children:
from duckietown_world import SE2Transform
sr = GroundTruth(a=(),
b=(child, ),
transform=SE2Transform.identity())
self.spatial_relations[child] = sr
def __post_init__(self):
from .transforms import Transform
for k, v in list(self.spatial_relations.items()):
if isinstance(v, Transform):
if k in self.children:
b: Tuple[str, ...] = (k,)
sr = GroundTruth(a=root, b=b, transform=v)
self.spatial_relations[k] = sr
else:
msg = f'What is the "{k}" referring to?'
raise ValueError(msg)
if not self.spatial_relations:
for child in self.children:
from duckietown_world import SE2Transform
sr = GroundTruth(a=root, b=(child,), transform=SE2Transform.identity())
self.spatial_relations[child] = sr
def interpret_map_data(data):
tiles = 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
if '/' in tile:
kind, orient = tile.split('/')
kind = kind.strip()
orient = orient.strip()
# angle = ['S', 'E', 'N', 'W'].index(orient)
drivable = True
elif '4' in tile:
kind = '4way'
# angle = 2
orient = 'N'
drivable = True
else:
kind = tile
# angle = 0
orient = 'S'
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)
# if drivable:
# tile['curves'] = self._get_curve(i, j)
# self.drivable_tiles.append(tile)
#
# self._load_objects(self.map_data)
#
# # Get the starting tile from the map, if specified
# self.start_tile = None
# if 'start_tile' in self.map_data:
# coords = self.map_data['start_tile']
# self.start_tile = self._get_tile(*coords)
# # Arrays for checking collisions with N static objects
# # (Dynamic objects done separately)
# # (N x 2): Object position used in calculating reward
# self.collidable_centers = []
#
# # (N x 2 x 4): 4 corners - (x, z) - for object's boundbox
# self.collidable_corners = []
#
# # (N x 2 x 2): two 2D norms for each object (1 per axis of boundbox)
# self.collidable_norms = []
#
# # (N): Safety radius for object used in calculating reward
# self.collidable_safety_radii = []
# For each object
for obj_idx, desc in enumerate(data.get('objects', [])):
kind = desc['kind']
pos = desc['pos']
rotate = desc['rotate']
transform = SE2Transform([float(pos[0]), float(tm.W - pos[1])], rotate)
# x, z = pos[0:2]
#
# i = int(np.floor(x))
# j = int(np.floor(z))
# dx = x - i
# dy = z - j
#
# z = pos[2] if len(pos) == 3 else 0.0
# optional = desc.get('optional', False)
# height = desc.get('height', None)
# pos = ROAD_TILE_SIZE * np.array((x, y, z))
# Load the mesh
# mesh = ObjMesh.get(kind)
# TODO
# if 'height' in desc:
# scale = desc['height'] / mesh.max_coords[1]
# else:
# scale = desc['scale']
# assert not ('height' in desc and 'scale' in desc), "cannot specify both height and scale"
# static = desc.get('static', True)
# obj_desc = {
# 'kind': kind,
# # 'mesh': mesh,
# 'pos': pos,
# 'scale': scale,
# 'y_rot': rotate,
# 'optional': optional,
# 'static': static,
# }
if kind == "trafficlight":
status = Constant("off")
obj = TrafficLight(status)
else:
kind2klass = {
'duckie': Duckie,
'cone': Cone,
'barrier': Barrier,
'building': Building,
'duckiebot': DB18,
'sign_left_T_intersect': SignLeftTIntersect,
'sign_right_T_intersect': SignRightTIntersect,
'sign_T_intersect': SignTIntersect,
'sign_4_way_intersect': Sign4WayIntersect,
'sign_t_light_ahead': SingTLightAhead,
'sign_stop': SignStop,
'tree': Tree,
'house': House,
'bus': Bus,
'truck': Truck,
}
if kind in kind2klass:
klass = kind2klass[kind]
obj = klass()
else:
logger.debug('Do not know special kind %s' % kind)
obj = GenericObject(kind=kind)
obj_name = 'ob%02d-%s' % (obj_idx, kind)
# tile = tm[(i, j)]
# transform = TileRelativeTransform([dx, dy], z, rotate)
tm.set_object(obj_name, obj, ground_truth=transform)
# obj = None
# if static:
# if kind == "trafficlight":
# obj = TrafficLightObj(obj_desc, self.domain_rand, SAFETY_RAD_MULT)
# else:
# obj = WorldObj(obj_desc, self.domain_rand, SAFETY_RAD_MULT)
# else:
# obj = DuckieObj(obj_desc, self.domain_rand, SAFETY_RAD_MULT, ROAD_TILE_SIZE)
#
# self.objects.append(obj)
# Compute collision detection information
# angle = rotate * (math.pi / 180)
# Find drivable tiles object could intersect with
return tm