Skip to content

Commit

Permalink
Allow programmatic access to p1_display internals. (#340)
Browse files Browse the repository at this point in the history
# Changes
- Added a note to `README.md` explaining Windows Python requirements
- Return `Analyzer` from `main()` so it may be accessed further when embedded within another application
- Constrain the 3D displacement Y axis to min 0
  • Loading branch information
adamshapiro0 authored Oct 28, 2024
2 parents 8839ea5 + 295a961 commit 969cf8a
Show file tree
Hide file tree
Showing 2 changed files with 24 additions and 9 deletions.
4 changes: 4 additions & 0 deletions python/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -88,6 +88,10 @@ FusionEngine message specification.

### Usage Instructions

> Note for Windows users: You must install Python using an installer from
> https://www.python.org/downloads/windows/ and select "Add python.exe to PATH". (Do not install Python using Microsoft
> Store.)
#### Install From PyPI

1. Install Python (3.6 or later) and pip.
Expand Down
29 changes: 20 additions & 9 deletions python/fusion_engine_client/analysis/analyzer.py
Original file line number Diff line number Diff line change
Expand Up @@ -814,6 +814,7 @@ def _plot_displacement(self, source, time, solution_type, displacement_enu_m, st
time_figure.update_layout(title_text=extra_text)

# Plot the data.
max_3d_diff_m = [0.0]
def _plot_data(name, idx, marker_style=None):
style = {'mode': 'markers', 'marker': {'size': 8}, 'showlegend': True, 'legendgroup': name,
'hoverlabel': {'namelength': -1}}
Expand All @@ -828,7 +829,9 @@ def _plot_data(name, idx, marker_style=None):
topo_figure.add_trace(go.Scattergl(x=displacement_enu_m[0, idx], y=displacement_enu_m[1, idx],
name=name, text=text, **style), 1, 1)

time_figure.add_trace(go.Scattergl(x=time[idx], y=np.linalg.norm(displacement_enu_m[:, idx], axis=0),
displacement_3d_m = np.linalg.norm(displacement_enu_m[:, idx], axis=0)
max_3d_diff_m[0] = max(max_3d_diff_m[0], np.nanmax(displacement_3d_m))
time_figure.add_trace(go.Scattergl(x=time[idx], y=displacement_3d_m,
name=name, text=text, **style), 1, 1)
style['showlegend'] = False
time_figure.add_trace(go.Scattergl(x=time[idx], y=displacement_enu_m[0, idx], name=name,
Expand All @@ -847,6 +850,12 @@ def _plot_data(name, idx, marker_style=None):
for type, info in _SOLUTION_TYPE_MAP.items():
_plot_data(info.name, solution_type == type, marker_style=info.style)

# Set the 3D displacement Y-axis limits to start at 0 and encompass the data.
max_y = 1.2 * max_3d_diff_m[0]
if max_y == 0.0:
max_y = 1.0
time_figure['layout']['yaxis1'].update(range=[0, max_y])

name = source.replace(' ', '_').lower()
self._add_figure(name=f"{name}_top_down", figure=topo_figure, title=f"{source}: Top-Down (Topocentric)")
self._add_figure(name=f"{name}_vs_time", figure=time_figure, title=f"{source}: vs. Time")
Expand All @@ -858,32 +867,32 @@ def plot_stationary_position_error(self, truth_lla_deg):
@param truth_lla_deg The truth LLA location (in degrees/meters).
"""
truth_ecef_m = np.array(geodetic2ecef(*truth_lla_deg, deg=True))
self._plot_pose_displacement(title='Position Error', center_ecef_m=truth_ecef_m)
return self._plot_pose_displacement(title='Position Error', center_ecef_m=truth_ecef_m)

def plot_pose_displacement(self):
"""!
@brief Generate a topocentric (top-down) plot of position displacement, as well as plot of displacement over
time.
"""
self._plot_pose_displacement()
return self._plot_pose_displacement()

def _plot_pose_displacement(self, title='Pose Displacement', center_ecef_m=None):
if self.output_dir is None:
return
return None

# Read the pose data.
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:
self.logger.info('No pose data available. Skipping displacement plots.')
return
return None

# Remove invalid solutions.
valid_idx = np.logical_and(~np.isnan(pose_data.p1_time), pose_data.solution_type != SolutionType.Invalid)
if not np.any(valid_idx):
self.logger.info('No valid position solutions detected. Skipping displacement plots.')
return
return None

time = pose_data.p1_time[valid_idx] - float(self.t0)
solution_type = pose_data.solution_type[valid_idx]
Expand All @@ -906,6 +915,8 @@ def _plot_pose_displacement(self, title='Pose Displacement', center_ecef_m=None)
self._plot_displacement(source=title, title=axis_title, time=time, solution_type=solution_type,
displacement_enu_m=displacement_enu_m, std_enu_m=std_enu_m)

return displacement_enu_m

def plot_relative_position(self):
"""!
@brief Generate a topocentric (top-down) plot of relative position vs base station, as well as plot of relative
Expand Down Expand Up @@ -2812,7 +2823,7 @@ def _assign_colors(cls, elements, num_colors=None):
return {e: colors[i % len(colors)] for i, e in enumerate(elements)}


def main():
def main(args=None):
parser = ArgumentParser(description="""\
Load and display information stored in a FusionEngine binary file.
""")
Expand Down Expand Up @@ -2908,7 +2919,7 @@ def main():
'-v', '--verbose', action='count', default=0,
help="Print verbose/trace debugging messages.")

options = parser.parse_args()
options = parser.parse_args(args=args)

# Configure logging.
if options.verbose >= 1:
Expand Down Expand Up @@ -3042,7 +3053,7 @@ def main():
analyzer.generate_index(auto_open=not options.no_index)

_logger.info("Output stored in '%s'." % os.path.abspath(output_dir))

return analyzer

if __name__ == "__main__":
main()

0 comments on commit 969cf8a

Please sign in to comment.