mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-20 00:51:41 +00:00
[backend] Check file existance, use underscores in camera unique name (#33)
* Add more comments and logging, fix configManager barf, add underscores * apply spotless * Add more verbose logging * Update VisionSourceManager.java
This commit is contained in:
@@ -80,13 +80,12 @@ public class Main {
|
||||
var usbSrc = (USBCameraSource) src;
|
||||
collectedSources.put(usbSrc, usbSrc.configuration.pipelineSettings);
|
||||
logger.trace(
|
||||
() -> {
|
||||
return "Matched config for camera \""
|
||||
+ src.getFrameProvider().getName()
|
||||
+ "\" and loaded "
|
||||
+ usbSrc.configuration.pipelineSettings.size()
|
||||
+ " pipelines";
|
||||
});
|
||||
() ->
|
||||
"Matched config for camera \""
|
||||
+ src.getFrameProvider().getName()
|
||||
+ "\" and loaded "
|
||||
+ usbSrc.configuration.pipelineSettings.size()
|
||||
+ " pipelines");
|
||||
}
|
||||
|
||||
logger.info("Adding " + collectedSources.size() + " configs to VMM.");
|
||||
|
||||
@@ -33,12 +33,20 @@ import org.photonvision.vision.processes.PipelineManager;
|
||||
public class CameraConfiguration {
|
||||
private static final Logger logger = new Logger(CameraConfiguration.class, LogGroup.Camera);
|
||||
|
||||
/** Name as reported by CSCore */
|
||||
public String baseName = "";
|
||||
|
||||
/** Name used to title the subfolder of this config */
|
||||
public String uniqueName = "";
|
||||
|
||||
/** User-set nickname */
|
||||
public String nickname = "";
|
||||
public double FOV = 70;
|
||||
|
||||
/** Can be either path (ex /dev/videoX) or index (ex 1). */
|
||||
public String path = "";
|
||||
|
||||
public CameraType cameraType = CameraType.UsbCamera;
|
||||
public double FOV = 70;
|
||||
public CameraCalibrationCoefficients calibration;
|
||||
public List<Integer> cameraLeds = new ArrayList<>();
|
||||
public int currentPipelineIndex = -1;
|
||||
|
||||
@@ -22,10 +22,7 @@ import java.io.File;
|
||||
import java.io.IOException;
|
||||
import java.nio.file.Files;
|
||||
import java.nio.file.Path;
|
||||
import java.util.Arrays;
|
||||
import java.util.HashMap;
|
||||
import java.util.List;
|
||||
import java.util.Objects;
|
||||
import java.util.*;
|
||||
import java.util.stream.Collectors;
|
||||
import org.photonvision.common.logging.LogGroup;
|
||||
import org.photonvision.common.logging.Logger;
|
||||
@@ -172,7 +169,8 @@ public class ConfigManager {
|
||||
// Delete old pipe configs so that we don't get any conflicts
|
||||
try {
|
||||
var pipelineFolder = Path.of(subdir.toString(), "pipelines");
|
||||
Files.list(pipelineFolder).forEach(it -> it.toFile().delete());
|
||||
if (pipelineFolder.toFile().exists())
|
||||
Files.list(pipelineFolder).map(Path::toFile).filter(File::exists).forEach(File::delete);
|
||||
} catch (IOException e) {
|
||||
logger.error("Exception while deleting old configs!");
|
||||
e.printStackTrace();
|
||||
@@ -240,26 +238,31 @@ public class ConfigManager {
|
||||
|
||||
// Load pipelines by mapping the files within the pipelines subdir
|
||||
// to their deserialized equivalents
|
||||
var pipelineSubdirectory = Path.of(subdir.toString(), "pipelines");
|
||||
List<CVPipelineSettings> settings =
|
||||
Files.list(Path.of(subdir.toString(), "pipelines"))
|
||||
.filter(p -> p.toFile().isFile())
|
||||
.map(
|
||||
p -> {
|
||||
var relativizedFilePath =
|
||||
getRootFolder().toAbsolutePath().relativize(p).toString();
|
||||
try {
|
||||
return JacksonUtils.deserialize(p, CVPipelineSettings.class);
|
||||
} catch (JsonProcessingException e) {
|
||||
logger.error("Exception while deserializing " + relativizedFilePath);
|
||||
e.printStackTrace();
|
||||
} catch (IOException e) {
|
||||
logger.warn(
|
||||
"Could not load pipeline at " + relativizedFilePath + "! Skipping...");
|
||||
}
|
||||
return null;
|
||||
})
|
||||
.filter(Objects::nonNull)
|
||||
.collect(Collectors.toList());
|
||||
pipelineSubdirectory.toFile().exists()
|
||||
? Files.list(pipelineSubdirectory)
|
||||
.filter(p -> p.toFile().isFile())
|
||||
.map(
|
||||
p -> {
|
||||
var relativizedFilePath =
|
||||
getRootFolder().toAbsolutePath().relativize(p).toString();
|
||||
try {
|
||||
return JacksonUtils.deserialize(p, CVPipelineSettings.class);
|
||||
} catch (JsonProcessingException e) {
|
||||
logger.error("Exception while deserializing " + relativizedFilePath);
|
||||
e.printStackTrace();
|
||||
} catch (IOException e) {
|
||||
logger.warn(
|
||||
"Could not load pipeline at "
|
||||
+ relativizedFilePath
|
||||
+ "! Skipping...");
|
||||
}
|
||||
return null;
|
||||
})
|
||||
.filter(Objects::nonNull)
|
||||
.collect(Collectors.toList())
|
||||
: Collections.emptyList();
|
||||
|
||||
loadedConfig.driveModeSettings = driverMode;
|
||||
loadedConfig.addPipelineSettings(settings);
|
||||
|
||||
@@ -1,25 +0,0 @@
|
||||
/*
|
||||
* Copyright (C) 2020 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.server;
|
||||
|
||||
public class Main {
|
||||
|
||||
public static void main(String[] args) {
|
||||
Server.main(5800);
|
||||
}
|
||||
}
|
||||
@@ -37,89 +37,143 @@ public class VisionSourceManager {
|
||||
|
||||
private static final Logger logger = new Logger(VisionSourceManager.class, LogGroup.Camera);
|
||||
|
||||
public static List<VisionSource> loadAllSources(
|
||||
Collection<CameraConfiguration> camerasConfiguration) {
|
||||
/**
|
||||
* 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) {
|
||||
logger.trace(() -> "Loading all sources...");
|
||||
|
||||
List<UsbCameraInfo> usbCamInfos = Arrays.asList(UsbCamera.enumerateUsbCameras());
|
||||
logger.trace(() -> "CSCore detected " + usbCamInfos.size() + " USB cameras!");
|
||||
|
||||
for (var usbCamInfo : usbCamInfos) {
|
||||
logger.info(
|
||||
"Adding local video device - \"" + usbCamInfo.name + "\" at \"" + usbCamInfo.path + "\"");
|
||||
}
|
||||
|
||||
return LoadAllSources(camerasConfiguration, usbCamInfos);
|
||||
return LoadAllSources(loadedConfigs, usbCamInfos);
|
||||
}
|
||||
|
||||
/**
|
||||
* Load vision sources based on currently connected hardware and preexisting configs.
|
||||
*
|
||||
* @param loadedConfigs The configs loaded from disk.
|
||||
* @param detectedCamInfos The {@link UsbCameraInfo}s detected by {@link
|
||||
* UsbCamera#enumerateUsbCameras()}.
|
||||
*/
|
||||
public static List<VisionSource> LoadAllSources(
|
||||
Collection<CameraConfiguration> camerasConfiguration, List<UsbCameraInfo> usbCameraInfos) {
|
||||
var UsbCamerasConfiguration =
|
||||
camerasConfiguration.stream()
|
||||
Collection<CameraConfiguration> loadedConfigs, List<UsbCameraInfo> detectedCamInfos) {
|
||||
var loadedUsbCamConfigs =
|
||||
loadedConfigs.stream()
|
||||
.filter(configuration -> configuration.cameraType == CameraType.UsbCamera)
|
||||
.collect(Collectors.toList());
|
||||
// var HttpCamerasConfiguration = camerasConfiguration.stream().filter(configuration ->
|
||||
// configuration.cameraType == CameraType.HttpCamera);
|
||||
var matchedCameras = matchUSBCameras(usbCameraInfos, UsbCamerasConfiguration);
|
||||
return loadUSBCameraSources(matchedCameras);
|
||||
var matchedCameras = matchUSBCameras(detectedCamInfos, loadedUsbCamConfigs);
|
||||
|
||||
// turn the matched cameras into VisionSources
|
||||
return loadVisionSourcesFromCamConfigs(matchedCameras);
|
||||
}
|
||||
|
||||
private static NetworkFrameProvider loadHTTPCamera(CameraConfiguration config) {
|
||||
throw new NotImplementedException("");
|
||||
}
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
private static List<CameraConfiguration> matchUSBCameras(
|
||||
List<UsbCameraInfo> infos, List<CameraConfiguration> cameraConfigurationList) {
|
||||
ArrayList<UsbCameraInfo> loopableInfo = new ArrayList<>(infos);
|
||||
List<UsbCameraInfo> detectedCamInfos, List<CameraConfiguration> loadedUsbCamConfigs) {
|
||||
ArrayList<UsbCameraInfo> detectedCameraList = new ArrayList<>(detectedCamInfos);
|
||||
List<CameraConfiguration> cameraConfigurations = new ArrayList<>();
|
||||
|
||||
for (CameraConfiguration config : cameraConfigurationList) {
|
||||
// loop over all the configs loaded from disk
|
||||
for (CameraConfiguration config : loadedUsbCamConfigs) {
|
||||
UsbCameraInfo cameraInfo;
|
||||
if (!StringUtils.isNumeric(config.path)) {
|
||||
// matching by path
|
||||
|
||||
// Load by path vs by index -- if the path is numeric we'll match by index
|
||||
if (StringUtils.isNumeric(config.path)) {
|
||||
// match by index
|
||||
var index = Integer.parseInt(config.path);
|
||||
logger.trace(
|
||||
"Trying to find a match for loaded camera " + config.baseName + " with index " + index);
|
||||
cameraInfo =
|
||||
loopableInfo.stream()
|
||||
.filter(usbCameraInfo -> usbCameraInfo.path.equals(config.path))
|
||||
detectedCameraList.stream()
|
||||
.filter(usbCameraInfo -> usbCameraInfo.dev == index)
|
||||
.findFirst()
|
||||
.orElse(null);
|
||||
} else {
|
||||
// match by index
|
||||
// matching by path
|
||||
logger.trace(
|
||||
"Trying to find a match for loaded camera "
|
||||
+ config.baseName
|
||||
+ " with path "
|
||||
+ config.path);
|
||||
cameraInfo =
|
||||
loopableInfo.stream()
|
||||
.filter(usbCameraInfo -> usbCameraInfo.dev == Integer.parseInt(config.path))
|
||||
detectedCameraList.stream()
|
||||
.filter(usbCameraInfo -> usbCameraInfo.path.equals(config.path))
|
||||
.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) {
|
||||
loopableInfo.remove(cameraInfo);
|
||||
logger.trace("Matched the config for " + config.baseName + " to a physical camera!");
|
||||
detectedCameraList.remove(cameraInfo);
|
||||
cameraConfigurations.add(config);
|
||||
}
|
||||
}
|
||||
for (UsbCameraInfo info : loopableInfo) {
|
||||
// create new camera config for all new cameras
|
||||
String name = info.name.replaceAll("[^\\x00-\\x7F]", "");
|
||||
String uniqueName = name;
|
||||
int suffix = 0;
|
||||
|
||||
// If we have any unmatched cameras left, create a new CameraConfiguration for them here.
|
||||
logger.trace(
|
||||
"After matching loaded configs " + detectedCameraList.size() + " cameras were unmatched.");
|
||||
for (UsbCameraInfo info : detectedCameraList) {
|
||||
// create new camera config for all new cameras
|
||||
String baseName =
|
||||
info.name.replaceAll("[^\\x00-\\x7F]", ""); // Remove all non-ASCII characters
|
||||
String uniqueName = baseName.replaceAll(" ", "_"); // Replace spaces with underscores;
|
||||
|
||||
int suffix = 0;
|
||||
while (containsName(cameraConfigurations, uniqueName)) {
|
||||
suffix++;
|
||||
uniqueName = String.format("%s (%d)", uniqueName, suffix);
|
||||
}
|
||||
|
||||
logger.info("Creating a new camera config for camera " + uniqueName);
|
||||
|
||||
CameraConfiguration configuration =
|
||||
new CameraConfiguration(name, uniqueName, uniqueName, info.path);
|
||||
new CameraConfiguration(baseName, uniqueName, uniqueName, info.path);
|
||||
cameraConfigurations.add(configuration);
|
||||
}
|
||||
|
||||
logger.trace("Matched or created " + cameraConfigurations.size() + " camera configs!");
|
||||
return cameraConfigurations;
|
||||
}
|
||||
|
||||
private static List<VisionSource> loadUSBCameraSources(List<CameraConfiguration> configurations) {
|
||||
private static List<VisionSource> loadVisionSourcesFromCamConfigs(
|
||||
List<CameraConfiguration> camConfigs) {
|
||||
List<VisionSource> usbCameraSources = new ArrayList<>();
|
||||
configurations.forEach(
|
||||
configuration -> usbCameraSources.add(new USBCameraSource(configuration)));
|
||||
camConfigs.forEach(configuration -> usbCameraSources.add(new USBCameraSource(configuration)));
|
||||
return usbCameraSources;
|
||||
}
|
||||
|
||||
private static boolean containsName(final List<CameraConfiguration> list, final String name) {
|
||||
return list.stream().anyMatch(configuration -> configuration.uniqueName.equals(name));
|
||||
/**
|
||||
* Check if a given config list contains the given unique name.
|
||||
*
|
||||
* @param configList A list of camera configs.
|
||||
* @param uniqueName The unique name.
|
||||
* @return If the list of configs contains the unique name.
|
||||
*/
|
||||
private static boolean containsName(
|
||||
final List<CameraConfiguration> configList, final String uniqueName) {
|
||||
return configList.stream()
|
||||
.anyMatch(configuration -> configuration.uniqueName.equals(uniqueName));
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user