From 43ec5be01fa32965a2a4c9b42b0bfbf8a1d43dde Mon Sep 17 00:00:00 2001
From: Chrislai_502 <chrislai_502@berkeley.edu>
Date: Fri, 2 Jun 2023 15:46:17 -1000
Subject: [PATCH 1/8] Initial Implementation

---
 ros2bag_image_extractor.py | 234 +++++++++++++++++++++++++------------
 1 file changed, 162 insertions(+), 72 deletions(-)

diff --git a/ros2bag_image_extractor.py b/ros2bag_image_extractor.py
index 94693fb..e2777dc 100644
--- a/ros2bag_image_extractor.py
+++ b/ros2bag_image_extractor.py
@@ -16,6 +16,7 @@
 from rosbags.rosbag2 import Reader
 from rosbags.serde import deserialize_cdr
 from cv_bridge import CvBridge
+import rosbag2_py
 
 
 # ---------------------------------------------------------------------------- #
@@ -65,7 +66,7 @@ def undistort(input, distortion_data):
 args = arg_parser.parse_args()
 
 OUTPUT_DIR  = args.output_dir
-ROSBAG_FILE = args.rosbag_file_path
+ROSBAG_FILE_PATH = args.rosbag_file_path
 
 if args.undistort:
     dir_path(args.camera_info_path)
@@ -79,75 +80,164 @@ def undistort(input, distortion_data):
 # ---------------------------------------------------------------------------- #
 
 # ---------------- Create reader instance and open for reading --------------- #
-with Reader(ROSBAG_FILE) as reader:
-
-    camera_found = False
-
-    for connection in reader.connections:
-        if args.verbose:
-            print(connection.topic," : ", connection.msgtype)
-        if 'camera' in connection.topic:
-            camera_found = True
-
-    if camera_found is False:
-        exit()
-
-    # Iterator dictionary to go over camera messages
-    iterator = {
-        '/camera/rear_left/image/compressed'            : 0,
-        '/camera/rear_right/image/compressed'           : 0,
-        '/camera/front_left/image/compressed'           : 0,
-        '/camera/front_left_center/image/compressed'    : 0,
-        '/camera/front_right_center/image/compressed'   : 0,
-        '/camera/front_right/image/compressed'          : 0,
-        '/vimba_rear_left/image'            : 0,
-        '/vimba_rear_right/image'           : 0,
-        '/vimba_front_left/image'           : 0,
-        '/vimba_front_left_center/image'    : 0,
-        '/vimba_front_right_center/image'   : 0,
-        '/vimba_front_right/image'          : 0,
-    }
-
-    for connection, timestamp, rawdata in reader.messages():
-        if connection.topic in iterator.keys():
-            
-            # Update iterator for this topic
-            iterator[connection.topic] += 1
-
-            # Extract message from rosbag
-            msg = deserialize_cdr(rawdata, connection.msgtype)
-            output_topic = None
-            if (connection.msgtype == "sensor_msgs/msg/CompressedImage"):
-                np_arr = np.frombuffer(msg.data, np.uint8)
-                cv2_msg = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
-                output_topic = connection.topic[7:-16]
-            else:
-                # Convert to cv2 image
-                cv2_msg = bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
-                output_topic = connection.topic[:-5]
-
-            # Create a directory for topic in output dir if it does not exist
-            output_directory = OUTPUT_DIR + output_topic
-            if not os.path.exists(output_directory):
-                if args.verbose:
-                    print("Creating Directory: ", output_directory)
-                os.mkdir(output_directory)
-
-            output_file_path = output_directory + 'Image' + '_' + '{0:010d}'.format(iterator[connection.topic]) + '_' + str(msg.header.stamp.sec) + '_' + str(msg.header.stamp.nanosec) + '.jpg'
-            
-            # Undistort Image before Saving
-            if args.undistort:
-                # If distortion parameters not loaded then load them once
-                if connection.topic[1:-6] not in distortion_dict:
-                    yaml_file_path = args.camera_info_path + connection.topic[1:-6] + '.yaml'
-                    distortion_fp = open(file_path(yaml_file_path))
-                    distortion_dict[connection.topic[1:-6]] = yaml.safe_load(distortion_fp)
-                cv2_msg = undistort(cv2_msg, distortion_dict[connection.topic[1:-6]])
-
-            # Save Image
+# with Reader(ROSBAG_FILE) as reader:
+# Check if the extension is a db3 or mcap
+files = os.listdir(ROSBAG_FILE_PATH)
+for file in files:
+    if file.endswith(".db3"):
+        store_type = "sqlite3"
+        print("Detected Input bag is a db3 file.")
+    elif file.endswith(".mcap"):
+        store_type = "mcap"
+        print("Detected Input bag is a mcap file.")
+if not store_type:
+    print("Error: Input bag is not a db3 or mcap file.")
+    exit()
+
+reader = rosbag2_py.SequentialReader()
+
+# Opens the bag files and sets the converter options
+try:
+    reader.open(
+        rosbag2_py.StorageOptions(uri=ROSBAG_FILE_PATH, storage_id=store_type),
+        rosbag2_py.ConverterOptions(
+            input_serialization_format="cdr", output_serialization_format="cdr"
+        ),
+    )
+except Exception as e:
+    print(e)
+    exit()
+
+camera_found = False
+
+# for connection in reader.connections:
+#     if args.verbose:
+#         print(connection.topic," : ", connection.msgtype)
+#     if 'camera' in connection.topic:
+#         camera_found = True
+
+# if camera_found is False:
+#     print("Error: No camera topics found in bag. Exiting...")
+#     exit()
+
+# Iterator dictionary to go over camera messages
+iterator = {
+    '/camera/rear_left/image/compressed'            : 0,
+    '/camera/rear_right/image/compressed'           : 0,
+    '/camera/front_left/image/compressed'           : 0,
+    '/camera/front_left_center/image/compressed'    : 0,
+    '/camera/front_right_center/image/compressed'   : 0,
+    '/camera/front_right/image/compressed'          : 0,
+    '/vimba_rear_left/image'            : 0,
+    '/vimba_rear_right/image'           : 0,
+    '/vimba_front_left/image'           : 0,
+    '/vimba_front_left_center/image'    : 0,
+    '/vimba_front_right_center/image'   : 0,
+    '/vimba_front_right/image'          : 0,
+}
+
+# Checking if all the topics that are going to be collected are in the bag
+TOPIC_TYPES = reader.get_all_topics_and_types()
+TYPE_MAP = {TOPIC_TYPES[i].name: TOPIC_TYPES[i].type for i in range(len(TOPIC_TYPES))}
+if iterator:
+    for topic in iterator.keys():
+        if topic not in TYPE_MAP:
+            print("ERROR: The topic ", topic, " is not in the input bag.")
+            exit()
+        print("Topic ", topic, " found in the input bag.")
+else:
+    print("FATAL ERROR: TOPICS_TO_EXTRACT not defined.")
+
+while reader.has_next():
+    
+    # Read the next message
+    topic_name, data, timestamp = reader.read_next()
+    
+    if topic_name in iterator.keys():
+        # Update iterator for this topic
+        iterator[topic_name] += 1
+
+        # Extract message from rosbag
+        output_topic = None
+        if (TYPE_MAP[topic_name] == "sensor_msgs/msg/CompressedImage"):
+            np_arr = np.frombuffer(data, np.uint8)
+            cv2_msg = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
+            output_topic = topic_name[7:-16]
+        else:
+            # Convert to cv2 image
+            cv2_msg = bridge.imgmsg_to_cv2(data, desired_encoding='bgr8')
+            output_topic = topic_name[:-5]
+
+        # Create a directory for topic in output dir if it does not exist
+        output_directory = OUTPUT_DIR + output_topic
+        if not os.path.exists(output_directory):
             if args.verbose:
-                print('Saving ' + output_file_path)
-
-            if not cv2.imwrite(output_file_path, cv2_msg):
-                raise Exception("Could not write image")
-            
\ No newline at end of file
+                print("Creating Directory: ", output_directory)
+            os.mkdir(output_directory)
+
+        output_file_path = output_directory + 'Image' + '_' + '{0:010d}'.format(iterator[connection.topic]) + '_' + str(msg.header.stamp.sec) + '_' + str(msg.header.stamp.nanosec) + '.jpg'
+        
+        # Undistort Image before Saving
+        if args.undistort:
+            # If distortion parameters not loaded then load them once
+            if topic_name[1:-6] not in distortion_dict:
+                yaml_file_path = args.camera_info_path + topic_name[1:-6] + '.yaml'
+                distortion_fp = open(file_path(yaml_file_path))
+                distortion_dict[topic_name[1:-6]] = yaml.safe_load(distortion_fp)
+            cv2_msg = undistort(cv2_msg, distortion_dict[topic_name[1:-6]])
+
+        # Save Image
+        if args.verbose:
+            print('Saving ' + output_file_path)
+
+        if not cv2.imwrite(output_file_path, cv2_msg):
+            raise Exception("Could not write image")
+        
+# Close the bag file
+del reader
+        
+
+# OLD_CODE
+# for connection, timetimestamp, rawdatastamp, rawdata in reader.messages():
+#     if connection.topic in iterator.keys():
+        
+#         # Update iterator for this topic
+#         iterator[connection.topic] += 1
+
+#         # Extract message from rosbag
+#         msg = deserialize_cdr(rawdata, connection.msgtype)
+#         output_topic = None
+#         if (connection.msgtype == "sensor_msgs/msg/CompressedImage"):
+#             np_arr = np.frombuffer(msg.data, np.uint8)
+#             cv2_msg = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
+#             output_topic = connection.topic[7:-16]
+#         else:
+#             # Convert to cv2 image
+#             cv2_msg = bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
+#             output_topic = connection.topic[:-5]
+
+#         # Create a directory for topic in output dir if it does not exist
+#         output_directory = OUTPUT_DIR + output_topic
+#         if not os.path.exists(output_directory):
+#             if args.verbose:
+#                 print("Creating Directory: ", output_directory)
+#             os.mkdir(output_directory)
+
+#         output_file_path = output_directory + 'Image' + '_' + '{0:010d}'.format(iterator[connection.topic]) + '_' + str(msg.header.stamp.sec) + '_' + str(msg.header.stamp.nanosec) + '.jpg'
+        
+#         # Undistort Image before Saving
+#         if args.undistort:
+#             # If distortion parameters not loaded then load them once
+#             if connection.topic[1:-6] not in distortion_dict:
+#                 yaml_file_path = args.camera_info_path + connection.topic[1:-6] + '.yaml'
+#                 distortion_fp = open(file_path(yaml_file_path))
+#                 distortion_dict[connection.topic[1:-6]] = yaml.safe_load(distortion_fp)
+#             cv2_msg = undistort(cv2_msg, distortion_dict[connection.topic[1:-6]])
+
+#         # Save Image
+#         if args.verbose:
+#             print('Saving ' + output_file_path)
+
+#         if not cv2.imwrite(output_file_path, cv2_msg):
+#             raise Exception("Could not write image")
+        
\ No newline at end of file

From c95444b88eee89f5f54c843121c3bfe3f9988e2e Mon Sep 17 00:00:00 2001
From: Chrislai502 <chrislai_502@berkeley.edu>
Date: Wed, 14 Jun 2023 14:36:56 -1000
Subject: [PATCH 2/8] Mcap support with compressed selection feature done

---
 .gitignore                 |   3 +
 multi_extract.sh           |  55 +++++++++++++----
 ros2bag_image_extractor.py | 118 ++++++++++++-------------------------
 3 files changed, 83 insertions(+), 93 deletions(-)

diff --git a/.gitignore b/.gitignore
index b6e4761..02e203a 100644
--- a/.gitignore
+++ b/.gitignore
@@ -127,3 +127,6 @@ dmypy.json
 
 # Pyre type checker
 .pyre/
+
+# Ignore output folder
+output/
diff --git a/multi_extract.sh b/multi_extract.sh
index dd51fab..f7a047d 100755
--- a/multi_extract.sh
+++ b/multi_extract.sh
@@ -3,12 +3,13 @@
 # ---------------------------------------------------------------------------- #
 #                               DEFAULT VARIABLES                              #
 # ---------------------------------------------------------------------------- #
-DATA_DIR_DEFAULT="/media/Public/ROSBAG_BACKUPS/rosbag2_2022_09_21-12_58_49"
+DATA_DIR_DEFAULT="/home/zhihao/rosbags/ALL_SIX_CAMERAS/"
 VERBOSE_DEFAULT=1
-UNDISTORT_DEFAULT=1
+UNDISTORT_DEFAULT=0
 CALIB_DIR_DEFAULT="/home/roar/ART/perception/Camera/Calibration_new/"
-OUTPUT_BASE_DIR_DEFAULT="/media/Public/Lucas_Oil/"
+OUTPUT_BASE_DIR_DEFAULT="/home/zhihao/chris/IAC_dataset_maker/output/"
 MAKE_VID_DEFAULT=1
+USE_COMPRESSED_DEFAULT=0
 
 # ---------------------------------------------------------------------------- #
 #                          PARSE ENVIRONMENT VARIABLES                         #
@@ -52,6 +53,18 @@ else
     echo "----------------------------------------------------------------------------"
 fi
 
+# ------------------------ USE_COMPRESSED SETTINGS --------------------------- #
+if [ -z ${USE_COMPRESSED+x} ]; then
+    echo "----------------------------------------------------------------------------"
+    USE_COMPRESSED=$USE_COMPRESSED_DEFAULT
+    echo "               USE_COMPRESSED not defined. Setting USE_COMPRESSED to $USE_COMPRESSED.                   "
+    echo "----------------------------------------------------------------------------"
+else
+    echo "----------------------------------------------------------------------------"
+    echo "                       USE_COMPRESSED has been defined as $USE_COMPRESSED                 "
+    echo "----------------------------------------------------------------------------"
+fi
+
 # --------------------------- UNDISTORTION SETTINGS -------------------------- #
 if [ -z ${UNDISTORT:+x} ]; then
     echo "----------------------------------------------------------------------------"
@@ -103,7 +116,7 @@ done
 # ---------------------------------------------------------------------------- #
 echo "\n\nSearching DATA_DIR for ROSBAGS now...\n"
 sleep 5s
-find "$DATA_DIR" -iname "*.db3" -print0 | xargs -0 -I file dirname file | sort | uniq | while read d; do
+find "$DATA_DIR" \( -iname "*.db3" -o -iname "*.mcap" \) -print0 | xargs -0 -I file dirname file | sort | uniq | while read d; do
     ROSBAG_NAME=$(basename "$d")
 
     # ---------------------------------------------------------------------------- #
@@ -120,30 +133,46 @@ find "$DATA_DIR" -iname "*.db3" -print0 | xargs -0 -I file dirname file | sort |
     echo "----------------------------------------------------------------------------"
 
     # -------------------------- Create output Directory ------------------------- #
-    mkdir -p $OUTPUT_DIR
+    mkdir -p "$OUTPUT_DIR"
 
     # ------------------ Begin Extraction Based on User Setting ------------------ #
     if [ $VERBOSE -eq 1 ]; then
         if [ $UNDISTORT -eq 1 ]; then
-            python3 ros2bag_image_extractor.py "$d" $OUTPUT_DIR -vup $CALIB_DIR
+            if [ $USE_COMPRESSED -eq 1 ]; then
+                python3 ros2bag_image_extractor.py "$d" "$OUTPUT_DIR" -vucp "$CALIB_DIR"
+            else 
+                python3 ros2bag_image_extractor.py "$d" "$OUTPUT_DIR" -vup "$CALIB_DIR"
+            fi
         else 
-            python3 ros2bag_image_extractor.py "$d" $OUTPUT_DIR -v
+            if [ $USE_COMPRESSED -eq 1 ]; then
+                python3 ros2bag_image_extractor.py "$d" "$OUTPUT_DIR" -vc
+            else
+                python3 ros2bag_image_extractor.py "$d" "$OUTPUT_DIR" -v
+            fi
         fi
     else
         if [ $UNDISTORT -eq 1 ]; then
-            python3 ros2bag_image_extractor.py "$d" $OUTPUT_DIR -up $CALIB_DIR
+            if [ $USE_COMPRESSED -eq 1 ]; then
+                python3 ros2bag_image_extractor.py "$d" "$OUTPUT_DIR" -ucp "$CALIB_DIR"
+            else 
+                python3 ros2bag_image_extractor.py "$d" "$OUTPUT_DIR" -up "$CALIB_DIR"
+            fi
         else
-            python3 ros2bag_image_extractor.py "$d" $OUTPUT_DIR
+            if [ $USE_COMPRESSED -eq 1 ]; then
+                python3 ros2bag_image_extractor.py "$d" "$OUTPUT_DIR" -c
+            else
+                python3 ros2bag_image_extractor.py "$d" "$OUTPUT_DIR"
+            fi
         fi
     fi
 
     # --------------------- Convert Extracted Images to Video -------------------- #
     if [ $MAKE_VID -eq 1 ]; then
-        for camera_output_dir in $OUTPUT_DIR/*/; do
-            cd $camera_output_dir
+        for camera_output_dir in "$OUTPUT_DIR"/*/; do
+            cd "$camera_output_dir"
             base_name=$(basename "$camera_output_dir")
-            ffmpeg -framerate 50 -pattern_type glob -i '*.jpg' -c:v libx264 -profile:v high -crf 20 -pix_fmt yuv420p ../$base_name.mp4
+            ffmpeg -framerate 50 -pattern_type glob -i '*.jpg' -c:v libx264 -profile:v high -crf 20 -pix_fmt yuv420p "../$base_name.mp4"
         done
     fi
 
-done
+done
\ No newline at end of file
diff --git a/ros2bag_image_extractor.py b/ros2bag_image_extractor.py
index e2777dc..12e7211 100644
--- a/ros2bag_image_extractor.py
+++ b/ros2bag_image_extractor.py
@@ -13,11 +13,13 @@
 import cv2
 
 # --------------------------- ROS related Libraries -------------------------- #
-from rosbags.rosbag2 import Reader
-from rosbags.serde import deserialize_cdr
 from cv_bridge import CvBridge
 import rosbag2_py
 
+from rclpy.serialization import deserialize_message
+from rosidl_runtime_py.utilities import get_message
+
+
 
 # ---------------------------------------------------------------------------- #
 #                                   Functions                                  #
@@ -59,6 +61,7 @@ def undistort(input, distortion_data):
 arg_parser.add_argument('rosbag_file_path', help='Path to rosbag to extract the data from', type=dir_path)
 arg_parser.add_argument('output_dir', help='Path to directory where extracted data should be stored', type=dir_path)
 arg_parser.add_argument('-u', "--undistort", action="store_true")
+arg_parser.add_argument('-c', "--compressed", action="store_true")
 arg_parser.add_argument('-p', '--camera_info_path', help="Path to folder containing yaml config files for camera info for all cameras", type=dir_path)
 arg_parser.add_argument('-v', "--verbose", action="store_true")
 
@@ -66,6 +69,7 @@ def undistort(input, distortion_data):
 args = arg_parser.parse_args()
 
 OUTPUT_DIR  = args.output_dir
+print("Output Directory: ", OUTPUT_DIR)
 ROSBAG_FILE_PATH = args.rosbag_file_path
 
 if args.undistort:
@@ -96,6 +100,7 @@ def undistort(input, distortion_data):
 
 reader = rosbag2_py.SequentialReader()
 
+# ----------------------------- OBTAIN ALL TOPICS ---------------------------- #
 # Opens the bag files and sets the converter options
 try:
     reader.open(
@@ -110,31 +115,25 @@ def undistort(input, distortion_data):
 
 camera_found = False
 
-# for connection in reader.connections:
-#     if args.verbose:
-#         print(connection.topic," : ", connection.msgtype)
-#     if 'camera' in connection.topic:
-#         camera_found = True
-
-# if camera_found is False:
-#     print("Error: No camera topics found in bag. Exiting...")
-#     exit()
-
 # Iterator dictionary to go over camera messages
-iterator = {
-    '/camera/rear_left/image/compressed'            : 0,
-    '/camera/rear_right/image/compressed'           : 0,
-    '/camera/front_left/image/compressed'           : 0,
-    '/camera/front_left_center/image/compressed'    : 0,
-    '/camera/front_right_center/image/compressed'   : 0,
-    '/camera/front_right/image/compressed'          : 0,
-    '/vimba_rear_left/image'            : 0,
-    '/vimba_rear_right/image'           : 0,
-    '/vimba_front_left/image'           : 0,
-    '/vimba_front_left_center/image'    : 0,
-    '/vimba_front_right_center/image'   : 0,
-    '/vimba_front_right/image'          : 0,
-}
+if args.compressed:
+    iterator = {
+        '/vimba_front_left_center/image/compressed'  :0,
+        '/vimba_front_right_center/image/compressed' :0,
+        '/vimba_front_left/image/compressed'         :0,
+        '/vimba_front_right/image/compressed'        :0,
+        '/vimba_rear_left/image/compressed'          :0,
+        '/vimba_rear_right/image/compressed'         :0,
+    }
+else:
+    iterator = {
+        '/vimba_rear_left/image'            : 0,
+        '/vimba_rear_right/image'           : 0,
+        '/vimba_front_left/image'           : 0,
+        '/vimba_front_left_center/image'    : 0,
+        '/vimba_front_right_center/image'   : 0,
+        '/vimba_front_right/image'          : 0,
+    }
 
 # Checking if all the topics that are going to be collected are in the bag
 TOPIC_TYPES = reader.get_all_topics_and_types()
@@ -158,25 +157,30 @@ def undistort(input, distortion_data):
         iterator[topic_name] += 1
 
         # Extract message from rosbag
+        msg_type = TYPE_MAP[topic_name]
+        msg_ser = get_message(msg_type)
+        msg = deserialize_message(data, msg_ser)
         output_topic = None
-        if (TYPE_MAP[topic_name] == "sensor_msgs/msg/CompressedImage"):
-            np_arr = np.frombuffer(data, np.uint8)
+        if (args.compressed and msg_type == "sensor_msgs/msg/CompressedImage"):
+            np_arr = np.frombuffer(msg.data, np.uint8)
             cv2_msg = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
-            output_topic = topic_name[7:-16]
+            output_topic = topic_name[7:-17]
         else:
             # Convert to cv2 image
-            cv2_msg = bridge.imgmsg_to_cv2(data, desired_encoding='bgr8')
-            output_topic = topic_name[:-5]
+            cv2_msg = bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
+            output_topic = topic_name[1:-6]
 
         # Create a directory for topic in output dir if it does not exist
-        output_directory = OUTPUT_DIR + output_topic
+        print("Output Topic: ", output_topic)
+        output_directory = os.path.join(OUTPUT_DIR, output_topic)
+        
         if not os.path.exists(output_directory):
             if args.verbose:
                 print("Creating Directory: ", output_directory)
             os.mkdir(output_directory)
 
-        output_file_path = output_directory + 'Image' + '_' + '{0:010d}'.format(iterator[connection.topic]) + '_' + str(msg.header.stamp.sec) + '_' + str(msg.header.stamp.nanosec) + '.jpg'
-        
+        output_file_path = os.path.join(output_directory, 'Image' + '_' + '{0:010d}'.format(iterator[topic_name]) + '_' + str(msg.header.stamp.sec) + '_' + str(msg.header.stamp.nanosec) + '.jpg')
+
         # Undistort Image before Saving
         if args.undistort:
             # If distortion parameters not loaded then load them once
@@ -195,49 +199,3 @@ def undistort(input, distortion_data):
         
 # Close the bag file
 del reader
-        
-
-# OLD_CODE
-# for connection, timetimestamp, rawdatastamp, rawdata in reader.messages():
-#     if connection.topic in iterator.keys():
-        
-#         # Update iterator for this topic
-#         iterator[connection.topic] += 1
-
-#         # Extract message from rosbag
-#         msg = deserialize_cdr(rawdata, connection.msgtype)
-#         output_topic = None
-#         if (connection.msgtype == "sensor_msgs/msg/CompressedImage"):
-#             np_arr = np.frombuffer(msg.data, np.uint8)
-#             cv2_msg = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
-#             output_topic = connection.topic[7:-16]
-#         else:
-#             # Convert to cv2 image
-#             cv2_msg = bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
-#             output_topic = connection.topic[:-5]
-
-#         # Create a directory for topic in output dir if it does not exist
-#         output_directory = OUTPUT_DIR + output_topic
-#         if not os.path.exists(output_directory):
-#             if args.verbose:
-#                 print("Creating Directory: ", output_directory)
-#             os.mkdir(output_directory)
-
-#         output_file_path = output_directory + 'Image' + '_' + '{0:010d}'.format(iterator[connection.topic]) + '_' + str(msg.header.stamp.sec) + '_' + str(msg.header.stamp.nanosec) + '.jpg'
-        
-#         # Undistort Image before Saving
-#         if args.undistort:
-#             # If distortion parameters not loaded then load them once
-#             if connection.topic[1:-6] not in distortion_dict:
-#                 yaml_file_path = args.camera_info_path + connection.topic[1:-6] + '.yaml'
-#                 distortion_fp = open(file_path(yaml_file_path))
-#                 distortion_dict[connection.topic[1:-6]] = yaml.safe_load(distortion_fp)
-#             cv2_msg = undistort(cv2_msg, distortion_dict[connection.topic[1:-6]])
-
-#         # Save Image
-#         if args.verbose:
-#             print('Saving ' + output_file_path)
-
-#         if not cv2.imwrite(output_file_path, cv2_msg):
-#             raise Exception("Could not write image")
-        
\ No newline at end of file

From 46735e194e43851eb73a0d6bfb4043eb1c2c13c1 Mon Sep 17 00:00:00 2001
From: Chrislai502 <chrislai_502@berkeley.edu>
Date: Wed, 14 Jun 2023 14:50:37 -1000
Subject: [PATCH 3/8] README.md

---
 README.md | 1 +
 1 file changed, 1 insertion(+)

diff --git a/README.md b/README.md
index 174d96b..4a1b2d6 100644
--- a/README.md
+++ b/README.md
@@ -60,6 +60,7 @@ Use the following steps to extract data from a rosbag, remove image distortions,
 
 # Contact
 * Aman Saraf | [amansaraf99@gmail.com](mailto:amansaraf99@gmail.com)
+* Chris Lai  | [chris.lai@berkeley.edu](mailto:chris.lai@berkeley.edu)
 
     
 

From 39104cd4745124887ccb2e63f0f462a728848295 Mon Sep 17 00:00:00 2001
From: KaushikKunal <kunalsingh747@gmail.com>
Date: Wed, 20 Sep 2023 02:24:09 -0700
Subject: [PATCH 4/8] Debugging, cd into wrong folder. If topic doesn't exist,
 just skip

---
 multi_extract.sh           | 32 +++++++++++++++++++++++---------
 ros2bag_image_extractor.py |  2 +-
 2 files changed, 24 insertions(+), 10 deletions(-)

diff --git a/multi_extract.sh b/multi_extract.sh
index f7a047d..d0839ae 100755
--- a/multi_extract.sh
+++ b/multi_extract.sh
@@ -3,11 +3,12 @@
 # ---------------------------------------------------------------------------- #
 #                               DEFAULT VARIABLES                              #
 # ---------------------------------------------------------------------------- #
-DATA_DIR_DEFAULT="/home/zhihao/rosbags/ALL_SIX_CAMERAS/"
+DATA_DIR_DEFAULT="/home/art-berk/rosbags/"
 VERBOSE_DEFAULT=1
-UNDISTORT_DEFAULT=0
-CALIB_DIR_DEFAULT="/home/roar/ART/perception/Camera/Calibration_new/"
-OUTPUT_BASE_DIR_DEFAULT="/home/zhihao/chris/IAC_dataset_maker/output/"
+UNDISTORT_DEFAULT=1
+# CALIB_DIR_DEFAULT="/home/roar/ART/perception/Camera/Calibration_new/"
+CALIB_DIR_DEFAULT="/home/art-berk/IAC_dataset_maker/putnam_calib/"
+OUTPUT_BASE_DIR_DEFAULT="/home/art-berk/IAC_dataset_maker/output/"
 MAKE_VID_DEFAULT=1
 USE_COMPRESSED_DEFAULT=0
 
@@ -103,8 +104,8 @@ else
 fi
 # --------------------- ENVIRONMENT VARIABLE PARSING END --------------------- #
 
-echo "\nPausing for 10 seconds. Press Ctrl+C to quit if the above settings are not correct.\n"
-for i in 1 2 3 4 5 6 7 8 9 10
+echo "\nPausing for 5 seconds. Press Ctrl+C to quit if the above settings are not correct.\n"
+for i in 1 2 3 4 5 
 do
   echo "$i seconds passed"
   sleep 1s
@@ -115,7 +116,8 @@ done
 #                 Find all rosbags at datadir and extract data                 #
 # ---------------------------------------------------------------------------- #
 echo "\n\nSearching DATA_DIR for ROSBAGS now...\n"
-sleep 5s
+root_dir=$(pwd)
+sleep 2s
 find "$DATA_DIR" \( -iname "*.db3" -o -iname "*.mcap" \) -print0 | xargs -0 -I file dirname file | sort | uniq | while read d; do
     ROSBAG_NAME=$(basename "$d")
 
@@ -134,8 +136,12 @@ find "$DATA_DIR" \( -iname "*.db3" -o -iname "*.mcap" \) -print0 | xargs -0 -I f
 
     # -------------------------- Create output Directory ------------------------- #
     mkdir -p "$OUTPUT_DIR"
-
+    echo 
     # ------------------ Begin Extraction Based on User Setting ------------------ #
+    cd "$root_dir"
+    current_dir=$(pwd)
+    echo "Current working directory is: $current_dir"
+    
     if [ $VERBOSE -eq 1 ]; then
         if [ $UNDISTORT -eq 1 ]; then
             if [ $USE_COMPRESSED -eq 1 ]; then
@@ -169,10 +175,18 @@ find "$DATA_DIR" \( -iname "*.db3" -o -iname "*.mcap" \) -print0 | xargs -0 -I f
     # --------------------- Convert Extracted Images to Video -------------------- #
     if [ $MAKE_VID -eq 1 ]; then
         for camera_output_dir in "$OUTPUT_DIR"/*/; do
+            echo "$OUTPUT_DIR"/*/
             cd "$camera_output_dir"
             base_name=$(basename "$camera_output_dir")
             ffmpeg -framerate 50 -pattern_type glob -i '*.jpg' -c:v libx264 -profile:v high -crf 20 -pix_fmt yuv420p "../$base_name.mp4"
+            cd -
         done
-    fi
+    
+        # for camera_output_dir in "$OUTPUT_DIR"/*/; do
+        #     cd "$camera_output_dir"
+        #     base_name=$(basename "${camera_output_dir%/}")
+        #     ffmpeg -framerate 50 -pattern_type glob -i '*.jpg' -c:v libx264 -profile:v high -crf 20 -pix_fmt yuv420p "../$base_name.mp4"
+        # done
 
+    fi
 done
\ No newline at end of file
diff --git a/ros2bag_image_extractor.py b/ros2bag_image_extractor.py
index 12e7211..4238991 100644
--- a/ros2bag_image_extractor.py
+++ b/ros2bag_image_extractor.py
@@ -142,7 +142,7 @@ def undistort(input, distortion_data):
     for topic in iterator.keys():
         if topic not in TYPE_MAP:
             print("ERROR: The topic ", topic, " is not in the input bag.")
-            exit()
+            # exit()
         print("Topic ", topic, " found in the input bag.")
 else:
     print("FATAL ERROR: TOPICS_TO_EXTRACT not defined.")

From 787e62efaf350dc4f1aa658df5350e85018ad4cb Mon Sep 17 00:00:00 2001
From: chrislai502 <chrislai_502@berkeley.edu>
Date: Wed, 20 Sep 2023 02:29:57 -0700
Subject: [PATCH 5/8] Previous commit was also chris, please do not set global
 configs guys

---
 multi_extract.sh | 1 -
 1 file changed, 1 deletion(-)

diff --git a/multi_extract.sh b/multi_extract.sh
index d0839ae..6d2f6b1 100755
--- a/multi_extract.sh
+++ b/multi_extract.sh
@@ -175,7 +175,6 @@ find "$DATA_DIR" \( -iname "*.db3" -o -iname "*.mcap" \) -print0 | xargs -0 -I f
     # --------------------- Convert Extracted Images to Video -------------------- #
     if [ $MAKE_VID -eq 1 ]; then
         for camera_output_dir in "$OUTPUT_DIR"/*/; do
-            echo "$OUTPUT_DIR"/*/
             cd "$camera_output_dir"
             base_name=$(basename "$camera_output_dir")
             ffmpeg -framerate 50 -pattern_type glob -i '*.jpg' -c:v libx264 -profile:v high -crf 20 -pix_fmt yuv420p "../$base_name.mp4"

From b4f5f455959c796852a211d9850428746d20fdc9 Mon Sep 17 00:00:00 2001
From: chrislai502 <chrislai_502@berkeley.edu>
Date: Wed, 20 Sep 2023 04:17:20 -0700
Subject: [PATCH 6/8] Solved file descriptor issue in ffmpeg

---
 multi_extract.sh           | 61 ++++++++++++++++++++++----------------
 ros2bag_image_extractor.py | 13 +++++++-
 2 files changed, 47 insertions(+), 27 deletions(-)

diff --git a/multi_extract.sh b/multi_extract.sh
index 6d2f6b1..4bed96a 100755
--- a/multi_extract.sh
+++ b/multi_extract.sh
@@ -4,7 +4,7 @@
 #                               DEFAULT VARIABLES                              #
 # ---------------------------------------------------------------------------- #
 DATA_DIR_DEFAULT="/home/art-berk/rosbags/"
-VERBOSE_DEFAULT=1
+VERBOSE_DEFAULT=0
 UNDISTORT_DEFAULT=1
 # CALIB_DIR_DEFAULT="/home/roar/ART/perception/Camera/Calibration_new/"
 CALIB_DIR_DEFAULT="/home/art-berk/IAC_dataset_maker/putnam_calib/"
@@ -118,8 +118,19 @@ done
 echo "\n\nSearching DATA_DIR for ROSBAGS now...\n"
 root_dir=$(pwd)
 sleep 2s
-find "$DATA_DIR" \( -iname "*.db3" -o -iname "*.mcap" \) -print0 | xargs -0 -I file dirname file | sort | uniq | while read d; do
-    ROSBAG_NAME=$(basename "$d")
+# find "$DATA_DIR" \( -iname "*.db3" -o -iname "*.mcap" \) -print0 | xargs -0 -I file dirname file | while read d; do
+# find "$DATA_DIR" \( -iname "*.db3" -o -iname "*.mcap" \) -print0 | xargs -0 -I file dirname file | sort -u | while IFS= read -r d; do
+#     echo "$d"
+# done
+find "$DATA_DIR" \( -iname "*.db3" -o -iname "*.mcap" \) -print0 | xargs -0 -I file dirname file | sort -u > /tmp/tempfile.txt
+
+
+# find "$DATA_DIR" \( -iname "*.db3" -o -iname "*.mcap" \) -print0 | xargs -0 -I file dirname file | sort -u | while IFS= read -r d; do
+while IFS= read -r line; do
+    echo "$line"
+
+    ROSBAG_NAME=$(basename "$line")
+    echo "ROSBAG_NAME $ROSBAG_NAME"
 
     # ---------------------------------------------------------------------------- #
     #                           NOTE: SPECIFY OUTPUT DIR                           #
@@ -130,44 +141,42 @@ find "$DATA_DIR" \( -iname "*.db3" -o -iname "*.mcap" \) -print0 | xargs -0 -I f
 
     # ------------------------------ Debug Verbosity ----------------------------- #
     echo "----------------------------------------------------------------------------"
-    echo "Found ROSBAG:              $d"
+    echo "Found ROSBAG:              $line"
     echo "Extracting images to:      $OUTPUT_DIR"
     echo "----------------------------------------------------------------------------"
 
     # -------------------------- Create output Directory ------------------------- #
     mkdir -p "$OUTPUT_DIR"
-    echo 
+
     # ------------------ Begin Extraction Based on User Setting ------------------ #
     cd "$root_dir"
-    current_dir=$(pwd)
-    echo "Current working directory is: $current_dir"
     
     if [ $VERBOSE -eq 1 ]; then
         if [ $UNDISTORT -eq 1 ]; then
             if [ $USE_COMPRESSED -eq 1 ]; then
-                python3 ros2bag_image_extractor.py "$d" "$OUTPUT_DIR" -vucp "$CALIB_DIR"
+                python3 ros2bag_image_extractor.py "$line" "$OUTPUT_DIR" -vucp "$CALIB_DIR"
             else 
-                python3 ros2bag_image_extractor.py "$d" "$OUTPUT_DIR" -vup "$CALIB_DIR"
+                python3 ros2bag_image_extractor.py "$line" "$OUTPUT_DIR" -vup "$CALIB_DIR"
             fi
         else 
             if [ $USE_COMPRESSED -eq 1 ]; then
-                python3 ros2bag_image_extractor.py "$d" "$OUTPUT_DIR" -vc
+                python3 ros2bag_image_extractor.py "$line" "$OUTPUT_DIR" -vc
             else
-                python3 ros2bag_image_extractor.py "$d" "$OUTPUT_DIR" -v
+                python3 ros2bag_image_extractor.py "$line" "$OUTPUT_DIR" -v
             fi
         fi
     else
         if [ $UNDISTORT -eq 1 ]; then
             if [ $USE_COMPRESSED -eq 1 ]; then
-                python3 ros2bag_image_extractor.py "$d" "$OUTPUT_DIR" -ucp "$CALIB_DIR"
+                python3 ros2bag_image_extractor.py "$line" "$OUTPUT_DIR" -ucp "$CALIB_DIR"
             else 
-                python3 ros2bag_image_extractor.py "$d" "$OUTPUT_DIR" -up "$CALIB_DIR"
+                python3 ros2bag_image_extractor.py "$line" "$OUTPUT_DIR" -up "$CALIB_DIR"
             fi
         else
             if [ $USE_COMPRESSED -eq 1 ]; then
-                python3 ros2bag_image_extractor.py "$d" "$OUTPUT_DIR" -c
+                python3 ros2bag_image_extractor.py "$line" "$OUTPUT_DIR" -c
             else
-                python3 ros2bag_image_extractor.py "$d" "$OUTPUT_DIR"
+                python3 ros2bag_image_extractor.py "$line" "$OUTPUT_DIR"
             fi
         fi
     fi
@@ -175,17 +184,17 @@ find "$DATA_DIR" \( -iname "*.db3" -o -iname "*.mcap" \) -print0 | xargs -0 -I f
     # --------------------- Convert Extracted Images to Video -------------------- #
     if [ $MAKE_VID -eq 1 ]; then
         for camera_output_dir in "$OUTPUT_DIR"/*/; do
-            cd "$camera_output_dir"
             base_name=$(basename "$camera_output_dir")
-            ffmpeg -framerate 50 -pattern_type glob -i '*.jpg' -c:v libx264 -profile:v high -crf 20 -pix_fmt yuv420p "../$base_name.mp4"
-            cd -
-        done
-    
-        # for camera_output_dir in "$OUTPUT_DIR"/*/; do
-        #     cd "$camera_output_dir"
-        #     base_name=$(basename "${camera_output_dir%/}")
-        #     ffmpeg -framerate 50 -pattern_type glob -i '*.jpg' -c:v libx264 -profile:v high -crf 20 -pix_fmt yuv420p "../$base_name.mp4"
-        # done
 
+            # Ah, that narrows things down a bit. The behavior you're seeing could be related to ffmpeg inadvertently reading from standard input, which affects the subsequent iterations of the loop that reads from /tmp/tempfile.txt.
+            # Here's a solution: redirect the standard input of ffmpeg to /dev/null. This ensures that ffmpeg won't interfere with the input being read by the loop.
+            # Modify the ffmpeg line as follows:
+            ffmpeg -framerate 50 -pattern_type glob -i "$camera_output_dir/*.jpg" -c:v libx264 -profile:v high -crf 20 -pix_fmt yuv420p "./output/$ROSBAG_NAME/$base_name.mp4" > /dev/null 2>&1 < /dev/null
+
+        done
     fi
-done
\ No newline at end of file
+    sleep 1s
+# done
+done < /tmp/tempfile.txt
+
+rm /tmp/tempfile.txt
\ No newline at end of file
diff --git a/ros2bag_image_extractor.py b/ros2bag_image_extractor.py
index 4238991..7647d5e 100644
--- a/ros2bag_image_extractor.py
+++ b/ros2bag_image_extractor.py
@@ -28,15 +28,19 @@
 # ------------------------ Check if directory is valid ----------------------- #
 def dir_path(string):
     if os.path.isdir(string):
+        print(f"Is a directory! {string}")
         return string
     else:
+        print(f"Is not a directory! {string}")
         raise NotADirectoryError(string)
 
 # ----------------------- Check if file path is valid ----------------------- #
 def file_path(string):
     if os.path.isfile(string):
+        print(f"Is a file! {string}")
         return string
     else:
+        print(f"Is not a string! {string}")
         raise FileNotFoundError(string)
 
 # ------------------------------ Undistort Image ----------------------------- #
@@ -147,6 +151,8 @@ def undistort(input, distortion_data):
 else:
     print("FATAL ERROR: TOPICS_TO_EXTRACT not defined.")
 
+counter = 0
+
 while reader.has_next():
     
     # Read the next message
@@ -171,7 +177,7 @@ def undistort(input, distortion_data):
             output_topic = topic_name[1:-6]
 
         # Create a directory for topic in output dir if it does not exist
-        print("Output Topic: ", output_topic)
+        # print("Output Topic: ", output_topic)
         output_directory = os.path.join(OUTPUT_DIR, output_topic)
         
         if not os.path.exists(output_directory):
@@ -191,8 +197,13 @@ def undistort(input, distortion_data):
             cv2_msg = undistort(cv2_msg, distortion_dict[topic_name[1:-6]])
 
         # Save Image
+        
+
         if args.verbose:
             print('Saving ' + output_file_path)
+        counter += 1
+        if counter %1000 == 0:
+            print(f"Processed {counter} Images")
 
         if not cv2.imwrite(output_file_path, cv2_msg):
             raise Exception("Could not write image")

From 31c283a4198a6a9483e7ea3e9e1f0d7858d9047e Mon Sep 17 00:00:00 2001
From: chrislai502 <chrislai_502@berkeley.edu>
Date: Thu, 2 Nov 2023 16:32:33 -0700
Subject: [PATCH 7/8] Handling edge cases where ROSBAG does not have images, or
 missing certain image topics. Cleaned up all error messages

---
 multi_extract.sh                           | 25 +++---
 organizer.py                               | 27 ++++++
 putnam_calib/vimba_front_left.yaml         | 23 +++++
 putnam_calib/vimba_front_left_center.yaml  | 23 +++++
 putnam_calib/vimba_front_right.yaml        | 23 +++++
 putnam_calib/vimba_front_right_center.yaml | 23 +++++
 ros2bag_image_extractor.py                 | 97 ++++++++++++++--------
 7 files changed, 194 insertions(+), 47 deletions(-)
 create mode 100644 organizer.py
 create mode 100644 putnam_calib/vimba_front_left.yaml
 create mode 100644 putnam_calib/vimba_front_left_center.yaml
 create mode 100644 putnam_calib/vimba_front_right.yaml
 create mode 100644 putnam_calib/vimba_front_right_center.yaml

diff --git a/multi_extract.sh b/multi_extract.sh
index 4bed96a..429e8fc 100755
--- a/multi_extract.sh
+++ b/multi_extract.sh
@@ -3,7 +3,7 @@
 # ---------------------------------------------------------------------------- #
 #                               DEFAULT VARIABLES                              #
 # ---------------------------------------------------------------------------- #
-DATA_DIR_DEFAULT="/home/art-berk/rosbags/"
+DATA_DIR_DEFAULT="/media/art-berk/DRIVE2_ART/rosbags/"
 VERBOSE_DEFAULT=0
 UNDISTORT_DEFAULT=1
 # CALIB_DIR_DEFAULT="/home/roar/ART/perception/Camera/Calibration_new/"
@@ -127,10 +127,9 @@ find "$DATA_DIR" \( -iname "*.db3" -o -iname "*.mcap" \) -print0 | xargs -0 -I f
 
 # find "$DATA_DIR" \( -iname "*.db3" -o -iname "*.mcap" \) -print0 | xargs -0 -I file dirname file | sort -u | while IFS= read -r d; do
 while IFS= read -r line; do
-    echo "$line"
 
     ROSBAG_NAME=$(basename "$line")
-    echo "ROSBAG_NAME $ROSBAG_NAME"
+    # echo "ROSBAG_NAME $ROSBAG_NAME"
 
     # ---------------------------------------------------------------------------- #
     #                           NOTE: SPECIFY OUTPUT DIR                           #
@@ -182,17 +181,19 @@ while IFS= read -r line; do
     fi
 
     # --------------------- Convert Extracted Images to Video -------------------- #
-    if [ $MAKE_VID -eq 1 ]; then
-        for camera_output_dir in "$OUTPUT_DIR"/*/; do
-            base_name=$(basename "$camera_output_dir")
+    if [ ! -d "$OUTPUT_DIR" ] || [ -z "$(ls -A $OUTPUT_DIR)" ]; then # Check if the output directory is not empty first
+        if [ $MAKE_VID -eq 1 ]; then
+            for camera_output_dir in "$OUTPUT_DIR"/*/; do
+                base_name=$(basename "$camera_output_dir")
 
-            # Ah, that narrows things down a bit. The behavior you're seeing could be related to ffmpeg inadvertently reading from standard input, which affects the subsequent iterations of the loop that reads from /tmp/tempfile.txt.
-            # Here's a solution: redirect the standard input of ffmpeg to /dev/null. This ensures that ffmpeg won't interfere with the input being read by the loop.
-            # Modify the ffmpeg line as follows:
-            ffmpeg -framerate 50 -pattern_type glob -i "$camera_output_dir/*.jpg" -c:v libx264 -profile:v high -crf 20 -pix_fmt yuv420p "./output/$ROSBAG_NAME/$base_name.mp4" > /dev/null 2>&1 < /dev/null
+                # Ah, that narrows things down a bit. The behavior you're seeing could be related to ffmpeg inadvertently reading from standard input, which affects the subsequent iterations of the loop that reads from /tmp/tempfile.txt.
+                # Here's a solution: redirect the standard input of ffmpeg to /dev/null. This ensures that ffmpeg won't interfere with the input being read by the loop.
+                # Modify the ffmpeg line as follows:
+                ffmpeg -framerate 50 -pattern_type glob -i "$camera_output_dir/*.jpg" -c:v libx264 -profile:v high -crf 20 -pix_fmt yuv420p "./output/$ROSBAG_NAME/$base_name.mp4" > /dev/null 2>&1 < /dev/null
 
-        done
-    fi
+            done
+        fi
+    fi 
     sleep 1s
 # done
 done < /tmp/tempfile.txt
diff --git a/organizer.py b/organizer.py
new file mode 100644
index 0000000..c88e0f1
--- /dev/null
+++ b/organizer.py
@@ -0,0 +1,27 @@
+import os
+import shutil
+
+# Define the paths
+first_path = '/home/art-berk/IAC_dataset_maker/output'
+second_path = '/media/art-berk/DRIVE2_ART/rosbags'
+
+# Ensure 'rosbag_with_images_extracted' directory exists
+destination_folder = os.path.join(second_path, 'rosbag_with_images_extracted')
+os.makedirs(destination_folder, exist_ok=True)
+
+# Get a set of all directory names in first path
+first_path_dirs = {dir_name for dir_name in os.listdir(first_path)
+                   if os.path.isdir(os.path.join(first_path, dir_name))}
+
+# Iterate over all directories in the second path
+for dir_name in os.listdir(second_path):
+    dir_path = os.path.join(second_path, dir_name)
+    # Check if it's a directory and not the destination directory
+    if dir_name not in first_path_dirs or not os.path.isdir(dir_path) or dir_path == destination_folder:
+        continue  # Skip if no match or it's not a directory or if it is the destination directory
+    
+    # Move matched directory to the 'rosbag_with_images_extracted' directory
+    shutil.move(dir_path, destination_folder)
+    print(f"Moved '{dir_name}' to '{destination_folder}'")
+
+print("Operation completed.")
diff --git a/putnam_calib/vimba_front_left.yaml b/putnam_calib/vimba_front_left.yaml
new file mode 100644
index 0000000..8b9a52c
--- /dev/null
+++ b/putnam_calib/vimba_front_left.yaml
@@ -0,0 +1,23 @@
+camera_matrix:
+  rows: 3
+  cols: 3
+  data:
+    - 243.762740
+    - 0.000000
+    - 259.440156
+    - 0.000000
+    - 244.236210
+    - 191.623041
+    - 0.000000
+    - 0.000000
+    - 1.000000
+
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data:
+    - -0.179154
+    - 0.041029
+    - -0.001240
+    - 0.000456
+    - 0.000000  
\ No newline at end of file
diff --git a/putnam_calib/vimba_front_left_center.yaml b/putnam_calib/vimba_front_left_center.yaml
new file mode 100644
index 0000000..60c4fcb
--- /dev/null
+++ b/putnam_calib/vimba_front_left_center.yaml
@@ -0,0 +1,23 @@
+camera_matrix:
+  rows: 3
+  cols: 3
+  data:
+    - 1637.855960 
+    - 0.000000
+    - 546.820824
+    - 0.000000
+    - 1629.665877
+    - 367.335716
+    - 0.000000
+    - 0.000000
+    - 1.000000
+
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data:
+    - -0.316097
+    - 0.461574
+    - -0.001655
+    - 0.004033
+    - 0.000000
diff --git a/putnam_calib/vimba_front_right.yaml b/putnam_calib/vimba_front_right.yaml
new file mode 100644
index 0000000..a7c7aad
--- /dev/null
+++ b/putnam_calib/vimba_front_right.yaml
@@ -0,0 +1,23 @@
+camera_matrix:
+  rows: 3
+  cols: 3
+  data:
+    - 241.574152
+    - 0.000000
+    - 245.622459
+    - 0.000000
+    - 240.962419
+    - 188.186666
+    - 0.000000
+    - 0.000000
+    - 1.000000
+
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data:
+    - -0.182866
+    - 0.042365
+    - -0.001724
+    - -0.000473
+    - 0.000000
diff --git a/putnam_calib/vimba_front_right_center.yaml b/putnam_calib/vimba_front_right_center.yaml
new file mode 100644
index 0000000..2285b26
--- /dev/null
+++ b/putnam_calib/vimba_front_right_center.yaml
@@ -0,0 +1,23 @@
+camera_matrix:
+  rows: 3
+  cols: 3
+  data:
+  - 1669.803681
+  - 0.000000
+  - 550.637949
+  - 0.000000
+  - 1667.774209
+  - 261.517346
+  - 0.000000
+  - 0.000000
+  - 1.000000
+
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data:
+  - -0.285679
+  - -0.003097
+  - -0.007689
+  - -0.000588
+  - 0.000000
\ No newline at end of file
diff --git a/ros2bag_image_extractor.py b/ros2bag_image_extractor.py
index 7647d5e..13117a4 100644
--- a/ros2bag_image_extractor.py
+++ b/ros2bag_image_extractor.py
@@ -28,19 +28,19 @@
 # ------------------------ Check if directory is valid ----------------------- #
 def dir_path(string):
     if os.path.isdir(string):
-        print(f"Is a directory! {string}")
+        # print(f"Is a directory! {string}")
         return string
     else:
-        print(f"Is not a directory! {string}")
+        # print(f"Is not a directory! {string}")
         raise NotADirectoryError(string)
 
 # ----------------------- Check if file path is valid ----------------------- #
 def file_path(string):
     if os.path.isfile(string):
-        print(f"Is a file! {string}")
+        # print(f"Is a file! {string}")
         return string
     else:
-        print(f"Is not a string! {string}")
+        # print(f"Is not a string! {string}")
         raise FileNotFoundError(string)
 
 # ------------------------------ Undistort Image ----------------------------- #
@@ -65,7 +65,7 @@ def undistort(input, distortion_data):
 arg_parser.add_argument('rosbag_file_path', help='Path to rosbag to extract the data from', type=dir_path)
 arg_parser.add_argument('output_dir', help='Path to directory where extracted data should be stored', type=dir_path)
 arg_parser.add_argument('-u', "--undistort", action="store_true")
-arg_parser.add_argument('-c', "--compressed", action="store_true")
+arg_parser.add_argument('-c', "--compressed", action="store_false")
 arg_parser.add_argument('-p', '--camera_info_path', help="Path to folder containing yaml config files for camera info for all cameras", type=dir_path)
 arg_parser.add_argument('-v', "--verbose", action="store_true")
 
@@ -73,7 +73,15 @@ def undistort(input, distortion_data):
 args = arg_parser.parse_args()
 
 OUTPUT_DIR  = args.output_dir
-print("Output Directory: ", OUTPUT_DIR)
+
+# Check if output directory exists
+if os.path.exists(OUTPUT_DIR):
+    # If it does exist, check if the directory is empty. If it is, just leave it. If not, just skip the rosbag.
+    if os.listdir(OUTPUT_DIR):
+        print("[script] Directory Exists and is Not Empty! Exiting...")
+        exit()
+
+print("[debug] Output Directory: ", OUTPUT_DIR)
 ROSBAG_FILE_PATH = args.rosbag_file_path
 
 if args.undistort:
@@ -91,15 +99,17 @@ def undistort(input, distortion_data):
 # with Reader(ROSBAG_FILE) as reader:
 # Check if the extension is a db3 or mcap
 files = os.listdir(ROSBAG_FILE_PATH)
+print(f"[script] ROSBAG filepath: {ROSBAG_FILE_PATH}")
 for file in files:
     if file.endswith(".db3"):
         store_type = "sqlite3"
-        print("Detected Input bag is a db3 file.")
+        print("[script] Detected Input bag is a db3 file.")
+        
     elif file.endswith(".mcap"):
         store_type = "mcap"
-        print("Detected Input bag is a mcap file.")
+        print("[script] Detected Input bag is a mcap file.")
 if not store_type:
-    print("Error: Input bag is not a db3 or mcap file.")
+    print(f"[script] FATAL ERROR: Input bag is not a db3 or mcap file")
     exit()
 
 reader = rosbag2_py.SequentialReader()
@@ -117,39 +127,51 @@ def undistort(input, distortion_data):
     print(e)
     exit()
 
-camera_found = False
 
-# Iterator dictionary to go over camera messages
+# Check if there are images in the ROSBAG, if not, skip!
+# Get all the topic types and 
 if args.compressed:
-    iterator = {
-        '/vimba_front_left_center/image/compressed'  :0,
-        '/vimba_front_right_center/image/compressed' :0,
-        '/vimba_front_left/image/compressed'         :0,
-        '/vimba_front_right/image/compressed'        :0,
-        '/vimba_rear_left/image/compressed'          :0,
-        '/vimba_rear_right/image/compressed'         :0,
+
+    image_topics = {
+        '/vimba_front_left_center/image/compressed',
+        '/vimba_front_right_center/image/compressed',
+        '/vimba_front_left/image/compressed', 
+        '/vimba_front_right/image/compressed',        
+        '/vimba_rear_left/image/compressed',       
+        '/vimba_rear_right/image/compressed',   
+        '/vimba_rear_left/image'            ,
+        '/vimba_rear_right/image'           ,
+        '/vimba_front_left/image'           ,
+        '/vimba_front_left_center/image'    ,
+        '/vimba_front_right_center/image'   ,
+        '/vimba_front_right/image'          
     }
 else:
-    iterator = {
-        '/vimba_rear_left/image'            : 0,
-        '/vimba_rear_right/image'           : 0,
-        '/vimba_front_left/image'           : 0,
-        '/vimba_front_left_center/image'    : 0,
-        '/vimba_front_right_center/image'   : 0,
-        '/vimba_front_right/image'          : 0,
+    image_topics = {
+        '/vimba_rear_left/image'            ,
+        '/vimba_rear_right/image'           ,
+        '/vimba_front_left/image'           ,
+        '/vimba_front_left_center/image'    ,
+        '/vimba_front_right_center/image'   ,
+        '/vimba_front_right/image'          
     }
 
-# Checking if all the topics that are going to be collected are in the bag
+
 TOPIC_TYPES = reader.get_all_topics_and_types()
 TYPE_MAP = {TOPIC_TYPES[i].name: TOPIC_TYPES[i].type for i in range(len(TOPIC_TYPES))}
-if iterator:
-    for topic in iterator.keys():
-        if topic not in TYPE_MAP:
-            print("ERROR: The topic ", topic, " is not in the input bag.")
-            # exit()
-        print("Topic ", topic, " found in the input bag.")
-else:
-    print("FATAL ERROR: TOPICS_TO_EXTRACT not defined.")
+
+iterator = dict()
+
+# Initialize an iterator based on whether or not the topic is in the rosbag
+for t in TYPE_MAP:
+    if t in image_topics:
+        iterator[t] = 0
+
+if len(iterator) == 0:
+    print("[script] No Images to extract from this rosbag. Exiting...")
+    # If no camera topics are found, close the ROSBAG and return. TODO: Check if ffmpeg is happy about this
+    del reader
+    exit()
 
 counter = 0
 
@@ -192,7 +214,12 @@ def undistort(input, distortion_data):
             # If distortion parameters not loaded then load them once
             if topic_name[1:-6] not in distortion_dict:
                 yaml_file_path = args.camera_info_path + topic_name[1:-6] + '.yaml'
-                distortion_fp = open(file_path(yaml_file_path))
+                try:
+                    distortion_fp = open(file_path(yaml_file_path))
+                except:
+                    del reader
+                    print("[script] FATAL ERROR: Opening .yaml file failed. NOT A FILE")
+                    exit()
                 distortion_dict[topic_name[1:-6]] = yaml.safe_load(distortion_fp)
             cv2_msg = undistort(cv2_msg, distortion_dict[topic_name[1:-6]])
 

From 07016b41dbcd5aea9bd4f014350266435871f69c Mon Sep 17 00:00:00 2001
From: chrislai502 <chrislai_502@berkeley.edu>
Date: Thu, 2 Nov 2023 16:53:13 -0700
Subject: [PATCH 8/8] cleanup, tested

---
 multi_extract.sh | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/multi_extract.sh b/multi_extract.sh
index 429e8fc..fa1ff32 100755
--- a/multi_extract.sh
+++ b/multi_extract.sh
@@ -198,4 +198,5 @@ while IFS= read -r line; do
 # done
 done < /tmp/tempfile.txt
 
-rm /tmp/tempfile.txt
\ No newline at end of file
+rm /tmp/tempfile.txt
+find $OUTPUT_BASE_DIR_DEFAULT -type d -empty -delete # Cleanup Empty Directories
\ No newline at end of file