def create_lane_highlight(poses_sequence, dw):
assert isinstance(poses_sequence, SampledSequence)
def mapi(v):
if isinstance(v, SE2Transform):
return v.as_SE2()
else:
return v
poses_sequence = poses_sequence.transform_values(mapi)
lane_pose_results = poses_sequence.transform_values(GetClosestLane(dw))
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([timestamp], [rt])
visualization.set_object('ls%s-%s-lane' % (i, name), lane_segment, ground_truth=s)
p = SampledSequence([timestamp], [lane_pose_result.center_point])
visualization.set_object('ls%s-%s-anchor' % (i, name), Anchor(), ground_truth=p)
return lane_pose_results
def read_simulator_log(filename):
from duckietown_world.world_duckietown import DB18, construct_map
duckietown_map = None
curpos_timestamps = []
curpos_values = []
timestamps_observations = []
observations = []
for ob in read_log(filename):
if ob.topic == 'map_info':
map_data = ob.data['map_data']
tile_size = ob.data['tile_size']
duckietown_map = construct_map(map_data, tile_size)
if ob.topic == 'observations':
timestamps_observations.append(ob.timestamp)
observations.append(ob.data)
# if ob.topic == 'misc':
# sim = ob.data['Simulator']
# cur_pos = sim['cur_pos']
# cur_angle = sim['cur_angle']
#
# curpos_values.append((cur_pos, cur_angle))
# curpos_timestamps.append(ob.timestamp)
if ob.topic == 'Simulator':
sim = ob.data
cur_pos = sim['cur_pos']
cur_angle = sim['cur_angle']
curpos_values.append((cur_pos, cur_angle))
curpos_timestamps.append(ob.timestamp)
if timestamps_observations:
logger.info('Found %d observations' % len(timestamps_observations))
observations = SampledSequence(timestamps_observations, observations)
else:
observations = None
if not duckietown_map:
msg = 'Could not find duckietown_map.'
raise Exception(msg)
transforms = []
for cur_pos, cur_angle in curpos_values:
transform = duckietown_map.se2_from_curpos(cur_pos, cur_angle)
transforms.append(transform)
trajectory = SampledSequence(curpos_timestamps, transforms)
if not curpos_timestamps:
msg = 'Could not find any position.'
raise Exception(msg)
robot = DB18()
duckietown_map.set_object('ego', robot, ground_truth=trajectory)
return SimulatorLog(duckietown=duckietown_map,
observations=observations,
trajectory=trajectory)
def accumulate(sequence):
""" Integrates with respect to time.
Sums the values along the horizontal. """
total = 0.0
timestamps = []
values = []
for t, v in sequence:
total += v
timestamps.append(t)
values.append(total)
return SampledSequence(timestamps, values)
def integrate(sequence):
total = 0.0
timestamps = []
values = []
for _ in iterate_with_dt(sequence):
v0 = _.v0
dt = _.dt
total += v0 * dt
timestamps.append(_.t0)
values.append(total)
return SampledSequence(timestamps, values)
def integrate(sequence):
""" Integrates with respect to time.
That is, it multiplies the value with the Delta T. """
total = 0.0
timestamps = []
values = []
for _ in iterate_with_dt(sequence):
v0 = _.v0
dt = _.dt
total += v0 * dt
timestamps.append(_.t0)
values.append(total)
return SampledSequence(timestamps, values)
def evaluate(self, context, result):
assert isinstance(result, RuleEvaluationResult)
interval = context.get_interval()
lane_pose_seq = context.get_lane_pose_seq()
timestamps = []
values = []
for i, timestamp in interval:
try:
name2lpr = lane_pose_seq.at(timestamp)
except UndefinedAtTime:
d = 0
else:
if name2lpr:
first = name2lpr[sorted(name2lpr)[0]]
assert isinstance(first, GetLanePoseResult)
lp = first.lane_pose
assert isinstance(lp, LanePose)
d = np.abs(lp.relative_heading)
else:
# no lp
d = 0
values.append(d)
timestamps.append(timestamp)
sequence = SampledSequence(timestamps, values)
cumulative = integrate(sequence)
dtot = cumulative.values[-1]
# result.set_metric((), dtot, sequence, description, cumulative=cumulative)
title = "Deviation from lane direction"
description = textwrap.dedent("""\
This metric describes the amount of deviation from the relative heading.
""")
result.set_metric(name=(),
total=dtot,
incremental=sequence,
title=title,
description=description,
cumulative=cumulative)
def evaluate(self, context, result):
assert isinstance(result, RuleEvaluationResult)
interval = context.get_interval()
lane_pose_seq = context.get_lane_pose_seq()
timestamps = []
values = []
for i, timestamp in interval:
try:
name2lpr = lane_pose_seq.at(timestamp)
except UndefinedAtTime:
d = 1
else:
if name2lpr:
d = 0
else:
# no lp
d = 1
values.append(d)
timestamps.append(timestamp)
sequence = SampledSequence(timestamps, values)
cumulative = integrate(sequence)
dtot = cumulative.values[-1]
title = "Drivable areas"
description = textwrap.dedent("""\
This metric computes whether the robot was in a drivable area.
Note that we check that the robot is in the lane in a correct heading
(up to 90deg deviation from the lane direction).
""")
result.set_metric(name=(),
total=dtot,
incremental=sequence,
title=title,
description=description,
cumulative=cumulative)
def evaluate(self, context, result):
assert isinstance(result, RuleEvaluationResult)
interval = context.get_interval()
lane_pose_seq = context.get_lane_pose_seq()
ego_pose_sequence = context.get_ego_pose_global()
timestamps = []
driven_any = []
driven_lanedir = []
tile_fqn2lane_fqn = {}
for idt in iterate_with_dt(interval):
t0, t1 = idt.v0, idt.v1 # not v
try:
name2lpr = lane_pose_seq.at(t0)
p0 = ego_pose_sequence.at(t0).as_SE2()
p1 = ego_pose_sequence.at(t1).as_SE2()
except UndefinedAtTime:
dr_any = dr_lanedir = 0.0
else:
prel = relative_pose(p0, p1)
translation, _ = geo.translation_angle_from_SE2(prel)
dr_any = np.linalg.norm(translation)
if name2lpr:
ds = []
for k, lpr in name2lpr.items():
if lpr.tile_fqn in tile_fqn2lane_fqn:
if lpr.lane_segment_fqn != tile_fqn2lane_fqn[
lpr.tile_fqn]:
# msg = 'Backwards detected'
# print(msg)
continue
tile_fqn2lane_fqn[lpr.tile_fqn] = lpr.lane_segment_fqn
assert isinstance(lpr, GetLanePoseResult)
c0 = lpr.center_point
ctas = geo.translation_angle_scale_from_E2(
c0.asmatrix2d().m)
c0_ = geo.SE2_from_translation_angle(
ctas.translation, ctas.angle)
prelc0 = relative_pose(c0_, p1)
tas = geo.translation_angle_scale_from_E2(prelc0)
# otherwise this lane should not be reported
# assert tas.translation[0] >= 0, tas
ds.append(tas.translation[0])
dr_lanedir = max(ds) if ds else 0.0
else:
# no lp
dr_lanedir = 0.0
driven_any.append(dr_any)
driven_lanedir.append(dr_lanedir)
timestamps.append(t0)
title = "Consecutive lane distance"
driven_lanedir_incremental = SampledSequence(timestamps,
driven_lanedir)
driven_lanedir_cumulative = accumulate(driven_lanedir_incremental)
description = textwrap.dedent("""\
This metric computes how far the robot drove
**in the direction of the correct lane**,
discounting whenever it was driven
in the wrong direction with respect to the start.
""")
result.set_metric(name=('driven_lanedir_consec', ),
total=driven_lanedir_cumulative.values[-1],
incremental=driven_lanedir_incremental,
title=title,
description=description,
cumulative=driven_lanedir_cumulative)
def draw_static(root,
output_dir,
pixel_size=(480, 480),
area=None,
images=None,
timeseries=None):
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)
if len(timestamps) == 0:
keyframes = SampledSequence([0], [0])
else:
keyframes = SampledSequence(range(len(timestamps)), timestamps)
# nkeyframes = len(keyframes)
if area is None:
areas = []
for i, t in keyframes:
root_t = root.filter_all(ChooseTime(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)
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)
except UndefinedAtTime:
pass
else:
img = Tag(name='img')
resized = get_resized_image(obs.bytes_contents, 200)
img.attrs['src'] = data_encoded_for_src(resized, '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'
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 = JSON.parse(localStorage.getItem('checkboxValues')) || {};
console.log(checkboxValues);
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;
localStorage.setItem("checkboxValues", JSON.stringify(checkboxValues));
}
document.addEventListener("DOMContentLoaded", function(event) {
for(var name in name2selector) {
console.log(name);
element = document.getElementById(name);
if(name in checkboxValues) {
element.checked = checkboxValues[name];
}
hideshow(element);
}
});
</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 evaluate(self, context, result):
assert isinstance(result, RuleEvaluationResult)
interval = context.get_interval()
lane_pose_seq = context.get_lane_pose_seq()
ego_pose_sequence = context.get_ego_pose_global()
timestamps = []
driven_any = []
driven_lanedir = []
for idt in iterate_with_dt(interval):
t0, t1 = idt.v0, idt.v1 # not v
try:
name2lpr = lane_pose_seq.at(t0)
p0 = ego_pose_sequence.at(t0).as_SE2()
p1 = ego_pose_sequence.at(t1).as_SE2()
except UndefinedAtTime:
dr_any = dr_lanedir = 0.0
else:
prel = relative_pose(p0, p1)
translation, _ = geo.translation_angle_from_SE2(prel)
dr_any = np.linalg.norm(translation)
if name2lpr:
ds = []
for k, lpr in name2lpr.items():
assert isinstance(lpr, GetLanePoseResult)
c0 = lpr.center_point
ctas = geo.translation_angle_scale_from_E2(
c0.asmatrix2d().m)
c0_ = geo.SE2_from_translation_angle(
ctas.translation, ctas.angle)
prelc0 = relative_pose(c0_, p1)
tas = geo.translation_angle_scale_from_E2(prelc0)
# otherwise this lane should not be reported
# assert tas.translation[0] >= 0, tas
ds.append(tas.translation[0])
dr_lanedir = max(ds)
else:
# no lp
dr_lanedir = 0.0
driven_any.append(dr_any)
driven_lanedir.append(dr_lanedir)
timestamps.append(idt.t0)
driven_any_incremental = SampledSequence(timestamps, driven_any)
driven_any_cumulative = integrate(driven_any_incremental)
title = "Distance"
description = textwrap.dedent("""\
This metric computes how far the robot drove.
""")
result.set_metric(name=('driven_any', ),
total=driven_any_cumulative.values[-1],
incremental=driven_any_incremental,
title=title,
description=description,
cumulative=driven_any_cumulative)
title = "Lane distance"
driven_lanedir_incremental = SampledSequence(timestamps,
driven_lanedir)
driven_lanedir_cumulative = integrate(driven_lanedir_incremental)
description = textwrap.dedent("""\
This metric computes how far the robot drove
**in the direction of the lane**.
""")
result.set_metric(name=('driven_lanedir', ),
total=driven_lanedir_cumulative.values[-1],
incremental=driven_lanedir_incremental,
title=title,
description=description,
cumulative=driven_lanedir_cumulative)