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 = lp.distance_from_center
else:
# no lp
d = 0
values.append(d)
timestamps.append(timestamp)
sequence = SampledSequence(timestamps, values)
cumulative = integrate(sequence)
dtot = cumulative.values[-1]
title = "Deviation from center line"
description = textwrap.dedent("""\
This metric describes the amount of deviation from the center line.
""")
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 draw_logs_main_(output, filename):
if output is None:
output = filename + '.out'
if not os.path.exists(output):
os.makedirs(output)
logger.info('processing file %s' % filename)
log = read_simulator_log(filename)
duckietown_env = log.duckietown
if log.observations:
images = {'observations': log.observations}
else:
images = None
interval = SampledSequence.from_iterator(
enumerate(log.trajectory.timestamps))
evaluated = evaluate_rules(poses_sequence=log.trajectory,
interval=interval,
world=duckietown_env,
ego_name='ego')
timeseries = make_timeseries(evaluated)
draw_static(duckietown_env, output, images=images, timeseries=timeseries)
return evaluated
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 check_car_dynamics_correct(klass, CarCommands, CarParams):
"""
:param klass: the implementation
:return:
"""
# load one of the maps (see the list using dt-world-draw-maps)
outdir = get_comptests_output_dir()
dw = load_map('udem1')
v = 5
# define a SampledSequence with timestamp, command
commands_sequence = SampledSequence.from_iterator([
# we set these velocities at 1.0
(1.0, CarCommands(0.1 * v, 0.1 * v)),
# at 2.0 we switch and so on
(2.0, CarCommands(0.1 * v, 0.4 * v)),
(4.0, CarCommands(0.1 * v, 0.4 * v)),
(5.0, CarCommands(0.1 * v, 0.2 * v)),
(6.0, CarCommands(0.1 * v, 0.1 * v)),
])
# we upsample the sequence by 5
commands_sequence = commands_sequence.upsample(5)
## Simulate the dynamics of the vehicle
# start from q0 and v0
q0 = geo.SE2_from_translation_angle([1.8, 0.7], 0)
v0 = geo.se2.zero()
c0 = q0, v0
# instantiate the class that represents the dynamics
dynamics = CarParams(10.0)
# this function integrates the dynamics
poses_sequence = get_robot_trajectory(dynamics, q0, commands_sequence)
#################
# Visualization and rule evaluation
# Creates an object 'duckiebot'
ego_name = 'duckiebot'
db = DB18() # class that gives the appearance
# convert from SE2 to SE2Transform representation
transforms_sequence = poses_sequence.transform_values(
SE2Transform.from_SE2)
# puts the object in the world with a certain "ground_truth" constraint
dw.set_object(ego_name, db, ground_truth=transforms_sequence)
# Rule evaluation (do not touch)
interval = SampledSequence.from_iterator(
enumerate(commands_sequence.timestamps))
evaluated = evaluate_rules(poses_sequence=transforms_sequence,
interval=interval,
world=dw,
ego_name=ego_name)
timeseries = make_timeseries(evaluated)
# Drawing
area = RectangularArea((0, 0), (3, 3))
draw_static(dw, outdir, area=area, timeseries=timeseries)
expected = {
1.0:
geo.SE2_from_translation_angle([1.8, 0.7], 0.0),
1.6:
geo.SE2_from_translation_angle([2.09998657, 0.70245831],
0.016389074695313716),
2.0:
geo.SE2_from_translation_angle([2.29993783, 0.70682836],
0.027315124492189525),
2.4:
geo.SE2_from_translation_angle([2.49991893, 0.70792121],
-0.016385672773040847),
2.8:
geo.SE2_from_translation_angle([2.69975684, 0.70027647],
-0.060086470038271216),
3.2:
geo.SE2_from_translation_angle([2.89906999, 0.68390873],
-0.10378726730350164),
3.6:
geo.SE2_from_translation_angle([3.09747779, 0.65884924],
-0.14748806456873198),
4.0:
geo.SE2_from_translation_angle([3.2946014, 0.62514586],
-0.19118886183396236),
4.6:
geo.SE2_from_translation_angle([3.58705642, 0.55852875],
-0.25674005773180786),
5.0:
geo.SE2_from_translation_angle([3.77932992, 0.50353298],
-0.3004408549970382),
6.0:
geo.SE2_from_translation_angle([4.2622088, 0.37429798],
-0.22257046876429318),
}
for t, expected_pose in expected.items():
assert np.allclose(poses_sequence.at(t=t), expected_pose), t
print('All tests passed successfully!')
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)