def from_yaml(cls, d0: EvaluationParametersDict) -> "EvaluationParameters":
d = dict(d0)
services_ = d.pop("services")
if not isinstance(services_, dict):
msg = "Expected dict"
raise ZValueError(msg, got=services_)
if not services_:
msg = "No services described."
raise ZValueError(msg)
version = d.pop("version", "3")
if d:
msg = "Extra field in the configuration"
raise ZValueError(msg, d0=d0, extra=list(d))
services = {}
for k, v in services_.items():
services[k] = ServiceDefinition.from_yaml(v)
# check that there is at least a service with the image called
# SUBMISSION_CONTAINER
n = 0
for service_definition in services.values():
if service_definition.image == ChallengesConstants.SUBMISSION_CONTAINER_TAG:
n += 1
# if n == 0:
# msg = 'I expect one of the services to have "image: %s".' % SUBMISSION_CONTAINER_TAG
# raise ValueError(msg)
# if n > 1:
# msg = 'Too many services with "image: %s".' % SUBMISSION_CONTAINER_TAG
# raise ValueError(msg)
return EvaluationParameters(services=services, version=version)
def __post_init__(self):
if self.order not in Score.ALLOWED:
msg = "Invalid value"
raise ZValueError(msg, order=self.order, allowed=Score.ALLOWED)
if self.discretization is not None:
discretization = float(self.discretization)
if discretization <= 0:
msg = "Need a strictly positive discretization"
raise ZValueError(msg, discretization=discretization)
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 __post_init__(self):
values = list(self.values)
timestamps = list(self.timestamps)
if len(timestamps) != len(values):
msg = "Length mismatch"
raise ZValueError(msg, timestamps=timestamps, values=values)
for t in timestamps:
if not isinstance(t, (float, int)):
msg = "I expected a number"
raise ZValueError(msg, t=t)
for i in range(len(timestamps) - 1):
dt = timestamps[i + 1] - timestamps[i]
if dt <= 0:
msg = f"Invalid dt = {dt} at i = {i}"
raise ZValueError(msg, timestamps=timestamps)
timestamps = list(map(Timestamp, timestamps))
self.timestamps = timestamps
self.values = values
def add(self, t: Timestamp, v: X):
# self.timestamps = self.timestamps or []
# self.values = self.values or []
# print(type(self), self.__dict__)
if self.timestamps:
if t == self.timestamps[-1]:
msg = "Repeated time stamp"
raise ZValueError(msg, t=t, timestamps=self.timestamps)
self.timestamps.append(t)
self.values.append(v)
def wrap_config_reader2(f, cls, data: dict, *args, **kwargs):
"""Decorator for a function that takes a (clsname, dict)"""
# def f2(x, *args, **kwargs):
if not isinstance(data, dict):
msg = "Expected dict"
raise ZValueError(msg, data=data)
def write(d: dict):
assert isinstance(d, dict)
# noinspection PyBroadException
try:
return json.dumps(d, indent=4)
except:
# try:
# return safe_yaml_dump(d, default_flow_style=False).encode('utf-8')
# except:
return str(d)
data2 = dict(**data)
try:
res = f(cls, data2, *args, **kwargs)
except KeyError as e:
msg = "Could not interpret the configuration data using %s:%s()" % (
cls.__name__,
f.__name__,
)
msg += '\nMissing configuration "%s". Specified: %s' % (e.args,
list(data))
msg += "\n\n" + indent(write(data), " > ") + "\n"
raise InvalidConfiguration(msg) from e
except InvalidConfiguration as e:
msg = "Could not interpret the configuration data using %s:%s()" % (
cls.__name__,
f.__name__,
)
msg += "\n\n" + indent(write(data), " > ") + "\n"
raise InvalidConfiguration(msg) from e
except BaseException as e:
msg = "Could not interpret the configuration data using %s:%s()" % (
cls.__name__,
f.__name__,
)
msg += "\n\n" + indent(write(data), " > ") + "\n"
# raise_wrapped(InvalidConfiguration, e, msg, compact=False)
raise InvalidConfiguration(msg) from e
#
# if data2:
# msg = "Unused fields %s " % list(data2)
# msg += "\n\n" + indent(write(data), " > ")
# raise InvalidConfiguration(msg)
return res
def f(ob: PlacedObject) -> PlacedObject:
# print(ob)
if hasattr(ob, "get_sampling_points"):
if not isinstance(ob, GenericSequence):
raise ZValueError(ob=ob)
sp = ob.get_sampling_points()
if sp == GenericSequence.CONTINUOUS:
pass
else:
points.update(sp)
# else:
# logger.info(f'not a seq: {type(ob)}')
return ob
def get_transform(desc: MapFormat1Object, W: int,
tile_size: float) -> SE2Transform:
rotate_deg = desc.get("rotate", 0)
rotate = np.deg2rad(rotate_deg)
if "pos" in desc:
pos = desc["pos"]
x = float(pos[0]) * tile_size
# account for non-righthanded
y = float(W - 1 - pos[1]) * tile_size
# account for non-righthanded
rotate = -rotate
transform = SE2Transform([x, y], rotate)
return transform
elif "pose" in desc:
# noinspection PyTypedDict
pose = Serializable.from_json_dict(desc["pose"])
return pose
elif "place" in desc:
# noinspection PyTypedDict
place = desc["place"]
tile_coords = tuple(place["tile"])
relative = Serializable.from_json_dict(place["relative"])
p, theta = relative.p, relative.theta
i, j = tile_coords
fx = (i + 0.5) * tile_size + p[0]
fy = (j + 0.5) * tile_size + p[1]
transform = SE2Transform([fx, fy], theta)
# logger.info(tile_coords=tile_coords, tile_size=tile_size, transform=transform)
return transform
elif "attach" in desc:
# noinspection PyTypedDict
attach = desc["attach"]
tile_coords = tuple(attach["tile"])
slot = str(attach["slot"])
x, y = get_xy_slot(slot)
i, j = tile_coords
u, v = (x + i) * tile_size, (y + j) * tile_size
transform = SE2Transform([u, v], rotate)
q = transform.as_SE2()
return SE2Transform.from_SE2(q)
else:
msg = "Cannot find positiong"
raise ZValueError(msg, desc=desc)
def read_rgba(fn: FilePath, resize: int) -> np.ndarray:
try:
im = Image.open(fn)
except Exception as e:
msg = f"Could not open filename {fn!r}"
raise ValueError(msg) from e
im = im.convert("RGBA")
im = im.resize((resize, resize))
data = np.array(im)
if data.ndim != 3:
raise ZValueError(fn=fn, shape=data.shape)
# assert data.ndim == 4
assert data.dtype == np.uint8
return data
def from_iterator(cls,
i: Iterator[X],
T: Type[X] = object) -> "SampledSequence[X]":
timestamps = []
values = []
for x in i:
if len(x) != 2:
raise ZValueError(x=x)
t = x[0]
v = x[1]
assert isinstance(t, (float, int)), type(t)
t = Timestamp(t)
timestamps.append(t)
values.append(v)
return SampledSequence[T](timestamps, values)
def dtserver_submit2(
*,
token: str,
challenges: List[ChallengeName],
data: SubmitDataDict,
impersonate: Optional[UserID] = None,
) -> Submit2ResponseDict:
if not isinstance(challenges, list):
msg = "Expected a list of strings"
raise ZValueError(msg, challenges=challenges)
endpoint = Endpoints.components
method = "POST"
data = {"challenges": challenges, "parameters": data}
add_impersonate_info(data, impersonate)
add_version_info(data)
return make_server_request(token, endpoint, data=data, method=method)
def check_loops(gltf: GLTF):
n = DiGraph()
all_nodes = list(range(len(gltf.model.nodes)))
for i in all_nodes:
n.add_node(i)
for node_index, node in enumerate(gltf.model.nodes):
if node.children:
for c in node.children:
n.add_edge(node_index, c)
try:
c = find_cycle(n)
except NetworkXNoCycle:
pass
else:
logger.info(c=c)
raise ZValueError("cycle found", c=c)
def sample_trees(po: dw.PlacedObject, tree_density: float,
tree_min_dist: float) -> List[SE2value]:
if tree_density == 0:
return []
def choose_grass(x: dw.PlacedObject) -> bool:
if isinstance(x, dw.Tile) and x.kind == "floor":
return True
else:
return False
def the_others(x: dw.PlacedObject) -> bool:
if isinstance(x, dw.Tile) and x.kind != "floor":
return True
else:
return False
choices = list(iterate_by_test(po, choose_grass))
non_grass = list(iterate_by_test(po, the_others))
# logger.info(choices=choices)
if not choices:
msg = "Cannot put trees in grass"
tiles = list(_.object.kind for _ in iterate_by_class(po, dw.Tile))
raise ZValueError(msg, tiles=tiles)
ntrees = int(len(choices) * tree_density)
results: List[SE2value] = []
def far_enough(pose_):
for p in results:
if distance_poses(p, pose_) < tree_min_dist:
return False
return True
options = list(range(len(choices)))
attitude = [None] * len(choices)
a: int
for a, c in enumerate(choices):
p1 = c.transform_sequence.asmatrix2d().m
min_distance = 100.0
sum_distances = 0.0
for k, s in enumerate(non_grass):
p2 = s.transform_sequence.asmatrix2d().m
d = distance_poses(p1, p2)
sum_distances += d
# if d < 0.4:
# logger.info(a=a, k=k, d=d)
# raise ZValueError()
min_distance = min(d, min_distance)
# noinspection PyTypeChecker
attitude[a] = 1.0 / sum_distances
# logger.info(attitude=attitude)
nplaced = [0] * len(choices)
for i in range(ntrees):
while True:
weights = 1.0 / (np.array(nplaced, dtype=float) * 3 + 1)
weights2 = weights * np.array(attitude)
weights3 = weights2 / np.sum(weights2)
chosen = choice(options, p=weights3)
itr = choices[chosen]
m1 = itr.transform_sequence.asmatrix2d().m
t = [random.uniform(-0.4, 0.4), random.uniform(-0.4, 0.4)]
pos_in_tile = SE2_from_translation_angle(t, 0.0)
pose = m1 @ pos_in_tile
if far_enough(pose):
results.append(pose)
nplaced[chosen] += 1
break
return results
def make_scenario(
yaml_str: str,
scenario_name: str,
only_straight: bool,
min_dist: float,
delta_y_m: float,
delta_theta_rad: float,
robots_pcs: List[RobotName],
robots_npcs: List[RobotName],
robots_parked: List[RobotName],
nduckies: int,
duckie_min_dist_from_other_duckie: float,
duckie_min_dist_from_robot: float,
tree_density: float,
tree_min_dist: float,
duckie_y_bounds: Sequence[float],
pc_robot_protocol: ProtocolDesc = PROTOCOL_NORMAL,
npc_robot_protocol: ProtocolDesc = PROTOCOL_FULL,
) -> Scenario:
yaml_data = yaml.load(yaml_str, Loader=yaml.SafeLoader)
if "objects" not in yaml_data:
yaml_data["objects"] = {}
objects = yaml_data["objects"]
if isinstance(objects, list):
raise ZValueError(yaml_data=yaml_data)
po = dw.construct_map(yaml_data)
num_pcs = len(robots_pcs)
num_npcs = len(robots_npcs)
num_parked = len(robots_parked)
nrobots = num_npcs + num_pcs + num_parked
all_robot_poses = sample_many_good_starting_poses(
po,
nrobots,
only_straight=only_straight,
min_dist=min_dist,
delta_theta_rad=delta_theta_rad,
delta_y_m=delta_y_m,
)
remaining_robot_poses = list(all_robot_poses)
poses_pcs = remaining_robot_poses[:num_pcs]
remaining_robot_poses = remaining_robot_poses[num_pcs:]
#
poses_npcs = remaining_robot_poses[:num_npcs]
remaining_robot_poses = remaining_robot_poses[num_npcs:]
#
poses_parked = remaining_robot_poses[:num_parked]
remaining_robot_poses = remaining_robot_poses[num_parked:]
assert len(remaining_robot_poses) == 0
COLORS_PLAYABLE = ["red", "green", "blue"]
COLOR_NPC = "blue"
COLOR_PARKED = "grey"
robots = {}
for i, robot_name in enumerate(robots_pcs):
pose = poses_pcs[i]
fpose = friendly_from_pose(pose)
velocity = FriendlyVelocity(0.0, 0.0, 0.0)
configuration = RobotConfiguration(fpose, velocity)
color = COLORS_PLAYABLE[i % len(COLORS_PLAYABLE)]
robots[robot_name] = ScenarioRobotSpec(
description=f"Playable robot {robot_name}",
controllable=True,
configuration=configuration,
# motion=None,
color=color,
protocol=pc_robot_protocol,
)
for i, robot_name in enumerate(robots_npcs):
pose = poses_npcs[i]
fpose = friendly_from_pose(pose)
velocity = FriendlyVelocity(0.0, 0.0, 0.0)
configuration = RobotConfiguration(fpose, velocity)
robots[robot_name] = ScenarioRobotSpec(
description=f"NPC robot {robot_name}",
controllable=True,
configuration=configuration,
color=COLOR_NPC,
protocol=npc_robot_protocol,
)
for i, robot_name in enumerate(robots_parked):
pose = poses_parked[i]
fpose = friendly_from_pose(pose)
velocity = FriendlyVelocity(0.0, 0.0, 0.0)
configuration = RobotConfiguration(fpose, velocity)
robots[robot_name] = ScenarioRobotSpec(
description=f"Parked robot {robot_name}",
controllable=False,
configuration=configuration,
# motion=MOTION_PARKED,
color=COLOR_PARKED,
protocol=None,
)
# logger.info(duckie_y_bounds=duckie_y_bounds)
names = [f"duckie{i:02d}" for i in range(nduckies)]
poses = sample_duckies_poses(
po,
nduckies,
robot_positions=all_robot_poses,
min_dist_from_other_duckie=duckie_min_dist_from_other_duckie,
min_dist_from_robot=duckie_min_dist_from_robot,
from_side_bounds=(duckie_y_bounds[0], duckie_y_bounds[1]),
delta_theta_rad=np.pi,
)
d = [ScenarioDuckieSpec("yellow", friendly_from_pose(_)) for _ in poses]
duckies = dict(zip(names, d))
trees = sample_trees(po, tree_density, tree_min_dist)
for i, pose_tree in enumerate(trees):
obname = f"tree_random_{i}"
st = dw.SE2Transform.from_SE2(pose_tree)
objects[obname] = {
"kind": "tree",
"pose": st.as_json_dict(),
"height": random.uniform(0.15, 0.4)
}
add_signs(po, objects)
yaml_str = yaml.dump(yaml_data)
# logger.info(trees=trees)
payload = {"info": "fill here the yaml"}
ms = Scenario(
payload_yaml=yaml.dump(payload),
scenario_name=scenario_name,
environment=yaml_str,
robots=robots,
duckies=duckies,
player_robots=list(robots_pcs),
)
return ms
def export_gltf(dm: DuckietownMap, outdir: str, background: bool = True):
gltf = GLTF()
# setattr(gltf, 'md52PV', {})
scene_index = add_scene(gltf, Scene(nodes=[]))
map_nodes = []
it: IterateByTestResult
tiles = list(iterate_by_class(dm, Tile))
if not tiles:
raise ZValueError("no tiles?")
for it in tiles:
tile = cast(Tile, it.object)
name = it.fqn[-1]
fn = tile.fn
fn_normal = tile.fn_normal
fn_emissive = tile.fn_emissive
fn_metallic_roughness = tile.fn_metallic_roughness
fn_occlusion = tile.fn_occlusion
material_index = make_material(
gltf,
doubleSided=False,
baseColorFactor=[1, 1, 1, 1.0],
fn=fn,
fn_normals=fn_normal,
fn_emissive=fn_emissive,
fn_metallic_roughness=fn_metallic_roughness,
fn_occlusion=fn_occlusion,
)
mi = get_square()
mesh_index = add_polygon(
gltf,
name + "-mesh",
vertices=mi.vertices,
texture=mi.textures,
colors=mi.color,
normals=mi.normals,
material=material_index,
)
node1_index = add_node(gltf, Node(mesh=mesh_index))
i, j = ij_from_tilename(name)
c = (i + j) % 2
color = [1, 0, 0, 1.0] if c else [0, 1, 0, 1.0]
add_back = False
if add_back:
material_back = make_material(gltf, doubleSided=False, baseColorFactor=color)
back_mesh_index = add_polygon(
gltf,
name + "-mesh",
vertices=mi.vertices,
texture=mi.textures,
colors=mi.color,
normals=mi.normals,
material=material_back,
)
flip = np.diag([1.0, 1.0, -1.0, 1.0])
flip[2, 3] = -0.01
back_index = add_node(gltf, Node(mesh=back_mesh_index, matrix=gm(flip)))
else:
back_index = None
tile_transform = it.transform_sequence
tile_matrix2d = tile_transform.asmatrix2d().m
s = dm.tile_size
scale = np.diag([s, s, s, 1])
tile_matrix = SE3_from_SE2(tile_matrix2d)
tile_matrix = tile_matrix @ scale @ SE3_rotz(-np.pi / 2)
tile_matrix_float = list(tile_matrix.T.flatten())
if back_index is not None:
children = [node1_index, back_index]
else:
children = [node1_index]
tile_node = Node(name=name, matrix=tile_matrix_float, children=children)
tile_node_index = add_node(gltf, tile_node)
map_nodes.append(tile_node_index)
if background:
bg_index = add_background(gltf)
add_node_to_scene(gltf, scene_index, bg_index)
exports = {
"Sign": export_sign,
# XXX: the tree model is crewed up
"Tree": export_tree,
# "Tree": None,
"Duckie": export_duckie,
"DB18": export_DB18,
# "Bus": export_bus,
# "Truck": export_truck,
# "House": export_house,
"TileMap": None,
"TrafficLight": export_trafficlight,
"LaneSegment": None,
"PlacedObject": None,
"DuckietownMap": None,
"Tile": None,
}
for it in iterate_by_class(dm, PlacedObject):
ob = it.object
K = type(ob).__name__
if isinstance(ob, Sign):
K = "Sign"
if K not in exports:
logger.warn(f"cannot convert {type(ob).__name__}")
continue
f = exports[K]
if f is None:
continue
thing_index = f(gltf, it.fqn[-1], ob)
tile_transform = it.transform_sequence
tile_matrix2d = tile_transform.asmatrix2d().m
tile_matrix = SE3_from_SE2(tile_matrix2d) @ SE3_rotz(np.pi / 2)
sign_node_index = add_node(gltf, Node(children=[thing_index]))
tile_matrix_float = list(tile_matrix.T.flatten())
tile_node = Node(name=it.fqn[-1], matrix=tile_matrix_float, children=[sign_node_index])
tile_node_index = add_node(gltf, tile_node)
map_nodes.append(tile_node_index)
mapnode = Node(name="tiles", children=map_nodes)
map_index = add_node(gltf, mapnode)
add_node_to_scene(gltf, scene_index, map_index)
# add_node_to_scene(model, scene_index, node1_index)
yfov = np.deg2rad(60)
camera = Camera(
name="perpcamera",
type="perspective",
perspective=PerspectiveCameraInfo(aspectRatio=4 / 3, yfov=yfov, znear=0.01, zfar=1000),
)
gltf.model.cameras.append(camera)
t = np.array([2, 2, 0.15])
matrix = look_at(pos=t, target=np.array([0, 2, 0]))
cam_index = add_node(gltf, Node(name="cameranode", camera=0, matrix=list(matrix.T.flatten())))
add_node_to_scene(gltf, scene_index, cam_index)
cleanup_model(gltf)
fn = os.path.join(outdir, "main.gltf")
make_sure_dir_exists(fn)
logger.info(f"writing to {fn}")
gltf.export(fn)
if True:
data = read_ustring_from_utf8_file(fn)
j = json.loads(data)
data2 = json.dumps(j, indent=2)
write_ustring_to_utf8_file(data2, fn)
fnb = os.path.join(outdir, "main.glb")
logger.info(f"writing to {fnb}")
gltf.export(fnb)
verify_trimesh = False
if verify_trimesh:
res = trimesh.load(fn)
# camera_pose, _ = res.graph['cameranode']
# logger.info(res=res)
import pyrender
scene = pyrender.Scene.from_trimesh_scene(res)
def get_interest_map(m: DuckietownMap, q: SE2value) -> DuckietownMap:
""" Returns the map of interest given a pose. """
components = get_map_components(m)
r = get_tile_at_point(m, q)
tile = r.i, r.j
for component_id, component in components.items():
if tile in component.tile_coords:
break
else:
msg = "Cannot find any component for this point."
raise ZValueError(msg, tile=tile, q=q, components=components)
tiles: Set[Tuple[int, int]] = component.tile_coords
tmin: Tuple[int, int] = min(tiles)
tmax: Tuple[int, int] = max(tiles)
M = 1
tmin = (tmin[0] - M, tmin[1] - M)
tmax = (tmax[0] + M, tmax[1] + M)
print(f"tmin tmax {tmin} {tmax}")
children2 = {}
for name, child in m.children.items():
if name == "tilemap":
tilemap = cast(TileMap, child)
tiles2 = {}
for tilename, tile in tilemap.children.items():
coords = ij_from_tilename(tilename)
if contained(tmin, coords, tmax):
tiles2[tilename] = tile
sr2 = {}
for sr_id, sr in list(tilemap.spatial_relations.items()):
first = sr.b[0]
if first in tiles2:
sr2[sr_id] = sr
tilemap2 = TileMap(H=tilemap.H,
W=tilemap.W,
children=tiles2,
spatial_relations=sr2)
children2[name] = tilemap2
else:
t: Transform = get_child_transform(m, name)
try:
tile_for_child = get_tile_at_point(m, t.as_SE2())
except NotInMap:
print(f"cutting {name}")
continue
c = tile_for_child.i, tile_for_child.j
if contained(tmin, c, tmax):
print(f"keep {name} {c}")
children2[name] = child
else:
print(f"cutting {name}")
# print(debug_print(children2))
sr2 = {}
for sr_id, sr in list(m.spatial_relations.items()):
first = sr.b[0]
if first in children2:
sr2[sr_id] = sr
return DuckietownMap(tile_size=m.tile_size,
children=children2,
spatial_relations=sr2)
