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)