diff --git a/python/fusion_engine_client/analysis/analyzer.py b/python/fusion_engine_client/analysis/analyzer.py
index bbf46c15..15c3a9d1 100755
--- a/python/fusion_engine_client/analysis/analyzer.py
+++ b/python/fusion_engine_client/analysis/analyzer.py
@@ -1243,6 +1243,9 @@ def _plot_data(name, selected_idx, flags, source_id, lla_deg, customdata_all, ma
# Read the pose data.
have_pose_data = False
+ primary_source_id = min(pose_source_ids)
+ overall_t_min = None
+ overall_t_max = None
for source_id in pose_source_ids:
result = self.reader.read(message_types=[PoseMessage], source_ids=[source_id], **self.params)
pose_data = result[PoseMessage.MESSAGE_TYPE]
@@ -1263,8 +1266,14 @@ def _plot_data(name, selected_idx, flags, source_id, lla_deg, customdata_all, ma
flags = pose_data.flags[valid_idx]
lla_deg = pose_data.lla_deg[:, valid_idx]
std_enu_m = pose_data.position_std_enu_m[:, valid_idx]
+ p1_time = pose_data.p1_time[valid_idx]
- customdata_all = _build_position_customdata(p1_time=pose_data.p1_time[valid_idx],
+ t_min = float(np.min(p1_time))
+ t_max = float(np.max(p1_time))
+ overall_t_min = t_min if overall_t_min is None else min(overall_t_min, t_min)
+ overall_t_max = t_max if overall_t_max is None else max(overall_t_max, t_max)
+
+ customdata_all = _build_position_customdata(p1_time=p1_time,
gps_time=pose_data.gps_time[valid_idx],
lla_deg=lla_deg, std_enu_m=std_enu_m)
@@ -1288,7 +1297,12 @@ def _plot_data(name, selected_idx, flags, source_id, lla_deg, customdata_all, ma
layout = go.Layout(
autosize=True,
hovermode='closest',
- title=title,
+ # Anchored to the plot area's own left edge (paper x=0) rather than left at the default (centered on
+ # the whole container) -- the container also includes the legend, which Plotly auto-widens to fit,
+ # so a container-centered title drifts right of where the map itself actually ends up.
+ title=dict(text=title, x=0, xanchor='left', xref='paper'),
+ # Reduce padding around the map, leaving enough space for the title.
+ margin=dict(l=16, r=16, t=70, b=8),
mapbox=dict(
accesstoken=mapbox_token,
bearing=0,
@@ -1305,27 +1319,57 @@ def _plot_data(name, selected_idx, flags, source_id, lla_deg, customdata_all, ma
figure = go.Figure(data=map_data, layout=layout)
figure['layout'].update(showlegend=True)
- # Add quality selection buttons.
- num_traces = len(figure.data)
- buttons = [dict(label='All', method='restyle', args=['visible', [True] * num_traces])]
- for name, indices in sorted(indices_by_engine.items()):
- if len(indices) == 0:
- continue
- visible = np.full((num_traces,), False)
- visible[indices] = True
- buttons.append(dict(label=name, method='restyle', args=['visible', visible]))
- figure['layout']['updatemenus'] = [{
- 'type': 'buttons',
- 'direction': 'left',
- 'buttons': buttons,
- 'x': 0.0,
- 'xanchor': 'left',
- 'y': 1.1,
- 'yanchor': 'top'
- }]
+ # Add selection buttons for different engines (nav engine, GNSS receiver, etc.).
+ if len(indices_by_engine) > 1:
+ num_traces = len(figure.data)
+ buttons = [dict(label='All', method='restyle', args=['visible', [True] * num_traces])]
+ for name, indices in sorted(indices_by_engine.items()):
+ if len(indices) == 0:
+ continue
+ visible = np.full((num_traces,), False)
+ visible[indices] = True
+ buttons.append(dict(label=name, method='restyle', args=['visible', visible]))
+ figure['layout']['updatemenus'] = [{
+ 'type': 'buttons',
+ 'direction': 'left',
+ 'buttons': buttons,
+ # Move the buttons inside the map to avoid overlap and reduce unused whitespace.
+ 'x': 1.0,
+ 'xanchor': 'right',
+ 'y': 0.99,
+ 'yanchor': 'top',
+ 'bgcolor': 'rgba(255,255,255,0.85)',
+ }]
+
+ # Speed profile (for the time slider below the map) is only computed for the default source -- it's a
+ # visual aid for picking a time range, not a plotted data source, so it doesn't need every source's data.
+ profile_time_sec, profile_speed_mps, profile_gps_time_sec, _ = \
+ self._estimate_speed_mps(source_id=primary_source_id, forward_only=False, signed=False)
+
+ slider_js = self._map_time_slider_js(t_min=overall_t_min, t_max=overall_t_max,
+ profile_time_sec=profile_time_sec,
+ profile_speed_mps=profile_speed_mps,
+ profile_gps_time_sec=profile_gps_time_sec)
+
+ # Make room for the slider *before* Plotly's own first render so the map doesn't appear full-size and then
+ # shrink after the time scale renders.
+ #
+ # The map itself starts hidden (`visibility:hidden`, which still reserves its final layout space, unlike
+ # `display:none`) -- even with the container correctly sized up front, Plotly's own WebGL/mapbox-gl
+ # rendering doesn't necessarily catch up to a resize() call within the same paint, so revealing it right
+ # away can still show one frame at the wrong (window-sized) dimensions overlapping the slider. It's
+ # revealed by JS (plotly_map_time_slider.js) once Plotly itself reports the post-resize redraw is done.
+ slider_head_css = """\
+
+"""
self._add_figure(name="map", figure=figure, title="Vehicle Trajectory (Map)", config={'scrollZoom': True},
- custom_hover=False)
+ custom_hover=False, inject_js=slider_js, inject_head=slider_head_css)
def plot_gnss_skyplot(self, decimate=True):
for source_id in self._get_gnss_antenna_source_ids():
@@ -2184,6 +2228,77 @@ def plot_wheel_data(self):
self._plot_wheel_ticks_or_speeds(source='vehicle', type='speed')
self._plot_wheel_ticks_or_speeds(source='vehicle', type='tick')
+ def _estimate_speed_mps(self, source_id, forward_only: bool = False, signed: bool = False):
+ """!
+ @brief Estimate speed (m/s) from the best available data, falling back through progressively coarser
+ sources.
+
+ Sources in order of priority:
+ 1. `PoseMessage.velocity_body_mps` -- forward (X) component only if `forward_only`, otherwise the
+ full 3D norm.
+ 2. `PoseAuxMessage.velocity_enu_mps` -- always an unsigned 3D norm; ENU velocity can't convey forward speed.
+ 3. Differential position -- unsigned 3D norm of consecutive ECEF position deltas (from
+ `PoseMessage.lla_deg`) divided by elapsed time. Coarser (no velocity filtering/smoothing) and one
+ sample shorter than the other sources (undefined at the first time).
+
+ @param source_id The pose source ID to read.
+ @param forward_only If `True`, return body forward velocity, if known, or 3D speed otherwise.
+ @param signed If `True`, return signed forward velocity (negative when reversing) instead of speed (unsigned)
+ when known. When using ENU velocity or differential position, speed is always unsigned.
+
+ @return `(p1_time, speed_mps, gps_time, source)`, all `None` if no usable data exists at all. `source` is
+ one of `'body'`, `'enu'`, or `'diff_position'`, identifying which tier was used. `gps_time` is
+ NaN-filled if the winning source has no way to recover real GPS time (only possible for `'enu'`,
+ and only when there's no `PoseMessage` in the log at all to borrow it from -- see below).
+ """
+ result = self.reader.read(message_types=[PoseMessage], source_ids=source_id, **self.params)
+ pose_data = result[PoseMessage.MESSAGE_TYPE]
+ have_pose = len(pose_data.p1_time) != 0
+
+ if have_pose and np.any(~np.isnan(pose_data.velocity_body_mps)):
+ if forward_only:
+ speed_mps = pose_data.velocity_body_mps[0, :]
+ if not signed:
+ speed_mps = np.abs(speed_mps)
+ else:
+ speed_mps = np.linalg.norm(pose_data.velocity_body_mps, axis=0)
+ return pose_data.p1_time, speed_mps, pose_data.gps_time, 'body'
+
+ result = self.reader.read(message_types=[PoseAuxMessage], source_ids=source_id, **self.params)
+ pose_aux_data = result[PoseAuxMessage.MESSAGE_TYPE]
+ if len(pose_aux_data.p1_time) != 0 and np.any(~np.isnan(pose_aux_data.velocity_enu_mps)):
+ self.logger.warning('Body velocity not available. Estimating |speed| from ENU velocity. May not '
+ 'match other speed sources when reversing.')
+ speed_mps = np.linalg.norm(pose_aux_data.velocity_enu_mps, axis=0)
+ # PoseAuxMessage doesn't carry GPS time itself, but it's emitted in lockstep with PoseMessage at the
+ # same P1 times -- if PoseMessage is present too (just without usable velocity), borrow its GPS time
+ # via interpolation rather than leaving this all NaN.
+ valid_gps_idx = np.logical_and(~np.isnan(pose_data.p1_time), ~np.isnan(pose_data.gps_time))
+ if have_pose and np.any(valid_gps_idx):
+ gps_time = np.interp(pose_aux_data.p1_time, pose_data.p1_time[valid_gps_idx],
+ pose_data.gps_time[valid_gps_idx])
+ else:
+ gps_time = np.full_like(pose_aux_data.p1_time, np.nan)
+ return pose_aux_data.p1_time, speed_mps, gps_time, 'enu'
+
+ if not have_pose:
+ return None, None, None, None
+
+ valid_idx = np.logical_and(~np.isnan(pose_data.p1_time), ~np.any(np.isnan(pose_data.lla_deg), axis=0))
+ if np.sum(valid_idx) < 2:
+ return None, None, None, None
+
+ self.logger.warning('Body and ENU velocity not available. Approximating |speed| from differential '
+ 'position.')
+ p1_time = pose_data.p1_time[valid_idx]
+ gps_time = pose_data.gps_time[valid_idx]
+ position_ecef_m = np.array(geodetic2ecef(lat=pose_data.lla_deg[0, valid_idx],
+ lon=pose_data.lla_deg[1, valid_idx],
+ alt=pose_data.lla_deg[2, valid_idx], deg=True))
+ dt_sec = np.diff(p1_time)
+ speed_mps = np.linalg.norm(np.diff(position_ecef_m, axis=1), axis=0) / dt_sec
+ return p1_time[1:], speed_mps, gps_time[1:], 'diff_position'
+
def _plot_wheel_ticks_or_speeds(self, source, type):
"""!
@brief Plot wheel speed or tick data.
@@ -2373,38 +2488,19 @@ def _get_time_source(meas_type, data):
# Note: Pose data is not read when plotting ticks (ticks do not plot in meters/second). If the wheel data is not
# in P1 time, we cannot compare against the pose data, which is.
if type == 'speed' and p1_time_present:
- nav_engine_p1_time = None
- nav_engine_speed_mps = None
-
- # If we have pose messages _and_ they contain body velocity, we can use that.
- #
- # Note that we are using this to compare vs wheel speeds, so we're only interested in forward speed here.
- result = self.reader.read(message_types=[PoseMessage], source_ids=self.default_source_id, **self.params)
- pose_data = result[PoseMessage.MESSAGE_TYPE]
- if len(pose_data.p1_time) != 0 and np.any(~np.isnan(pose_data.velocity_body_mps[0, :])):
- nav_engine_p1_time = pose_data.p1_time
- nav_engine_speed_mps = pose_data.velocity_body_mps[0, :]
- if data_signed:
- nav_engine_speed_name = 'Speed Estimate (Nav Engine)'
- else:
- nav_engine_speed_mps = np.abs(nav_engine_speed_mps)
- nav_engine_speed_name = '|Speed Estimate| (Nav Engine)'
- # Otherwise, if we have pose aux messages, read those and use the ENU velocity to estimate speed. Since we
- # don't know attitude, the best we can do is estimate 3D speed and assume it's primarily in the along-track
- # direction. This will also be an absolute value, so may not match the wheel data if it is signed and the
- # vehicle is going backward.
- else:
- result = self.reader.read(message_types=[PoseAuxMessage], source_ids=self.default_source_id,
- **self.params)
- pose_aux_data = result[PoseAuxMessage.MESSAGE_TYPE]
- if len(pose_aux_data.p1_time) != 0:
- self.logger.warning('Body forward velocity not available. Estimating |speed| from ENU velocity. '
- 'May not match wheel speeds when going backward.')
- nav_engine_p1_time = pose_aux_data.p1_time
- nav_engine_speed_mps = np.linalg.norm(pose_aux_data.velocity_enu_mps, axis=0)
- nav_engine_speed_name = '|3D Speed Estimate| (Nav Engine)'
+ # We're comparing this to wheel speed, so prefer a signed forward (body-frame X) estimate when real
+ # body velocity is available. The ENU-velocity and differential-position fallbacks (see
+ # _estimate_speed_mps()) can only ever produce an unsigned 3D speed -- attitude/heading isn't known
+ # from either -- so may not match the wheel data if it's signed and the vehicle is going backward.
+ nav_engine_p1_time, nav_engine_speed_mps, _, speed_source = \
+ self._estimate_speed_mps(source_id=self.default_source_id, forward_only=True, signed=data_signed)
if nav_engine_speed_mps is not None:
+ nav_engine_speed_name = {
+ 'body': 'Speed Estimate' if data_signed else '|Speed Estimate|',
+ 'enu': '|3D Speed Estimate|',
+ 'diff_position': '|Differential Position Speed Estimate|',
+ }[speed_source] + ' (Nav Engine)'
if use_time_type:
nav_time, _ = self._resolve_x_axis(p1_time=nav_engine_p1_time)
nav_kwargs = {'customdata': self._time_hover_customdata(p1_time=nav_engine_p1_time)}
@@ -3454,7 +3550,7 @@ def _add_page(self, name, html_body, title=None):
self.plots[name] = {'title': title, 'path': path}
def _add_figure(self, name, figure=None, title=None, config=None, inject_js: str = None,
- time_axis_type: Optional[str] = None, custom_hover: bool = True):
+ inject_head: str = None, time_axis_type: Optional[str] = None, custom_hover: bool = True):
"""!
@brief Generate an HTML file for the specified figure.
@@ -3464,7 +3560,13 @@ def _add_figure(self, name, figure=None, title=None, config=None, inject_js: str
@param config An optional dictionary containing Plotly.js figure config options to be included in the generated
JavaScript.
@param inject_js Custom Javascript to be injected into the generated HTML file (see @ref
- __write_html_and_inject_js()).
+ __write_html_and_inject_js()). Runs *after* `Plotly.newPlot()`, so it's too late to affect the
+ container's size before Plotly's own initial (auto-sized) render -- use `inject_head` for that.
+ @param inject_head Raw HTML (typically a `