Merge branch 'RobotAndPiSideCode'

Author: ds12a

PiSideCode/Calibration/calibration.py

@@ -296,7 +296,7 @@ def getReprojectionError(self) -> float:
 
         return totalError / len(self.objPoints)
 
-    # Load a calibration file 
+    # Load a calibration file
     def loadCalibration(self, file: str):
         with open(file, "r") as f:
             self.calibrationData = json.load(f)

PiSideCode/Camera/BootUp.py

@@ -13,7 +13,7 @@ def parseConfig(camPath):
         print("File Not Found")
         fileDump = {
             "Resolution": None,
-            "Gain": None,
+            "Brightness": None,
             "Exposure": None,
         }
 
@@ -23,8 +23,13 @@ def parseConfig(camPath):
             json.dump(fileDump, outFile)
         return fileDump
 
+
 # Write config data to a file following naming conventions
-def writeConfig(camPath, resolution, gain, exposure):
+def writeConfig(camPath, resolution, brightness, exposure):
     with open(f"./Camera/CameraConfigs/ConfigSettings_{camPath}.json", "w") as data:
-        fileDump = {"Resolution": resolution, "Gain": gain, "Exposure": exposure}
+        fileDump = {
+            "Resolution": resolution,
+            "Brightness": brightness,
+            "Exposure": exposure,
+        }
         json.dump(fileDump, data)

PiSideCode/Camera/CameraConfigs/ConfigSettings_Windows0.json

@@ -1 +1 @@
-{"Resolution": [640, 480], "Gain": 1.0, "Exposure": -6.0}
+{"Resolution": [640, 480], "Brightness": 1.0, "Exposure": -6.0}

PiSideCode/Camera/CameraConfigs/ConfigSettings_platform-fc800000-usb-usb-0-1-1-0-video-index0.json

@@ -1 +1 @@
-{"Resolution": [640, 480], "Gain": -1.0, "Exposure": 54.0}
+{"Resolution": [1600, 1300], "Brightness": 10.0, "Exposure": 105.0}

PiSideCode/Camera/CameraConfigs/ConfigSettings_platform-fc880000-usb-usb-0-1-1-0-video-index0.json

@@ -1 +1 @@
-{"Resolution": [1600, 1200], "Gain": 3.0, "Exposure": 3.0}
+{"Resolution": [1600, 1200], "Brightness": 3.0, "Exposure": 3.0}

PiSideCode/Camera/CameraConfigs/ConfigSettings_platform-xhci-hcd-9-auto-usb-0-1-1-0-video-index0.json

@@ -0,0 +1 @@
+{"Resolution": [1600, 1300], "Brightness": -1.0, "Exposure": 156.0}

PiSideCode/Camera/CameraConfigs/ConfigSettings_platform-xhci-hcd-9-auto-usb-0-1-2-1-0-video-index0.json

@@ -1 +1 @@
-{"Resolution": [960, 544], "Gain": -1.0, "Exposure": 156.0}
+{"Resolution": [960, 544], "Brightness": -1.0, "Exposure": 156.0}

PiSideCode/Camera/CameraConfigs/ConfigSettings_platform-xhci-hcd-9-auto-usb-0-1-3-1-0-video-index0.json

@@ -1 +1 @@
-{"Resolution": [640, 480], "Gain": -1.0, "Exposure": 45.0}
+{"Resolution": [640, 480], "Brightness": -1.0, "Exposure": 45.0}

PiSideCode/Camera/CameraInfo.py

@@ -8,6 +8,9 @@ def __init__(
         self.identifier = identifier
         self.resolution = resolution
         self.supportedResolutions = supportedResolutions
+        self.exposureRange = (0, 100, 1)
+        self.brightnessRange = (0, 240, 1)
+        self.validFormats = []
 
         # Calibration
         self.K = K

PiSideCode/Camera/camera.py

@@ -38,34 +38,84 @@ def __init__(self):
                     Cameras.logger.info(f"Camera found: {camPath}")
 
                     # Get supported resolutions using v4l2-ctl and a little regex
-                    supportedResolutions = sorted( # Sort values
+                    formatParams = subprocess.run(
+                        [
+                            "v4l2-ctl",
+                            "-d",
+                            path,
+                            "--list-formats-ext",
+                        ],
+                        capture_output=True,
+                    ).stdout.decode("utf-8")
+
+                    supportedResolutions = sorted(  # Sort values
                         list(
-                            set( # Unique values
+                            set(  # Unique values
                                 map(
                                     lambda x: (
                                         int(x.split("x")[0]),
                                         int(x.split("x")[1]),
                                     ),
                                     re.findall(
                                         "[0-9]+x[0-9]+",
-                                        subprocess.run(
-                                            [
-                                                "v4l2-ctl",
-                                                "-d",
-                                                path,
-                                                "--list-formats-ext",
-                                            ],
-                                            capture_output=True,
-                                        ).stdout.decode("utf-8"),
+                                        formatParams,
                                     ),
                                 )
                             )
                         )
                     )
 
+                    formats = set(
+                        map(
+                            lambda x: re.search("'....'", x).group().strip("'"),
+                            re.findall(
+                                ": '....'",
+                                formatParams,
+                            ),
+                        )
+                    )
+
+                    settingParams = subprocess.run(
+                        ["v4l2-ctl", "-d", path, "--list-ctrls-menus"],
+                        capture_output=True,
+                    ).stdout.decode("utf-8")
+
+                    exposureRange = tuple(
+                        map(
+                            lambda x: int(x.split("=")[-1]),
+                            re.search(
+                                "exposure_absolute .* min=[0-9]+ max=[0-9]+ step=[0-9]+",
+                                settingParams,
+                            )
+                            .group()
+                            .split()[-3:],
+                        )
+                    )
+
+                    brightnessRange = tuple(
+                        map(
+                            lambda x: int(x.split("=")[-1]),
+                            re.search(
+                                "brightness .* min=[0-9]+ max=[0-9]+ step=[0-9]+",
+                                settingParams,
+                            )
+                            .group()
+                            .split()[-3:],
+                        )
+                    )
+
                     Cameras.logger.info(
                         f"Supported resolutions: {supportedResolutions}"
                     )
+                    Cameras.logger.info(
+                        f"Supported formats: {formats}"
+                    )
+                    Cameras.logger.info(
+                        f"Supported exposures (min, max, step): {exposureRange}"
+                    )
+                    Cameras.logger.info(
+                        f"Supported brightnesses (min, max, step): {brightnessRange}"
+                    )
 
                     # Disable buffer so we always pull the latest image
                     cam.set(cv2.CAP_PROP_BUFFERSIZE, 1)
@@ -81,6 +131,9 @@ def __init__(self):
                     # Initialize CameraInfo object
                     self.info[camPath] = CameraInfo(cam, camPath, supportedResolutions)
                     self.info[camPath].resolution = self.getResolutions()[camPath]
+                    self.info[camPath].exposureRange = exposureRange
+                    self.info[camPath].brightnessRange = brightnessRange
+                    self.info[camPath].validFormats = formats
 
                     # Attempt to import config from file
                     self.importConfig(camPath)
@@ -89,13 +142,13 @@ def __init__(self):
                     writeConfig(
                         self.cleanIdentifier(camPath),
                         self.getResolutions()[camPath],
-                        self.getGains()[camPath],
+                        self.getBrightnesss()[camPath],
                         self.getExposures()[camPath],
                     )
 
         else:
             Cameras.logger.error("Unknown platform!")
-    
+
     def setCalibration(self, identifier, K, D):
         self.info[identifier].K = K
         self.info[identifier].D = D
@@ -118,6 +171,16 @@ def getFrames(self):
             frames[identifier] = img
         return frames
 
+    # Grab frames from each camera specifically for processing
+    def getFramesForProcessing(self):
+        frames = {}
+        connections = {}
+        for identifier, camInfo in self.info.items():
+            flag, img = camInfo.cam.read()
+            frames[identifier] = img
+            connections[identifier] = True if flag else False
+        return (connections, frames)
+
     # Sets resolution, video format, and FPS
     def setResolution(self, identifier, resolution):
         if resolution is None:
@@ -127,8 +190,10 @@ def setResolution(self, identifier, resolution):
         # os.system(f"v4l2-ctl -d /dev/v4l/by-path/{identifier} --set-fmt-video=width={resolution[0]},height={resolution[1]}")
         self.info[identifier].cam.set(cv2.CAP_PROP_FRAME_HEIGHT, resolution[1])
         self.info[identifier].cam.set(cv2.CAP_PROP_FRAME_WIDTH, resolution[0])
-        self.info[identifier].cam.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*"MJPG"))
-        self.info[identifier].cam.set(cv2.CAP_PROP_FPS, 30)
+        self.info[identifier].cam.set(
+            cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*("MJPG" if "MJPG" in self.info[identifier].validFormats else "GREY"))
+        )
+        self.info[identifier].cam.set(cv2.CAP_PROP_FPS, 30)  # Lower can be better
         resolution = tuple(resolution)
 
         # Test if resolution got set
@@ -145,31 +210,35 @@ def setResolution(self, identifier, resolution):
         writeConfig(
             self.cleanIdentifier(identifier),
             resolution,
-            self.getGains()[identifier],
+            self.getBrightnesss()[identifier],
             self.getExposures()[identifier],
         )
 
         Cameras.logger.info(f"Resolution set to {resolution} for {identifier}")
         return True
 
-    def setGain(self, identifier, gain):
-        if gain is None:
-            Cameras.logger.info("Gain not set")
+    def setBrightness(self, identifier, brightness):
+        if brightness is None:
+            Cameras.logger.info("Brightness not set")
             return False
 
-        # Set gain through command line
-        os.system(f"v4l2-ctl -d /dev/v4l/by-path/{identifier} --set-ctrl gain={gain}")
+        # Set brightness through command line
+        returned = os.system(
+            f"v4l2-ctl -d /dev/v4l/by-path/{identifier} --set-ctrl brightness={brightness}"
+        )
 
         # Check if it set, if so write it to a file
-        if self.info[identifier].cam.get(cv2.CAP_PROP_GAIN) != gain:
-            Cameras.logger.warning(f"Gain not set: {gain} not accepted")
+        if returned != 0:
+            Cameras.logger.warning(
+                f"Brightness not set: {brightness} not accepted on camera {identifier}"
+            )
             return False
         else:
-            Cameras.logger.info(f"Gain set to {gain}")
+            Cameras.logger.info(f"Brightness set to {brightness}")
             writeConfig(
                 self.cleanIdentifier(identifier),
                 self.getResolutions()[identifier],
-                gain,
+                brightness,
                 self.getExposures()[identifier],
             )
             return True
@@ -180,38 +249,40 @@ def setExposure(self, identifier, exposure):
             return False
 
         # Set exposure with a command
-        os.system(
-            f"v4l2-ctl -d /dev/v4l/by-path/{identifier} --set-ctrl exposure_auto=1 --set-ctrl exposure_absolute={exposure}"
+        returned = os.system(
+            f"v4l2-ctl -d /dev/v4l/by-path/{identifier} --set-ctrl exposure_absolute={exposure}"  #  --set-ctrl exposure_auto=1
         )
 
         # Check if it set, if so write it to a file
-        if self.info[identifier].cam.get(cv2.CAP_PROP_EXPOSURE) != exposure:
-            Cameras.logger.warning(f"Exposure not set: {exposure} not accepted")
+        if returned != 0:
+            Cameras.logger.warning(
+                f"Exposure not set: {exposure} not accepted on camera {identifier}"
+            )
             return False
         else:
             Cameras.logger.info(f"Exposure set to {exposure}")
 
             writeConfig(
                 self.cleanIdentifier(identifier),
                 self.getResolutions()[identifier],
-                self.getGains()[identifier],
+                self.getBrightnesss()[identifier],
                 exposure,
             )
             return True
 
     # Return a dictionary of all camera exposures
     def getExposures(self):
-        exposure = {}
-        for identifier, camInfo in self.info.items():
-            exposure[identifier] = camInfo.cam.get(cv2.CAP_PROP_EXPOSURE)
-        return exposure
-
-    # Return a dictionary of all camera gains
-    def getGains(self):
-        gain = {}
-        for identifier, camInfo in self.info.items():
-            gain[identifier] = camInfo.cam.get(cv2.CAP_PROP_GAIN)
-        return gain
+        return {
+            identifier: camInfo.cam.get(cv2.CAP_PROP_EXPOSURE)
+            for identifier, camInfo in self.info.items()
+        }
+
+    # Return a dictionary of all camera brightnesss
+    def getBrightnesss(self):
+        return {
+            identifier: camInfo.cam.get(cv2.CAP_PROP_BRIGHTNESS)
+            for identifier, camInfo in self.info.items()
+        }
 
     # Return a dictionary of all camera resolutions
     def getResolutions(self):
@@ -222,7 +293,13 @@ def getResolutions(self):
                 int(camInfo.cam.get(cv2.CAP_PROP_FRAME_HEIGHT)),
             )
 
-        return resolution
+        return {
+            identifier: (
+                int(camInfo.cam.get(cv2.CAP_PROP_FRAME_WIDTH)),
+                int(camInfo.cam.get(cv2.CAP_PROP_FRAME_HEIGHT)),
+            )
+            for identifier, camInfo in self.info.items()
+        }
 
     # Find a calibration for the camera
     def importCalibration(self, identifier):
@@ -246,7 +323,7 @@ def importCalibration(self, identifier):
                 f"Calibration not found for camera {identifier} at resolution {resolution}"
             )
             return False
-    
+
     def importConfig(self, camPath):
         # Attempt to import config from file
         Cameras.logger.info(f"Attempting to import config for {camPath}")
@@ -267,15 +344,13 @@ def importConfig(self, camPath):
                 camPath, config["Resolution"]
             ):  # Calls self.importCalibration iff resolution was set
                 self.importCalibration(camPath)
-            self.setGain(camPath, config["Gain"])
+            self.setBrightness(camPath, config["Brightness"])
             self.setExposure(camPath, config["Exposure"])
 
         else:
             self.importCalibration(camPath)
-            Cameras.logger.warning(
-                f"Camera config not found for camera {camPath}"
-            )
-        
+            Cameras.logger.warning(f"Camera config not found for camera {camPath}")
+
         return config
 
     @staticmethod