feat: ensure /tf and /tf_static topics are present, and adjust frame …

…timestamp comparison (#54) * raise an error when tf does not exit Signed-off-by: Shunsuke Miura <[email protected]> * chore: adjust frame timestamp comparison in _Rosbag2ToNonAnnotatedT4Converter Signed-off-by: Shunsuke Miura <[email protected]> * fix error message Signed-off-by: Shunsuke Miura <[email protected]> --------- Signed-off-by: Shunsuke Miura <[email protected]>
tier4 · Dec 6, 2023 · 691a176 · 691a176
1 parent af2224f
commit 691a176
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 1 deletion.
diff --git a/perception_dataset/rosbag2/rosbag2_reader.py b/perception_dataset/rosbag2/rosbag2_reader.py
@@ -73,6 +73,10 @@ def _get_starting_time(self) -> float:
 
     def _set_tf_buffer(self):
         """set /tf and /tf_static to tf_buffer"""
+        if "/tf" not in self._topic_name_to_topic_type:
+            raise ValueError(f"/tf is not in {self._bag_dir}")
+        if "/tf_static" not in self._topic_name_to_topic_type:
+            raise ValueError(f"/tf_static is not in {self._bag_dir}")
         for message in self.read_messages(topics=["/tf"]):
             for transform in message.transforms:
                 self._tf_buffer.set_transform(transform, "default_authority")

diff --git a/perception_dataset/rosbag2/rosbag2_to_non_annotated_t4_converter.py b/perception_dataset/rosbag2/rosbag2_to_non_annotated_t4_converter.py
@@ -608,7 +608,9 @@ def get_move_distance(trans1: Dict[str, float], trans2: Dict[str, float]) -> flo
                 lidar_unix_timestamp = misc_utils.nusc_timestamp_to_unix_timestamp(
                     sample_record.timestamp
                 )
-                while (image_unix_timestamp - prev_frame_unix_timestamp) > self._TIMESTAMP_DIFF:
+                while (
+                    image_unix_timestamp - prev_frame_unix_timestamp
+                ) > self._camera_latency + self._TIMESTAMP_DIFF:
                     dummy_image_timestamp = self._insert_dummy_image_frame(
                         image_msg,
                         sensor_channel,