From 589d392b4ccdc03faba63c5bcc0514be9ed25994 Mon Sep 17 00:00:00 2001
From: Marina Moreira <67443181+marinagmoreira@users.noreply.github.com>
Date: Mon, 6 Nov 2023 11:43:25 -0800
Subject: [PATCH] Merge merge_bag tools (#745)

* meging python and c++ tool - untested

* tested

* removed commented code

* using utilities instead of function

* running python3 + fixes

* fixing dependencies issues
---
 .../scripts/localization_common/utilities.py  |  5 +-
 tools/bag_processing/scripts/rosbag_merge.py  | 46 ++++++++++---------
 2 files changed, 27 insertions(+), 24 deletions(-)

diff --git a/localization/localization_common/scripts/localization_common/utilities.py b/localization/localization_common/scripts/localization_common/utilities.py
index 8c5ec28463..17708cc26f 100644
--- a/localization/localization_common/scripts/localization_common/utilities.py
+++ b/localization/localization_common/scripts/localization_common/utilities.py
@@ -20,8 +20,6 @@
 import os
 import subprocess
 
-import pandas as pd
-
 
 # Forward errors so we can recover failures
 # even when running commands through multiprocessing
@@ -68,6 +66,9 @@ def create_directory(directory=None):
 
 
 def load_dataframe(files):
+    # Importing the module only within this function
+    import pandas as pd
+
     dataframes = [pd.read_csv(file) for file in files]
     dataframe = pd.concat(dataframes)
     return dataframe
diff --git a/tools/bag_processing/scripts/rosbag_merge.py b/tools/bag_processing/scripts/rosbag_merge.py
index 055f1f1504..d24fa33335 100755
--- a/tools/bag_processing/scripts/rosbag_merge.py
+++ b/tools/bag_processing/scripts/rosbag_merge.py
@@ -24,9 +24,10 @@
 import os
 import re
 import string
+import subprocess
 import sys
 
-import rosbag
+import localization_common.utilities as lu
 
 
 # https://stackoverflow.com/a/4836734
@@ -59,29 +60,31 @@ def merge_bag(input_bag_prefix, input_bag_suffix, merged_bag, only_loc_topics=Fa
         merged_bag_name = input_bag_prefix + ".merged.bag"
 
     sorted_bag_names = natural_sort(bag_names)
+    merge_bags_command = (
+        "rosrun localization_node merge_bags "
+        + " ".join(sorted_bag_names)
+        + " -output_bag "
+        + str(merged_bag_name)
+    )
 
-    topics = None
     if only_loc_topics:
-        topics = [
-            "/hw/imu",
-            "/loc/of/features",
-            "/loc/ml/features",
-            "/loc/ar/features",
-            "/mgt/img_sampler/nav_cam/image_record",
-            "/graph_loc/state",
-            "/gnc/ekf",
-            "/sparse_mapping/pose",
-            "/mob/flight_mode",
-            "/beh/inspection/feedback",
-            "/beh/inspection/goal",
-            "/beh/inspection/result",
-        ]
+        merge_bags_command += (
+            " -save_topics"
+            + " '/hw/imu"
+            + " /loc/of/features"
+            + " /loc/ml/features"
+            + " /loc/ar/features"
+            + " /mgt/img_sampler/nav_cam/image_record"
+            + " /graph_loc/state"
+            + " /gnc/ekf"
+            + " /sparse_mapping/pose"
+            + " /mob/flight_mode"
+            + " /beh/inspection/feedback"
+            + " /beh/inspection/goal"
+            + " /beh/inspection/result'"
+        )
 
-    with rosbag.Bag(merged_bag_name, "w") as merged_bag:
-        for sorted_bag_name in sorted_bag_names:
-            with rosbag.Bag(sorted_bag_name, "r") as sorted_bag:
-                for topic, msg, t in sorted_bag.read_messages(topics):
-                    merged_bag.write(topic, msg, t)
+    lu.run_command_and_save_output(merge_bags_command)
 
 
 if __name__ == "__main__":
@@ -136,7 +139,6 @@ def merge_bag(input_bag_prefix, input_bag_suffix, merged_bag, only_loc_topics=Fa
 
     print(bag_names)
     for bag_name in bag_names:
-        print(bag_name)
         merge_bag(
             bag_name, args.input_bag_suffix, args.merged_bag, args.only_loc_topics
         )