diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java b/photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java index fac672636c..cfd3b677a8 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java @@ -137,7 +137,7 @@ public Map toHashMap() { generalSubmap.put( "gpuAcceleration", LibCameraJNI.isSupported() - ? "Zerocopy Libcamera on " + LibCameraJNI.getSensorModel().getFriendlyName() + ? "Zerocopy Libcamera Working" : ""); // TODO add support for other types of GPU accel generalSubmap.put("hardwareModel", hardwareConfig.deviceName); generalSubmap.put("hardwarePlatform", Platform.getPlatformName()); diff --git a/photon-core/src/main/java/org/photonvision/common/hardware/metrics/cmds/LinuxCmds.java b/photon-core/src/main/java/org/photonvision/common/hardware/metrics/cmds/LinuxCmds.java index 1a7d18f35e..9abff7ce9f 100644 --- a/photon-core/src/main/java/org/photonvision/common/hardware/metrics/cmds/LinuxCmds.java +++ b/photon-core/src/main/java/org/photonvision/common/hardware/metrics/cmds/LinuxCmds.java @@ -22,7 +22,7 @@ public class LinuxCmds extends CmdBase { public void initCmds(HardwareConfig config) { // CPU - cpuMemoryCommand = "awk '/MemTotal:/ {print int($2 / 1000);}' /proc/meminfo"; + cpuMemoryCommand = "free -m | awk 'FNR == 2 {print $3}'"; // TODO: boards have lots of thermal devices. Hard to pick the CPU @@ -32,7 +32,7 @@ public void initCmds(HardwareConfig config) { cpuUptimeCommand = "uptime -p | cut -c 4-"; // RAM - ramUsageCommand = "awk '/MemAvailable:/ {print int($2 / 1000);}' /proc/meminfo"; + ramUsageCommand = "free -m | awk 'FNR == 2 {print $3}'"; // Disk diskUsageCommand = "df ./ --output=pcent | tail -n +2"; diff --git a/photon-core/src/main/java/org/photonvision/common/hardware/metrics/cmds/PiCmds.java b/photon-core/src/main/java/org/photonvision/common/hardware/metrics/cmds/PiCmds.java index 6eb005acc5..ba72dc5531 100644 --- a/photon-core/src/main/java/org/photonvision/common/hardware/metrics/cmds/PiCmds.java +++ b/photon-core/src/main/java/org/photonvision/common/hardware/metrics/cmds/PiCmds.java @@ -25,7 +25,7 @@ public void initCmds(HardwareConfig config) { super.initCmds(config); // CPU - cpuMemoryCommand = "vcgencmd get_mem arm | grep -Eo '[0-9]+'"; + cpuMemoryCommand = "free -m | awk 'FNR == 2 {print $2}'"; cpuTemperatureCommand = "sed 's/.\\{3\\}$/.&/' /sys/class/thermal/thermal_zone0/temp"; cpuThrottleReasonCmd = "if (( $(( $(vcgencmd get_throttled | grep -Eo 0x[0-9a-fA-F]*) & 0x01 )) != 0x00 )); then echo \"LOW VOLTAGE\"; " diff --git a/photon-core/src/main/java/org/photonvision/raspi/LibCameraJNI.java b/photon-core/src/main/java/org/photonvision/raspi/LibCameraJNI.java index 8a12aeabfa..d97d102681 100644 --- a/photon-core/src/main/java/org/photonvision/raspi/LibCameraJNI.java +++ b/photon-core/src/main/java/org/photonvision/raspi/LibCameraJNI.java @@ -30,8 +30,6 @@ public class LibCameraJNI { private static boolean libraryLoaded = false; private static final Logger logger = new Logger(LibCameraJNI.class, LogGroup.Camera); - public static final Object CAMERA_LOCK = new Object(); - public static synchronized void forceLoad() throws IOException { if (libraryLoaded) return; @@ -93,8 +91,13 @@ public String getFriendlyName() { } } - public static SensorModel getSensorModel() { - int model = getSensorModelRaw(); + public static SensorModel getSensorModel(long r_ptr) { + int model = getSensorModelRaw(r_ptr); + return SensorModel.values()[model]; + } + + public static SensorModel getSensorModel(String name) { + int model = getSensorModelRaw(name); return SensorModel.values()[model]; } @@ -107,54 +110,64 @@ public static boolean isSupported() { private static native boolean isLibraryWorking(); - public static native int getSensorModelRaw(); + public static native int getSensorModelRaw(long r_ptr); + + public static native int getSensorModelRaw(String name); // ======================================================== // /** * Creates a new runner with a given width/height/fps * + * @param the path / name of the camera as given from libcamera. * @param width Camera video mode width in pixels * @param height Camera video mode height in pixels * @param fps Camera video mode FPS - * @return success of creating a camera object + * @return the runner pointer for the camera. */ - public static native boolean createCamera(int width, int height, int rotation); + public static native long createCamera(String name, int width, int height, int rotation); /** * Starts the camera thresholder and display threads running. Make sure that this function is * called synchronously with stopCamera and returnFrame! */ - public static native boolean startCamera(); + public static native boolean startCamera(long r_ptr); /** Stops the camera runner. Make sure to call prior to destroying the camera! */ - public static native boolean stopCamera(); + public static native boolean stopCamera(long r_ptr); // Destroy all native resources associated with a camera. Ensure stop is called prior! - public static native boolean destroyCamera(); + public static native boolean destroyCamera(long r_ptr); // ======================================================== // // Set thresholds on [0..1] public static native boolean setThresholds( - double hl, double sl, double vl, double hu, double su, double vu, boolean hueInverted); + long r_ptr, + double hl, + double sl, + double vl, + double hu, + double su, + double vu, + boolean hueInverted); - public static native boolean setAutoExposure(boolean doAutoExposure); + public static native boolean setAutoExposure(long r_ptr, boolean doAutoExposure); // Exposure time, in microseconds - public static native boolean setExposure(int exposureUs); + public static native boolean setExposure(long r_ptr, int exposureUs); // Set brightness on [-1, 1] - public static native boolean setBrightness(double brightness); + public static native boolean setBrightness(long r_ptr, double brightness); // Unknown ranges for red and blue AWB gain - public static native boolean setAwbGain(double red, double blue); + public static native boolean setAwbGain(long r_ptr, double red, double blue); /** * Get the time when the first pixel exposure was started, in the same timebase as libcamera gives * the frame capture time. Units are nanoseconds. */ - public static native long getFrameCaptureTime(); + public static native long getFrameCaptureTime(long p_ptr); /** * Get the current time, in the same timebase as libcamera gives the frame capture time. Units are @@ -162,33 +175,36 @@ public static native boolean setThresholds( */ public static native long getLibcameraTimestamp(); - public static native long setFramesToCopy(boolean copyIn, boolean copyOut); + public static native long setFramesToCopy(long r_ptr, boolean copyIn, boolean copyOut); // Analog gain multiplier to apply to all color channels, on [1, Big Number] - public static native boolean setAnalogGain(double analog); + public static native boolean setAnalogGain(long r_ptr, double analog); /** Block until a new frame is available from native code. */ - public static native boolean awaitNewFrame(); + public static native long awaitNewFrame(long r_ptr); /** * Get a pointer to the most recent color mat generated. Call this immediately after * awaitNewFrame, and call only once per new frame! */ - public static native long takeColorFrame(); + public static native long takeColorFrame(long pair_ptr); /** * Get a pointer to the most recent processed mat generated. Call this immediately after * awaitNewFrame, and call only once per new frame! */ - public static native long takeProcessedFrame(); + public static native long takeProcessedFrame(long pair_ptr); /** * Set the GPU processing type we should do. Enum of [none, HSV, greyscale, adaptive threshold]. */ - public static native boolean setGpuProcessType(int type); + public static native boolean setGpuProcessType(long r_ptr, int type); + + public static native int getGpuProcessType(long p_ptr); - public static native int getGpuProcessType(); + /** Release a pair pointer back to the libcamera driver code to be filled again */ + public static native boolean releasePair(long p_ptr); - // Release a frame pointer back to the libcamera driver code to be filled again */ - // public static native long returnFrame(long frame); + /** Get an array containing the names/ids/paths of all connected CSI cameras from libcamera. */ + public static native String[] getCameraNames(); } diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/CameraInfo.java b/photon-core/src/main/java/org/photonvision/vision/camera/CameraInfo.java new file mode 100644 index 0000000000..568453bdfe --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/vision/camera/CameraInfo.java @@ -0,0 +1,83 @@ +/* + * 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 . + */ + +package org.photonvision.vision.camera; + +import edu.wpi.first.cscore.UsbCameraInfo; +import java.util.Arrays; + +public class CameraInfo extends UsbCameraInfo { + public final CameraType cameraType; + + public CameraInfo( + int dev, String path, String name, String[] otherPaths, int vendorId, int productId) { + super(dev, path, name, otherPaths, vendorId, productId); + cameraType = CameraType.UsbCamera; + } + + public CameraInfo( + int dev, + String path, + String name, + String[] otherPaths, + int vendorId, + int productId, + CameraType cameraType) { + super(dev, path, name, otherPaths, vendorId, productId); + this.cameraType = cameraType; + } + + public CameraInfo(UsbCameraInfo info) { + super(info.dev, info.path, info.name, info.otherPaths, info.vendorId, info.productId); + cameraType = CameraType.UsbCamera; + } + + /** + * @return True, if this camera is reported from V4L and is a CSI camera. + */ + public boolean getIsV4lCsiCamera() { + return (Arrays.stream(otherPaths).anyMatch(it -> it.contains("csi-video")) + || getBaseName().equals("unicam")); + } + + /** + * @return The base name of the camera aka the name as just ascii. + */ + public String getBaseName() { + return name.replaceAll("[^\\x00-\\x7F]", ""); + } + + /** + * @param baseName + * @return Returns a human readable name + */ + public String getHumanReadableName() { + return getBaseName().replaceAll(" ", "_"); + } + + @Override + public boolean equals(Object o) { + if (o == this) return true; + if (!(o instanceof UsbCameraInfo || o instanceof CameraInfo)) return false; + UsbCameraInfo other = (UsbCameraInfo) o; + return path.equals(other.path) + // && a.dev == b.dev (dev is not constant in Windows) + && name.equals(other.name) + && productId == other.productId + && vendorId == other.vendorId; + } +} diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/LibcameraGpuSettables.java b/photon-core/src/main/java/org/photonvision/vision/camera/LibcameraGpuSettables.java index 2fdde391d2..45aefb5fc0 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/LibcameraGpuSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/LibcameraGpuSettables.java @@ -34,11 +34,13 @@ public class LibcameraGpuSettables extends VisionSourceSettables { private boolean lastAutoExposureActive; private int lastGain = 50; private Pair lastAwbGains = new Pair<>(18, 18); - private boolean m_initialized = false; + public long r_ptr = 0; private final LibCameraJNI.SensorModel sensorModel; - private ImageRotationMode m_rotationMode; + private ImageRotationMode m_rotationMode = ImageRotationMode.DEG_0; + + public final Object CAMERA_LOCK = new Object(); public void setRotation(ImageRotationMode rotationMode) { if (rotationMode != m_rotationMode) { @@ -53,7 +55,7 @@ public LibcameraGpuSettables(CameraConfiguration configuration) { videoModes = new HashMap<>(); - sensorModel = LibCameraJNI.getSensorModel(); + sensorModel = LibCameraJNI.getSensorModel(configuration.path); if (sensorModel == LibCameraJNI.SensorModel.IMX219) { // Settings for the IMX219 sensor, which is used on the Pi Camera Module v2 @@ -121,7 +123,7 @@ public double getFOV() { @Override public void setAutoExposure(boolean cameraAutoExposure) { lastAutoExposureActive = cameraAutoExposure; - LibCameraJNI.setAutoExposure(cameraAutoExposure); + LibCameraJNI.setAutoExposure(r_ptr, cameraAutoExposure); } @Override @@ -147,7 +149,7 @@ public void setExposure(double exposure) { } lastManualExposure = exposure; - var success = LibCameraJNI.setExposure((int) Math.round(exposure) * 800); + var success = LibCameraJNI.setExposure(r_ptr, (int) Math.round(exposure) * 800); if (!success) LibcameraGpuSource.logger.warn("Couldn't set Pi Camera exposure"); } @@ -155,7 +157,7 @@ public void setExposure(double exposure) { public void setBrightness(int brightness) { lastBrightness = brightness; double realBrightness = MathUtils.map(brightness, 0.0, 100.0, -1.0, 1.0); - var success = LibCameraJNI.setBrightness(realBrightness); + var success = LibCameraJNI.setBrightness(r_ptr, realBrightness); if (!success) LibcameraGpuSource.logger.warn("Couldn't set Pi Camera brightness"); } @@ -163,7 +165,7 @@ public void setBrightness(int brightness) { public void setGain(int gain) { lastGain = gain; // TODO units here seem odd -- 5ish seems legit? So divide by 10 - var success = LibCameraJNI.setAnalogGain(gain / 10.0); + var success = LibCameraJNI.setAnalogGain(r_ptr, gain / 10.0); if (!success) LibcameraGpuSource.logger.warn("Couldn't set Pi Camera gain"); } @@ -185,7 +187,7 @@ public void setBlueGain(int blue) { public void setAwbGain(int red, int blue) { if (sensorModel != LibCameraJNI.SensorModel.OV9281) { - var success = LibCameraJNI.setAwbGain(red / 10.0, blue / 10.0); + var success = LibCameraJNI.setAwbGain(r_ptr, red / 10.0, blue / 10.0); if (!success) LibcameraGpuSource.logger.warn("Couldn't set Pi Camera AWB gains"); } } @@ -201,29 +203,35 @@ protected void setVideoModeInternal(VideoMode videoMode) { // We need to make sure that other threads don't try to do anything funny while we're recreating // the camera - synchronized (LibCameraJNI.CAMERA_LOCK) { - if (m_initialized) { + synchronized (CAMERA_LOCK) { + if (r_ptr != 0) { logger.debug("Stopping libcamera"); - if (!LibCameraJNI.stopCamera()) { + if (!LibCameraJNI.stopCamera(r_ptr)) { logger.error("Couldn't stop a zero copy Pi Camera while switching video modes"); } logger.debug("Destroying libcamera"); - if (!LibCameraJNI.destroyCamera()) { + if (!LibCameraJNI.destroyCamera(r_ptr)) { logger.error("Couldn't destroy a zero copy Pi Camera while switching video modes"); } } logger.debug("Creating libcamera"); - if (!LibCameraJNI.createCamera( - mode.width, mode.height, (m_rotationMode == ImageRotationMode.DEG_180 ? 180 : 0))) { + r_ptr = + LibCameraJNI.createCamera( + getConfiguration().path, + mode.width, + mode.height, + (m_rotationMode == ImageRotationMode.DEG_180 ? 180 : 0)); + if (r_ptr == 0) { logger.error("Couldn't create a zero copy Pi Camera while switching video modes"); + if (!LibCameraJNI.destroyCamera(r_ptr)) { + logger.error("Couldn't destroy a zero copy Pi Camera while switching video modes"); + } } logger.debug("Starting libcamera"); - if (!LibCameraJNI.startCamera()) { + if (!LibCameraJNI.startCamera(r_ptr)) { logger.error("Couldn't start a zero copy Pi Camera while switching video modes"); } - - m_initialized = true; } // We don't store last settings on the native side, and when you change video mode these get @@ -234,7 +242,7 @@ protected void setVideoModeInternal(VideoMode videoMode) { setGain(lastGain); setAwbGain(lastAwbGains.getFirst(), lastAwbGains.getSecond()); - LibCameraJNI.setFramesToCopy(true, true); + LibCameraJNI.setFramesToCopy(r_ptr, true, true); currentVideoMode = mode; } @@ -243,4 +251,8 @@ protected void setVideoModeInternal(VideoMode videoMode) { public HashMap getAllVideoModes() { return videoModes; } + + public LibCameraJNI.SensorModel getModel() { + return sensorModel; + } } diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/provider/LibcameraGpuFrameProvider.java b/photon-core/src/main/java/org/photonvision/vision/frame/provider/LibcameraGpuFrameProvider.java index 7b76f658ad..36ebe47cfb 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/provider/LibcameraGpuFrameProvider.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/provider/LibcameraGpuFrameProvider.java @@ -20,6 +20,7 @@ import org.opencv.core.Mat; import org.photonvision.common.util.math.MathUtils; import org.photonvision.raspi.LibCameraJNI; +import org.photonvision.raspi.LibCameraJNI.SensorModel; import org.photonvision.vision.camera.LibcameraGpuSettables; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameProvider; @@ -47,35 +48,38 @@ public String getName() { @Override public Frame get() { - // We need to make sure that other threads don't try to change video modes while we're waiting + // We need to make sure that other threads don't try to change video modes while + // we're waiting // for a frame // System.out.println("GET!"); - synchronized (LibCameraJNI.CAMERA_LOCK) { - var success = LibCameraJNI.awaitNewFrame(); + synchronized (settables.CAMERA_LOCK) { + var p_ptr = LibCameraJNI.awaitNewFrame(settables.r_ptr); - if (!success) { + if (p_ptr == 0) { System.out.println("No new frame"); return new Frame(); } - var colorMat = new CVMat(new Mat(LibCameraJNI.takeColorFrame())); - var processedMat = new CVMat(new Mat(LibCameraJNI.takeProcessedFrame())); + var colorMat = new CVMat(new Mat(LibCameraJNI.takeColorFrame(p_ptr))); + var processedMat = new CVMat(new Mat(LibCameraJNI.takeProcessedFrame(p_ptr))); // System.out.println("Color mat: " + colorMat.getMat().size()); // Imgcodecs.imwrite("color" + i + ".jpg", colorMat.getMat()); // Imgcodecs.imwrite("processed" + (i) + ".jpg", processedMat.getMat()); - int itype = LibCameraJNI.getGpuProcessType(); + int itype = LibCameraJNI.getGpuProcessType(p_ptr); FrameThresholdType type = FrameThresholdType.NONE; if (itype < FrameThresholdType.values().length && itype >= 0) { type = FrameThresholdType.values()[itype]; } var now = LibCameraJNI.getLibcameraTimestamp(); - var capture = LibCameraJNI.getFrameCaptureTime(); + var capture = LibCameraJNI.getFrameCaptureTime(p_ptr); var latency = (now - capture); + LibCameraJNI.releasePair(p_ptr); + return new Frame( colorMat, processedMat, @@ -87,7 +91,9 @@ public Frame get() { @Override public void requestFrameThresholdType(FrameThresholdType type) { - LibCameraJNI.setGpuProcessType(type.ordinal()); + if (settables.getModel() == SensorModel.OV9281 && type.equals(FrameThresholdType.GREYSCALE)) + LibCameraJNI.setGpuProcessType(settables.r_ptr, 4); // 4 = Grayscale pass through. + else LibCameraJNI.setGpuProcessType(settables.r_ptr, type.ordinal()); } @Override @@ -98,6 +104,7 @@ public void requestFrameRotation(ImageRotationMode rotationMode) { @Override public void requestHsvSettings(HSVParams params) { LibCameraJNI.setThresholds( + settables.r_ptr, params.getHsvLower().val[0] / 180.0, params.getHsvLower().val[1] / 255.0, params.getHsvLower().val[2] / 255.0, @@ -109,6 +116,6 @@ public void requestHsvSettings(HSVParams params) { @Override public void requestFrameCopies(boolean copyInput, boolean copyOutput) { - LibCameraJNI.setFramesToCopy(copyInput, copyOutput); + LibCameraJNI.setFramesToCopy(settables.r_ptr, copyInput, copyOutput); } } diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java index 970b1a6ae7..a84e6aba4e 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java @@ -18,13 +18,11 @@ package org.photonvision.vision.processes; import edu.wpi.first.cscore.UsbCamera; -import edu.wpi.first.cscore.UsbCameraInfo; import java.util.ArrayList; import java.util.Arrays; import java.util.Collection; import java.util.List; import java.util.concurrent.CopyOnWriteArrayList; -import java.util.function.Supplier; import java.util.stream.Collectors; import org.photonvision.common.configuration.CameraConfiguration; import org.photonvision.common.configuration.ConfigManager; @@ -35,6 +33,7 @@ import org.photonvision.common.logging.Logger; import org.photonvision.common.util.TimedTaskManager; import org.photonvision.raspi.LibCameraJNI; +import org.photonvision.vision.camera.CameraInfo; import org.photonvision.vision.camera.CameraQuirk; import org.photonvision.vision.camera.CameraType; import org.photonvision.vision.camera.LibcameraGpuSource; @@ -44,9 +43,11 @@ public class VisionSourceManager { private static final Logger logger = new Logger(VisionSourceManager.class, LogGroup.Camera); private static final List deviceBlacklist = List.of("bcm2835-isp"); - final List knownUsbCameras = new CopyOnWriteArrayList<>(); + final List knownCameras = new CopyOnWriteArrayList<>(); + final List unmatchedLoadedConfigs = new CopyOnWriteArrayList<>(); private boolean hasWarned; + private boolean hasWarnedNoCameras = false; private String ignoredCamerasRegex = ""; private static class SingletonHolder { @@ -60,7 +61,7 @@ public static VisionSourceManager getInstance() { VisionSourceManager() {} public void registerTimedTask() { - TimedTaskManager.getInstance().addTask("VisionSourceManager", this::tryMatchUSBCams, 3000); + TimedTaskManager.getInstance().addTask("VisionSourceManager", this::tryMatchCams, 3000); } public void registerLoadedConfigs(CameraConfiguration... configs) { @@ -77,11 +78,37 @@ public void registerLoadedConfigs(Collection configs) { unmatchedLoadedConfigs.addAll(configs); } - protected Supplier> cameraInfoSupplier = - () -> List.of(UsbCamera.enumerateUsbCameras()); + /** + * Pre filter out any csi cameras to return just USB Cameras. Allow defining the camerainfo. + * + * @return a list containing usbcamerainfo. + */ + protected List getConnectedUSBCameras() { + List cameraInfos = + List.of(UsbCamera.enumerateUsbCameras()).stream() + .map(c -> new CameraInfo(c)) + .collect(Collectors.toList()); + return cameraInfos; + } + + /** + * Retrieve the list of csi cameras from libcamera. + * + * @return a list containing csicamerainfo. + */ + protected List getConnectedCSICameras() { + List cameraInfos = new ArrayList(); + if (LibCameraJNI.isSupported()) + for (String path : LibCameraJNI.getCameraNames()) { + String name = LibCameraJNI.getSensorModel(path).getFriendlyName(); + cameraInfos.add( + new CameraInfo(-1, path, name, new String[] {}, -1, -1, CameraType.ZeroCopyPicam)); + } + return cameraInfos; + } - protected void tryMatchUSBCams() { - var visionSourceList = tryMatchUSBCamImpl(); + protected void tryMatchCams() { + var visionSourceList = tryMatchCamImpl(); if (visionSourceList == null) return; logger.info("Adding " + visionSourceList.size() + " configs to VMM."); @@ -94,62 +121,57 @@ protected void tryMatchUSBCams() { "fullsettings", ConfigManager.getInstance().getConfig().toHashMap())); } - protected List tryMatchUSBCamImpl() { - return tryMatchUSBCamImpl(true); + protected List tryMatchCamImpl() { + return tryMatchCamImpl(null); } - protected List tryMatchUSBCamImpl(boolean createSources) { - // Detect cameras using CSCore - List connectedCameras = - new ArrayList<>(filterAllowedDevices(cameraInfoSupplier.get())); - - // Remove all known devices - var notYetLoadedCams = new ArrayList(); - for (var connectedCam : connectedCameras) { - boolean cameraIsUnknown = true; - for (UsbCameraInfo knownCam : this.knownUsbCameras) { - if (usbCamEquals(knownCam, connectedCam)) { - cameraIsUnknown = false; - break; - } - } - if (cameraIsUnknown) { - notYetLoadedCams.add(connectedCam); - } + /** + * @param cameraInfos Used to feed camera info for unit tests. + * @return New VisionSources. + */ + protected List tryMatchCamImpl(ArrayList cameraInfos) { + boolean createSources = true; + List connectedCameras; + if (cameraInfos == null) { + // Detect USB cameras using CSCore + connectedCameras = new ArrayList<>(filterAllowedDevices(getConnectedUSBCameras())); + // Detect CSI cameras using libcamera + connectedCameras.addAll(new ArrayList<>(filterAllowedDevices(getConnectedCSICameras()))); + } else { + connectedCameras = new ArrayList<>(filterAllowedDevices(cameraInfos)); + createSources = + false; // Dont create sources if we are using supplied camerainfo for unit tests. } - if (notYetLoadedCams.isEmpty()) return null; - - if (connectedCameras.isEmpty()) { - logger.warn( - "No USB cameras were detected! Check that all cameras are connected, and that the path is correct."); + // Return no new sources because there are no new sources + if (connectedCameras.isEmpty() && !cameraInfos.isEmpty()) { + if (hasWarnedNoCameras) { + logger.warn( + "No cameras were detected! Check that all cameras are connected, and that the path is correct."); + hasWarnedNoCameras = true; + } return null; - } - logger.debug("Matching " + notYetLoadedCams.size() + " new cameras!"); + } else hasWarnedNoCameras = false; - // Sort out just the USB cams - var usbCamConfigs = new ArrayList<>(); - for (var config : unmatchedLoadedConfigs) { - if (config.cameraType == CameraType.UsbCamera) usbCamConfigs.add(config); - } + // Remove any known cameras. + connectedCameras.removeIf(c -> knownCameras.contains(c)); + + // All cameras are already loaded return no new sources. + if (connectedCameras.isEmpty()) return null; + + logger.debug("Matching " + connectedCameras.size() + " new cameras!"); // Debug prints - for (var info : notYetLoadedCams) { + for (var info : connectedCameras) { logger.info("Adding local video device - \"" + info.name + "\" at \"" + info.path + "\""); } - if (!usbCamConfigs.isEmpty()) - logger.debug("Trying to match " + usbCamConfigs.size() + " unmatched configs..."); + if (!unmatchedLoadedConfigs.isEmpty()) + logger.debug("Trying to match " + unmatchedLoadedConfigs.size() + " unmatched configs..."); // Match camera configs to physical cameras - - List matchedCameras; - - if (!createSources) { - matchedCameras = matchUSBCameras(notYetLoadedCams, unmatchedLoadedConfigs, false); - } else { - matchedCameras = matchUSBCameras(notYetLoadedCams, unmatchedLoadedConfigs); - } + List matchedCameras = + matchCameras(connectedCameras, unmatchedLoadedConfigs); unmatchedLoadedConfigs.removeAll(matchedCameras); if (!unmatchedLoadedConfigs.isEmpty() && !hasWarned) { @@ -167,11 +189,8 @@ protected List tryMatchUSBCamImpl(boolean createSources) { } // We add the matched cameras to the known camera list - for (var cam : notYetLoadedCams) { - if (this.knownUsbCameras.stream().noneMatch(it -> usbCamEquals(it, cam))) { - this.knownUsbCameras.add(cam); - } - } + this.knownCameras.addAll(connectedCameras); + if (matchedCameras.isEmpty()) return null; // for unit tests only! @@ -201,115 +220,64 @@ protected List tryMatchUSBCamImpl(boolean createSources) { * disk. * * @param detectedCamInfos Information about currently connected USB cameras. - * @param loadedUsbCamConfigs The USB {@link CameraConfiguration}s loaded from disk. + * @param loadedCamConfigs The USB {@link CameraConfiguration}s loaded from disk. * @return the matched configurations. */ - protected List matchUSBCameras( - List detectedCamInfos, List loadedUsbCamConfigs) { - return matchUSBCameras(detectedCamInfos, loadedUsbCamConfigs, true); - } - - /** - * Create {@link CameraConfiguration}s based on a list of detected USB cameras and the configs on - * disk. - * - * @param detectedCamInfos Information about currently connected USB cameras. - * @param loadedUsbCamConfigs The USB {@link CameraConfiguration}s loaded from disk. - * @param useJNI If false, this is a unit test and the JNI should not be used for CSI devices. - * @return the matched configurations. - */ - private List matchUSBCameras( - List detectedCamInfos, - List loadedUsbCamConfigs, - boolean useJNI) { + public List matchCameras( + List detectedCamInfos, List loadedCamConfigs) { var detectedCameraList = new ArrayList<>(detectedCamInfos); - ArrayList cameraConfigurations = new ArrayList<>(); + ArrayList cameraConfigurations = new ArrayList(); + ArrayList unloadedConfigs = + new ArrayList(loadedCamConfigs); - List unmatchedAfterByID = new ArrayList<>(loadedUsbCamConfigs); + if (detectedCameraList.size() > 0 || unloadedConfigs.size() > 0) + cameraConfigurations.addAll(matchByPathByID(detectedCameraList, unloadedConfigs)); + else logger.debug("Skipping matchByPath no configs or cameras left to match"); - // loop over all the configs loaded from disk, attempting to match each camera - // to a config by path-by-id on linux - for (CameraConfiguration config : loadedUsbCamConfigs) { - UsbCameraInfo cameraInfo; + if (detectedCameraList.size() > 0 || unloadedConfigs.size() > 0) + cameraConfigurations.addAll(matchByPath(detectedCameraList, unloadedConfigs)); + else logger.debug("Skipping matchByPath no configs or cameras left to match"); - if (config.otherPaths.length == 0) { - logger.debug("No valid path-by-id found for config with name " + config.baseName); - } else { - // attempt matching by path and basename - logger.debug( - "Trying to find a match for loaded camera " - + config.baseName - + " with path-by-id " - + config.otherPaths[0]); - cameraInfo = - detectedCameraList.stream() - .filter( - usbCameraInfo -> - usbCameraInfo.otherPaths.length != 0 - && usbCameraInfo.otherPaths[0].equals(config.otherPaths[0]) - && cameraNameToBaseName(usbCameraInfo.name).equals(config.baseName)) - .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!"); - detectedCameraList.remove(cameraInfo); - unmatchedAfterByID.remove(config); - cameraConfigurations.add(mergeInfoIntoConfig(config, cameraInfo)); - } - } - } + if (detectedCameraList.size() > 0 || unloadedConfigs.size() > 0) + cameraConfigurations.addAll(matchByName(detectedCameraList, unloadedConfigs)); + else logger.debug("Skipping matchByName no configs or cameras left to match"); - if (!unmatchedAfterByID.isEmpty() && !detectedCameraList.isEmpty()) { - logger.debug("Match by path-by-id failed, falling back to path-only matching"); - - List unmatchedAfterByPath = new ArrayList<>(loadedUsbCamConfigs); - - // now attempt to match the cameras and configs remaining by normal path - for (CameraConfiguration config : unmatchedAfterByID) { - UsbCameraInfo cameraInfo; - - // attempt matching by path and basename - logger.debug( - "Trying to find a match for loaded camera " - + config.baseName - + " with path " - + config.path); - cameraInfo = - detectedCameraList.stream() - .filter( - usbCameraInfo -> - usbCameraInfo.path.equals(config.path) - && cameraNameToBaseName(usbCameraInfo.name).equals(config.baseName)) - .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!"); - detectedCameraList.remove(cameraInfo); - unmatchedAfterByPath.remove(config); - cameraConfigurations.add(mergeInfoIntoConfig(config, cameraInfo)); - } - } - - if (!unmatchedAfterByPath.isEmpty() && !detectedCameraList.isEmpty()) { - logger.debug("Match by ID and path failed, falling back to name-only matching"); - - // if both path and ID based matching fails, attempt basename only match - for (CameraConfiguration config : unmatchedAfterByPath) { - UsbCameraInfo cameraInfo; + if (detectedCameraList.size() > 0) + cameraConfigurations.addAll( + createConfigsForCameras(detectedCameraList, unloadedConfigs, cameraConfigurations)); - logger.debug("Trying to find a match for loaded camera with name " + config.baseName); + logger.debug("Matched or created " + cameraConfigurations.size() + " camera configs!"); + return cameraConfigurations; + } + // loop over all the configs loaded from disk, attempting to match each camera + // to a config by path-by-id on linux + private List matchByPathByID( + List detectedCamInfos, List unloadedConfigs) { + List ret = new ArrayList(); + List unloadedConfigsCopy = + new ArrayList(unloadedConfigs); + + for (CameraConfiguration config : unloadedConfigsCopy) { + // Only run match path by id if the camera is not a CSI camera. + if (config.cameraType != CameraType.ZeroCopyPicam) { + CameraInfo cameraInfo; + if (config.otherPaths.length == 0) { + logger.debug("No valid path-by-id found for config with name " + config.baseName); + } else { + // attempt matching by path and basename + logger.debug( + "Trying to find a match for loaded camera " + + config.baseName + + " with path-by-id " + + config.otherPaths[0]); cameraInfo = - detectedCameraList.stream() + detectedCamInfos.stream() .filter( usbCameraInfo -> - cameraNameToBaseName(usbCameraInfo.name).equals(config.baseName)) + usbCameraInfo.otherPaths.length != 0 + && usbCameraInfo.otherPaths[0].equals(config.otherPaths[0]) + && usbCameraInfo.getBaseName().equals(config.baseName)) .findFirst() .orElse(null); @@ -317,60 +285,123 @@ && cameraNameToBaseName(usbCameraInfo.name).equals(config.baseName)) // and add it to the output if (cameraInfo != null) { logger.debug("Matched the config for " + config.baseName + " to a physical camera!"); - detectedCameraList.remove(cameraInfo); - cameraConfigurations.add(mergeInfoIntoConfig(config, cameraInfo)); + ret.add(mergeInfoIntoConfig(config, cameraInfo)); + detectedCamInfos.remove(cameraInfo); + unloadedConfigs.remove(config); } } } } + return ret; + } - // If we have any unmatched cameras left, create a new CameraConfiguration for - // them here. + private List matchByPath( + List detectedCamInfos, List unloadedConfigs) { + List ret = new ArrayList(); + List unloadedConfigsCopy = + new ArrayList(unloadedConfigs); + // now attempt to match the cameras and configs remaining by normal path + for (CameraConfiguration config : unloadedConfigsCopy) { + CameraInfo cameraInfo; + + // attempt matching by path and basename + logger.debug( + "Trying to find a match for loaded camera " + + config.baseName + + " with path " + + config.path); + cameraInfo = + detectedCamInfos.stream() + .filter( + usbCameraInfo -> + usbCameraInfo.path.equals(config.path) + && usbCameraInfo.getBaseName().equals(config.baseName)) + .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!"); + ret.add(mergeInfoIntoConfig(config, cameraInfo)); + detectedCamInfos.remove(cameraInfo); + unloadedConfigs.remove(config); + } + } + return ret; + } + + // Try matching cameras to configs by name. + private List matchByName( + List detectedCamInfos, List unloadedConfigs) { + List ret = new ArrayList(); + List unloadedConfigsCopy = + new ArrayList(unloadedConfigs); + // if both path and ID based matching fails, attempt basename only match + for (CameraConfiguration config : unloadedConfigsCopy) { + CameraInfo cameraInfo; + + logger.debug("Trying to find a match for loaded camera with name " + config.baseName); + + cameraInfo = + detectedCamInfos.stream() + .filter(CameraInfo -> CameraInfo.getBaseName().equals(config.baseName)) + .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!"); + ret.add(mergeInfoIntoConfig(config, cameraInfo)); + detectedCamInfos.remove(cameraInfo); + unloadedConfigs.remove(config); + } + } + return ret; + } + + // If we have any unmatched cameras left, create a new CameraConfiguration for + // them here. + private List createConfigsForCameras( + List detectedCameraList, + List loadedCamConfigs, + List loadedConfigs) { + List ret = new ArrayList(); logger.debug( "After matching loaded configs " + detectedCameraList.size() + " cameras were unmatched."); - for (UsbCameraInfo info : detectedCameraList) { + for (CameraInfo info : detectedCameraList) { // create new camera config for all new cameras - String baseName = cameraNameToBaseName(info.name); - String uniqueName = baseNameToUniqueName(baseName); + String baseName = info.getBaseName(); + String uniqueName = info.getHumanReadableName(); int suffix = 0; - while (containsName(cameraConfigurations, uniqueName) || containsName(uniqueName)) { + while (containsName(loadedConfigs, uniqueName) || containsName(uniqueName)) { suffix++; uniqueName = String.format("%s (%d)", uniqueName, suffix); } logger.info("Creating a new camera config for camera " + uniqueName); - // HACK -- for picams, we want to use the camera model String nickname = uniqueName; - if (isCsiCamera(info)) { - if (useJNI) { - nickname = LibCameraJNI.getSensorModel().toString(); - } else { - nickname = "CSICAM-DEV"; - } - } CameraConfiguration configuration = new CameraConfiguration(baseName, uniqueName, nickname, info.path, info.otherPaths); - cameraConfigurations.add(configuration); - } - logger.debug("Matched or created " + cameraConfigurations.size() + " camera configs!"); - return cameraConfigurations; - } + configuration.cameraType = info.cameraType; - private boolean isCsiCamera(UsbCameraInfo configuration) { - return (Arrays.stream(configuration.otherPaths).anyMatch(it -> it.contains("csi-video")) - || cameraNameToBaseName(configuration.name).equals("unicam")); + ret.add(configuration); + } + return ret; } - private CameraConfiguration mergeInfoIntoConfig(CameraConfiguration cfg, UsbCameraInfo info) { + private CameraConfiguration mergeInfoIntoConfig(CameraConfiguration cfg, CameraInfo info) { if (!cfg.path.equals(info.path)) { logger.debug("Updating path config from " + cfg.path + " to " + info.path); cfg.path = info.path; } cfg.otherPaths = info.otherPaths; + cfg.cameraType = info.cameraType; if (cfg.otherPaths.length != info.otherPaths.length) { logger.debug( @@ -400,94 +431,40 @@ public void setIgnoredCamerasRegex(String ignoredCamerasRegex) { this.ignoredCamerasRegex = ignoredCamerasRegex; } - private List filterAllowedDevices(List allDevices) { - List filteredDevices = new ArrayList<>(); - List badDevices = new ArrayList<>(); - + /** + * Filter out any blacklisted or ignored devices. + * + * @param allDevices + * @return list of devices with blacklisted or ingore devices removed. + */ + private List filterAllowedDevices(List allDevices) { + List filteredDevices = new ArrayList<>(); for (var device : allDevices) { - // Filter devices that are physically the same device but may show up as multiple devices that - // really cant be accessed. First noticed with raspi 5 and ov5647. - - List paths = new ArrayList<>(); - - boolean skip = false; - if (device.otherPaths.length != 0) { - // Use the other paths to filter out devices that share the same path other than the index - // select only the lowest index. - // A ov5647 on a raspi 5 would show another path as - // platform-1000880000.pisp_be-video-index0, - // platform-1000880000.pisp_be-video-index4, and platform-1000880000.pisp_be-video-index5. - // This code will remove "indexX" from all the other paths from all the devices and make - // sure - // that we only take one camera stream from each device the stream with the lowest index. - for (String p : device.otherPaths) { - paths.add(p.split("index")[0]); - } - for (var otherDevice : filteredDevices) { - if (otherDevice.otherPaths.length == 0) continue; - List otherPaths = new ArrayList<>(); - for (String p : otherDevice.otherPaths) { - otherPaths.add(p.split("index")[0]); - } - if (paths.containsAll(otherPaths)) { - if (otherDevice.dev >= device.dev) { - badDevices.add(otherDevice); - } else { - skip = true; - break; - } - } - } - } - - filteredDevices.removeAll(badDevices); if (deviceBlacklist.contains(device.name)) { logger.trace( "Skipping blacklisted device: \"" + device.name + "\" at \"" + device.path + "\""); } else if (device.name.matches(ignoredCamerasRegex)) { logger.trace("Skipping ignored device: \"" + device.name + "\" at \"" + device.path); - } else if (!skip) { + } else if (device.getIsV4lCsiCamera()) { + } else { filteredDevices.add(device); logger.trace( "Adding local video device - \"" + device.name + "\" at \"" + device.path + "\""); - } else { - logger.trace("Skipping duplicate device: \"" + device.name + "\" at \"" + device.path); } } return filteredDevices; } - private boolean usbCamEquals(UsbCameraInfo a, UsbCameraInfo b) { - return a.path.equals(b.path) - // && a.dev == b.dev (dev is not constant in Windows) - && a.name.equals(b.name) - && a.productId == b.productId - && a.vendorId == b.vendorId; - } - - // Remove all non-ASCII characters - private static String cameraNameToBaseName(String cameraName) { - return cameraName.replaceAll("[^\\x00-\\x7F]", ""); - } - - // Replace spaces with underscores - private static String baseNameToUniqueName(String baseName) { - return baseName.replaceAll(" ", "_"); - } - private static List loadVisionSourcesFromCamConfigs( List camConfigs) { var cameraSources = new ArrayList(); for (var configuration : camConfigs) { logger.debug("Creating VisionSource for " + configuration); - // Picams should have csi-video in the path - boolean is_picam = - (Arrays.stream(configuration.otherPaths).anyMatch(it -> it.contains("csi-video")) - || configuration.baseName.equals("unicam")); boolean is_pi = Platform.isRaspberryPi(); - if (is_picam && is_pi) { - configuration.cameraType = CameraType.ZeroCopyPicam; + + if (configuration.cameraType == CameraType.ZeroCopyPicam && is_pi) { + // If the camera was loaded from libcamera then create its source using libcamera. var piCamSrc = new LibcameraGpuSource(configuration); cameraSources.add(piCamSrc); } else { diff --git a/photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java b/photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java index 4a995d26fd..487a9e5ae9 100644 --- a/photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java @@ -20,21 +20,21 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertTrue; -import edu.wpi.first.cscore.UsbCameraInfo; import java.util.ArrayList; import org.junit.jupiter.api.Test; import org.photonvision.common.configuration.CameraConfiguration; import org.photonvision.common.configuration.ConfigManager; +import org.photonvision.vision.camera.CameraInfo; +import org.photonvision.vision.camera.CameraType; public class VisionSourceManagerTest { @Test public void visionSourceTest() { var inst = new VisionSourceManager(); - var infoList = new ArrayList(); - inst.cameraInfoSupplier = () -> infoList; + var cameraInfos = new ArrayList(); ConfigManager.getInstance().load(); - inst.tryMatchUSBCamImpl(); + inst.tryMatchCamImpl(cameraInfos); var config3 = new CameraConfiguration( @@ -51,40 +51,39 @@ public void visionSourceTest() { "dev/video2", new String[] {"by-id/321"}); - UsbCameraInfo info1 = new UsbCameraInfo(0, "dev/video0", "testVideo", new String[0], 1, 2); + CameraInfo info1 = new CameraInfo(0, "dev/video0", "testVideo", new String[0], 1, 2); - infoList.add(info1); + cameraInfos.add(info1); inst.registerLoadedConfigs(config3, config4); - inst.tryMatchUSBCamImpl(false); + inst.tryMatchCamImpl(cameraInfos); - assertTrue(inst.knownUsbCameras.contains(info1)); + assertTrue(inst.knownCameras.contains(info1)); assertEquals(2, inst.unmatchedLoadedConfigs.size()); - UsbCameraInfo info2 = - new UsbCameraInfo(0, "dev/video1", "secondTestVideo", new String[0], 2, 3); + CameraInfo info2 = new CameraInfo(0, "dev/video1", "secondTestVideo", new String[0], 2, 3); - infoList.add(info2); + cameraInfos.add(info2); - var cams = inst.matchUSBCameras(infoList, inst.unmatchedLoadedConfigs); + var cams = inst.matchCameras(cameraInfos, inst.unmatchedLoadedConfigs); // assertEquals("testVideo (1)", cams.get(0).uniqueName); // Proper suffixing - inst.tryMatchUSBCamImpl(false); + inst.tryMatchCamImpl(cameraInfos); - assertTrue(inst.knownUsbCameras.contains(info2)); + assertTrue(inst.knownCameras.contains(info2)); assertEquals(2, inst.unmatchedLoadedConfigs.size()); - UsbCameraInfo info3 = - new UsbCameraInfo(0, "dev/video2", "thirdTestVideo", new String[] {"by-id/123"}, 3, 4); + CameraInfo info3 = + new CameraInfo(0, "dev/video2", "thirdTestVideo", new String[] {"by-id/123"}, 3, 4); - UsbCameraInfo info4 = - new UsbCameraInfo(0, "dev/video3", "fourthTestVideo", new String[] {"by-id/321"}, 5, 6); + CameraInfo info4 = + new CameraInfo(0, "dev/video3", "fourthTestVideo", new String[] {"by-id/321"}, 5, 6); - infoList.add(info4); + cameraInfos.add(info4); - cams = inst.matchUSBCameras(infoList, inst.unmatchedLoadedConfigs); + cams = inst.matchCameras(cameraInfos, inst.unmatchedLoadedConfigs); var cam4 = cams.stream() @@ -96,14 +95,14 @@ public void visionSourceTest() { assertEquals(cam4.nickname, config4.nickname); - infoList.add(info3); + cameraInfos.add(info3); - cams = inst.matchUSBCameras(infoList, inst.unmatchedLoadedConfigs); + cams = inst.matchCameras(cameraInfos, inst.unmatchedLoadedConfigs); - inst.tryMatchUSBCamImpl(false); + inst.tryMatchCamImpl(cameraInfos); - assertTrue(inst.knownUsbCameras.contains(info2)); - assertTrue(inst.knownUsbCameras.contains(info3)); + assertTrue(inst.knownCameras.contains(info2)); + assertTrue(inst.knownCameras.contains(info3)); var cam3 = cams.stream() @@ -121,8 +120,8 @@ public void visionSourceTest() { assertEquals(cam3.nickname, config3.nickname); assertEquals(cam4.nickname, config4.nickname); - UsbCameraInfo info5 = - new UsbCameraInfo( + CameraInfo info5 = + new CameraInfo( 2, "/dev/video2", "Left Camera", @@ -132,13 +131,13 @@ public void visionSourceTest() { }, 7, 8); - infoList.add(info5); - inst.tryMatchUSBCamImpl(false); + cameraInfos.add(info5); + inst.tryMatchCamImpl(cameraInfos); - assertTrue(inst.knownUsbCameras.contains(info5)); + assertTrue(inst.knownCameras.contains(info5)); - UsbCameraInfo info6 = - new UsbCameraInfo( + CameraInfo info6 = + new CameraInfo( 3, "dev/video3", "Right Camera", @@ -148,52 +147,118 @@ public void visionSourceTest() { }, 9, 10); - infoList.add(info6); - inst.tryMatchUSBCamImpl(false); + cameraInfos.add(info6); + inst.tryMatchCamImpl(cameraInfos); - assertTrue(inst.knownUsbCameras.contains(info6)); + assertTrue(inst.knownCameras.contains(info6)); // RPI 5 CSI Tests - UsbCameraInfo info7 = - new UsbCameraInfo( + // CSI CAMERAS SHOULD NOT BE LOADED LIKE THIS THEY SHOULD GO THROUGH LIBCAM. + CameraInfo info7 = + new CameraInfo( 4, "dev/video4", "CSICAM-DEV", // Typically rp1-cfe for unit test changed to CSICAM-DEV new String[] {"/dev/v4l/by-path/platform-1f00110000.csi-video-index0"}, 11, 12); - infoList.add(info7); - inst.tryMatchUSBCamImpl(false); + cameraInfos.add(info7); + inst.tryMatchCamImpl(cameraInfos); - assertTrue(inst.knownUsbCameras.contains(info7)); + assertTrue(!inst.knownCameras.contains(info7)); // This camera should not be recognized/used. - UsbCameraInfo info8 = - new UsbCameraInfo( + CameraInfo info8 = + new CameraInfo( 5, "dev/video8", "CSICAM-DEV", // Typically rp1-cfe for unit test changed to CSICAM-DEV new String[] {"/dev/v4l/by-path/platform-1f00110000.csi-video-index4"}, 13, 14); - infoList.add(info8); - inst.tryMatchUSBCamImpl(false); + cameraInfos.add(info8); + inst.tryMatchCamImpl(cameraInfos); - assertTrue(!inst.knownUsbCameras.contains(info8)); // This camera should not be recognized/used. + assertTrue(!inst.knownCameras.contains(info8)); // This camera should not be recognized/used. - UsbCameraInfo info9 = - new UsbCameraInfo( + CameraInfo info9 = + new CameraInfo( 6, "dev/video9", "CSICAM-DEV", // Typically rp1-cfe for unit test changed to CSICAM-DEV new String[] {"/dev/v4l/by-path/platform-1f00110000.csi-video-index5"}, 15, 16); - infoList.add(info9); - inst.tryMatchUSBCamImpl(false); + cameraInfos.add(info9); + inst.tryMatchCamImpl(cameraInfos); - assertTrue(!inst.knownUsbCameras.contains(info9)); // This camera should not be recognized/used. - assertEquals(7, inst.knownUsbCameras.size()); + assertTrue(!inst.knownCameras.contains(info9)); // This camera should not be recognized/used. + assertEquals(6, inst.knownCameras.size()); + assertEquals(0, inst.unmatchedLoadedConfigs.size()); + + // RPI LIBCAMERA CSI CAMERA TESTS + CameraInfo info10 = + 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); + cameraInfos.add(info10); + inst.tryMatchCamImpl(cameraInfos); + + assertTrue(inst.knownCameras.contains(info10)); + assertEquals(7, inst.knownCameras.size()); + assertEquals(0, inst.unmatchedLoadedConfigs.size()); + + CameraInfo info11 = + 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); + cameraInfos.add(info11); + inst.tryMatchCamImpl(cameraInfos); + + assertTrue(inst.knownCameras.contains(info11)); + assertEquals(8, inst.knownCameras.size()); + assertEquals(0, inst.unmatchedLoadedConfigs.size()); + + CameraInfo info12 = + new CameraInfo( + -1, + " /base/axi/pcie@120000/rp1/i2c@80000/ov5647@36", + "Camera Module v1", + new String[] {}, + -1, + -1, + CameraType.ZeroCopyPicam); + cameraInfos.add(info12); + inst.tryMatchCamImpl(cameraInfos); + + assertTrue(inst.knownCameras.contains(info12)); + assertEquals(9, inst.knownCameras.size()); + assertEquals(0, inst.unmatchedLoadedConfigs.size()); + + CameraInfo info13 = + new CameraInfo( + -1, + "/base/axi/pcie@120000/rp1/i2c@88000/imx708@1a", + "Camera Module v3", + new String[] {}, + -1, + -1, + CameraType.ZeroCopyPicam); + cameraInfos.add(info13); + inst.tryMatchCamImpl(cameraInfos); + + assertTrue(inst.knownCameras.contains(info13)); + assertEquals(10, inst.knownCameras.size()); assertEquals(0, inst.unmatchedLoadedConfigs.size()); } } diff --git a/photon-server/src/main/resources/nativelibraries/libphotonlibcamera.so b/photon-server/src/main/resources/nativelibraries/libphotonlibcamera.so index c2d0d9b496..fe65d58132 100644 Binary files a/photon-server/src/main/resources/nativelibraries/libphotonlibcamera.so and b/photon-server/src/main/resources/nativelibraries/libphotonlibcamera.so differ