def __init__(self, width, control_points, *args, **kwargs):
PlacedObject.__init__(self, *args, **kwargs)
self.width = width
self.control_points = control_points
for p in control_points:
check_isinstance(p, SE2Transform)
def get_tile_slots():
LM = 0.5 # half tile
# tile_offset
to = 0.20
# tile_curb
tc = 0.05
positions = {
0: (+to, +tc),
1: (+tc, +to),
2: (-tc, +to),
3: (-to, +tc),
4: (-to, -tc),
5: (-tc, -to),
6: (+tc, -to),
7: (+to, -tc),
}
po = PlacedObject()
for i, (x, y) in positions.items():
name = str(i)
# if name in self.children:
# continue
sl = SignSlot()
# theta = np.deg2rad(theta_deg)
theta = 0
t = SE2Transform((-LM + x, -LM + y), theta)
# noinspection PyTypeChecker
po.set_object(name, sl, ground_truth=t)
return po
def __init__(self, width, length, height, *args, color: str = None, **kwargs):
# noinspection PyArgumentList
PlacedObject.__init__(self, *args, **kwargs)
self.width = width
self.height = height
self.length = length
self.color = color or "red"
def __init__(self,
width,
length,
height,
*args,
color: str = None,
**kwargs):
PlacedObject.__init__(self, *args, **kwargs)
self.width = width
self.height = height
self.length = length
self.color = color or "red"
def wb2():
root = PlacedObject()
for map_name in list_maps():
tm = load_map(map_name)
root.set_object(map_name, tm)
d = root.as_json_dict()
# print(json.dumps(d, indent=4))
# print(yaml.safe_dump(d, default_flow_style=False))
# print('------')
r1 = Serializable.from_json_dict(d)
d1 = r1.as_json_dict()
def __init__(self, kind, drivable, **kwargs):
PlacedObject.__init__(self, **kwargs)
self.kind = kind
self.drivable = drivable
from duckietown_world.world_duckietown.map_loading import get_texture_file
try:
self.fn = get_texture_file(kind)
except KeyError:
msg = 'Cannot find texture for %s' % kind
logger.warning(msg)
self.fn = None
def m1():
outdir = get_comptests_output_dir()
gm = load_map("udem1")
# dw = DuckietownWorld()
# for map_name, tm in gym_maps.items():
# DW.root.set_object(map_name, tm)
root = PlacedObject()
world = PlacedObject()
root.set_object("world", world)
origin = SE2Transform([1, 10], np.deg2rad(10))
world.set_object("tile_map",
gm,
ground_truth=Constant[SE2Transform](origin))
# d = dw.as_json_dict()
# print(json.dumps(d, indent=4))
# print(yaml.safe_dump(d, default_flow_style=False))
#
G = get_meausurements_graph(root)
fn = os.path.join(outdir, "out1.pdf")
plot_measurement_graph(root, G, fn)
def __init__(self, kind, drivable, **kwargs):
# noinspection PyArgumentList
PlacedObject.__init__(self, **kwargs)
self.kind = kind
self.drivable = drivable
self.fn_emissive = get_if_exists(self.style, kind, "emissive")
self.fn_normal = get_if_exists(self.style, kind, "normals")
self.fn = get_if_exists(self.style, kind, "texture")
self.fn_metallic_roughness = get_if_exists(self.style, kind, "metallic_roughness")
self.fn_occlusion = get_if_exists(self.style, kind, "occlusion")
if not "slots" in self.children:
slots = get_tile_slots()
# noinspection PyTypeChecker
self.set_object("slots", slots, ground_truth=SE2Transform.identity())
def __init__(self, width, control_points, *args, **kwargs):
PlacedObject.__init__(self, *args, **kwargs)
self.width = float(width)
self.control_points = control_points
for p in control_points:
check_isinstance(p, SE2Transform)
for i in range(len(control_points) - 1):
a = control_points[i]
b = control_points[i + 1]
ta, _ = geo.translation_angle_from_SE2(a.as_SE2())
tb, _ = geo.translation_angle_from_SE2(b.as_SE2())
d = np.linalg.norm(ta - tb)
if d < 0.001:
msg = 'Two points are two close: \n%s\n%s' % (a, b)
raise ValueError(msg)
def __init__(self, kind, drivable, **kwargs):
# noinspection PyArgumentList
PlacedObject.__init__(self, **kwargs)
self.kind = kind
self.drivable = drivable
from duckietown_world.world_duckietown.map_loading import get_texture_file
try:
self.fn = get_texture_file(kind)
except KeyError as e:
msg = f"Cannot find texture for tile of type {kind}"
logger.warning(msg, e=e)
self.fn = None
# if kind in ['asphalt']:
if not "slots" in self.children:
slots = get_tile_slots()
self.set_object("slots", slots, ground_truth=SE2Transform.identity())
def __init__(self, kind, drivable, **kwargs):
PlacedObject.__init__(self, **kwargs)
self.kind = kind
self.drivable = drivable
from duckietown_world.world_duckietown.map_loading import get_texture_file
try:
self.fn = get_texture_file(kind)
except KeyError:
msg = 'Cannot find texture for %s' % kind
logger.warning(msg)
self.fn = None
# if kind in ['asphalt']:
if not 'slots' in self.children:
slots = get_tile_slots()
self.set_object('slots',
slots,
ground_truth=SE2Transform.identity())
def create_lane_highlight(poses_sequence: SampledSequence, dw):
def mapi(v):
if isinstance(v, SE2Transform):
return v.as_SE2()
else:
return v
poses_sequence = poses_sequence.transform_values(mapi, np.ndarray)
lane_pose_results = poses_sequence.transform_values(
GetClosestLane(dw), object)
visualization = PlacedObject()
dw.set_object("visualization",
visualization,
ground_truth=SE2Transform.identity())
for i, (timestamp, name2pose) in enumerate(lane_pose_results):
for name, lane_pose_result in name2pose.items():
assert isinstance(lane_pose_result, GetLanePoseResult)
lane_segment = lane_pose_result.lane_segment
rt = lane_pose_result.lane_segment_transform
s = SampledSequence[Transform]([timestamp], [rt])
visualization.set_object("ls%s-%s-lane" % (i, name),
lane_segment,
ground_truth=s)
p = SampledSequence[Transform]([timestamp],
[lane_pose_result.center_point])
visualization.set_object("ls%s-%s-anchor" % (i, name),
Anchor(),
ground_truth=p)
return lane_pose_results
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 __init__(self, status=None, **kwargs):
if status is None:
status = Constant("off")
PlacedObject.__init__(self, **kwargs)
self.status = status
def draw_static(
root: PlacedObject,
output_dir: str,
pixel_size: Tuple[int, int] = (480, 480),
area=None,
images=None,
timeseries=None,
height_of_stored_images: Optional[int] = None,
main_robot_name: Optional[str] = None,
) -> Sequence[str]:
from duckietown_world.world_duckietown import get_sampling_points, ChooseTime
images = images or {}
timeseries = timeseries or {}
if not os.path.exists(output_dir):
os.makedirs(output_dir)
fn_svg = os.path.join(output_dir, "drawing.svg")
fn_html = os.path.join(output_dir, "drawing.html")
timestamps = get_sampling_points(root)
# logger.info(f'timestamps: {timestamps}')
if len(timestamps) == 0:
keyframes = SampledSequence[Timestamp]([0], [0])
else:
keyframes = SampledSequence[Timestamp](range(len(timestamps)),
timestamps)
# nkeyframes = len(keyframes)
if area is None:
areas = []
all_keyframes = keyframes.values
keyframes_for_extent = [all_keyframes[0], all_keyframes[-1]]
for t in keyframes_for_extent:
root_t = root.filter_all(ChooseTime(t))
# print(i, root_t)
rarea = get_extent_points(root_t)
areas.append(rarea)
area = reduce(RectangularArea.join, areas)
logger.info("area: %s" % area)
drawing, base = get_basic_upright2(fn_svg, area, pixel_size)
# drawing.add(drawing.defs())
gmg = drawing.g()
base.add(gmg)
static, dynamic = get_static_and_dynamic(root)
t0 = keyframes.values[0]
root_t0 = root.filter_all(ChooseTime(t0))
g_static = drawing.g()
g_static.attribs["class"] = "static"
draw_recursive(drawing, root_t0, g_static, draw_list=static)
base.add(g_static)
obs_div = Tag(name="div")
imagename2div = {}
for name in images:
imagename2div[name] = Tag(name="div")
obs_div.append(imagename2div[name])
# logger.debug('dynamic: %s' % dynamic)
for i, t in keyframes:
g_t = drawing.g()
g_t.attribs["class"] = "keyframe keyframe%d" % i
root_t = root.filter_all(ChooseTime(t))
draw_recursive(drawing, root_t, g_t, draw_list=dynamic)
base.add(g_t)
for name, sequence in images.items():
try:
obs = sequence.at(t)
updated = True
except UndefinedAtTime:
obs = sequence.at_or_previous(t)
updated = False
img = Tag(name="img")
if isinstance(obs, bytes):
data = obs
else:
data = obs.bytes_contents
if height_of_stored_images is not None:
data = get_resized_image(data, height_of_stored_images)
img.attrs["src"] = data_encoded_for_src(data, "image/jpeg")
# print('image %s %s: %.4fMB ' % (i, t, len(resized) / (1024 * 1024.0)))
img.attrs["class"] = "keyframe keyframe%d" % i
img.attrs["visualize"] = "hide"
img.attrs["updated"] = int(updated)
imagename2div[name].append(img)
other = ""
# language=html
visualize_controls = """\
<style>
*[visualize_parts=false] {
display: none;
}
</style>
<p>
<input id='checkbox-static' type="checkbox" onclick="hideshow(this);" checked>static data</input>
<input id='checkbox-textures' type="checkbox" onclick="hideshow(this);" checked>textures</input>
<input id='checkbox-axes' type="checkbox" onclick="hideshow(this);">axes</input>
<br/>
<input id='checkbox-lane_segments' type="checkbox" onclick="hideshow(this);">map lane
segments</input>
(<input id='checkbox-lane_segments-control_points' type="checkbox" onclick="hideshow(
this);">control
points</input>)</p>
</p>
<p>
<input id='checkbox-vehicles' type="checkbox" onclick="hideshow(this);" checked>vehicles</input>
<input id='checkbox-duckies' type="checkbox" onclick="hideshow(this);" checked>duckies</input>
<input id='checkbox-signs' type="checkbox" onclick="hideshow(this);" checked>signs</input>
<input id='checkbox-sign-papers' type="checkbox" onclick="hideshow(this);" checked>signs
textures</input>
<input id='checkbox-decorations' type="checkbox" onclick="hideshow(this);"
checked>decorations</input>
</p>
<p>
<input id='checkbox-current_lane' type="checkbox" onclick="hideshow(this);">current lane</input>
<input id='checkbox-anchors' type="checkbox" onclick="hideshow(this);">anchor point</input>
</p>
<script>
var checkboxValues = null;
name2selector = {
"checkbox-static": "g.static",
"checkbox-textures": "g.static .tile-textures",
"checkbox-axes": "g.axes",
"checkbox-lane_segments": "g.static .LaneSegment",
"checkbox-lane_segments-control_points": " .control-point",
"checkbox-current_lane": "g.keyframe .LaneSegment",
"checkbox-duckies": ".Duckie",
"checkbox-signs": ".Sign",
"checkbox-sign-papers": ".Sign .sign-paper",
"checkbox-vehicles": ".Vehicle",
"checkbox-decorations": ".Decoration",
'checkbox-anchors': '.Anchor',
};
function hideshow(element) {
console.log(element);
element_name = element.id;
console.log(element_name);
selector = name2selector[element_name];
checked = element.checked;
console.log(selector);
console.log(checked);
elements = document.querySelectorAll(selector);
elements.forEach(_ => _.setAttribute('visualize_parts', checked));
checkboxValues[element_name] = checked;
try {
localStorage.setItem("checkboxValues", JSON.stringify(checkboxValues));
} catch (error) {
console.log('cannot save preferences.');
console.log(error);
}
}
function init() {
for(var name in name2selector) {
console.log(name);
element = document.getElementById(name);
if(name in checkboxValues) {
element.checked = checkboxValues[name];
}
hideshow(element);
}
}
document.addEventListener("DOMContentLoaded", function(event) {
init();
});
try {
checkboxValues = JSON.parse(localStorage.getItem('checkboxValues')) || {};
} catch (error) {
console.log('cannot load preferences.');
console.log(error);
checkboxValues = {}
}
init();
console.log(checkboxValues);
</script>
"""
div_timeseries = str(make_tabs(timeseries))
obs_div = str(obs_div)
html = make_html_slider(
drawing,
keyframes,
obs_div=obs_div,
other=other,
div_timeseries=div_timeseries,
visualize_controls=visualize_controls,
)
with open(fn_html, "w") as f:
f.write(html)
# language=css
style = """
.sign-paper {
display: none;
}
g.axes, .LaneSegment {
display: none;
}
"""
drawing.defs.add(drawing.style(style))
drawing.save(pretty=True)
logger.info("Written SVG to %s" % fn_svg)
logger.info("Written HTML to %s" % fn_html)
return [fn_svg, fn_html]
def __init__(self, tile_size: float, *args, **kwargs):
self.tile_size = tile_size
# noinspection PyArgumentList
PlacedObject.__init__(self, *args, **kwargs)
def wb1():
outdir = get_comptests_output_dir()
root = PlacedObject()
tile_map = create_map(H=3, W=3)
world = PlacedObject()
root.set_object('world', world)
placement = Constant[SE2Transform](SE2Transform.identity())
world.set_object('map1', tile_map, ground_truth=placement)
ego = PlacedObject()
world_coordinates = Constant[SE2Transform](SE2Transform([0, 0], 0))
world.set_object('ego', ego, ground_truth=world_coordinates)
d = root.as_json_dict()
# print(json.dumps(DW.root.as_json_dict(), indent=4))
# print(yaml.safe_dump(d, default_flow_style=False))
# print('------')
r1 = Serializable.from_json_dict(d)
# print('read: %s' % r1)
d1 = r1.as_json_dict()
def __init__(self, status, **kwargs):
PlacedObject.__init__(self, **kwargs)
self.status = status
def __init__(self, tile_size, *args, **kwargs):
self.tile_size = tile_size
PlacedObject.__init__(self, *args, **kwargs)
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)
def __init__(self, status=None, **kwargs):
# if status is None:
# status = Constant[TrafficLightStatus]("off")
# noinspection PyArgumentList
PlacedObject.__init__(self, **kwargs)
self.status = status
def __init__(self, width, length, height, *args, **kwargs):
PlacedObject.__init__(self, *args, **kwargs)
self.width = width
self.height = height
self.length = length
def __init__(self, status=None, **kwargs):
# if status is None:
# status = Constant[TrafficLightStatus]("off")
PlacedObject.__init__(self, **kwargs)
self.status = status
