From 7acb283813aecbc57c6061a709233e2dafa7f03e Mon Sep 17 00:00:00 2001
From: Felix Kloss <felix.widmaier@tuebingen.mpg.de>
Date: Thu, 14 Sep 2023 11:44:47 +0200
Subject: [PATCH] Add camera image brightness test

With this, we can hopefully auto-detect broken light panels.
---
 CHANGELOG.md                            |  4 +-
 scripts/trifingerpro_post_submission.py | 56 +++++++++++++++++++++++++
 2 files changed, 59 insertions(+), 1 deletion(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index ef70ec5..3777ca0 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -8,7 +8,9 @@
   [plotext](https://github.com/piccolomo/plotext) to show the plot directly in
   the terminal or save as file to /tmp if plotext is not available.
 - Check camera sharpness in `trifinger_post_submission.py`.  This should alert us early,
-  if a lense comes loose.
+  if a lens comes loose.
+- Check camera brightness in `trifinger_post_submission.py` to auto-detect
+  broken light panels.
 
 ### Changed
 - plot_post_submission_log.py now plots multiple log files side by side instead of
diff --git a/scripts/trifingerpro_post_submission.py b/scripts/trifingerpro_post_submission.py
index 89cad78..78efc94 100644
--- a/scripts/trifingerpro_post_submission.py
+++ b/scripts/trifingerpro_post_submission.py
@@ -349,6 +349,57 @@ def record_camera_observations(
     return observation_buffer
 
 
+def check_camera_brightness(
+    observations: typing.Sequence[tricamera.TriCameraObjectObservation],
+    log: logging.Logger,
+) -> bool:
+    """Check if the camera images match the expected brightness.
+
+    If they are too dark, it might mean that one of the light panels is broken.
+
+    Args:
+        observations: Sequence of camera observations.
+        log: Logger instance to log results.
+
+    Returns:
+        True if test is successful, False if there is any issue.
+    """
+    # On robots with all four panels on, brightness mean is typically in the range of
+    # 90-95.  On a robot that has 90 with all on, turning one panel off reduced it to
+    # 75.  Set the threshold based on this information.  If it results in frequent false
+    # positives, we can lower it a bit.
+    BRIGHTNESS_MEAN_TRHESHOLD = 87.0
+
+    all_cameras_means = []
+    for obs in observations:
+        # NOTE for brightness estimation, we don't need to debayer, we can just
+        # use the raw data
+        image_means = [np.mean(camera.image) for camera in obs.cameras]
+        all_cameras_means.append(np.mean(image_means))
+
+    total_mean = np.mean(all_cameras_means)
+
+    if total_mean >= BRIGHTNESS_MEAN_TRHESHOLD:
+        log.info(
+            SM(
+                "Image brightness is okay",
+                mean_brightness=total_mean,
+                limit=BRIGHTNESS_MEAN_TRHESHOLD,
+            )
+        )
+    else:
+        log.error(
+            SM(
+                "Image brightness is too low.  Lighting should be checked.",
+                mean_brightness=total_mean,
+                limit=BRIGHTNESS_MEAN_TRHESHOLD,
+            )
+        )
+        return False
+
+    return True
+
+
 def check_camera_sharpness(
     observations: typing.Sequence[tricamera.TriCameraObjectObservation],
     log: logging.Logger,
@@ -622,6 +673,11 @@ def main():
         args.object, num_observations=30
     )
 
+    if not check_camera_brightness(
+        camera_observations, logging.getLogger("camera_brightness")
+    ):
+        sys.exit(2)
+
     if not check_camera_sharpness(
         camera_observations, logging.getLogger("camera_sharpness")
     ):