fix(perception_replayer): order rosbags by starting_time (#43)

* feat(simulator_compatibility_test): temporariy remove tests for control_mode_command (#34) Signed-off-by: Ryohsuke Mitsudome <[email protected]> Signed-off-by: temkei.kem <[email protected]> * fix: order rosbags by starting_time instead of file name. Signed-off-by: temkei.kem <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: Ryohsuke Mitsudome <[email protected]> Signed-off-by: temkei.kem <[email protected]> Co-authored-by: Ryohsuke Mitsudome <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Jun 10, 2024 · 84190d2 · 84190d2
1 parent 6c31a30
commit 84190d2
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 3 deletions.
diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py
@@ -32,6 +32,7 @@
 from rosbag2_py import StorageFilter
 from rosidl_runtime_py.utilities import get_message
 from sensor_msgs.msg import PointCloud2
+from utils import get_starting_time
 from utils import open_reader
 
 
@@ -82,9 +83,13 @@ def __init__(self, args, name):
         # load rosbag
         print("Stared loading rosbag")
         if os.path.isdir(args.bag):
-            for bag_file in sorted(os.listdir(args.bag)):
-                if bag_file.endswith(self.args.rosbag_format):
-                    self.load_rosbag(args.bag + "/" + bag_file)
+            bags = [
+                os.path.join(args.bag, base_name)
+                for base_name in os.listdir(args.bag)
+                if base_name.endswith(args.rosbag_format)
+            ]
+            for bag_file in sorted(bags, key=get_starting_time):
+                self.load_rosbag(bag_file)
         else:
             self.load_rosbag(args.bag)
         print("Ended loading rosbag")

diff --git a/planning/planning_debug_tools/scripts/perception_replayer/utils.py b/planning/planning_debug_tools/scripts/perception_replayer/utils.py
@@ -26,6 +26,11 @@
 from tf_transformations import quaternion_from_euler
 
 
+def get_starting_time(uri: str):
+    info = rosbag2_py.Info().read_metadata(uri, "sqlite3")
+    return info.starting_time
+
+
 def get_rosbag_options(path, serialization_format="cdr"):
     storage_options = rosbag2_py.StorageOptions(uri=path, storage_id="sqlite3")