fix: resolve ValueError if there is no observed NavSatFix messages (#175

) Signed-off-by: ktro2828 <[email protected]> Co-authored-by: Shunsuke Miura <[email protected]>
tier4 · Dec 9, 2024 · bdb58fc · bdb58fc
1 parent 2743645
commit bdb58fc
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 8 deletions.
diff --git a/perception_dataset/ros2/oxts_msgs/ins_handler.py b/perception_dataset/ros2/oxts_msgs/ins_handler.py
@@ -418,19 +418,20 @@ def get_odometries(self) -> List[Odometry]:
     def get_imus(self) -> List[Imu]:
         return self._buffer["imu"]
 
-    def lookup_nav_sat_fixes(self, stamp: RosTime) -> NavSatFix:
+    def lookup_nav_sat_fixes(self, stamp: RosTime) -> Optional[NavSatFix]:
         return self.interpolate_nav_sat_fixes(stamp)
 
     def interpolate_nav_sat_fixes(
         self, query_stamp: Union[RosTime, List[RosTime]]
-    ) -> Union[NavSatFix | List[NavSatFix]]:
+    ) -> Optional[Union[NavSatFix | List[NavSatFix]]]:
         """Interpolate NavSatFix.
 
         Args:
             query_stamp (RosTime | List[RosTime]): Query stamp(s).
 
         Returns:
-            Union[NavSatFix, List[NavSatFix]]: Interpolated message(s).
+            Optional[Union[NavSatFix, List[NavSatFix]]]: Interpolated message(s).
+                Note that it returns `None`, if there is no observed NavSatFix messages.
 
         Warnings:
             If the value in `query_stamps` is out of range of the observed timestamps,
@@ -454,6 +455,9 @@ def interpolate_nav_sat_fixes(
             timestamps.append(stamp_to_unix_timestamp(msg.header.stamp))
             geo_coordinates.append((msg.latitude, msg.longitude, msg.altitude))
 
+        if len(timestamps) == 0:
+            return None
+
         if min(query_timestamps) < min(timestamps) or max(timestamps) < max(query_timestamps):
             warnings.warn(
                 "The value in `query_timestamps` is out of range of the observed timestamps, "

diff --git a/perception_dataset/rosbag2/rosbag2_to_non_annotated_t4_converter.py b/perception_dataset/rosbag2/rosbag2_to_non_annotated_t4_converter.py
@@ -872,11 +872,15 @@ def _generate_ego_pose(self, stamp: builtin_interfaces.msg.Time) -> str:
                     "ay": ego_state.accel.y,
                     "az": ego_state.accel.z,
                 },
-                geocoordinate={
-                    "latitude": geocoordinate.latitude,
-                    "longitude": geocoordinate.longitude,
-                    "altitude": geocoordinate.altitude,
-                },
+                geocoordinate=(
+                    {
+                        "latitude": geocoordinate.latitude,
+                        "longitude": geocoordinate.longitude,
+                        "altitude": geocoordinate.altitude,
+                    }
+                    if geocoordinate is not None
+                    else None
+                ),
             )
         else:
             transform_stamped = self._bag_reader.get_transform_stamped(