Skip to content

Commit

Permalink
Return calculated ENU displacement from plot_stationary_position_erro…
Browse files Browse the repository at this point in the history
…r().
  • Loading branch information
adamshapiro0 committed Oct 28, 2024
1 parent a482191 commit 05a88ff
Showing 1 changed file with 7 additions and 5 deletions.
12 changes: 7 additions & 5 deletions python/fusion_engine_client/analysis/analyzer.py
Original file line number Diff line number Diff line change
Expand Up @@ -858,32 +858,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 +906,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

0 comments on commit 05a88ff

Please sign in to comment.