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
def evaluate(self, agent, outdir, episodes=None):
"""
Evaluates the agent on the map inicialised in __init__
:param agent: Agent to be evaluated, passed to self._collect_trajectory(agent,...)
:param outdir: Directory for logged outputs (trajectory plots + numeric data)
:param episodes: Number of evaluation episodes, if None, it is determined based on self.start_poses
"""
if not os.path.exists(outdir):
os.makedirs(outdir)
if episodes is None:
episodes = len(self.start_poses.get(self.map_name, []))
totals = {}
for i in range(episodes):
episode_path, episode_orientations, episode_timestamps = self._collect_trajectory(
agent, i)
logger.info("Episode {}/{} sampling completed".format(
i + 1, episodes))
if len(episode_timestamps) <= 1:
continue
episode_path = np.stack(episode_path)
episode_orientations = np.stack(episode_orientations)
# Convert them to SampledSequences
transforms_sequence = []
for j in range(len(episode_path)):
transforms_sequence.append(
SE2Transform(episode_path[j], episode_orientations[j]))
transforms_sequence = SampledSequence.from_iterator(
enumerate(transforms_sequence))
transforms_sequence.timestamps = episode_timestamps
_outdir = outdir
if outdir is not None and episodes > 1:
_outdir = os.path.join(outdir, "Trajectory_{}".format(i + 1))
evaluated = self._eval_poses_sequence(transforms_sequence,
outdir=_outdir)
logger.info("Episode {}/{} plotting completed".format(
i + 1, episodes))
totals = self._extract_total_episode_eval_metrics(
evaluated, totals, display_outputs=True)
# Calculate the median total metrics
median_totals = {}
mean_totals = {}
stdev_totals = {}
for key, value in totals.items():
median_totals[key] = np.median(value)
mean_totals[key] = np.mean(value)
stdev_totals[key] = np.std(value)
# Save results to file
with open(os.path.join(outdir, "total_metrics.json"),
"w") as json_file:
json.dump(
{
'median_totals': median_totals,
'mean_totals': mean_totals,
'stdev_totals': stdev_totals,
'episode_totals': totals
},
json_file,
indent=2)
logger.info("\nMedian total metrics: \n {}".format(
pretty_print(median_totals)))
logger.info("\nMean total metrics: \n {}".format(
pretty_print(mean_totals)))
logger.info("\nStandard deviation of total metrics: \n {}".format(
pretty_print(stdev_totals)))
slot = int(slot)
rotation = rotation or 0
rotation = int(rotation)
# bug in the map file
# rotation = rotation + 90
sign = dict(kind=kind,
tag={
'~TagInstance':
dict(tag_id=int(tag_ID),
family='36h11',
size=APRIL_TAG_SIZE)
},
attach=dict(tile=[x, y], slot=slot))
sign['pose'] = SE2Transform([0, 0], np.deg2rad(rotation)).as_json_dict()
objects['tag%02d' % i] = sign
in_file = open("AprilTag position duckietown.csv", "rb")
reader = csv.reader(in_file)
data = list(reader)[1:]
CM = 0.01
origin = -5 * CM, -15.5 * CM
# Tag ID,Quadrant,x location,Y-location,Roation
# FAMILY = 'default'
# SIZE = 0.035
for entry in data:
tag_ID, quadrant, x, y, rotation = entry
x = float(x) + origin[0]