Fixed handling of pose messages without GPS time when generating a KML file.

python/examples/extract_position_data.py

@@ -172,31 +172,49 @@
     logger.info("Generating '%s'." % path)
     with open(path, 'w') as f:
         f.write(KML_TEMPLATE_START)
+
         # Extract the first and last valid position.
         valid_solutions = np.where(pose_data.solution_type != SolutionType.Invalid)[0]
         first_valid_pose = pose_data.messages[valid_solutions[0]]
         last_valid_pose = pose_data.messages[valid_solutions[-1]]
 
-        f.write(KML_TEMPLATE_LOOKAT %
-          {'latitude': first_valid_pose.lla_deg[0],
-          'longitude': first_valid_pose.lla_deg[1],
-          'altitude': first_valid_pose.lla_deg[2] - first_valid_pose.undulation_m,
-          'begin_time': str(first_valid_pose.gps_time.as_utc().isoformat()),
-          'end_time': str(last_valid_pose.gps_time.as_utc().isoformat())}
-        )
+        # Extract the start/end GPS times. Note that the device may not have GPS time at the start of a log, even if it
+        # has a valid position, if it started up inside a parking garage, etc. Similarly, it might not have GPS time at
+        # the end of the log if it was reset and didn't have enough time to reinitialize time. So we can't simply look
+        # at [first,last]_valid_pose.gps_time since it might be invalid.
+        #
+        # Instead, we can ask the DataLoader class to convert P1 time to GPS time for us. If it is still not able to
+        # convert either time, the returned Timestamp object will be invalid. Timestamp.as_utc() will return None below
+        # and we will not be able to put a timestamp on the KML entry.
+        gps_times = reader.convert_to_gps_time((first_valid_pose.p1_time, last_valid_pose.p1_time),
+                                               return_timestamp=True)
+
+        def _to_time_str(time: Timestamp):
+            utc_time = time.as_utc()
+            return utc_time.isoformat() if utc_time is not None else ""
+
+        f.write(KML_TEMPLATE_LOOKAT % {
+            'latitude': first_valid_pose.lla_deg[0],
+            'longitude': first_valid_pose.lla_deg[1],
+            'altitude': first_valid_pose.lla_deg[2] - first_valid_pose.undulation_m,
+            'begin_time': _to_time_str(gps_times[0]),
+            'end_time': _to_time_str(gps_times[1]),
+        })
+
         for pose in pose_data.messages:
-          # IMPORTANT: KML heights are specified in MSL, so we convert the ellipsoid heights to orthometric below using
-          # the reported geoid undulation (geoid height). Undulation values come from a geoid model, and are not
-          # typically precise. When analyzing position performance compared with another device, we strongly recommend
-          # that you do the performance using ellipsoid heights. When comparing in MSL, if the geoid models used by the
-          # two devices are not exactly the same, the heights may differ by multiple meters.
-          #
-          # Only write KML entries with valid GPS time.
-          if pose.gps_time.as_utc() is not None:
-            f.write(KML_TEMPLATE %
-                    {'timestamp': '\n'.join([str(pose.gps_time.as_utc().isoformat())]),
-                    'solution_type': int(pose.solution_type),
-                    'coordinates': '\n'.join(['%.8f,%.8f,%.8f' % (pose.lla_deg[1], pose.lla_deg[0], pose.lla_deg[2] - pose.undulation_m)])})
+            # IMPORTANT: KML heights are specified in MSL, so we convert the ellipsoid heights to orthometric below
+            # using the reported geoid undulation (geoid height). Undulation values come from a geoid model, and are not
+            # typically precise. When analyzing position performance compared with another device, we strongly recommend
+            # that you do the performance using ellipsoid heights. When comparing in MSL, if the geoid models used by
+            # the two devices are not exactly the same, the heights may differ by multiple meters.
+            #
+            # Only write KML entries with valid GPS time.
+            f.write(KML_TEMPLATE % {
+                'timestamp': _to_time_str(pose.gps_time),
+                'solution_type': int(pose.solution_type),
+                'coordinates': '%.8f,%.8f,%.8f' % (pose.lla_deg[1], pose.lla_deg[0],
+                                                   pose.lla_deg[2] - pose.undulation_m),
+            })
 
         f.write(KML_TEMPLATE_END)
 

python/fusion_engine_client/analysis/data_loader.py

@@ -625,8 +625,8 @@ def get_input_path(self):
     def _convert_time(self, conversion_type: TimeConversionType,
                       times: Union[Iterable[Union[datetime, gpstime, Timestamp, float]],
                                    Union[datetime, gpstime, Timestamp, float]],
-                      assume_utc: bool = False) ->\
-            np.ndarray:
+                      assume_utc: bool = False, return_timestamp: bool = False) ->\
+            Union[np.ndarray, Timestamp]:
         """!
         @brief Convert UTC or GPS timestamps to P1 time or Convert UTC or P1 timestamps to GPS time.
 
@@ -643,8 +643,11 @@ def _convert_time(self, conversion_type: TimeConversionType,
                  seconds.
                - For `datetime`, if `tzinfo` is not set, assume it is `timezone.utc`. Otherwise, interpret the timestamp
                  in the local timezone.
+        @param return_timestamp If `True`, return `Timestamp` objects instead of floating point values.
 
-        @return A numpy array containing time values (in seconds), or `nan` if the value could not be converted.
+        @return If `return_timestamp == False`, a numpy array containing time values (in seconds), or `nan` if the value
+                could not be converted. If `return_timestamp == True`, a list of @ref
+                fusion_engine_client.messages.timestamps.Timestamp objects.
         """
         # Load pose messages, which contain the relationship between P1 and GPS time. Omit any NAN values from the
         # reference timestamps.
@@ -721,10 +724,10 @@ def _to_gps_or_p1(value):
                 if p1_ref_sec is None:
                     time_sec[gps_idx] = np.nan
                 else:
-                    # The relationship between P1 time and GPS time should not change rapidly since P1 time is rate-steered
-                    # to align with GPS time. As a result, it should be safe to extrapolate between gaps in P1 or GPS times,
-                    # as long as the gaps aren't extremely large. NumPy's interp() function does not extrapolate, so we
-                    # instead use SciPy's function.
+                    # The relationship between P1 time and GPS time should not change rapidly since P1 time is
+                    # rate-steered to align with GPS time. As a result, it should be safe to extrapolate between gaps in
+                    # P1 or GPS times, as long as the gaps aren't extremely large. NumPy's interp() function does not
+                    # extrapolate, so we instead use SciPy's function.
                     f = sp.interpolate.interp1d(gps_ref_sec, p1_ref_sec, fill_value='extrapolate')
                     time_sec[gps_idx] = f(time_sec[gps_idx])
         elif conversion_type == TimeConversionType.P1_TO_GPS:
@@ -737,16 +740,17 @@ def _to_gps_or_p1(value):
                     f = sp.interpolate.interp1d(p1_ref_sec, gps_ref_sec, fill_value='extrapolate')
                     time_sec[p1_idx] = f(time_sec[p1_idx])
 
+        result = [Timestamp(t) for t in time_sec] if return_timestamp else time_sec
         if return_scalar:
-            return time_sec[0]
+            return result[0]
         else:
-            return time_sec
+            return result
 
     def convert_to_p1_time(self,
                            times: Union[Iterable[Union[datetime, gpstime, Timestamp, float]],
                                         Union[datetime, gpstime, Timestamp, float]],
-                           assume_utc: bool = False) ->\
-            np.ndarray:
+                           assume_utc: bool = False, return_timestamp: bool = False) ->\
+            Union[np.ndarray, Timestamp]:
         """!
         @brief Convert UTC or GPS timestamps to P1 time.
 
@@ -762,16 +766,18 @@ def convert_to_p1_time(self,
                  seconds.
                - For `datetime`, if `tzinfo` is not set, assume it is `timezone.utc`. Otherwise, interpret the timestamp
                  in the local timezone.
+        @param return_timestamp If `True`, return `Timestamp` objects instead of floating point values.
 
         @return A numpy array containing P1 time values (in seconds), or `nan` if the value could not be converted.
         """
-        return self._convert_time(conversion_type=TimeConversionType.GPS_TO_P1, times=times, assume_utc=assume_utc)
+        return self._convert_time(conversion_type=TimeConversionType.GPS_TO_P1, times=times, assume_utc=assume_utc,
+                                  return_timestamp=return_timestamp)
 
     def convert_to_gps_time(self,
                             times: Union[Iterable[Union[datetime, gpstime, Timestamp, float]],
                                          Union[datetime, gpstime, Timestamp, float]],
-                            assume_utc: bool = False) ->\
-            np.ndarray:
+                            assume_utc: bool = False, return_timestamp: bool = False) ->\
+            Union[np.ndarray, Timestamp]:
         """!
         @brief Convert UTC or P1 timestamps to GPS time.
 
@@ -787,10 +793,12 @@ def convert_to_gps_time(self,
                  seconds.
                - For `datetime`, if `tzinfo` is not set, assume it is `timezone.utc`. Otherwise, interpret the timestamp
                  in the local timezone.
+        @param return_timestamp If `True`, return `Timestamp` objects instead of floating point values.
 
         @return A numpy array containing GPS time values (in seconds), or `nan` if the value could not be converted.
         """
-        return self._convert_time(conversion_type=TimeConversionType.P1_TO_GPS, times=times, assume_utc=assume_utc)
+        return self._convert_time(conversion_type=TimeConversionType.P1_TO_GPS, times=times, assume_utc=assume_utc,
+                                  return_timestamp=return_timestamp)
 
     @classmethod
     def time_align_data(cls, data: dict, mode: TimeAlignmentMode = TimeAlignmentMode.INSERT,