def sample_good_starting_pose(m: PlacedObject, only_straight: bool, along_lane: float) -> geo.SE2value:
""" Samples a good starting pose on a straight lane """
choices = list(iterate_by_class(m, LaneSegment))
if only_straight:
choices = [_ for _ in choices if is_straight(_)]
choice = random.choice(choices)
ls: LaneSegment = choice.object
lp: LanePose = ls.lane_pose(along_lane, 0.0, 0.0)
rel: SE2Transform = ls.SE2Transform_from_lane_pose(lp)
m1 = choice.transform_sequence.asmatrix2d().m
m2 = rel.asmatrix2d().m
g = np.dot(m1, m2)
t, a, s = geo.translation_angle_scale_from_E2(g)
g = geo.SE2_from_translation_angle(t, a)
return g
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)