Get Pat/FR-07 moving with OAK cone detection

config/fr07-followpath.json

@@ -0,0 +1,70 @@
+{
+  "version": 2,
+  "robot": {
+    "modules": {
+      "app": {
+          "driver": "osgar.followpath:FollowPath",
+          "in": ["emergency_stop", "pose2d", "path"],
+          "out": ["desired_speed"],
+          "init": {
+            "max_speed": 0.1,
+            "path": [[0, 0], [1, 0], [1, 1]],
+            "timeout": 10
+          }
+      },
+      "platform": {
+        "driver": "osgar.platforms.yuhesen:FR07",
+        "in": ["can"],
+        "out": ["can"],
+        "init": {}
+      },
+      "can": {
+        "driver": "pcan",
+        "in": ["can"],
+        "out": ["can"],
+        "init": {}
+      },
+      "oak": {
+        "driver": "osgar.drivers.oak_camera:OakCamera",
+        "init": {
+          "fps": 10,
+          "is_color": true,
+          "is_depth": true,
+          "laser_projector_current": 1200,
+          "is_imu_enabled": true,
+          "number_imu_records": 10,
+          "disable_magnetometer_fusion": false,
+          "cam_ip": "169.254.1.222",
+          "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
+        }
+      },
+      "gps": {
+          "driver": "gps",
+          "in": ["raw"],
+          "out": ["position"],
+          "init": {}
+      },
+      "gps_serial": {
+          "driver": "serial",
+          "in": [],
+          "out": ["raw"],
+          "init": {"port": "/dev/ttyUSB0", "speed": 4800}
+      }
+    },
+    "links": [
+      ["can.can", "platform.can"],
+      ["platform.can", "can.can"],
+      ["app.desired_speed", "platform.desired_steering"],
+      ["platform.emergency_stop", "app.emergency_stop"],
+      ["platform.pose2d", "app.pose2d"],
+      ["gps_serial.raw", "gps.raw"]
+    ]
+  }
+}

config/fr07-go.json

@@ -23,14 +23,48 @@
         "in": ["can"],
         "out": ["can"],
         "init": {}
+      },
+      "oak": {
+        "driver": "osgar.drivers.oak_camera:OakCamera",
+        "init": {
+          "fps": 10,
+          "is_color": true,
+          "is_depth": true,
+          "laser_projector_current": 1200,
+          "is_imu_enabled": true,
+          "number_imu_records": 10,
+          "disable_magnetometer_fusion": false,
+          "cam_ip": "169.254.1.222",
+          "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
+        }
+      },
+      "gps": {
+          "driver": "gps",
+          "in": ["raw"],
+          "out": ["position"],
+          "init": {}
+      },
+      "gps_serial": {
+          "driver": "serial",
+          "in": [],
+          "out": ["raw"],
+          "init": {"port": "/dev/ttyUSB0", "speed": 4800}
       }
     },
     "links": [
       ["can.can", "platform.can"],
       ["platform.can", "can.can"],
       ["app.desired_speed", "platform.desired_steering"],
       ["platform.emergency_stop", "app.emergency_stop"],
-      ["platform.pose2d", "app.pose2d"]
+      ["platform.pose2d", "app.pose2d"],
+      ["gps_serial.raw", "gps.raw"]
     ]
   }
 }

osgar/logger.py

@@ -38,6 +38,7 @@
 from ast import literal_eval
 import mmap
 import pathlib
+import gzip
 
 g_logger = logging.getLogger(__name__)
 
@@ -160,7 +161,10 @@ class LogReader:
     def __init__(self, filename, follow=False, only_stream_id=None):
         self.filename = filename
         self.follow = follow
-        self.f = open(self.filename, 'rb')
+        if str(self.filename).endswith('gz'):  # support both string and pathlib.Path
+            self.f = gzip.open(self.filename)
+        else:
+            self.f = open(self.filename, 'rb')
         data = self._read(4)
         assert data == MAGIC, data
 

osgar/obstdet3d.py

@@ -0,0 +1,25 @@
+"""
+  Obstacle Detection 3D
+"""
+import numpy as np
+
+from osgar.node import Node
+
+
+class ObstacleDetector3D(Node):
+    def __init__(self, config, bus):
+        super().__init__(config, bus)
+        bus.register('obstacle')
+
+    def on_depth(self, data):
+        assert data.shape == (400, 640), data.shape
+        selection = data[150:250, 300:340]
+        mask = selection > 0  # not valid data?
+        if mask.max() == True:
+            dist = selection[mask].min() / 1000
+        else:
+            dist = 0.0
+        self.publish('obstacle', float(dist))
+#        print(self.time, dist)
+
+# vim: expandtab sw=4 ts=4

osgar/tools/lidarview.py

@@ -147,12 +147,25 @@ def draw(foreground, pose, scan, poses=[], image=None, bbox=None, callback=None,
 
         foreground.blit(cameraView, (0, 0))
 
-        for b in bbox:
-            assert len(b) > 5, b
-            name, x, y, width, height = b[:5]
-            color = (0, 255, 0)
-            rect = pygame.Rect(x, y, width, height)
-            pygame.draw.rect(image, color, rect, 2)
+        def frameNorm(w, h, bbox):
+            normVals = np.full(len(bbox), w)
+            normVals[::2] = h
+            return (np.clip(np.array(bbox), 0, 1) * normVals).astype(int)
+
+        for frame_detections in bbox:
+            for detection in frame_detections:
+                if len(detection) == 0:
+                    continue  # empty bbox is allowed now
+                # old format (b=detection)
+                #assert len(b) > 5, b
+                #name, x, y, width, height = b[:5]
+                # new (temporary?) format
+                w, h = image.get_size()
+                a, b, c, d = frameNorm(h, h, detection[2]).tolist()
+                name, x, y, width, height = detection[0],  a + (w - h) / 2, b, c - a, d - b
+                color = (0, 255, 0)
+                rect = pygame.Rect(x, y, width, height)
+                pygame.draw.rect(image, color, rect, 4)
 
     if callback is not None:
         debug_poly = []