Auto reconnect to unconnected cameras (#40)

* Auto reconnect to unconnected cameras

* Refactor vision source manager to not suck

* Filter allowed video modes

* Run spotless

* Fix filtering

* Update VisionSourceManager.java

* Remove debug stuff

* Add unit tests, rebase

* run spotless

* Fix config test

* Fix settings not being saved

* run spotless

* oop

* kil debug prints if we have no usb cams to match

* whoooops

* add braudcast

* Actually save pipeline settings

* fix memes

* get right coeffs

* Run spotless

* run spotless

* rebase and add some prints

* Sorry min

* Oops reimpl matching

* Oops
This commit is contained in:
Matt
2020-08-29 16:30:33 -07:00
committed by GitHub
parent 8718be7a94
commit 7f4df2ff4d
7 changed files with 243 additions and 149 deletions

View File

@@ -34,9 +34,9 @@ import org.photonvision.common.networking.NetworkManager;
import org.photonvision.common.util.TestUtils;
import org.photonvision.server.Server;
import org.photonvision.vision.camera.FileVisionSource;
import org.photonvision.vision.camera.USBCameraSource;
import org.photonvision.vision.pipeline.CVPipelineSettings;
import org.photonvision.vision.pipeline.ReflectivePipelineSettings;
import org.photonvision.vision.processes.VisionModule;
import org.photonvision.vision.processes.VisionModuleManager;
import org.photonvision.vision.processes.VisionSource;
import org.photonvision.vision.processes.VisionSourceManager;
@@ -84,58 +84,44 @@ public class Main {
return true;
}
private static HashMap<VisionSource, List<CVPipelineSettings>> gatherSources() {
private static void addTestModeSources() {
var collectedSources = new HashMap<VisionSource, List<CVPipelineSettings>>();
if (!isTestMode) {
var camConfigs = ConfigManager.getInstance().getConfig().getCameraConfigurations();
logger.info("Loaded " + camConfigs.size() + " configs from disk.");
var sources = VisionSourceManager.loadAllSources(camConfigs.values());
for (var src : sources) {
var usbSrc = (USBCameraSource) src;
collectedSources.put(usbSrc, usbSrc.configuration.pipelineSettings);
logger.debug(
() ->
"Matched config for camera \""
+ src.getFrameProvider().getName()
+ "\" and loaded "
+ usbSrc.configuration.pipelineSettings.size()
+ " pipelines");
}
} else {
var camConf2019 =
new CameraConfiguration("WPI2019", TestUtils.getTestMode2019ImagePath().toString());
camConf2019.FOV = TestUtils.WPI2019Image.FOV;
camConf2019.calibrations.add(TestUtils.get2019LifeCamCoeffs(true));
var camConf2019 =
new CameraConfiguration("WPI2019", TestUtils.getTestMode2019ImagePath().toString());
camConf2019.FOV = TestUtils.WPI2019Image.FOV;
camConf2019.calibrations.add(TestUtils.get2019LifeCamCoeffs(true));
var pipeline2019 = new ReflectivePipelineSettings();
pipeline2019.pipelineNickname = "CargoShip";
pipeline2019.targetModel = TargetModel.get2019Target();
var pipeline2019 = new ReflectivePipelineSettings();
pipeline2019.pipelineNickname = "CargoShip";
pipeline2019.targetModel = TargetModel.get2019Target();
var psList2019 = new ArrayList<CVPipelineSettings>();
psList2019.add(pipeline2019);
var psList2019 = new ArrayList<CVPipelineSettings>();
psList2019.add(pipeline2019);
var fvs2019 = new FileVisionSource(camConf2019);
var fvs2019 = new FileVisionSource(camConf2019);
var camConf2020 =
new CameraConfiguration("WPI2020", TestUtils.getTestMode2020ImagePath().toString());
camConf2020.FOV = TestUtils.WPI2020Image.FOV;
camConf2019.calibrations.add(TestUtils.get2019LifeCamCoeffs(true));
var camConf2020 =
new CameraConfiguration("WPI2020", TestUtils.getTestMode2020ImagePath().toString());
camConf2020.FOV = TestUtils.WPI2020Image.FOV;
camConf2019.calibrations.add(TestUtils.get2019LifeCamCoeffs(true));
var pipeline2020 = new ReflectivePipelineSettings();
pipeline2020.pipelineNickname = "OuterPort";
pipeline2020.targetModel = TargetModel.get2020Target();
camConf2019.calibrations.add(TestUtils.get2019LifeCamCoeffs(true));
var pipeline2020 = new ReflectivePipelineSettings();
pipeline2020.pipelineNickname = "OuterPort";
pipeline2020.targetModel = TargetModel.get2020Target();
camConf2020.calibrations.add(TestUtils.get2020LifeCamCoeffs(true));
var psList2020 = new ArrayList<CVPipelineSettings>();
psList2020.add(pipeline2020);
var psList2020 = new ArrayList<CVPipelineSettings>();
psList2020.add(pipeline2020);
var fvs2020 = new FileVisionSource(camConf2020);
var fvs2020 = new FileVisionSource(camConf2020);
collectedSources.put(fvs2019, psList2019);
collectedSources.put(fvs2020, psList2020);
}
return collectedSources;
collectedSources.put(fvs2019, psList2019);
collectedSources.put(fvs2020, psList2020);
// logger.info("Adding " + allSources.size() + " configs to VMM.");
VisionModuleManager.getInstance().addSources(collectedSources).forEach(VisionModule::start);
ConfigManager.getInstance().addCameraConfigurations(collectedSources);
}
public static void main(String[] args) {
@@ -145,7 +131,7 @@ public class Main {
logger.error("Failed to parse command-line options!", e);
}
System.out.println("Running in " + (isRelease ? "release" : "development") + " mode!");
logger.info("Running in " + (isRelease ? "release" : "development") + " mode!");
var logLevel = (isRelease || printDebugLogs) ? LogLevel.INFO : LogLevel.DEBUG;
Logger.setLevel(LogGroup.Camera, logLevel);
Logger.setLevel(LogGroup.WebServer, logLevel);
@@ -169,22 +155,32 @@ public class Main {
}
ConfigManager.getInstance().load(); // init config manager
ConfigManager.getInstance().requestSave();
NetworkManager.getInstance().initialize(false);
NetworkTablesManager.getInstance()
.setConfig(ConfigManager.getInstance().getConfig().getNetworkConfig());
HashMap<VisionSource, List<CVPipelineSettings>> allSources = gatherSources();
// HashMap<VisionSource, List<CVPipelineSettings>> allSources = gatherSources();
logger.info("Adding " + allSources.size() + " configs to VMM.");
VisionModuleManager.getInstance().addSources(allSources);
ConfigManager.getInstance().addCameraConfigurations(allSources);
// logger.info("Adding " + allSources.size() + " configs to VMM.");
// VisionModuleManager.getInstance().addSources(allSources);
// ConfigManager.getInstance().addCameraConfigurations(allSources);
if (!isTestMode) {
VisionSourceManager.getInstance()
.registerLoadedConfigs(
ConfigManager.getInstance().getConfig().getCameraConfigurations().values());
VisionSourceManager.getInstance().registerTimedTask();
} else {
addTestModeSources();
}
// Add hardware config to hardware manager
HardwareManager.getInstance()
.setConfig(ConfigManager.getInstance().getConfig().getHardwareConfig());
VisionModuleManager.getInstance().startModules();
Server.main(DEFAULT_WEBPORT);
}
}

View File

@@ -29,7 +29,6 @@ import org.photonvision.common.dataflow.DataChangeService;
import org.photonvision.common.dataflow.events.OutgoingUIEvent;
import org.photonvision.common.dataflow.networktables.NTDataPublisher;
import org.photonvision.common.dataflow.websocket.UIDataPublisher;
import org.photonvision.common.hardware.HardwareManager;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.SerializationUtils;
@@ -131,6 +130,8 @@ public class VisionModule {
logger.info("Setting FOV of vendor camera to " + fov);
visionSource.getSettables().setFOV(fov);
}
saveAndBroadcastAll();
}
void setDriverMode(boolean isDriverMode) {
@@ -312,7 +313,7 @@ public class VisionModule {
ret.calibrations = calList;
ret.isFovConfigurable =
!(HardwareManager.getInstance().getConfig().hasPresetFOV()
!(ConfigManager.getInstance().getConfig().getHardwareConfig().hasPresetFOV()
&& cameraQuirks.hasQuirk(CameraQuirk.PiCam));
return ret;

View File

@@ -52,20 +52,16 @@ public class VisionModuleManager {
return visionModules.get(i);
}
public void addSources(HashMap<VisionSource, List<CVPipelineSettings>> visionSources) {
public List<VisionModule> addSources(
HashMap<VisionSource, List<CVPipelineSettings>> visionSources) {
var addedModules = new ArrayList<VisionModule>();
for (var entry : visionSources.entrySet()) {
var visionSource = entry.getKey();
var pipelineManager = new PipelineManager(entry.getValue());
var module = new VisionModule(pipelineManager, visionSource, visionModules.size());
visionModules.add(module);
// todo: logging
}
}
public void startModules() {
for (var visionModule : visionModules) {
visionModule.start();
// todo: logging
addedModules.add(module);
}
return addedModules;
}
}

View File

@@ -20,67 +20,150 @@ package org.photonvision.vision.processes;
import edu.wpi.cscore.UsbCamera;
import edu.wpi.cscore.UsbCameraInfo;
import java.util.*;
import java.util.stream.Collectors;
import java.util.concurrent.CopyOnWriteArrayList;
import java.util.function.Supplier;
import org.photonvision.common.configuration.CameraConfiguration;
import org.photonvision.common.configuration.ConfigManager;
import org.photonvision.common.dataflow.DataChangeService;
import org.photonvision.common.dataflow.events.OutgoingUIEvent;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.TimedTaskManager;
import org.photonvision.vision.camera.CameraType;
import org.photonvision.vision.camera.USBCameraSource;
import org.photonvision.vision.pipeline.CVPipelineSettings;
public class VisionSourceManager {
private static final Logger logger = new Logger(VisionSourceManager.class, LogGroup.Camera);
private static final List<String> deviceBlacklist = List.of("bcm2835-isp");
/**
* Load vision sources based on currently connected hardware.
*
* @param loadedConfigs The {@link CameraConfiguration}s loaded from disk.
*/
public static List<VisionSource> loadAllSources(Collection<CameraConfiguration> loadedConfigs) {
return loadAllSources(
loadedConfigs, filterAllowedDevices(Arrays.asList(UsbCamera.enumerateUsbCameras())));
final List<UsbCameraInfo> knownUsbCameras = new CopyOnWriteArrayList<>();
final List<CameraConfiguration> unmatchedLoadedConfigs = new CopyOnWriteArrayList<>();
private static class SingletonHolder {
private static final VisionSourceManager INSTANCE = new VisionSourceManager();
}
public static VisionSourceManager getInstance() {
return SingletonHolder.INSTANCE;
}
VisionSourceManager() {}
public void registerTimedTask() {
TimedTaskManager.getInstance().addTask("VisionSourceManager", this::tryMatchUSBCams, 3000);
}
public void registerLoadedConfigs(CameraConfiguration... configs) {
registerLoadedConfigs(Arrays.asList(configs));
}
/**
* Load vision sources based on given cameras and configs.
* Register new camera configs loaded from disk. This will add them to the list of configs to try
* to match, and also automatically spawn new vision processes as necessary.
*
* @param loadedConfigs The configs loaded from disk.
* @param detectedCamInfos The cameras to attempt connection to.
* @param configs The loaded camera configs.
*/
public static List<VisionSource> loadAllSources(
Collection<CameraConfiguration> loadedConfigs, List<UsbCameraInfo> detectedCamInfos) {
var loadedUsbCamConfigs =
loadedConfigs.stream()
.filter(configuration -> configuration.cameraType == CameraType.UsbCamera)
.collect(Collectors.toList());
var matchedCameras = matchUSBCameras(detectedCamInfos, loadedUsbCamConfigs);
// turn the matched cameras into VisionSources
return loadVisionSourcesFromCamConfigs(matchedCameras);
public void registerLoadedConfigs(Collection<CameraConfiguration> configs) {
unmatchedLoadedConfigs.addAll(configs);
}
private static List<UsbCameraInfo> filterAllowedDevices(List<UsbCameraInfo> allDevices) {
List<UsbCameraInfo> filteredDevices = new ArrayList<>();
for (var device : allDevices) {
var deviceInfoStr =
"\""
+ device.name
+ "\" at \""
+ device.path
+ "\" with USB ID \""
+ device.vendorId
+ ":"
+ device.productId
+ "\"";
if (deviceBlacklist.contains(device.name)) {
logger.info("Skipping blacklisted device - " + deviceInfoStr);
} else {
filteredDevices.add(device);
logger.info("Adding local video device - " + deviceInfoStr);
protected Supplier<List<UsbCameraInfo>> cameraInfoSupplier =
() -> List.of(UsbCamera.enumerateUsbCameras());
protected void tryMatchUSBCams() {
var visionSourceMap = tryMatchUSBCamImpl();
if (visionSourceMap == null) return;
logger.info("Adding " + visionSourceMap.size() + " configs to VMM.");
ConfigManager.getInstance().addCameraConfigurations(visionSourceMap);
var addedSources = VisionModuleManager.getInstance().addSources(visionSourceMap);
addedSources.forEach(VisionModule::start);
DataChangeService.getInstance()
.publishEvent(
new OutgoingUIEvent<>(
"fullsettings", ConfigManager.getInstance().getConfig().toHashMap()));
}
protected HashMap<VisionSource, List<CVPipelineSettings>> tryMatchUSBCamImpl() {
// Detect cameras using CSCore
List<UsbCameraInfo> connectedCameras =
new ArrayList<>(filterAllowedDevices(cameraInfoSupplier.get()));
// Remove all known devices
var notYetLoadedCams = new ArrayList<UsbCameraInfo>();
for (var connectedCam : connectedCameras) {
boolean cameraIsUnknown = true;
for (UsbCameraInfo knownCam : this.knownUsbCameras) {
if (usbCamEquals(knownCam, connectedCam)) {
cameraIsUnknown = false;
break;
}
}
if (cameraIsUnknown) {
notYetLoadedCams.add(connectedCam);
}
}
return filteredDevices;
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 null;
}
logger.debug("Matching " + notYetLoadedCams.size() + " new cameras!");
// Sort out just the USB cams
var usbCamConfigs = new ArrayList<>();
for (var config : unmatchedLoadedConfigs) {
if (config.cameraType == CameraType.UsbCamera) usbCamConfigs.add(config);
}
// Debug prints
for (var info : notYetLoadedCams) {
logger.info("Adding local video device - \"" + info.name + "\" at \"" + info.path + "\"");
}
if (!usbCamConfigs.isEmpty())
logger.debug("Trying to match " + usbCamConfigs.size() + " unmatched configs...");
// Match camera configs to physical cameras
var matchedCameras = matchUSBCameras(notYetLoadedCams, unmatchedLoadedConfigs);
unmatchedLoadedConfigs.removeAll(matchedCameras);
if (!unmatchedLoadedConfigs.isEmpty())
logger.warn(
() ->
"After matching, "
+ unmatchedLoadedConfigs.size()
+ " configs remained unmatched. Is your camera disconnected?");
// 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);
}
}
if (matchedCameras.isEmpty()) return null;
// Turn these camera configs into vision sources
var sources = loadVisionSourcesFromCamConfigs(matchedCameras);
// These sources can be turned into USB cameras, which can be added to the config manager
var visionSourceMap = new HashMap<VisionSource, List<CVPipelineSettings>>();
for (var src : sources) {
var usbSrc = (USBCameraSource) src;
visionSourceMap.put(usbSrc, usbSrc.configuration.pipelineSettings);
logger.debug(
() ->
"Matched config for camera \""
+ src.getFrameProvider().getName()
+ "\" and loaded "
+ usbSrc.configuration.pipelineSettings.size()
+ " pipelines");
}
return visionSourceMap;
}
/**
@@ -89,11 +172,12 @@ public class VisionSourceManager {
*
* @param detectedCamInfos Information about currently connected USB cameras.
* @param loadedUsbCamConfigs The USB {@link CameraConfiguration}s loaded from disk.
* @return the matched configurations.
*/
private static List<CameraConfiguration> matchUSBCameras(
private List<CameraConfiguration> matchUSBCameras(
List<UsbCameraInfo> detectedCamInfos, List<CameraConfiguration> loadedUsbCamConfigs) {
ArrayList<UsbCameraInfo> detectedCameraList = new ArrayList<>(detectedCamInfos);
List<CameraConfiguration> cameraConfigurations = new ArrayList<>();
var detectedCameraList = new ArrayList<>(detectedCamInfos);
ArrayList<CameraConfiguration> cameraConfigurations = new ArrayList<>();
// loop over all the configs loaded from disk
for (CameraConfiguration config : loadedUsbCamConfigs) {
@@ -160,6 +244,35 @@ public class VisionSourceManager {
return cameraConfigurations;
}
private List<VisionSource> loadVisionSourcesFromCamConfigs(List<CameraConfiguration> camConfigs) {
List<VisionSource> usbCameraSources = new ArrayList<>();
camConfigs.forEach(configuration -> usbCameraSources.add(new USBCameraSource(configuration)));
return usbCameraSources;
}
private List<UsbCameraInfo> filterAllowedDevices(List<UsbCameraInfo> allDevices) {
List<UsbCameraInfo> filteredDevices = new ArrayList<>();
for (var device : allDevices) {
if (deviceBlacklist.contains(device.name)) {
logger.trace(
"Skipping blacklisted device: \"" + device.name + "\" at \"" + device.path + "\"");
} else {
filteredDevices.add(device);
logger.trace(
"Adding local video 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
&& 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]", "");
@@ -170,13 +283,6 @@ public class VisionSourceManager {
return baseName.replaceAll(" ", "_");
}
private static List<VisionSource> loadVisionSourcesFromCamConfigs(
List<CameraConfiguration> camConfigs) {
List<VisionSource> usbCameraSources = new ArrayList<>();
camConfigs.forEach(configuration -> usbCameraSources.add(new USBCameraSource(configuration)));
return usbCameraSources;
}
/**
* Check if a given config list contains the given unique name.
*
@@ -184,7 +290,7 @@ public class VisionSourceManager {
* @param uniqueName The unique name.
* @return If the list of configs contains the unique name.
*/
private static boolean containsName(
private boolean containsName(
final List<CameraConfiguration> configList, final String uniqueName) {
return configList.stream()
.anyMatch(configuration -> configuration.uniqueName.equals(uniqueName));

View File

@@ -45,6 +45,7 @@ public class ConfigTest {
TestUtils.loadLibraries();
configMgr = new ConfigManager(Path.of("testconfigdir"));
configMgr.load();
Logger.setLevel(LogGroup.General, LogLevel.TRACE);
REFLECTIVE_PIPELINE_SETTINGS = new ReflectivePipelineSettings();

View File

@@ -114,12 +114,12 @@ public class VisionModuleManagerTest {
TestUtils.WPI2019Image.FOV)),
List.of());
VisionModuleManager.getInstance().addSources(sources);
var modules = VisionModuleManager.getInstance().addSources(sources);
var module0DataConsumer = new TestDataConsumer();
VisionModuleManager.getInstance().visionModules.get(0).addResultConsumer(module0DataConsumer);
VisionModuleManager.getInstance().startModules();
modules.forEach(VisionModule::start);
sleep(500);

View File

@@ -17,14 +17,15 @@
package org.photonvision.vision.processes;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.cscore.UsbCameraInfo;
import java.util.List;
import org.junit.jupiter.api.Assertions;
import java.util.ArrayList;
import org.junit.jupiter.api.BeforeEach;
import org.junit.jupiter.api.Test;
import org.photonvision.common.configuration.CameraConfiguration;
import org.photonvision.common.util.TestUtils;
import org.photonvision.vision.camera.USBCameraSource;
public class VisionSourceManagerTest {
@BeforeEach
@@ -32,39 +33,32 @@ public class VisionSourceManagerTest {
TestUtils.loadLibraries();
}
final List<UsbCameraInfo> usbCameraInfos =
List.of(
new UsbCameraInfo(0, "/this-is-a-real-path", "cameraByPath", new String[] {""}, 1, 1),
new UsbCameraInfo(2, "/this-is-a-fake-path1", "cameraById", new String[] {""}, 420, 1),
new UsbCameraInfo(1, "/this-is-a-real-path2", "cameraByPath", new String[] {""}, 1, 1),
new UsbCameraInfo(3, "/this-is-a-fake-path2", "cameraById", new String[] {""}, 420, 1),
new UsbCameraInfo(4, "/fake-path420", "notExisting", new String[] {""}, 420, 1),
new UsbCameraInfo(5, "/fake-path421", "notExisting", new String[] {""}, 420, 1));
final List<CameraConfiguration> camConfig =
List.of(
new CameraConfiguration("cameraByPath", "dank meme", "good name", "/this-is-a-real-path"),
new CameraConfiguration(
"cameraByPath", "dank meme2", "very original", "/this-is-a-real-path2"),
new CameraConfiguration("cameraById", "camera", "my camera", "2"),
new CameraConfiguration("cameraById", "camera2", "my camera", "3"));
final List<USBCameraSource> usbCameraSources =
List.of(
new USBCameraSource(camConfig.get(0)),
new USBCameraSource(camConfig.get(1)),
new USBCameraSource(camConfig.get(2)),
new USBCameraSource(camConfig.get(3)),
new USBCameraSource(
new CameraConfiguration("notExisting", "notExisting", "notExisting", "4")),
new USBCameraSource(
new CameraConfiguration("notExisting", "notExisting (1)", "notExisting (1)", "5")));
@Test
public void visionSourceTest() {
List<VisionSource> i = VisionSourceManager.loadAllSources(camConfig, usbCameraInfos);
for (var source : i) {
Assertions.assertEquals(source, usbCameraSources.get(i.indexOf(source)));
}
var inst = new VisionSourceManager();
var infoList = new ArrayList<UsbCameraInfo>();
inst.cameraInfoSupplier = () -> infoList;
inst.tryMatchUSBCamImpl();
var config = new CameraConfiguration("secondTestVideo", "dev/video1");
UsbCameraInfo info1 = new UsbCameraInfo(0, "dev/video0", "testVideo", new String[0], 1, 2);
infoList.add(info1);
inst.registerLoadedConfigs(config);
inst.tryMatchUSBCamImpl();
inst.tryMatchUSBCamImpl();
assertTrue(inst.knownUsbCameras.contains(info1));
assertEquals(1, inst.unmatchedLoadedConfigs.size());
UsbCameraInfo info2 =
new UsbCameraInfo(0, "dev/video1", "secondTestVideo", new String[0], 2, 1);
infoList.add(info2);
inst.tryMatchUSBCamImpl();
assertTrue(inst.knownUsbCameras.contains(info2));
assertEquals(2, inst.knownUsbCameras.size());
assertEquals(0, inst.unmatchedLoadedConfigs.size());
}
}