Dev/adjust tcn coco saving

angel_system/activity_classification/tcn_hpl/predict.py

@@ -464,19 +464,29 @@ def collect(
                 raise RuntimeError(
                     "No video set before results collection. See `set_video` method."
                 )
-            packet = dict(
+            # get the global id for the image from the frame number
+
+            # add the image
+            img = dict(
                 video_id=self._vid,
                 frame_index=frame_index,
-                activity_pred=activity_pred,
-                activity_conf=list(activity_conf_vec),
             )
             if name is not None:
-                packet["name"] = name
+                img["name"] = name
             if file_name is not None:
-                packet["file_name"] = file_name
+                img["file_name"] = file_name
             if activity_gt is not None:
-                packet["activity_gt"] = activity_gt
-            self._dset.add_image(**packet)
+                img["activity_gt"] = activity_gt
+            # save the gid from the image to link to the annot
+            gid = self._dset.add_image(**img)
+
+            # add the annotation
+            self._dset.add_annotation(
+                image_id=gid,
+                category_id=activity_pred,
+                score=activity_conf_vec[activity_pred],
+                prob=list(activity_conf_vec),
+            )
 
     def write_file(self):
         """

ros/angel_system_nodes/angel_system_nodes/activity_classification/activity_classifier_tcn.py

@@ -102,6 +102,10 @@
 # activity prediction for the "live" image will not occur until object
 # detections are predicted for that frame.
 PARAM_WINDOW_LEADS_WITH_OBJECTS = "window_leads_with_objects"
+# Debug file saved out to the filesystem for understanding the node's
+# inputs when it decides not to create an activity classification.
+# the format will be csv with a list of the object detections and the pose
+PARAM_DEBUG_FILE = "debug_file"
 
 
 class NoActivityClassification(Exception):
@@ -148,6 +152,7 @@ def __init__(self):
                 (PARAM_TOPIC, "medical"),
                 (PARAM_POSE_REPEAT_RATE, 0),
                 (PARAM_WINDOW_LEADS_WITH_OBJECTS, False),
+                (PARAM_DEBUG_FILE, ""),
             ],
         )
         self._img_ts_topic = param_values[PARAM_IMG_TS_TOPIC]
@@ -166,6 +171,12 @@ def __init__(self):
 
         self._window_lead_with_objects = param_values[PARAM_WINDOW_LEADS_WITH_OBJECTS]
 
+        self._debug_file = param_values[PARAM_DEBUG_FILE]
+        # clear the file if it exists (since we are appending to it)
+        if self._debug_file != "":
+            with open(self._debug_file, "w") as f:
+                f.write("")
+
         self.topic = param_values[PARAM_TOPIC]
         # Load in TCN classification model and weights
         with SimpleTimer("Loading inference module", log.info):
@@ -649,12 +660,29 @@ def rt_loop(self):
 
                         self._activity_publisher.publish(act_msg)
                     except NoActivityClassification:
+                        # collect the results if we are saving to coco file
+                        if self._results_collector:
+                            # Prepare output message
+                            activity_msg = ActivityDetection()
+                            # set the only needed items for collection
+                            activity_msg.source_stamp_end_frame = window.frames[-1][0]
+                            activity_msg.conf_vec = [0.0 for x in self._model.classes]
+                            self._collect_results(activity_msg)
+
                         # No ramifications, but don't publish activity message.
                         log.warn(
                             "Runtime loop window processing function did "
                             "not yield an activity classification for "
                             "publishing."
                         )
+                        if self._debug_file != "":
+                            # save the info for why this window was not processed
+                            repr = window.__repr__()
+                            with open(self._debug_file, "a") as f:
+                                f.write(
+                                    f"timestamp: {self.get_clock().now().to_msg()}\n"
+                                )
+                                f.write(f"{repr}\n")
 
                     # This window has completed processing - record its leading
                     # timestamp now.

ros/angel_utils/python/angel_utils/activity_classification.py

@@ -192,13 +192,17 @@ def queue_image(
         # self.get_logger_fn().info(f"self.frames[-1][0] header stamp: {self.frames[-1][0]}")
         with self.__state_lock:
             # before the current lead frame?
-            if self.frames and time_to_int(img_header_stamp) <= time_to_int(
-                self.frames[-1][0]
+            if (
+                self.frames
+                and self.frames[-1][2] == image_frame_number
+                and time_to_int(img_header_stamp) <= time_to_int(self.frames[-1][0])
             ):
                 self.get_logger_fn().warn(
                     f"Input image frame was NOT after the previous latest: "
                     f"(prev) {time_to_int(self.frames[-1][0])} "
-                    f"!< {time_to_int(img_header_stamp)} (new)"
+                    f"!< {time_to_int(img_header_stamp)} (new)\n"
+                    f"frame number: {image_frame_number}\n"
+                    f"prev frame number: {self.frames[-1][2]}"
                 )
                 return False
             self.frames.append((img_header_stamp, img_mat, image_frame_number))

ros/angel_utils/scripts/convert_video_to_ros_bag.py

@@ -1,4 +1,12 @@
 #!/usr/bin/env python3
+"""
+Convert a video (mp4) or a series of images into a ROS bag.
+
+Example running (inside ROS environment):
+ros2 run angel_utils convert_video_to_ros_bag.py \
+  --video-fn video.mp4 \
+  --output-bag-folder ros_bags/new_bag
+"""
 import argparse
 from glob import glob
 from pathlib import Path