From 75b1c7c4ba85d535b657eebfeb0a3e0b6060a6bb Mon Sep 17 00:00:00 2001
From: ShaneSmiskol <shane@smiskol.com>
Date: Wed, 26 May 2021 17:17:04 -0700
Subject: [PATCH] Improve focus when using snapshot (#21031)

old-commit-hash: a4571443578d53d414ebc3384414e8502ee3c528
---
 selfdrive/camerad/snapshot/snapshot.py | 46 ++++++++++++++++----------
 1 file changed, 29 insertions(+), 17 deletions(-)

diff --git a/selfdrive/camerad/snapshot/snapshot.py b/selfdrive/camerad/snapshot/snapshot.py
index eb7bb93af3..52c67d65e3 100755
--- a/selfdrive/camerad/snapshot/snapshot.py
+++ b/selfdrive/camerad/snapshot/snapshot.py
@@ -1,18 +1,21 @@
 #!/usr/bin/env python3
 import os
-import signal
 import subprocess
 import time
 
 import numpy as np
 from PIL import Image
+from typing import List
 
 import cereal.messaging as messaging
-from common.basedir import BASEDIR
 from common.params import Params
+from common.realtime import DT_MDL
 from common.transformations.camera import eon_f_frame_size, eon_d_frame_size, leon_d_frame_size, tici_f_frame_size
 from selfdrive.hardware import TICI
 from selfdrive.controls.lib.alertmanager import set_offroad_alert
+from selfdrive.manager.process_config import managed_processes
+
+LM_THRESH = 120  # defined in selfdrive/camerad/imgproc/utils.h
 
 
 def jpeg_write(fn, dat):
@@ -29,7 +32,13 @@ def extract_image(dat, frame_sizes):
   return np.dstack([r, g, b])
 
 
-def get_snapshots(frame="roadCameraState", front_frame="driverCameraState"):
+def rois_in_focus(lapres: List[float]) -> float:
+  sz = len(lapres)
+  return sum([1. / sz for sharpness in
+              lapres if sharpness >= LM_THRESH])
+
+
+def get_snapshots(frame="roadCameraState", front_frame="driverCameraState", focus_perc_threshold=0.):
   frame_sizes = [eon_f_frame_size, eon_d_frame_size, leon_d_frame_size, tici_f_frame_size]
   frame_sizes = {w * h: (w, h) for (w, h) in frame_sizes}
 
@@ -39,10 +48,17 @@ def get_snapshots(frame="roadCameraState", front_frame="driverCameraState"):
   if front_frame is not None:
     sockets.append(front_frame)
 
+  # wait 4 sec from camerad startup for focus and exposure
   sm = messaging.SubMaster(sockets)
-  while min(sm.logMonoTime.values()) == 0:
+  while sm[sockets[0]].frameId < int(4. / DT_MDL):
     sm.update()
 
+  start_t = time.monotonic()
+  while time.monotonic() - start_t < 10:
+    sm.update()
+    if min(sm.rcv_frame.values()) > 1 and rois_in_focus(sm[frame].sharpnessScore) >= focus_perc_threshold:
+      break
+
   rear = extract_image(sm[frame].image, frame_sizes) if frame is not None else None
   front = extract_image(sm[front_frame].image, frame_sizes) if front_frame is not None else None
   return rear, front
@@ -63,7 +79,6 @@ def snapshot():
   # Check if camerad is already started
   try:
     subprocess.check_call(["pgrep", "camerad"])
-
     print("Camerad already running")
     params.put_bool("IsTakingSnapshot", False)
     params.delete("Offroad_IsTakingSnapshot")
@@ -71,23 +86,19 @@ def snapshot():
   except subprocess.CalledProcessError:
     pass
 
-  env = os.environ.copy()
-  env["SEND_ROAD"] = "1"
-  env["SEND_WIDE_ROAD"] = "1"
+  os.environ["SEND_ROAD"] = "1"
+  os.environ["SEND_WIDE_ROAD"] = "1"
 
   if front_camera_allowed:
-    env["SEND_DRIVER"] = "1"
-
-  proc = subprocess.Popen(os.path.join(BASEDIR, "selfdrive/camerad/camerad"),
-                          cwd=os.path.join(BASEDIR, "selfdrive/camerad"), env=env)
-  time.sleep(3.0)
+    os.environ["SEND_DRIVER"] = "1"
 
+  managed_processes['camerad'].start()
   frame = "wideRoadCameraState" if TICI else "roadCameraState"
   front_frame = "driverCameraState" if front_camera_allowed else None
-  rear, front = get_snapshots(frame, front_frame)
+  focus_perc_threshold = 0. if TICI else 10 / 12.
 
-  proc.send_signal(signal.SIGINT)
-  proc.communicate()
+  rear, front = get_snapshots(frame, front_frame, focus_perc_threshold)
+  managed_processes['camerad'].stop()
 
   params.put_bool("IsTakingSnapshot", False)
   set_offroad_alert("Offroad_IsTakingSnapshot", False)
@@ -103,6 +114,7 @@ if __name__ == "__main__":
   if pic is not None:
     print(pic.shape)
     jpeg_write("/tmp/back.jpg", pic)
-    jpeg_write("/tmp/front.jpg", fpic)
+    if fpic is not None:
+      jpeg_write("/tmp/front.jpg", fpic)
   else:
     print("Error taking snapshot")