Move camerad to system/ (#24836)

* mv camerad

* add hardware symlink

* fix unit tests
old-commit-hash: 6123ab3d1c901ed3763e1a7cb8e1aac3f6b8fda3

system/camerad/test/.gitignore

@@ -0,0 +1 @@
+jpegs/

system/camerad/test/ae_gray_test.cc

@@ -0,0 +1,67 @@
+// unittest for set_exposure_target
+
+#include "ae_gray_test.h"
+
+#include <cassert>
+
+#include <cmath>
+#include <cstring>
+
+#include "common/util.h"
+#include "system/camerad/cameras/camera_common.h"
+
+int main() {
+  // set up fake camerabuf
+  CameraBuf cb = {};
+  VisionBuf vb = {};
+  uint8_t * fb_y = new uint8_t[W*H];
+  vb.y = fb_y;
+  cb.cur_yuv_buf = &vb;
+  cb.rgb_width = W;
+  cb.rgb_height = H;
+
+  printf("AE test patterns %dx%d\n", cb.rgb_width, cb.rgb_height);
+
+  // mix of 5 tones
+  uint8_t l[5] = {0, 24, 48, 96, 235}; // 235 is yuv max
+
+  bool passed = true;
+  float rtol = 0.05;
+  // generate pattern and calculate EV
+  int cnt = 0;
+  for (int i_0=0; i_0<TONE_SPLITS; i_0++) {
+    for (int i_1=0; i_1<TONE_SPLITS; i_1++) {
+      for (int i_2=0; i_2<TONE_SPLITS; i_2++) {
+        for (int i_3=0; i_3<TONE_SPLITS; i_3++) {
+          int h_0 = i_0 * H / TONE_SPLITS;
+          int h_1 = i_1 * (H - h_0) / TONE_SPLITS;
+          int h_2 = i_2 * (H - h_0 - h_1) / TONE_SPLITS;
+          int h_3 = i_3 * (H - h_0 - h_1 - h_2) / TONE_SPLITS;
+          int h_4 = H - h_0 - h_1 - h_2 - h_3;
+          memset(&fb_y[0], l[0], h_0*W);
+          memset(&fb_y[h_0*W], l[1], h_1*W);
+          memset(&fb_y[h_0*W+h_1*W], l[2], h_2*W);
+          memset(&fb_y[h_0*W+h_1*W+h_2*W], l[3], h_3*W);
+          memset(&fb_y[h_0*W+h_1*W+h_2*W+h_3*W], l[4], h_4*W);
+          float ev = set_exposure_target((const CameraBuf*) &cb, 0, W-1, 1, 0, H-1, 1);
+          // printf("%d/%d/%d/%d/%d ev is %f\n", h_0, h_1, h_2, h_3, h_4, ev);
+          // printf("%f\n", ev);
+
+          // compare to gt
+          float evgt = gts[cnt];
+          if (fabs(ev - evgt) > rtol*evgt) {
+            passed = false;
+          }
+
+          // report
+          printf("%d/%d/%d/%d/%d: ev %f, gt %f, err %f\n", h_0, h_1, h_2, h_3, h_4, ev, evgt, fabs(ev - evgt) / (evgt != 0 ? evgt : 0.00001f));
+          cnt++;
+        }
+      }
+    }
+  }
+  assert(passed);
+
+  delete[] fb_y;
+  return 0;
+}

system/camerad/test/ae_gray_test.h

@@ -0,0 +1,18 @@
+#pragma once
+
+#define W 240
+#define H 160
+
+#define TONE_SPLITS 3
+
+float gts[TONE_SPLITS*TONE_SPLITS*TONE_SPLITS*TONE_SPLITS] = {
+  0.917969,0.917969,0.375000,0.917969,0.375000,0.375000,0.187500,0.187500,0.187500,0.917969,
+  0.375000,0.375000,0.187500,0.187500,0.187500,0.187500,0.187500,0.187500,0.093750,0.093750,
+  0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.917969,0.375000,0.375000,
+  0.187500,0.187500,0.187500,0.187500,0.187500,0.187500,0.093750,0.093750,0.093750,0.093750,
+  0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,
+  0.093750,0.093750,0.093750,0.093750,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+  0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+  0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+  0.000000
+};

system/camerad/test/camera_test.h

@@ -0,0 +1,25 @@
+// TODO: cleanup AE tests
+// needed by camera_common.cc
+void camera_autoexposure(CameraState *s, float grey_frac) {}
+void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {}
+void cameras_open(MultiCameraState *s) {}
+void cameras_run(MultiCameraState *s) {}
+
+typedef struct CameraState {
+  int camera_num;
+  CameraInfo ci;
+
+  int fps;
+  float digital_gain = 0;
+
+  CameraBuf buf;
+} CameraState;
+
+typedef struct MultiCameraState {
+  CameraState road_cam;
+  CameraState driver_cam;
+
+  PubMaster *pm = nullptr;
+} MultiCameraState;
+
+

system/camerad/test/check_skips.py

@@ -0,0 +1,27 @@
+#!/usr/bin/env python3
+# type: ignore
+import cereal.messaging as messaging
+
+all_sockets = ['roadCameraState', 'driverCameraState', 'wideRoadCameraState']
+prev_id = [None,None,None]
+this_id = [None,None,None]
+dt = [None,None,None]
+num_skipped = [0,0,0]
+
+if __name__ == "__main__":
+  sm = messaging.SubMaster(all_sockets)
+  while True:
+    sm.update()
+
+    for i in range(len(all_sockets)):
+      if not sm.updated[all_sockets[i]]:
+        continue
+      this_id[i] = sm[all_sockets[i]].frameId
+      if prev_id[i] is None:
+        prev_id[i] = this_id[i]
+        continue
+      dt[i] = this_id[i] - prev_id[i]
+      if dt[i] != 1:
+        num_skipped[i] += dt[i] - 1
+        print(all_sockets[i] ,dt[i] - 1, num_skipped[i])
+      prev_id[i] = this_id[i]

system/camerad/test/frame_test.py

@@ -0,0 +1,37 @@
+#!/usr/bin/env python3
+import numpy as np
+import cereal.messaging as messaging
+from PIL import ImageFont, ImageDraw, Image
+
+font = ImageFont.truetype("arial", size=72)
+def get_frame(idx):
+  img = np.zeros((874, 1164, 3), np.uint8)
+  img[100:400, 100:100+(idx % 10) * 100] = 255
+
+  # big number
+  im2 = Image.new("RGB", (200, 200))
+  draw = ImageDraw.Draw(im2)
+  draw.text((10, 100), "%02d" % idx, font=font)
+  img[400:600, 400:600] = np.array(im2.getdata()).reshape((200, 200, 3))
+  return img.tostring()
+
+if __name__ == "__main__":
+  from common.realtime import Ratekeeper
+  rk = Ratekeeper(20)
+
+  pm = messaging.PubMaster(['roadCameraState'])
+  frm = [get_frame(x) for x in range(30)]
+  idx = 0
+  while 1:
+    print("send %d" % idx)
+    dat = messaging.new_message('roadCameraState')
+    dat.valid = True
+    dat.frame = {
+      "frameId": idx,
+      "image": frm[idx % len(frm)],
+    }
+    pm.send('roadCameraState', dat)
+
+    idx += 1
+    rk.keep_time()
+    #time.sleep(1.0)

system/camerad/test/get_thumbnails_for_segment.py

@@ -0,0 +1,32 @@
+#!/usr/bin/env python3
+
+import os
+
+from tqdm import tqdm
+
+from common.file_helpers import mkdirs_exists_ok
+from tools.lib.logreader import LogReader
+from tools.lib.route import Route
+
+import argparse
+
+if __name__ == "__main__":
+  parser = argparse.ArgumentParser()
+  parser.add_argument("route", help="The route name")
+  parser.add_argument("segment", type=int,  help="The index of the segment")
+  args = parser.parse_args()
+
+  out_path = os.path.join("jpegs", f"{args.route.replace('|', '_')}_{args.segment}")
+  mkdirs_exists_ok(out_path)
+
+  r = Route(args.route)
+  path = r.log_paths()[args.segment] or r.qlog_paths()[args.segment]
+  lr = list(LogReader(path))
+
+  for msg in tqdm(lr):
+    if msg.which() == 'thumbnail':
+      with open(os.path.join(out_path, f"{msg.thumbnail.frameId}.jpg"), 'wb') as f:
+        f.write(msg.thumbnail.thumbnail)
+    elif msg.which() == 'navThumbnail':
+      with open(os.path.join(out_path, f"nav_{msg.navThumbnail.frameId}.jpg"), 'wb') as f:
+        f.write(msg.navThumbnail.thumbnail)

system/camerad/test/stress_restart.sh

@@ -0,0 +1,9 @@
+#!/bin/sh
+cd ..
+while :; do
+  ./camerad &
+  pid="$!"
+  sleep 2
+  kill -2 $pid
+  wait $pid
+done

system/camerad/test/test_camerad.py

@@ -0,0 +1,63 @@
+#!/usr/bin/env python3
+
+import time
+import unittest
+
+import cereal.messaging as messaging
+from system.hardware import TICI
+from selfdrive.test.helpers import with_processes
+
+TEST_TIMESPAN = 30 # random.randint(60, 180) # seconds
+SKIP_FRAME_TOLERANCE = 0
+LAG_FRAME_TOLERANCE = 2 # ms
+
+FPS_BASELINE = 20
+CAMERAS = {
+  "roadCameraState": FPS_BASELINE,
+  "driverCameraState": FPS_BASELINE // 2,
+}
+
+if TICI:
+  CAMERAS["driverCameraState"] = FPS_BASELINE
+  CAMERAS["wideRoadCameraState"] = FPS_BASELINE
+
+class TestCamerad(unittest.TestCase):
+  @classmethod
+  def setUpClass(cls):
+    if not TICI:
+      raise unittest.SkipTest
+
+  @with_processes(['camerad'])
+  def test_frame_packets(self):
+    print("checking frame pkts continuity")
+    print(TEST_TIMESPAN)
+
+    sm = messaging.SubMaster([socket_name for socket_name in CAMERAS])
+
+    last_frame_id = dict.fromkeys(CAMERAS, None)
+    last_ts = dict.fromkeys(CAMERAS, None)
+    start_time_sec = time.time()
+    while time.time()- start_time_sec < TEST_TIMESPAN:
+      sm.update()
+
+      for camera in CAMERAS:
+        if sm.updated[camera]:
+          ct = (sm[camera].timestampEof if not TICI else sm[camera].timestampSof) / 1e6
+          if last_frame_id[camera] is None:
+            last_frame_id[camera] = sm[camera].frameId
+            last_ts[camera] = ct
+            continue
+
+          dfid = sm[camera].frameId - last_frame_id[camera]
+          self.assertTrue(abs(dfid - 1) <= SKIP_FRAME_TOLERANCE, "%s frame id diff is %d" % (camera, dfid))
+
+          dts = ct - last_ts[camera]
+          self.assertTrue(abs(dts - (1000/CAMERAS[camera])) < LAG_FRAME_TOLERANCE, f"{camera} frame t(ms) diff is {dts:f}")
+
+          last_frame_id[camera] = sm[camera].frameId
+          last_ts[camera] = ct
+
+      time.sleep(0.01)
+
+if __name__ == "__main__":
+  unittest.main()

system/camerad/test/test_exposure.py

@@ -0,0 +1,57 @@
+#!/usr/bin/env python3
+import time
+import unittest
+import numpy as np
+
+from selfdrive.test.helpers import with_processes
+from system.camerad.snapshot.snapshot import get_snapshots
+
+from system.hardware import TICI
+
+TEST_TIME = 45
+REPEAT = 5
+
+class TestCamerad(unittest.TestCase):
+  @classmethod
+  def setUpClass(cls):
+    if not TICI:
+      raise unittest.SkipTest
+
+  def _numpy_rgb2gray(self, im):
+    ret = np.clip(im[:,:,2] * 0.114 + im[:,:,1] * 0.587 + im[:,:,0] * 0.299, 0, 255).astype(np.uint8)
+    return ret
+
+  def _is_exposure_okay(self, i, med_mean=np.array([[0.2,0.4],[0.2,0.6]])):
+    h, w = i.shape[:2]
+    i = i[h//10:9*h//10,w//10:9*w//10]
+    med_ex, mean_ex = med_mean
+    i = self._numpy_rgb2gray(i)
+    i_median = np.median(i) / 255.
+    i_mean = np.mean(i) / 255.
+    print([i_median, i_mean])
+    return med_ex[0] < i_median < med_ex[1] and mean_ex[0] < i_mean < mean_ex[1]
+
+  @with_processes(['camerad'])
+  def test_camera_operation(self):
+    passed = 0
+    start = time.time()
+    while time.time() - start < TEST_TIME and passed < REPEAT:
+      rpic, dpic = get_snapshots(frame="roadCameraState", front_frame="driverCameraState")
+
+      res = self._is_exposure_okay(rpic)
+      res = res and self._is_exposure_okay(dpic)
+
+      if TICI:
+        wpic, _ = get_snapshots(frame="wideRoadCameraState")
+        res = res and self._is_exposure_okay(wpic)
+
+      if passed > 0 and not res:
+        passed = -passed # fails test if any failure after first sus
+        break
+
+      passed += int(res)
+      time.sleep(2)
+    self.assertGreaterEqual(passed, REPEAT)
+
+if __name__ == "__main__":
+  unittest.main()