RedRoad experiment ver0

export_video.sh

@@ -0,0 +1,7 @@
+LOGFILE=$1
+FILENAME="${LOGFILE##*/}"
+NAME="${FILENAME%.*}"
+python -m osgar.logger $LOGFILE --raw --stream oak.color > upload/$NAME.h26x
+ffmpeg -i upload/$NAME.h26x -c copy upload/$NAME.mp4
+rm upload/$NAME.h26x
+

robotem-rovne/config/matty-redroad.json

@@ -0,0 +1,91 @@
+{
+  "version": 2,
+  "robot": {
+    "modules": {
+      "app": {
+          "driver": "robotem-rovne.main:RobotemRovne",
+          "in": ["emergency_stop", "pose2d", "nn_mask", "nmea_data", "orientation_list"],
+          "out": ["desired_steering"],
+          "init": {
+            "max_speed": 0.5,
+            "stop_dist": -1,
+            "terminate_on_stop": true
+          }
+      },
+      "platform": {
+        "driver": "osgar.platforms.matty:Matty",
+        "in": ["esp_data"],
+        "out": ["esp_data"],
+        "init": {}
+      },
+      "timer": {
+        "driver": "timer",
+        "in": [],
+        "out": ["tick"],
+        "init": {
+          "sleep": 0.1
+        }
+      },
+      "gps": {
+        "driver": "gps",
+        "in": ["raw"],
+        "out": ["nmea_data"],
+        "init": {
+        }
+      },
+      "serial": {
+          "driver": "serial",
+          "in": ["raw"],
+          "out": ["raw"],
+          "init": {"port": "/dev/esp32", "speed": 115200}
+      },
+      "oak": {
+        "driver": "osgar.drivers.oak_camera:OakCamera",
+        "init": {
+          "fps": 10,
+          "is_color": true,
+          "video_encoder": "h265",
+          "h264_bitrate": 2000,
+          "is_depth": true,
+          "is_stereo_images": false,
+          "laser_projector_current": 0,
+          "flood_light_current": 500,
+          "is_imu_enabled": true,
+          "number_imu_records": 10,
+          "disable_magnetometer_fusion": true,
+          "mono_resolution": "THE_400_P",
+          "color_resolution": "THE_1080_P",
+          "color_manual_focus": 130,
+          "stereo_median_filter": "KERNEL_3x3",
+          "stereo_mode": "HIGH_ACCURACY",
+          "stereo_extended_disparity": false,
+          "stereo_subpixel": false,
+          "stereo_left_right_check": true,
+          "sleep_on_start_sec": 3,
+          "model": {
+            "blob": "models/robotourist-083-redroad-20260116-224x224-rgb-chw-1shave.blob"
+          },
+          "nn_config": {
+            "output_format": "LayerFp16",
+            "NN_family": "robotourist",
+            "input_size": "224x224"
+          }
+        }
+      }
+    },
+    "links": [
+      ["app.desired_steering", "platform.desired_steering"],
+      ["platform.pose2d", "app.pose2d"],
+      ["platform.emergency_stop", "app.emergency_stop"],
+      ["serial.raw", "platform.esp_data"],
+      ["platform.esp_data", "serial.raw"],
+      ["platform.gps_serial", "gps.raw"],
+      ["timer.tick", "platform.tick"],
+
+      ["gps.nmea_data", "app.nmea_data"],
+
+      ["oak.nn_mask", "app.nn_mask"],
+      ["oak.orientation_list", "app.orientation_list"]
+    ]
+  }
+}

robotem-rovne/config/test_redroad.json

@@ -0,0 +1,39 @@
+{
+  "version": 2,
+  "robot": {
+    "modules": {
+      "oak": {
+        "driver": "osgar.drivers.oak_camera:OakCamera",
+        "init": {
+          "fps": 10,
+          "is_color": true,
+          "video_encoder": "mjpeg",
+          "is_depth": false,
+          "laser_projector_current": 0,
+          "flood_light_current": 500,
+          "is_imu_enabled": true,
+          "number_imu_records": 10,
+          "disable_magnetometer_fusion": true,
+          "mono_resolution": "THE_400_P",
+          "color_resolution": "THE_1080_P",
+          "color_manual_focus": 130,
+          "stereo_median_filter": "KERNEL_3x3",
+          "stereo_mode": "HIGH_ACCURACY",
+          "stereo_extended_disparity": false,
+          "stereo_subpixel": false,
+          "stereo_left_right_check": true,
+          "model": {
+            "blob": "models/robotourist-083-redroad-20260116-224x224-rgb-chw-1shave.blob"
+          },
+          "nn_config": {
+            "output_format": "LayerFp16",
+            "NN_family": "robotourist",
+            "input_size": "224x224"
+          }
+        }
+      }
+    },
+    "links": [
+    ]
+  }
+}

robotem-rovne/main.py

@@ -80,15 +80,16 @@ def on_nmea_data(self, data):
 
     def on_nn_mask(self, data):
         self.last_nn_mask = data.copy()  # make sure you modify only own copy
-        assert self.last_nn_mask.shape == (120, 160), self.last_nn_mask.shape
-        self.last_nn_mask[:60, :] = 0  # remove sky detections
+#        assert self.last_nn_mask.shape == (120, 160), self.last_nn_mask.shape
+        height, width = self.last_nn_mask.shape
+        self.last_nn_mask[:height//2, :] = 0  # remove sky detections
 
         center_y, center_x = mask_center(self.last_nn_mask)
-        dead = 10
+        dead = width//16 # was 10
         turn_angle = math.radians(20)
-        if center_x > 80 + dead:
+        if center_x > width//2 + dead:
             self.last_dir = -turn_angle
-        elif center_x < 80 - dead:
+        elif center_x < width//2 - dead:
             self.last_dir = turn_angle
         else:
             self.last_dir = 0  # straight

robotem-rovne/view_mask.py

@@ -18,21 +18,23 @@
 
 
 def read_h264_image(data, i_frame_only=True):
-    assert data.startswith(bytes.fromhex('00000001 0950')) or data.startswith(bytes.fromhex('00000001 0930')), data[
-                                                                                                               :20].hex()
-    if data.startswith(bytes.fromhex('00000001 0950')):
+    is_h264 = data.startswith(bytes.fromhex('00000001 0950')) or data.startswith(bytes.fromhex('00000001 0930'))
+    is_h265 = data.startswith(bytes.fromhex('00000001 460150')) or data.startswith(bytes.fromhex('00000001 460130'))
+    assert is_h264 or is_h265, data[:20].hex()
+
+    if data.startswith(bytes.fromhex('00000001 0950')) or data.startswith(bytes.fromhex('00000001 460150')):
         # I - key frame
-        with open('tmp.h264', 'wb') as f:
+        with open('tmp.h26x', 'wb') as f:
             f.write(data)
-    elif data.startswith(bytes.fromhex('00000001 0930')):
+    elif data.startswith(bytes.fromhex('00000001 0930')) or data.startswith(bytes.fromhex('00000001 460130')):
         # P-frame}
         if i_frame_only:
             return None
-        with open('tmp.h264', 'ab') as f:
+        with open('tmp.h26x', 'ab') as f:
             f.write(data)
     else:
         assert 0, f'Unexpected data {data[:20].hex()}'
-    cap = cv2.VideoCapture('tmp.h264')
+    cap = cv2.VideoCapture('tmp.h26x')
     image = None
     ret = True
     while ret:
@@ -43,8 +45,11 @@ def read_h264_image(data, i_frame_only=True):
     return image
 
 
-def read_logfile(logfile, writer=None, add_time=True):
-    nn_mask_stream = lookup_stream_id(logfile, 'oak.nn_mask')
+def read_logfile(logfile, writer=None, add_time=True, threshold=None):
+    if threshold is None:
+        nn_mask_stream = lookup_stream_id(logfile, 'oak.nn_mask')
+    else:
+        nn_mask_stream = lookup_stream_id(logfile, 'oak.redroad')
     img_stream = lookup_stream_id(logfile, 'oak.color')
     pose2d_stream = lookup_stream_id(logfile, 'platform.pose2d')
     total_duration, total_dist = get_time_and_dist(logfile, 'platform.pose2d')
@@ -59,14 +64,17 @@ def read_logfile(logfile, writer=None, add_time=True):
                 if img is None:
                     continue
                 mask = deserialize(data)
-                assert mask.shape == (120, 160), mask.shape
-                mask[:60, :] = 0  # remove sky detections
+                if threshold is not None:
+                    mask = (mask > threshold).astype(np.uint8)
+                assert mask.shape in [(120, 160), (112,112)], mask.shape
+                orig_height, orig_width = mask.shape
+#                mask[:height//2, :] = 0  # remove sky detections
                 center_y, center_x = mask_center(mask)
-                scale = 12  # 160 -> 640 -> 1920
-                center_x *= scale
-                center_y *= 9  # scale
                 mask = cv2.resize(mask, (1920, 1080))
                 height, width = mask.shape
+                scale = width // orig_width  # 160 -> 640 -> 1920
+                center_x *= scale
+                center_y *= height // orig_height
                 colored_mask = np.zeros((height, width, 3), dtype=np.uint8)
                 colored_mask[mask == 1] = [0, 0, 255]
                 overlay = cv2.addWeighted(img, 1, colored_mask, 0.7, 0)
@@ -135,6 +143,7 @@ def main():
     parser = argparse.ArgumentParser(description=__doc__)
     parser.add_argument('logfile', nargs='+', help='recorded log file(s)')
     parser.add_argument('--create-video', help='filename of output video')
+    parser.add_argument('--threshold', '-t', type=int, help='threshold value for redroad detection')
     args = parser.parse_args()
 
     if args.create_video is not None:
@@ -148,7 +157,7 @@ def main():
         writer = None  # no video writer
 
     for logfile in args.logfile:
-        read_logfile(logfile, writer=writer)
+        read_logfile(logfile, writer=writer, threshold=args.threshold)
 
     if writer is not None:
         writer.release()