From b6857c31bce90da759a9e5432b3e32c707bae270 Mon Sep 17 00:00:00 2001
From: 09ubberboy90 <2330834a@student.gla.ac.uk>
Date: Thu, 26 Jan 2023 16:44:05 +0000
Subject: [PATCH] Individual topic processing Handles numpy array error Reduces
 ram usage by individual processing

---
 ros2bag_convert/main.py                       |  9 ++----
 .../ros2bag_convert/message_converter.py      |  8 ++++-
 ros2bag_convert/ros2bag_convert/read_bag.py   | 31 ++++++++++++++++++-
 3 files changed, 40 insertions(+), 8 deletions(-)

diff --git a/ros2bag_convert/main.py b/ros2bag_convert/main.py
index ca007e2..7d6635c 100644
--- a/ros2bag_convert/main.py
+++ b/ros2bag_convert/main.py
@@ -1,5 +1,5 @@
-from .ros2bag_convert import read_bag,save_csv_file
-import sys
+from .ros2bag_convert import read_bag
+import sys, os
 
 argvs = sys.argv
 argc = len(argvs)
@@ -11,10 +11,7 @@ def main():
         print('Usage: #ros2bag-convert xxx.db3')
         quit()
     file_url = argvs[1]
-    data = read_bag.read_from_all_topics(file_url,True)
-    for i in range(len(data)):
-        topic_name,topic_type = data[i][0],data[i][1]
-        save_csv_file.save_csv_file(data[i][2:],file_url[:file_url.rfind("/")]+""+topic_name+".csv")
+    read_bag.read_write_from_all_topics(file_url,True)
 
 if __name__ == '__main__':
     main()
diff --git a/ros2bag_convert/ros2bag_convert/message_converter.py b/ros2bag_convert/ros2bag_convert/message_converter.py
index 0f0bc78..8e63715 100644
--- a/ros2bag_convert/ros2bag_convert/message_converter.py
+++ b/ros2bag_convert/ros2bag_convert/message_converter.py
@@ -255,8 +255,14 @@ def _convert_from_ros_type(field_type, field_value):
         field_value = list(field_value)
     elif _is_field_type_an_array(field_type):
         field_value = _convert_from_ros_array(field_type, field_value)
+    elif field_type == np.ndarray or type(field_value) == np.ndarray:
+        print("Unsupported type: ", field_type)
+        return None
     else:
-        field_value = convert_ros_message_to_dictionary(field_value)
+        try:
+            field_value = convert_ros_message_to_dictionary(field_value)
+        except:
+            print("field_type: ", field_type)
 
     return field_value
 
diff --git a/ros2bag_convert/ros2bag_convert/read_bag.py b/ros2bag_convert/ros2bag_convert/read_bag.py
index 7417a75..dad6c23 100644
--- a/ros2bag_convert/ros2bag_convert/read_bag.py
+++ b/ros2bag_convert/ros2bag_convert/read_bag.py
@@ -5,8 +5,9 @@
 from rclpy.serialization import deserialize_message
 # import sys
 # sys.path.append("/home/ros2/rosbag2_csv/rosbag2/")
-from . import message_converter
+from . import message_converter, save_csv_file
 import json
+import os
 
 def connect(sqlite_file):
     """ Make connection to an SQLite database file. """
@@ -214,3 +215,31 @@ def read_from_all_topics(bag_file, print_out=False):
     close(connection)
 
     return data
+
+def read_write_from_all_topics(bag_file, print_out=False):
+    """ Returns all timestamps and messages from all topics.
+    """
+    # Connect to the database
+    connection, cursor = connect(bag_file)
+
+    # Get all topics names and types
+    topic_names = get_all_topics_names(cursor, print_out=False)
+    print(topic_names)
+    topic_types = get_all_msgs_types(cursor, print_out=False)
+
+    # Get all messages types
+    topic_types = get_all_msgs_types(cursor, print_out=False)
+
+    # Get all timestamps and messages for each topic
+    for i in range(len(topic_names)):
+        timestamps, messages = read_from_topic(bag_file, topic_names[i], print_out)
+        topic_name,topic_type = topic_names[i],topic_types[i]
+        tmp = topic_name.split("/")
+        path, topic_name = "/".join(tmp[:-1]),tmp[-1]
+        path = bag_file[:bag_file.rfind("/")]+""+path
+        os.makedirs(path, exist_ok=True)
+        save_csv_file.save_csv_file([timestamps, messages],path + "/" + topic_name+".csv")
+
+    # Close connection to the database
+    close(connection)
+