Switch to Compressed Depth Images (#132)

* Switch the compressedDepth

* If food on fork has too few points, treat it as no FoF after acquisition

Author: amalnanavati

feeding_web_app_ros2_test/feeding_web_app_ros2_test/DummyRealSense.py

@@ -1,6 +1,7 @@
 #!/usr/bin/env python3
 
 # Standard imports
+import array
 import threading
 
 # Third-party imports
@@ -13,6 +14,16 @@
 from sensor_msgs.msg import CompressedImage, Image, CameraInfo
 
 
+# The fixed header that ROS2 Humble's compressed depth image transport plugin prepends to
+# the data. The exact value was empirically determined, but the below link shows the code
+# that prepends additional data:
+#
+# https://github.com/ros-perception/image_transport_plugins/blob/5ef79d74c4347e6a2d151df63230d5fea1357137/compressed_depth_image_transport/src/codec.cpp#L337
+COMPRESSED_DEPTH_16UC1_HEADER = array.array(
+    "B", [0, 0, 0, 0, 46, 32, 133, 4, 192, 24, 60, 78]
+)
+
+
 class DummyRealSense(Node):
     """
     Reads in a video file and publishes the frames as ROS2 messages.
@@ -80,7 +91,7 @@ def __init__(self):
             CompressedImage, "~/compressed_image", 1
         )
         self.aligned_depth_publisher = self.create_publisher(
-            Image, "~/aligned_depth", 1
+            CompressedImage, "~/aligned_depth", 1
         )
         self.camera_info_publisher = self.create_publisher(
             CameraInfo, "~/camera_info", 1
@@ -163,11 +174,24 @@ def publish_frames(self):
             )
 
             # Configure the depth Image message
-            depth_frame_msg = self.bridge.cv2_to_imgmsg(self.depth_frame, "passthrough")
+            depth_frame_msg = CompressedImage()
             depth_frame_msg.header.frame_id = "camera_color_optical_frame"
             depth_frame_msg.header.stamp = (
                 self.get_clock().now() - rclpy.duration.Duration(seconds=0.15)
             ).to_msg()
+            depth_frame_msg.format = "16UC1; compressedDepth"
+            success, data = cv2.imencode(
+                ".png",
+                self.depth_frame,
+                # PNG compression 1 is the best speed setting, and is the setting
+                # we use for our RealSense.
+                [cv2.IMWRITE_PNG_COMPRESSION, 1],
+            )
+            if not success:
+                raise RuntimeError("Failed to compress image")
+            depth_frame_msg.data = (
+                COMPRESSED_DEPTH_16UC1_HEADER.tobytes() + data.tobytes()
+            )
 
             # Configure the Camera Info
             self.camera_info_msg.header.stamp = depth_frame_msg.header.stamp

feeding_web_app_ros2_test/launch/feeding_web_app_dummy_nodes_launch.xml

@@ -22,7 +22,7 @@
     <node pkg="feeding_web_app_ros2_test" exec="DummyRealSense" name="DummyRealSense">
       <remap from="~/image" to="/camera/color/image_raw"/>
       <remap from="~/compressed_image" to="/camera/color/image_raw/compressed"/>
-      <remap from="~/aligned_depth" to="/camera/aligned_depth_to_color/image_raw"/>
+      <remap from="~/aligned_depth" to="/camera/aligned_depth_to_color/image_raw/compressedDepth"/>
       <remap from="~/camera_info" to="/camera/color/camera_info"/>
       <remap from="~/aligned_depth/camera_info" to="/camera/aligned_depth_to_color/camera_info"/>
       <param name="fps" value="15"/>

feedingwebapp/src/Pages/Home/MealStates/BiteAcquisitionCheck.jsx

@@ -137,24 +137,24 @@ const BiteAcquisitionCheck = () => {
     (message) => {
       console.log('Got food-on-fork detection message', message)
       // Only auto-continue if the previous state was Bite Acquisition
-      if (biteAcquisitionCheckAutoContinue && prevMealState === MEAL_STATE.R_BiteAcquisition && message.status === 1) {
+      if (biteAcquisitionCheckAutoContinue && prevMealState === MEAL_STATE.R_BiteAcquisition) {
         let callbackFn = null
-        if (message.probability < biteAcquisitionCheckAutoContinueProbThreshLower) {
+        if (message.status === -1 || (message.status === 1 && message.probability < biteAcquisitionCheckAutoContinueProbThreshLower)) {
           console.log('No FoF. Auto-continuing in ', remainingSeconds, ' seconds')
           if (timerWasForFof.current === true) {
             clearTimer()
           }
           timerWasForFof.current = false
           callbackFn = acquisitionFailure
-        } else if (message.probability > biteAcquisitionCheckAutoContinueProbThreshUpper) {
+        } else if (message.status === 1 && message.probability > biteAcquisitionCheckAutoContinueProbThreshUpper) {
           console.log('FoF. Auto-continuing in ', remainingSeconds, ' seconds')
           if (timerWasForFof.current === false) {
             clearTimer()
           }
           timerWasForFof.current = true
           callbackFn = acquisitionSuccess
         } else {
-          console.log('Not auto-continuing due to probability between thresholds')
+          console.log('Not auto-continuing due to probability between thresholds or errors')
           clearTimer()
         }
         // Don't override an existing timer