Merge branch 'PhotonVision:master' into master

devTools/calibrationUtils.py

@@ -162,9 +162,9 @@ def pose_to_rt(pose: Pose3d):
         "indices_point_camintrinsics_camextrinsics": None,
         "lensmodel": model,
         "imagersizes": np.array([imagersize], dtype=np.int32),
-        "calobject_warp": np.array(cal.calobjectWarp)
-        if len(cal.calobjectWarp) > 0
-        else None,
+        "calobject_warp": (
+            np.array(cal.calobjectWarp) if len(cal.calobjectWarp) > 0 else None
+        ),
         # We always do all the things
         "do_optimize_intrinsics_core": True,
         "do_optimize_intrinsics_distortions": True,

photon-core/src/main/java/org/photonvision/vision/camera/CameraInfo.java

@@ -108,7 +108,7 @@ public boolean equals(Object obj) {
     public String toString() {
         return "CameraInfo [cameraType="
                 + cameraType
-                + "baseName="
+                + ", baseName="
                 + getBaseName()
                 + ", vid="
                 + vendorId

photon-core/src/main/java/org/photonvision/vision/processes/CameraMatchingOptions.java

@@ -0,0 +1,57 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.processes;
+
+import java.util.List;
+import org.photonvision.vision.camera.CameraType;
+
+public class CameraMatchingOptions {
+    public CameraMatchingOptions(
+            boolean checkUSBPath,
+            boolean checkVidPid,
+            boolean checkBaseName,
+            boolean checkPath,
+            CameraType... allowedTypes) {
+        this.checkUSBPath = checkUSBPath;
+        this.checkVidPid = checkVidPid;
+        this.checkBaseName = checkBaseName;
+        this.checkPath = checkPath;
+        this.allowedTypes = List.of(allowedTypes);
+    }
+
+    public final boolean checkUSBPath;
+    public final boolean checkVidPid;
+    public final boolean checkBaseName;
+    public final boolean checkPath;
+    public final List<CameraType> allowedTypes;
+
+    @Override
+    public String toString() {
+        return "CameraMatchingOptions [checkUSBPath="
+                + checkUSBPath
+                + ", checkVidPid="
+                + checkVidPid
+                + ", checkBaseName="
+                + checkBaseName
+                + ", checkPath="
+                + checkPath
+                + ", allowedTypes="
+                + allowedTypes
+                + "]";
+    }
+}

photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java

@@ -255,6 +255,8 @@ private final Predicate<CameraInfo> getCameraMatcher(
                 matches &= (physicalCamera.path.equals(savedConfig.path));
             }
 
+            matches &= (physicalCamera.cameraType == savedConfig.cameraType);
+
             return matches;
         };
     }
@@ -309,51 +311,64 @@ public List<CameraConfiguration> matchCameras(
         ArrayList<CameraConfiguration> unloadedConfigs =
                 new ArrayList<CameraConfiguration>(loadedCamConfigs);
 
-        if (detectedCameraList.size() > 0 || unloadedConfigs.size() > 0) {
-            logger.info("Matching by usb port & name & USB VID/PID...");
-            cameraConfigurations.addAll(
-                    matchCamerasByStrategy(detectedCameraList, unloadedConfigs, true, true, true, false));
-        }
+        logger.info("Matching CSI cameras by port & base name...");
+        cameraConfigurations.addAll(
+                matchCamerasByStrategy(
+                        detectedCameraList,
+                        unloadedConfigs,
+                        new CameraMatchingOptions(false, false, true, true, CameraType.ZeroCopyPicam)));
+
+        logger.info("Matching USB cameras by usb port & name & USB VID/PID...");
+        cameraConfigurations.addAll(
+                matchCamerasByStrategy(
+                        detectedCameraList,
+                        unloadedConfigs,
+                        new CameraMatchingOptions(true, true, true, false, CameraType.UsbCamera)));
 
         // On windows, the v4l path is actually useful and tells us the port the camera is physically
         // connected to which is neat
         if (Platform.isWindows() && !matchCamerasOnlyByPath) {
-            if (detectedCameraList.size() > 0 || unloadedConfigs.size() > 0) {
-                logger.info("Matching by windows-path & USB VID/PID only...");
-                cameraConfigurations.addAll(
-                        matchCamerasByStrategy(detectedCameraList, unloadedConfigs, false, true, true, true));
-            }
-        }
-
-        if (detectedCameraList.size() > 0 || unloadedConfigs.size() > 0) {
-            logger.info("Matching by usb port & USB VID/PID...");
+            logger.info("Matching USB cameras by windows-path & USB VID/PID only...");
             cameraConfigurations.addAll(
-                    matchCamerasByStrategy(detectedCameraList, unloadedConfigs, true, true, false, false));
+                    matchCamerasByStrategy(
+                            detectedCameraList,
+                            unloadedConfigs,
+                            new CameraMatchingOptions(false, true, true, true, CameraType.UsbCamera)));
         }
 
+        logger.info("Matching USB cameras by usb port & USB VID/PID...");
+        cameraConfigurations.addAll(
+                matchCamerasByStrategy(
+                        detectedCameraList,
+                        unloadedConfigs,
+                        new CameraMatchingOptions(true, true, false, false, CameraType.UsbCamera)));
+
         // Legacy migration -- VID/PID will be unset, so we have to try with our most relaxed strategy
         // at least once. We _should_ still have a valid USB path (assuming cameras have not moved), so
         // try that first, then fallback to base name only beloow
-        if (detectedCameraList.size() > 0 || unloadedConfigs.size() > 0) {
-            logger.info("Matching by base-name & usb port...");
-            cameraConfigurations.addAll(
-                    matchCamerasByStrategy(detectedCameraList, unloadedConfigs, true, false, true, false));
-        }
+        logger.info("Matching USB cameras by base-name & usb port...");
+        cameraConfigurations.addAll(
+                matchCamerasByStrategy(
+                        detectedCameraList,
+                        unloadedConfigs,
+                        new CameraMatchingOptions(true, false, true, false, CameraType.UsbCamera)));
 
         // handle disabling only-by-base-name matching
         if (!matchCamerasOnlyByPath) {
-            if (detectedCameraList.size() > 0 || unloadedConfigs.size() > 0) {
-                logger.info("Matching by base-name & USB VID/PID only...");
-                cameraConfigurations.addAll(
-                        matchCamerasByStrategy(detectedCameraList, unloadedConfigs, false, true, true, false));
-            }
+            logger.info("Matching USB cameras by base-name & USB VID/PID only...");
+            cameraConfigurations.addAll(
+                    matchCamerasByStrategy(
+                            detectedCameraList,
+                            unloadedConfigs,
+                            new CameraMatchingOptions(false, true, true, false, CameraType.UsbCamera)));
 
             // Legacy migration for if no USB VID/PID set
-            if (detectedCameraList.size() > 0 || unloadedConfigs.size() > 0) {
-                logger.info("Matching by base-name only...");
-                cameraConfigurations.addAll(
-                        matchCamerasByStrategy(detectedCameraList, unloadedConfigs, false, false, true, false));
-            }
+            logger.info("Matching USB cameras by base-name only...");
+            cameraConfigurations.addAll(
+                    matchCamerasByStrategy(
+                            detectedCameraList,
+                            unloadedConfigs,
+                            new CameraMatchingOptions(false, false, true, false, CameraType.UsbCamera)));
         } else logger.info("Skipping match by filepath/vid/pid, disabled by user");
 
         if (detectedCameraList.size() > 0) {
@@ -392,41 +407,46 @@ public List<CameraConfiguration> matchCameras(
     private List<CameraConfiguration> matchCamerasByStrategy(
             List<CameraInfo> detectedCamInfos,
             List<CameraConfiguration> unloadedConfigs,
-            boolean checkUSBPath,
-            boolean checkVidPid,
-            boolean checkBaseName,
-            boolean checkPath) {
+            CameraMatchingOptions matchingOptions) {
         List<CameraConfiguration> ret = new ArrayList<CameraConfiguration>();
         List<CameraConfiguration> unloadedConfigsCopy =
                 new ArrayList<CameraConfiguration>(unloadedConfigs);
 
+        if (unloadedConfigsCopy.isEmpty()) return List.of();
+
+        logger.debug("Matching with options " + matchingOptions.toString());
+
         for (CameraConfiguration config : unloadedConfigsCopy) {
-            // Only run match path by id if the camera is not a CSI camera.
-            if (config.cameraType != CameraType.ZeroCopyPicam) {
+            // Only run match path by id if the camera type is allowed. This allows us to specify matching
+            // behavior per-camera-type
+            if (matchingOptions.allowedTypes.contains(config.cameraType)) {
                 logger.debug(
                         String.format(
-                                "Trying to find a match for loaded camera %s by strategy (path %s vid/pid %s basename %s path %s) with camera config: %s",
-                                config.baseName,
-                                checkUSBPath,
-                                checkVidPid,
-                                checkBaseName,
-                                checkPath,
-                                camCfgToString(config)));
+                                "Trying to find a match for loaded camera %s (%s) with camera config: %s",
+                                config.baseName, config.uniqueName, camCfgToString(config)));
 
                 // Get matcher and filter against it, picking out the first match
                 Predicate<CameraInfo> matches =
-                        getCameraMatcher(config, checkUSBPath, checkVidPid, checkBaseName, checkPath);
+                        getCameraMatcher(
+                                config,
+                                matchingOptions.checkUSBPath,
+                                matchingOptions.checkVidPid,
+                                matchingOptions.checkBaseName,
+                                matchingOptions.checkPath);
                 var cameraInfo = detectedCamInfos.stream().filter(matches).findFirst().orElse(null);
 
                 // If we actually matched a camera to a config, remove that camera from the list
                 // and add it to the output
                 if (cameraInfo != null) {
-                    logger.debug("Matched the config for " + config.baseName + " to a physical camera!");
+                    logger.debug(
+                            "Matched the config for "
+                                    + config.uniqueName
+                                    + " to the physical camera config above!");
                     ret.add(mergeInfoIntoConfig(config, cameraInfo));
                     detectedCamInfos.remove(cameraInfo);
                     unloadedConfigs.remove(config);
                 } else {
-                    logger.debug("No camera found for the config " + config.baseName);
+                    logger.debug("No camera found for the config " + config.uniqueName);
                 }
             }
         }
@@ -443,7 +463,10 @@ private List<CameraConfiguration> createConfigsForCameras(
             List<CameraConfiguration> loadedConfigs) {
         List<CameraConfiguration> ret = new ArrayList<CameraConfiguration>();
         logger.debug(
-                "After matching loaded configs " + detectedCameraList.size() + " cameras were unmatched.");
+                "After matching loaded configs, these configs remained unmatched: "
+                        + detectedCameraList.stream()
+                                .map(n -> String.valueOf(n))
+                                .collect(Collectors.joining("-", "{", "}")));
         for (CameraInfo info : detectedCameraList) {
             // create new camera config for all new cameras
             String baseName = info.getBaseName();

photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java

@@ -433,6 +433,73 @@ public void testInhibitPathChangeIdenticalCams() {
         }
     }
 
+    @Test
+    public void testCSICameraMatching() {
+        Logger.setLevel(LogGroup.Camera, LogLevel.DEBUG);
+
+        // List of known cameras
+        var cameraInfos = new ArrayList<CameraInfo>();
+
+        var inst = new VisionSourceManager();
+        ConfigManager.getInstance().clearConfig();
+        ConfigManager.getInstance().load();
+        ConfigManager.getInstance().getConfig().getNetworkConfig().matchCamerasOnlyByPath = false;
+
+        CameraInfo info1 =
+                new CameraInfo(
+                        -1,
+                        "/base/soc/i2c0mux/i2c@0/ov9281@60",
+                        "OV9281", // Typically rp1-cfe for unit test changed to CSICAM-DEV
+                        new String[] {},
+                        -1,
+                        -1,
+                        CameraType.ZeroCopyPicam);
+
+        CameraInfo info2 =
+                new CameraInfo(
+                        -1,
+                        "/base/soc/i2c0mux/i2c@1/ov9281@60",
+                        "OV9281", // Typically rp1-cfe for unit test changed to CSICAM-DEV
+                        new String[] {},
+                        -1,
+                        -1,
+                        CameraType.ZeroCopyPicam);
+
+        var camera1_saved_config =
+                new CameraConfiguration(
+                        "OV9281", "OV9281", "test-1", "/base/soc/i2c0mux/i2c@0/ov9281@60", new String[0]);
+        camera1_saved_config.cameraType = CameraType.ZeroCopyPicam;
+        camera1_saved_config.usbVID = -1;
+        camera1_saved_config.usbPID = -1;
+
+        var camera2_saved_config =
+                new CameraConfiguration(
+                        "OV9281", "OV9281 (1)", "test-2", "/base/soc/i2c0mux/i2c@1/ov9281@60", new String[0]);
+        camera2_saved_config.usbVID = -1;
+        camera2_saved_config.usbPID = -1;
+        camera2_saved_config.cameraType = CameraType.ZeroCopyPicam;
+
+        cameraInfos.add(info1);
+        cameraInfos.add(info2);
+
+        // Try matching with both cameras being "known"
+        inst.registerLoadedConfigs(camera1_saved_config, camera2_saved_config);
+        var ret1 = inst.tryMatchCamImpl(cameraInfos);
+
+        // Our cameras should be "known"
+        assertTrue(inst.knownCameras.contains(info1));
+        assertTrue(inst.knownCameras.contains(info2));
+        assertEquals(2, inst.knownCameras.size());
+        assertEquals(2, ret1.size());
+
+        // Exactly one camera should have the path we put in
+        for (int i = 0; i < cameraInfos.size(); i++) {
+            var testPath = cameraInfos.get(i).path;
+            assertEquals(
+                    1, ret1.stream().filter(it -> testPath.equals(it.cameraConfiguration.path)).count());
+        }
+    }
+
     @Test
     public void testIdenticalCameras() {
         Logger.setLevel(LogGroup.Camera, LogLevel.DEBUG);