Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-20 00:51:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

3d, camera calibration, backend settings hookup (#80)

Browse Source 
* Implement new UI backend stuff

* Kinda partially add resolution accuracy list

* camera calibration go brrrrrrrr

* ayyyy calibration works

* Maybe fix grouping

* Reorganize camera view

* Fix settings not getting sent

* Make pretty (#4)

* Reorganize camera view

* Apply some cosmetic layout changes to the cameras page

* Fix pipeline rollback bug when starting on non-dashboard pages

Co-authored-by: Matt <matthew.morley.ca@gmail.com>

* Fix naming mismatch

* Mostly make stuff work

* rename robot-relative pose to camera-relative pose

* SolvePNP memes, fix isFovConfigurable

* Change config path to photonvision_config

* netmask go poof, fix zip download?

* Update index.js

* Fix multi cam stuff?

* Use LinearFilter instead

* Fix multicam

* aaa

* start adding restart device and restart program, fix square size bug

* Add some debug stuff

* oop

* Start fixing tests

* Fix tests

* Make target box proportinal

* run spotless

* Make crosshair h o t p i n k

* Address review comments

* Address review 2 electric booaloo

* Possibly implement vendor FOV?

* Make centroid crosshair gren

* actually use FOV

* Fix tests

* actually fix tests

Co-authored-by: Declan Freeman-Gleason <declanfreemangleason@gmail.com>
This commit is contained in:
Matt
2020-08-14 12:39:21 -07:00
committed by GitHub
parent 86ea661ed9
commit b3436765e1
86 changed files with 2106 additions and 1173 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
photon-server/src/main/java/org/photonvision/Main.java
View File

@@ -25,6 +25,7 @@ import org.apache.commons.cli.*;
import org.photonvision.common.configuration.CameraConfiguration;
import org.photonvision.common.configuration.ConfigManager;
import org.photonvision.common.dataflow.networktables.NetworkTablesManager;
import org.photonvision.common.hardware.HardwareManager;
import org.photonvision.common.hardware.Platform;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.LogLevel;
@@ -106,12 +107,11 @@ public class Main {
var camConf2019 =
new CameraConfiguration("WPI2019", TestUtils.getTestMode2019ImagePath().toString());
camConf2019.FOV = TestUtils.WPI2019Image.FOV;
camConf2019.calibration = TestUtils.get2019LifeCamCoeffs(true);
camConf2019.calibrations.add(TestUtils.get2019LifeCamCoeffs(true));
var pipeline2019 = new ReflectivePipelineSettings();
pipeline2019.pipelineNickname = "CargoShip";
pipeline2019.targetModel = TargetModel.get2019Target();
pipeline2019.cameraCalibration = camConf2019.calibration;
var psList2019 = new ArrayList<CVPipelineSettings>();
psList2019.add(pipeline2019);
@@ -121,12 +121,12 @@ public class Main {
var camConf2020 =
new CameraConfiguration("WPI2020", TestUtils.getTestMode2020ImagePath().toString());
camConf2020.FOV = TestUtils.WPI2020Image.FOV;
camConf2020.calibration = TestUtils.get2020LifeCamCoeffs(true);
camConf2019.calibrations.add(TestUtils.get2019LifeCamCoeffs(true));
var pipeline2020 = new ReflectivePipelineSettings();
pipeline2020.pipelineNickname = "OuterPort";
pipeline2020.targetModel = TargetModel.get2020Target();
pipeline2020.cameraCalibration = camConf2020.calibration;
camConf2019.calibrations.add(TestUtils.get2019LifeCamCoeffs(true));
var psList2020 = new ArrayList<CVPipelineSettings>();
psList2020.add(pipeline2020);
@@ -177,6 +177,10 @@ public class Main {
VisionModuleManager.getInstance().addSources(allSources);
ConfigManager.getInstance().addCameraConfigurations(allSources);
// Add hardware config to hardware manager
HardwareManager.getInstance()
.setConfig(ConfigManager.getInstance().getConfig().getHardwareConfig());
VisionModuleManager.getInstance().startModules();
Server.main(DEFAULT_WEBPORT);
}

24
photon-server/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java
View File

@@ -20,6 +20,7 @@ package org.photonvision.common.configuration;
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnore;
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import java.util.ArrayList;
import java.util.List;
import org.photonvision.common.logging.LogGroup;
@@ -47,9 +48,10 @@ public class CameraConfiguration {
public CameraType cameraType = CameraType.UsbCamera;
public double FOV = 70;
public CameraCalibrationCoefficients calibration;
public final List<CameraCalibrationCoefficients> calibrations;
public List<Integer> cameraLeds = new ArrayList<>();
public int currentPipelineIndex = -1;
public int currentPipelineIndex = 0;
public Rotation2d camPitch = new Rotation2d();
@JsonIgnore // this ignores the pipes as we serialize them to their own subfolder
public List<CVPipelineSettings> pipelineSettings = new ArrayList<>();
@@ -66,6 +68,7 @@ public class CameraConfiguration {
this.uniqueName = uniqueName;
this.nickname = nickname;
this.path = path;
this.calibrations = new ArrayList<>();
logger.debug(
"Creating USB camera configuration for "
@@ -85,18 +88,20 @@ public class CameraConfiguration {
@JsonProperty("FOV") double FOV,
@JsonProperty("path") String path,
@JsonProperty("cameraType") CameraType cameraType,
@JsonProperty("calibration") CameraCalibrationCoefficients calibration,
@JsonProperty("calibration") List<CameraCalibrationCoefficients> calibrations,
@JsonProperty("cameraLEDs") List<Integer> cameraLeds,
@JsonProperty("currentPipelineIndex") int currentPipelineIndex) {
@JsonProperty("currentPipelineIndex") int currentPipelineIndex,
@JsonProperty("camPitch") Rotation2d camPitch) {
this.baseName = baseName;
this.uniqueName = uniqueName;
this.nickname = nickname;
this.FOV = FOV;
this.path = path;
this.cameraType = cameraType;
this.calibration = calibration;
this.calibrations = calibrations != null ? calibrations : new ArrayList<>();
this.cameraLeds = cameraLeds;
this.currentPipelineIndex = currentPipelineIndex;
this.camPitch = camPitch;
logger.debug(
"Creating camera configuration for "
@@ -134,4 +139,13 @@ public class CameraConfiguration {
public void setPipelineSettings(List<CVPipelineSettings> settings) {
pipelineSettings = settings;
}
public void addCalibration(CameraCalibrationCoefficients calibration) {
logger.info("adding calibration " + calibration.resolution);
calibrations.stream()
.filter(it -> it.resolution.equals(calibration.resolution))
.findAny()
.ifPresent(calibrations::remove);
calibrations.add(calibration);
}
}

104
photon-server/src/main/java/org/photonvision/common/configuration/ConfigManager.java
View File

@@ -22,25 +22,30 @@ import java.io.File;
import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.Path;
import java.time.LocalDateTime;
import java.time.format.DateTimeFormatter;
import java.util.*;
import java.util.stream.Collectors;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.file.FileUtils;
import org.photonvision.common.util.file.JacksonUtils;
import org.photonvision.vision.pipeline.CVPipelineSettings;
import org.photonvision.vision.pipeline.DriverModePipelineSettings;
import org.photonvision.vision.processes.VisionSource;
import org.zeroturnaround.zip.ZipUtil;
public class ConfigManager {
private static final Logger logger = new Logger(ConfigManager.class, LogGroup.General);
private static ConfigManager INSTANCE;
private PhotonConfiguration config;
final File rootFolder;
private final File hardwareConfigFile;
private final File networkConfigFile;
private final File camerasFolder;
final File configDirectoryFile;
public static ConfigManager getInstance() {
if (INSTANCE == null) {
INSTANCE = new ConfigManager(getRootFolder());
@@ -48,34 +53,58 @@ public class ConfigManager {
return INSTANCE;
}
public static void saveUploadedSettingsZip(File uploadPath) {
logger.info(uploadPath.getAbsolutePath());
var folderPath = Path.of(System.getProperty("java.io.tmpdir"), "photonvision").toFile();
folderPath.mkdirs();
ZipUtil.unpack(uploadPath, folderPath);
FileUtils.deleteDirectory(getRootFolder());
try {
org.apache.commons.io.FileUtils.copyDirectory(folderPath, getRootFolder().toFile());
} catch (IOException e) {
e.printStackTrace();
}
System.exit(666);
}
public PhotonConfiguration getConfig() {
return config;
}
private static Path getRootFolder() {
return Path.of("photonvision");
return Path.of("photonvision_config");
}
ConfigManager(Path rootFolder) {
this.rootFolder = new File(rootFolder.toUri());
ConfigManager(Path configDirectoryFile) {
this.configDirectoryFile = new File(configDirectoryFile.toUri());
this.hardwareConfigFile =
new File(Path.of(rootFolder.toString(), "hardwareConfig.json").toUri());
new File(Path.of(configDirectoryFile.toString(), "hardwareConfig.json").toUri());
this.networkConfigFile =
new File(Path.of(rootFolder.toString(), "networkSettings.json").toUri());
this.camerasFolder = new File(Path.of(rootFolder.toString(), "cameras").toUri());
new File(Path.of(configDirectoryFile.toString(), "networkSettings.json").toUri());
this.camerasFolder = new File(Path.of(configDirectoryFile.toString(), "cameras").toUri());
load();
}
private void load() {
logger.info("Loading settings...");
if (!rootFolder.exists()) {
if (rootFolder.mkdirs()) {
if (!configDirectoryFile.exists()) {
if (configDirectoryFile.mkdirs()) {
logger.debug("Root config folder did not exist. Created!");
} else {
logger.error("Failed to create root config folder!");
}
}
if (!configDirectoryFile.canWrite()) {
logger.debug("Making root dir writeable...");
try {
var success = configDirectoryFile.setWritable(true);
if (success) logger.debug("Set root dir writeable!");
else logger.error("Could not make root dir writeable!");
} catch (SecurityException e) {
logger.error("Could not make root dir writeable!", e);
}
}
HardwareConfig hardwareConfig;
NetworkConfig networkConfig;
@@ -130,16 +159,19 @@ public class ConfigManager {
logger.info("Saving settings...");
try {
JacksonUtils.serializer(hardwareConfigFile.toPath(), config.getHardwareConfig());
JacksonUtils.serialize(hardwareConfigFile.toPath(), config.getHardwareConfig());
} catch (IOException e) {
logger.error("Could not save hardware config!", e);
}
try {
JacksonUtils.serializer(networkConfigFile.toPath(), config.getNetworkConfig());
JacksonUtils.serialize(networkConfigFile.toPath(), config.getNetworkConfig());
} catch (IOException e) {
logger.error("Could not save network config!", e);
}
// Delete old configs
FileUtils.deleteDirectory(camerasFolder.toPath());
// save all of our cameras
var cameraConfigMap = config.getCameraConfigurations();
for (var subdirName : cameraConfigMap.keySet()) {
@@ -152,25 +184,16 @@ public class ConfigManager {
}
try {
JacksonUtils.serializer(Path.of(subdir.toString(), "config.json"), camConfig);
JacksonUtils.serialize(Path.of(subdir.toString(), "config.json"), camConfig);
} catch (IOException e) {
logger.error("Could not save config.json for " + subdir);
logger.error("Could not save config.json for " + subdir, e);
}
try {
JacksonUtils.serializer(
JacksonUtils.serialize(
Path.of(subdir.toString(), "drivermode.json"), camConfig.driveModeSettings);
} catch (IOException e) {
logger.error("Could not save drivermode.json for " + subdir);
}
// Delete old pipe configs so that we don't get any conflicts
try {
var pipelineFolder = Path.of(subdir.toString(), "pipelines");
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);
logger.error("Could not save drivermode.json for " + subdir, e);
}
for (var pipe : camConfig.pipelineSettings) {
@@ -182,7 +205,7 @@ public class ConfigManager {
}
try {
JacksonUtils.serializer(pipePath, pipe);
JacksonUtils.serialize(pipePath, pipe);
} catch (IOException e) {
logger.error("Could not save " + pipe.pipelineNickname + ".json!", e);
}
@@ -207,6 +230,7 @@ public class ConfigManager {
cameraConfigPath.toAbsolutePath(), CameraConfiguration.class);
} catch (JsonProcessingException e) {
logger.error("Camera config deserialization failed!", e);
e.printStackTrace();
}
if (loadedConfig == null) { // If the file could not be deserialized
logger.warn("Could not load camera " + subdir + "'s config.json! Loading " + "default");
@@ -243,7 +267,11 @@ public class ConfigManager {
.map(
p -> {
var relativizedFilePath =
rootFolder.toPath().toAbsolutePath().relativize(p).toString();
configDirectoryFile
.toPath()
.toAbsolutePath()
.relativize(p)
.toString();
try {
return JacksonUtils.deserialize(p, CVPipelineSettings.class);
} catch (JsonProcessingException e) {
@@ -284,4 +312,28 @@ public class ConfigManager {
getConfig().addCameraConfig(uniqueName, config);
save();
}
public File getSettingsFolderAsZip() {
File out = Path.of(System.getProperty("java.io.tmpdir"), "photonvision-settings.zip").toFile();
try {
ZipUtil.pack(configDirectoryFile, out);
} catch (Exception e) {
e.printStackTrace();
}
return out;
}
public void setNetworkSettings(NetworkConfig networkConfig) {
getConfig().setNetworkConfig(networkConfig);
save();
}
public Path getLogPath() {
var dateString = DateTimeFormatter.ofPattern("yyyy-M-d_hh-mm-ss").format(LocalDateTime.now());
var logFile =
Path.of(configDirectoryFile.toString(), "logs", "photonvision-" + dateString + ".log")
.toFile();
if (!logFile.getParentFile().exists()) logFile.getParentFile().mkdirs();
return logFile.toPath();
}
}

70
photon-server/src/main/java/org/photonvision/common/configuration/HardwareConfig.java
View File

@@ -17,12 +17,8 @@
package org.photonvision.common.configuration;
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonProperty;
import java.util.ArrayList;
import java.util.Map;
@SuppressWarnings("unused")
public class HardwareConfig {
public final String deviceName;
@@ -47,6 +43,10 @@ public class HardwareConfig {
public final String gpuTempCommand;
public final String ramUtilCommand;
// Device stuff
public final String restartHardwareCommand;
public final double vendorFOV; // -1 for unmanaged
public HardwareConfig() {
deviceName = "";
deviceLogoPath = "";
@@ -66,32 +66,54 @@ public class HardwareConfig {
gpuTempCommand = "";
ramUtilCommand = "";
ledBlinkCommand = "";
restartHardwareCommand = "";
vendorFOV = -1;
}
@JsonCreator
@SuppressWarnings("unused")
public HardwareConfig(
@JsonProperty("deviceName") String deviceName,
@JsonProperty("deviceLogoPath") String deviceLogoPath,
@JsonProperty("supportURL") String supportURL,
@JsonProperty("hardware") Map<String, ?> hardware,
@JsonProperty("metrics") Map<String, ?> metrics) {
String deviceName,
String deviceLogoPath,
String supportURL,
ArrayList<Integer> ledPins,
String ledSetCommand,
boolean ledsCanDim,
ArrayList<Integer> ledPWMRange,
String ledPWMSetRange,
int ledPWMFrequency,
String ledDimCommand,
String ledBlinkCommand,
String cpuTempCommand,
String cpuMemoryCommand,
String cpuUtilCommand,
String gpuMemoryCommand,
String gpuTempCommand,
String ramUtilCommand,
String restartHardwareCommand,
double vendorFOV) {
this.deviceName = deviceName;
this.deviceLogoPath = deviceLogoPath;
this.supportURL = supportURL;
this.ledPins = (ArrayList<Integer>) hardware.get("leds");
this.ledSetCommand = (String) hardware.get("ledSetCommand");
this.ledsCanDim = (Boolean) hardware.get("ledsCanDim");
this.ledPWMRange = (ArrayList<Integer>) hardware.get("ledPWMRange");
this.ledPWMSetRange = (String) hardware.get("ledPWMSetRange");
this.ledPWMFrequency = (Integer) hardware.get("ledPWMFrequency");
this.ledDimCommand = (String) hardware.get("ledDimCommand");
this.ledBlinkCommand = (String) hardware.get("ledBlinkCommand");
this.ledPins = ledPins;
this.ledSetCommand = ledSetCommand;
this.ledsCanDim = ledsCanDim;
this.ledPWMRange = ledPWMRange;
this.ledPWMSetRange = ledPWMSetRange;
this.ledPWMFrequency = ledPWMFrequency;
this.ledDimCommand = ledDimCommand;
this.ledBlinkCommand = ledBlinkCommand;
this.cpuTempCommand = cpuTempCommand;
this.cpuMemoryCommand = cpuMemoryCommand;
this.cpuUtilCommand = cpuUtilCommand;
this.gpuMemoryCommand = gpuMemoryCommand;
this.gpuTempCommand = gpuTempCommand;
this.ramUtilCommand = ramUtilCommand;
this.restartHardwareCommand = restartHardwareCommand;
this.vendorFOV = vendorFOV;
}
this.cpuTempCommand = (String) metrics.get("cpuTemp");
this.cpuMemoryCommand = (String) metrics.get("cpuMemory");
this.cpuUtilCommand = (String) metrics.get("cpuUtil");
this.gpuMemoryCommand = (String) metrics.get("gpuMemory");
this.gpuTempCommand = (String) metrics.get("gpuUtil");
this.ramUtilCommand = (String) metrics.get("ramUtil");
public final boolean hasPresetFOV() {
return vendorFOV > 0;
}
}

42
photon-server/src/main/java/org/photonvision/common/configuration/NetworkConfig.java
View File

@@ -18,23 +18,57 @@
package org.photonvision.common.configuration;
import java.util.HashMap;
import java.util.Map;
import org.photonvision.common.networking.NetworkMode;
public class NetworkConfig {
public int teamNumber = 1;
public NetworkMode connectionType = NetworkMode.DHCP;
public String ip = "";
public String gateway = "";
public String staticIp = "";
public String netmask = "";
public String hostname = "photonvision";
// TODO implement networking
public boolean shouldManage;
public NetworkConfig() {}
public NetworkConfig(
int teamNumber,
NetworkMode connectionType,
String staticIp,
String netmask,
String hostname,
boolean shouldManage) {
this.teamNumber = teamNumber;
this.connectionType = connectionType;
this.staticIp = staticIp;
this.netmask = netmask;
this.hostname = hostname;
this.shouldManage = shouldManage;
}
public static NetworkConfig fromHashMap(Map<String, Object> map) {
// teamNumber (int), supported (bool), connectionType (int),
// staticIp (str), netmask (str), hostname (str)
var ret = new NetworkConfig();
ret.teamNumber = Integer.parseInt(map.get("teamNumber").toString());
ret.shouldManage = (Boolean) map.get("supported");
ret.connectionType = NetworkMode.values()[(Integer) map.get("connectionType")];
ret.staticIp = (String) map.get("staticIp");
ret.netmask = (String) map.get("netmask");
ret.hostname = (String) map.get("hostname");
return ret;
}
public HashMap<String, Object> toHashMap() {
HashMap<String, Object> tmp = new HashMap<>();
tmp.put("teamNumber", teamNumber);
tmp.put("supported", shouldManage);
tmp.put("connectionType", connectionType.ordinal());
tmp.put("ip", ip);
tmp.put("gateway", gateway);
tmp.put("staticIp", staticIp);
tmp.put("netmask", netmask);
tmp.put("hostname", hostname);
return tmp;
}
}

22
photon-server/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java
View File

@@ -21,6 +21,7 @@ import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.stream.Collectors;
import org.photonvision.PhotonVersion;
import org.photonvision.common.util.SerializationUtils;
import org.photonvision.vision.processes.VisionModule;
import org.photonvision.vision.processes.VisionModuleManager;
@@ -35,6 +36,10 @@ public class PhotonConfiguration {
return networkConfig;
}
public void setNetworkConfig(NetworkConfig networkConfig) {
this.networkConfig = networkConfig;
}
public HashMap<String, CameraConfiguration> getCameraConfigurations() {
return cameraConfigurations;
}
@@ -54,6 +59,7 @@ public class PhotonConfiguration {
}
private HardwareConfig hardwareConfig;
private NetworkConfig networkConfig;
private HashMap<String, CameraConfiguration> cameraConfigurations;
@@ -73,8 +79,9 @@ public class PhotonConfiguration {
public Map<String, Object> toHashMap() {
Map<String, Object> map = new HashMap<>();
var settingsSubmap = new HashMap<String, Object>();
map.put("networkSettings", networkConfig.toHashMap());
settingsSubmap.put("networkSettings", networkConfig.toHashMap());
map.put(
"cameraSettings",
VisionModuleManager.getInstance().getModules().stream()
@@ -82,6 +89,17 @@ public class PhotonConfiguration {
.map(SerializationUtils::objectToHashMap)
.collect(Collectors.toList()));
settingsSubmap.put("lighting", SerializationUtils.objectToHashMap(hardwareConfig));
var generalSubmap = new HashMap<String, Object>();
generalSubmap.put("version", PhotonVersion.versionString);
generalSubmap.put("gpuAcceleration", false); // TODO gpu accel and accel type
generalSubmap.put("gpuAccelerationType", "Unknown");
generalSubmap.put("hardwareModel", "Unknown"); // TODO hardware model and platform
generalSubmap.put("hardwarePlatform", "Unknown");
settingsSubmap.put("general", generalSubmap);
map.put("settings", settingsSubmap);
return map;
}
@@ -95,5 +113,7 @@ public class PhotonConfiguration {
public HashMap<Integer, HashMap<String, Object>> videoFormatList;
public int outputStreamPort;
public int inputStreamPort;
public List<HashMap<String, Object>> calibrations;
public boolean isFovConfigurable = true;
}
}

1
photon-server/src/main/java/org/photonvision/common/dataflow/DataChangeService.java
View File

@@ -68,6 +68,7 @@ public class DataChangeService {
}
} catch (Exception e) {
logger.error("Exception when dispatching event!", e);
e.printStackTrace();
}
}
}

10
photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
View File

@@ -52,8 +52,6 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
private final Supplier<Integer> pipelineIndexSupplier;
private final BooleanSupplier driverModeSupplier;
private String currentCameraNickname;
public NTDataPublisher(
String cameraNickname,
Supplier<Integer> pipelineIndexSupplier,
@@ -65,7 +63,6 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
this.driverModeSupplier = driverModeSupplier;
this.driverModeConsumer = driverModeConsumer;
currentCameraNickname = cameraNickname;
updateCameraNickname(cameraNickname);
updateEntries();
}
@@ -146,7 +143,6 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
removeEntries();
subTable = rootTable.getSubTable(newCameraNickname);
updateEntries();
currentCameraNickname = newCameraNickname;
}
@Override
@@ -170,9 +166,9 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
targetAreaEntry.forceSetDouble(bestTarget.getArea());
targetSkewEntry.forceSetDouble(bestTarget.getSkew());
var poseX = bestTarget.getRobotRelativePose().getTranslation().getX();
var poseY = bestTarget.getRobotRelativePose().getTranslation().getY();
var poseRot = bestTarget.getRobotRelativePose().getRotation().getDegrees();
var poseX = bestTarget.getCameraToTarget().getTranslation().getX();
var poseY = bestTarget.getCameraToTarget().getTranslation().getY();
var poseRot = bestTarget.getCameraToTarget().getRotation().getDegrees();
targetPoseEntry.forceSetDoubleArray(new double[] {poseX, poseY, poseRot});
} else {
targetPitchEntry.forceSetDouble(0);

1
photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java
View File

@@ -27,6 +27,7 @@ import org.photonvision.common.logging.Logger;
import org.photonvision.common.scripting.ScriptEventType;
import org.photonvision.common.scripting.ScriptManager;
// TODO refactor this to be a singleton
public class NetworkTablesManager {
private NetworkTablesManager() {}

10
photon-server/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java
View File

@@ -18,7 +18,6 @@
package org.photonvision.common.dataflow.websocket;
import com.fasterxml.jackson.core.JsonProcessingException;
import edu.wpi.first.wpilibj.MedianFilter;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.HashMap;
@@ -31,10 +30,7 @@ import org.photonvision.vision.pipeline.result.CVPipelineResult;
public class UIDataPublisher implements CVPipelineResultConsumer {
private static final Logger logger = new Logger(UIDataPublisher.class, LogGroup.VisionModule);
// TODO check if this is the right spot to do FPS calculation
private final MedianFilter fpsAverager = new MedianFilter(10);
private final int index;
private long lastRunTime = 0;
private long lastUIResultUpdateTime = 0;
public UIDataPublisher(int index) {
@@ -45,16 +41,12 @@ public class UIDataPublisher implements CVPipelineResultConsumer {
public void accept(CVPipelineResult result) {
var now = System.currentTimeMillis();
var fps = fpsAverager.calculate(1000.0 / (now - lastRunTime));
lastRunTime = now;
// only update the UI at 15hz
if (lastUIResultUpdateTime + 1000.0 / 15.0 > now) return;
if (lastUIResultUpdateTime + 1000.0 / 10.0 > now) return;
var uiMap = new HashMap<Integer, HashMap<String, Object>>();
var dataMap = new HashMap<String, Object>();
dataMap.put("fps", fps);
dataMap.put("latency", result.getLatencyMillis());
var targets = result.targets;

3
photon-server/src/main/java/org/photonvision/common/hardware/GPIO/GPIOBase.java
View File

@@ -26,6 +26,7 @@ import org.photonvision.common.util.ShellExec;
public abstract class GPIOBase {
private static final Logger logger = new Logger(GPIOBase.class, LogGroup.General);
private static final ShellExec runCommand = new ShellExec(true, true);
public static HashMap<String, String> commands =
new HashMap<>() {
@@ -39,8 +40,6 @@ public abstract class GPIOBase {
}
};
private static final ShellExec runCommand = new ShellExec(true, true);
public static String execute(String command) {
try {
runCommand.executeBashCommand(command);

27
photon-server/src/main/java/org/photonvision/common/hardware/HardwareManager.java
View File

@@ -17,6 +17,7 @@
package org.photonvision.common.hardware;
import java.io.IOException;
import java.util.HashMap;
import org.photonvision.common.configuration.HardwareConfig;
import org.photonvision.common.hardware.GPIO.CustomGPIO;
@@ -24,12 +25,20 @@ import org.photonvision.common.hardware.GPIO.GPIOBase;
import org.photonvision.common.hardware.GPIO.PiGPIO;
import org.photonvision.common.hardware.metrics.MetricsBase;
import org.photonvision.common.hardware.metrics.MetricsPublisher;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.ShellExec;
public class HardwareManager {
HardwareConfig hardwareConfig;
private static final HashMap<Integer, GPIOBase> LEDs = new HashMap<>();
private final HashMap<Integer, GPIOBase> LEDs = new HashMap<>();
private final ShellExec shellExec = new ShellExec(true, false);
private final Logger logger = new Logger(HardwareManager.class, LogGroup.General);
public static HardwareManager getInstance() {
if (Singleton.INSTANCE == null) {
Singleton.INSTANCE = new HardwareManager();
}
return Singleton.INSTANCE;
}
@@ -52,6 +61,7 @@ public class HardwareManager {
// Start hardware metrics thread
MetricsPublisher.getInstance().startTask();
}
/** Example: HardwareManager.getInstance().getPWM(port).dimLEDs(int dimValue); */
public GPIOBase getGPIO(int pin) {
return LEDs.get(pin);
@@ -81,7 +91,20 @@ public class HardwareManager {
LEDs.values().forEach(GPIOBase::shutdown);
}
public boolean restartDevice() {
try {
return shellExec.executeBashCommand(hardwareConfig.restartHardwareCommand) == 0;
} catch (IOException e) {
logger.error("Could not restart device!", e);
return false;
}
}
public HardwareConfig getConfig() {
return hardwareConfig;
}
private static class Singleton {
private static final HardwareManager INSTANCE = new HardwareManager();
private static HardwareManager INSTANCE;
}
}

15
photon-server/src/main/java/org/photonvision/common/hardware/Platform.java
View File

@@ -36,8 +36,9 @@ public enum Platform {
// Completely unsupported
UNSUPPORTED("Unsupported Platform");
private static final ShellExec shell = new ShellExec(true, false);
public final String value;
public final boolean isRoot = checkForRoot();
public static final boolean isRoot = checkForRoot();
Platform(String value) {
this.value = value;
@@ -54,21 +55,21 @@ public enum Platform {
return this == WINDOWS_64 || this == WINDOWS_32;
}
public boolean isLinux() {
return this == LINUX_64 || this == LINUX_RASPBIAN || this == LINUX_ARM64;
public static boolean isLinux() {
return getCurrentPlatform() == LINUX_64
|| getCurrentPlatform() == LINUX_RASPBIAN
|| getCurrentPlatform() == LINUX_ARM64;
}
public static boolean isRaspberryPi() {
return CurrentPlatform.equals(LINUX_RASPBIAN);
}
private static ShellExec shell = new ShellExec(true, false);
@SuppressWarnings("StatementWithEmptyBody")
private boolean checkForRoot() {
private static boolean checkForRoot() {
if (isLinux()) {
try {
shell.execute("id", null, true, "-u");
shell.executeBashCommand("id -u");
} catch (IOException e) {
e.printStackTrace();
}

14
photon-server/src/main/java/org/photonvision/common/hardware/metrics/CPU.java → photon-server/src/main/java/org/photonvision/common/hardware/metrics/CPUMetrics.java
View File

@@ -17,28 +17,22 @@
package org.photonvision.common.hardware.metrics;
public class CPU extends MetricsBase {
public class CPUMetrics extends MetricsBase {
private CPU() {}
public static CPU getInstance() {
return Singleton.INSTANCE;
}
public CPUMetrics() {}
public double getMemory() {
if (cpuMemoryCommand.isEmpty()) return 0;
return execute(cpuMemoryCommand);
}
// TODO: Command should return in Celsius
public double getTemp() {
if (cpuTemperatureCommand.isEmpty()) return 0;
return execute(cpuTemperatureCommand) / 1000;
}
public double getUtilization() {
return execute(cpuUtilizationCommand);
}
private static class Singleton {
public static final CPU INSTANCE = new CPU();
}
}

13
photon-server/src/main/java/org/photonvision/common/hardware/metrics/GPU.java → photon-server/src/main/java/org/photonvision/common/hardware/metrics/GPUMetrics.java
View File

@@ -17,14 +17,7 @@
package org.photonvision.common.hardware.metrics;
public class GPU extends MetricsBase {
private GPU() {}
public static GPU getInstance() {
return Singleton.INSTANCE;
}
public class GPUMetrics extends MetricsBase {
public double getMemory() {
return execute(gpuMemoryCommand);
}
@@ -32,8 +25,4 @@ public class GPU extends MetricsBase {
public double getTemp() {
return execute(gpuTemperatureCommand) / 10;
}
private static class Singleton {
public static final GPU INSTANCE = new GPU();
}
}

24
photon-server/src/main/java/org/photonvision/common/hardware/metrics/MetricsPublisher.java
View File

@@ -28,18 +28,18 @@ import org.photonvision.server.UIUpdateType;
public class MetricsPublisher {
private final HashMap<String, Double> metrics;
private static final Logger logger = new Logger(MetricsPublisher.class, LogGroup.General);
private static CPU cpu;
private static GPU gpu;
private static RAM ram;
private static CPUMetrics cpuMetrics;
private static GPUMetrics gpuMetrics;
private static RAMMetrics ramMetrics;
public static MetricsPublisher getInstance() {
return Singleton.INSTANCE;
}
private MetricsPublisher() {
cpu = CPU.getInstance();
gpu = GPU.getInstance();
ram = RAM.getInstance();
cpuMetrics = new CPUMetrics();
gpuMetrics = new GPUMetrics();
ramMetrics = new RAMMetrics();
metrics = new HashMap<>();
}
@@ -49,12 +49,12 @@ public class MetricsPublisher {
.addTask(
"Metrics",
() -> {
metrics.put("cpuTemp", cpu.getTemp());
metrics.put("cpuUtil", cpu.getUtilization());
metrics.put("cpuMem", cpu.getMemory());
metrics.put("gpuTemp", gpu.getTemp());
metrics.put("gpuMem", gpu.getMemory());
metrics.put("ramUtil", ram.getUsedRam());
metrics.put("cpuTemp", cpuMetrics.getTemp());
metrics.put("cpuUtil", cpuMetrics.getUtilization());
metrics.put("cpuMem", cpuMetrics.getMemory());
metrics.put("gpuTemp", gpuMetrics.getTemp());
metrics.put("gpuMem", gpuMetrics.getMemory());
metrics.put("ramUtil", ramMetrics.getUsedRam());
DataChangeService.getInstance()
.publishEvent(

13
photon-server/src/main/java/org/photonvision/common/hardware/metrics/RAM.java → photon-server/src/main/java/org/photonvision/common/hardware/metrics/RAMMetrics.java
View File

@@ -17,19 +17,10 @@
package org.photonvision.common.hardware.metrics;
public class RAM extends MetricsBase {
private RAM() {}
public static RAM getInstance() {
return Singleton.INSTANCE;
}
public class RAMMetrics extends MetricsBase {
// TODO: Output in MBs for consistency
public double getUsedRam() {
if (ramUsageCommand.isEmpty()) return 0;
return execute(ramUsageCommand) / 1000;
}
private static class Singleton {
public static final RAM INSTANCE = new RAM();
}
}

49
photon-server/src/main/java/org/photonvision/common/logging/Logger.java
View File

@@ -17,21 +17,18 @@
package org.photonvision.common.logging;
import java.io.IOException;
import java.io.PrintWriter;
import java.io.StringWriter;
import java.nio.ByteBuffer;
import java.nio.channels.AsynchronousFileChannel;
import java.io.*;
import java.nio.file.Path;
import java.nio.file.StandardOpenOption;
import java.text.SimpleDateFormat;
import java.util.ArrayList;
import java.util.Date;
import java.util.HashMap;
import java.util.List;
import java.util.function.Supplier;
import org.photonvision.common.configuration.ConfigManager;
import org.photonvision.common.dataflow.DataChangeService;
import org.photonvision.common.dataflow.events.OutgoingUIEvent;
import org.photonvision.common.util.TimedTaskManager;
import org.photonvision.server.SocketHandler;
import org.photonvision.server.UIUpdateType;
@@ -102,6 +99,7 @@ public class Logger {
static {
currentAppenders.add(new ConsoleLogAppender());
currentAppenders.add(new UILogAppender());
addFileAppender(ConfigManager.getInstance().getLogPath());
}
@SuppressWarnings("ResultOfMethodCallIgnored")
@@ -115,7 +113,7 @@ public class Logger {
e.printStackTrace();
}
}
currentAppenders.add(new AsyncFileLogAppender(logFilePath));
currentAppenders.add(new FileLogAppender(logFilePath));
}
public static void setLevel(LogGroup group, LogLevel newLevel) {
@@ -177,7 +175,7 @@ public class Logger {
*/
public void error(String message, Throwable t) {
log(message, LogLevel.ERROR);
log(convertStackTraceToString(t), LogLevel.ERROR, LogLevel.TRACE);
log(convertStackTraceToString(t), LogLevel.ERROR, LogLevel.DEBUG);
}
public void warn(Supplier<String> messageSupplier) {
@@ -239,25 +237,40 @@ public class Logger {
var messageMap = new SocketHandler.UIMap();
messageMap.put("logMessage", message);
messageMap.put("logLevel", level.code);
var superMap = new SocketHandler.UIMap();
superMap.put("logMessage", messageMap);
DataChangeService.getInstance()
.publishEvent(new OutgoingUIEvent<>(UIUpdateType.BROADCAST, "log", messageMap, null));
.publishEvent(new OutgoingUIEvent<>(UIUpdateType.BROADCAST, "log", superMap, null));
}
}
private static class AsyncFileLogAppender implements LogAppender {
private final Path filePath;
private static class FileLogAppender implements LogAppender {
private OutputStream out;
public AsyncFileLogAppender(Path logFilePath) {
this.filePath = logFilePath;
public FileLogAppender(Path logFilePath) {
try {
this.out = new FileOutputStream(logFilePath.toFile());
TimedTaskManager.getInstance()
.addTask(
"FileLogAppender",
() -> {
try {
out.flush();
} catch (IOException ignored) {
}
},
30000L);
} catch (FileNotFoundException e) {
out = null;
System.err.println("Unable to log to file " + logFilePath.toString());
}
}
@Override
public void log(String message, LogLevel level) {
try (AsynchronousFileChannel asyncFile =
AsynchronousFileChannel.open(
filePath, StandardOpenOption.WRITE, StandardOpenOption.CREATE)) {
asyncFile.write(ByteBuffer.wrap(message.getBytes()), 0);
message += "\n";
try {
out.write(message.getBytes());
} catch (IOException e) {
e.printStackTrace();
}

3
photon-server/src/main/java/org/photonvision/common/networking/LinuxNetworking.java
View File

@@ -84,7 +84,7 @@ public class LinuxNetworking extends SysNetworking {
}
@Override
public boolean setStatic(String ipAddress, String netmask, String gateway) {
public boolean setStatic(String ipAddress, String netmask) {
setDHCP(); // clean up old static interface
File dhcpConf = new File(PATH);
try {
@@ -93,7 +93,6 @@ public class LinuxNetworking extends SysNetworking {
InetAddress iNetMask = InetAddress.getByName(netmask);
int prefix = convertNetmaskToCIDR(iNetMask);
lines.add("static ip_address=" + ipAddress + "/" + prefix);
lines.add("static routers=" + gateway);
FileUtils.writeLines(dhcpConf, lines);
return true;
} catch (IOException e) {

16
photon-server/src/main/java/org/photonvision/common/networking/NetworkInterface.java
View File

@@ -27,25 +27,23 @@ public class NetworkInterface {
public final String name;
public final String displayName;
public final String IPAddress;
public final String Netmask;
public final String Gateway;
public final String Broadcast;
public final String ipAddress;
public final String netmask;
public final String broadcast;
public NetworkInterface(java.net.NetworkInterface inetface, InterfaceAddress ifaceAddress) {
name = inetface.getName();
displayName = inetface.getDisplayName();
var inetAddress = ifaceAddress.getAddress();
IPAddress = inetAddress.getHostAddress();
Netmask = getIPv4LocalNetMask(ifaceAddress);
ipAddress = inetAddress.getHostAddress();
netmask = getIPv4LocalNetMask(ifaceAddress);
// TODO: (low) hack to "get" gateway, this is gross and bad, pls fix
var splitIPAddr = IPAddress.split("\\.");
var splitIPAddr = ipAddress.split("\\.");
splitIPAddr[3] = "1";
Gateway = String.join(".", splitIPAddr);
splitIPAddr[3] = "255";
Broadcast = String.join(".", splitIPAddr);
broadcast = String.join(".", splitIPAddr);
}
private static String getIPv4LocalNetMask(InterfaceAddress interfaceAddress) {

33
photon-server/src/main/java/org/photonvision/common/networking/NetworkManager.java
View File

@@ -17,7 +17,17 @@
package org.photonvision.common.networking;
import java.io.IOException;
import org.photonvision.common.configuration.ConfigManager;
import org.photonvision.common.hardware.Platform;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.ShellExec;
public class NetworkManager {
private static final Logger logger = new Logger(NetworkManager.class, LogGroup.General);
private NetworkManager() {}
private static class SingletonHolder {
@@ -35,5 +45,28 @@ public class NetworkManager {
if (!isManaged) {
return;
}
var config = ConfigManager.getInstance().getConfig().getNetworkConfig();
if (Platform.isLinux()) {
if (!Platform.isRoot) {
logger.error("Cannot manage network without root!");
return;
}
if (config.connectionType == NetworkMode.DHCP) {
return; // TODO do we need to reconnect or something?
} else if (config.connectionType == NetworkMode.STATIC) {
try {
new ShellExec()
.executeBashCommand("ip addr add " + config.staticIp + "/24" + " dev eth0");
} catch (IOException e) {
e.printStackTrace();
}
}
}
}
public void reinitialize() {
initialize(ConfigManager.getInstance().getConfig().getNetworkConfig().shouldManage);
}
}

2
photon-server/src/main/java/org/photonvision/common/networking/SysNetworking.java
View File

@@ -78,7 +78,7 @@ public abstract class SysNetworking {
public abstract boolean setHostname(String hostname);
public abstract boolean setStatic(String ipAddress, String netmask, String gateway);
public abstract boolean setStatic(String ipAddress, String netmask);
public abstract List<java.net.NetworkInterface> getNetworkInterfaces() throws SocketException;
}

3
photon-server/src/main/java/org/photonvision/common/scripting/ScriptManager.java
View File

@@ -102,8 +102,7 @@ public class ScriptManager {
}
try {
JacksonUtils.serializer(
scriptConfigPath, eventsConfig.toArray(new ScriptConfig[0]), true);
JacksonUtils.serialize(scriptConfigPath, eventsConfig.toArray(new ScriptConfig[0]), true);
} catch (IOException e) {
logger.error("Failed to initialize!", e);
}

20
photon-server/src/main/java/org/photonvision/common/util/file/FileUtils.java
View File

@@ -24,6 +24,7 @@ import java.nio.file.Path;
import java.nio.file.attribute.PosixFileAttributes;
import java.nio.file.attribute.PosixFilePermission;
import java.util.Arrays;
import java.util.Comparator;
import java.util.HashSet;
import java.util.Set;
import org.photonvision.common.hardware.Platform;
@@ -38,6 +39,25 @@ public class FileUtils {
private static final Set<PosixFilePermission> allReadWriteExecutePerms =
new HashSet<>(Arrays.asList(PosixFilePermission.values()));
public static void deleteDirectory(Path path) {
try {
// create a stream
var files = Files.walk(path);
// delete directory including files and sub-folders
files
.sorted(Comparator.reverseOrder())
.map(Path::toFile)
.filter(File::isFile)
.forEach(File::delete);
// close the stream
files.close();
} catch (IOException e) {
logger.error("Exception deleting files in " + path + "!", e);
}
}
public static void setFilePerms(Path path) throws IOException {
if (!Platform.CurrentPlatform.isWindows()) {
File thisFile = path.toFile();

18
photon-server/src/main/java/org/photonvision/common/util/file/JacksonUtils.java
View File

@@ -32,11 +32,11 @@ import java.io.IOException;
import java.nio.file.Path;
public class JacksonUtils {
public static <T> void serializer(Path path, T object) throws IOException {
serializer(path, object, false);
public static <T> void serialize(Path path, T object) throws IOException {
serialize(path, object, false);
}
public static <T> void serializer(Path path, T object, boolean forceSync) throws IOException {
public static <T> void serialize(Path path, T object, boolean forceSync) throws IOException {
PolymorphicTypeValidator ptv =
BasicPolymorphicTypeValidator.builder().allowIfBaseType(object.getClass()).build();
ObjectMapper objectMapper =
@@ -93,7 +93,17 @@ public class JacksonUtils {
}
private static void saveJsonString(String json, Path path, boolean forceSync) throws IOException {
FileOutputStream fileOutputStream = new FileOutputStream(path.toFile());
var file = path.toFile();
if (file.getParentFile() != null && !file.getParentFile().exists()) {
file.getParentFile().mkdirs();
}
if (!file.exists()) {
if (!file.canWrite()) {
file.setWritable(true);
}
file.createNewFile();
}
FileOutputStream fileOutputStream = new FileOutputStream(file);
fileOutputStream.write(json.getBytes());
fileOutputStream.flush();
if (forceSync) {

134
photon-server/src/main/java/org/photonvision/server/RequestHandler.java
View File

@@ -17,46 +17,136 @@
package org.photonvision.server;
import com.fasterxml.jackson.core.JsonProcessingException;
import com.fasterxml.jackson.databind.ObjectMapper;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import io.javalin.http.Context;
import java.io.File;
import java.io.FileInputStream;
import java.io.IOException;
import java.nio.file.Path;
import java.util.HashMap;
import java.util.Map;
import org.apache.commons.io.FileUtils;
import org.photonvision.common.configuration.ConfigManager;
import org.photonvision.common.configuration.NetworkConfig;
import org.photonvision.common.dataflow.networktables.NetworkTablesManager;
import org.photonvision.common.hardware.HardwareManager;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.networking.NetworkManager;
import org.photonvision.vision.processes.VisionModuleManager;
public class RequestHandler {
private static final Logger logger = new Logger(RequestHandler.class, LogGroup.WebServer);
private static final ObjectMapper kObjectMapper = new ObjectMapper();
/** Parses and saves general settings to the config manager. */
public static void onGeneralSettings(Context context) {
return;
public static void onSettingUpload(Context ctx) {
var file = ctx.uploadedFile("zipData");
if (file != null) {
var tempZipPath =
new File(Path.of(System.getProperty("java.io.tmpdir"), file.getFilename()).toString());
tempZipPath.getParentFile().mkdirs();
try {
FileUtils.copyInputStreamToFile(file.getContent(), tempZipPath);
} catch (IOException e) {
logger.error("Exception uploading settings file!");
e.printStackTrace();
}
ConfigManager.saveUploadedSettingsZip(tempZipPath);
// restartDevice();
} else {
logger.error("Couldn't read uploaded settings ZIP! Ignoring.");
}
}
/** Parses and saves camera settings (FOV and tilt) to the current camera. */
public static void onCameraSettings(Context context) {
return;
@SuppressWarnings("unchecked")
public static void onGeneralSettings(Context context) throws JsonProcessingException {
Map<String, Object> map =
(Map<String, Object>) kObjectMapper.readValue(context.body(), Map.class);
var networking =
(Map<String, Object>)
map.get("networkSettings"); // teamNumber (int), supported (bool), connectionType (int),
// staticIp (str), netmask (str), gateway (str), hostname (str)
var lighting =
(Map<String, Object>) map.get("lighting"); // supported (true/false), brightness (int)
// TODO do stuff with lighting
var networkConfig = NetworkConfig.fromHashMap(networking);
ConfigManager.getInstance().setNetworkSettings(networkConfig);
ConfigManager.getInstance().save();
NetworkManager.getInstance().reinitialize();
NetworkTablesManager.setClientMode(null); // TODO
logger.info("Responding to general settings with http 200");
context.status(200);
}
/** Duplicates the selected camera */
public static void onDuplicatePipeline(Context context) {
return;
@SuppressWarnings("unchecked")
public static void onCameraSettingsSave(Context context) {
try {
var settingsAndIndex = kObjectMapper.readValue(context.body(), Map.class);
logger.info("Got cam setting json from frontend!\n" + settingsAndIndex.toString());
var settings = (HashMap<String, Object>) settingsAndIndex.get("settings");
int index = (Integer) settingsAndIndex.get("index");
// The only settings we actually care about are FOV and pitch
var fov = Double.parseDouble(settings.get("fov").toString());
var pitch =
Rotation2d.fromDegrees(Double.parseDouble(settings.get("tiltDegrees").toString()));
logger.info(
String.format(
"Setting camera %s's fov to %s w/pitch %s", index, fov, pitch.getDegrees()));
var module = VisionModuleManager.getInstance().getModule(index);
module.setFovAndPitch(fov, pitch);
module.saveModule();
} catch (JsonProcessingException e) {
logger.error("Got invalid camera setting JSON from frontend!");
e.printStackTrace();
}
}
public static void onCalibrationStart(Context context) {
return;
public static void onSettingsDownload(Context ctx) {
logger.info("exporting settings to download...");
try {
var zip = ConfigManager.getInstance().getSettingsFolderAsZip();
var stream = new FileInputStream(zip);
logger.info("Uploading settings with size " + stream.available());
ctx.result(stream);
ctx.contentType("application/zip");
ctx.header("Content-Disposition: attachment; filename=\"photonvision-settings-export.zip\"");
ctx.status(200);
} catch (IOException e) {
e.printStackTrace();
ctx.status(501);
logger.error("Got bad recode from zip to byte");
}
}
public static void onSnapshot(Context context) {
return;
public static void onCalibrationEnd(Context ctx) {
var index = Integer.parseInt(ctx.body());
var calData = VisionModuleManager.getInstance().getModule(index).endCalibration();
if (calData == null) {
ctx.status(500);
return;
}
ctx.result(String.valueOf(calData.standardDeviation));
ctx.status(200);
}
public static void onCalibrationEnding(Context context) {
return;
public static void restartDevice(Context ctx) {
ctx.status(HardwareManager.getInstance().restartDevice() ? 200 : 500);
}
/** Parses and saves the current 3d settings to the current pipeline. */
public static void onPnpModel(Context context) {
return;
}
public static void onInstallOrUpdate(Context context) {
return;
/**
* Note that this doesn't actually restart the program itself -- instead, it relies on systemd or
* an equivalent.
*/
public static void restartProgram(Context ctx) {
ctx.status(200);
System.exit(0);
}
}

14
photon-server/src/main/java/org/photonvision/server/Server.java
View File

@@ -68,14 +68,14 @@ public class Server {
ws.onBinaryMessage(socketHandler::onBinaryMessage);
});
/*API Events*/
app.post("/api/settings/import", RequestHandler::onSettingUpload);
app.get("/api/settings/photonvision_config.zip", RequestHandler::onSettingsDownload);
app.post("/api/settings/camera", RequestHandler::onCameraSettingsSave);
app.post("/api/settings/general", RequestHandler::onGeneralSettings);
app.post("/api/settings/camera", RequestHandler::onCameraSettings);
app.post("/api/vision/duplicate", RequestHandler::onDuplicatePipeline);
app.post("/api/settings/startCalibration", RequestHandler::onCalibrationStart);
app.post("/api/settings/snapshot", RequestHandler::onSnapshot);
app.post("/api/settings/endCalibration", RequestHandler::onCalibrationEnding);
app.post("/api/vision/pnpModel", RequestHandler::onPnpModel);
app.post("/api/install", RequestHandler::onInstallOrUpdate);
app.post("/api/settings/endCalibration", RequestHandler::onCalibrationEnd);
app.post("/api/restartDevice", RequestHandler::restartDevice);
app.post("api/restartProgram", RequestHandler::restartProgram);
app.start(port);
}
}

32
photon-server/src/main/java/org/photonvision/server/SocketHandler.java
View File

@@ -20,10 +20,15 @@ package org.photonvision.server;
import com.fasterxml.jackson.core.JsonProcessingException;
import com.fasterxml.jackson.core.type.TypeReference;
import com.fasterxml.jackson.databind.ObjectMapper;
import io.javalin.websocket.*;
import io.javalin.websocket.WsBinaryMessageContext;
import io.javalin.websocket.WsCloseContext;
import io.javalin.websocket.WsConnectContext;
import io.javalin.websocket.WsContext;
import java.io.IOException;
import java.nio.ByteBuffer;
import java.util.*;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.concurrent.CopyOnWriteArrayList;
import org.apache.commons.lang3.tuple.Pair;
import org.msgpack.jackson.dataformat.MessagePackFactory;
@@ -33,7 +38,6 @@ import org.photonvision.common.dataflow.events.IncomingWebSocketEvent;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.vision.pipeline.PipelineType;
import org.photonvision.vision.processes.PipelineManager;
@SuppressWarnings("rawtypes")
public class SocketHandler {
@@ -177,6 +181,22 @@ public class SocketHandler {
dcService.publishEvent(newPipelineEvent);
break;
}
case SMT_DUPLICATEPIPELINE:
{
var pipeIndex = (Integer) entryValue;
logger.info("Duplicating pipe@index" + pipeIndex + " for camera " + cameraIndex);
var newPipelineEvent =
new IncomingWebSocketEvent<>(
DataChangeDestination.DCD_ACTIVEMODULE,
"duplicatePipeline",
pipeIndex,
cameraIndex,
context);
dcService.publishEvent(newPipelineEvent);
break;
}
case SMT_COMMAND:
{
var cmd = SocketMessageCommandType.fromEntryKey((String) entryValue);
@@ -223,13 +243,13 @@ public class SocketHandler {
dcService.publishEvent(changePipelineEvent);
break;
}
case SMT_ISPNPCALIBRATION:
case SMT_STARTPNPCALIBRATION:
{
var changePipelineEvent =
new IncomingWebSocketEvent<>(
DataChangeDestination.DCD_ACTIVEMODULE,
"changePipeline",
PipelineManager.CAL_3D_INDEX,
"startcalibration",
(Map) entryValue,
cameraIndex,
context);
dcService.publishEvent(changePipelineEvent);

5
photon-server/src/main/java/org/photonvision/server/SocketMessageType.java
View File

@@ -31,8 +31,9 @@ public enum SocketMessageType {
SMT_CURRENTCAMERA("currentCamera"),
SMT_PIPELINESETTINGCHANGE("changePipelineSetting"),
SMT_CURRENTPIPELINE("currentPipeline"),
SMT_ISPNPCALIBRATION("isPNPCalibration"),
SMT_TAKECALIBRATIONSNAPSHOT("takeCalibrationSnapshot");
SMT_STARTPNPCALIBRATION("startPnpCalibration"),
SMT_TAKECALIBRATIONSNAPSHOT("takeCalibrationSnapshot"),
SMT_DUPLICATEPIPELINE("duplicatePipeline");
public final String entryKey;

13
photon-server/src/main/java/org/photonvision/vision/camera/FileVisionSource.java
View File

@@ -19,6 +19,7 @@ package org.photonvision.vision.camera;
import edu.wpi.cscore.VideoMode;
import edu.wpi.cscore.VideoMode.PixelFormat;
import java.nio.file.Path;
import java.util.HashMap;
import org.photonvision.common.configuration.CameraConfiguration;
import org.photonvision.vision.frame.FrameProvider;
@@ -35,7 +36,13 @@ public class FileVisionSource implements VisionSource {
public FileVisionSource(CameraConfiguration cameraConfiguration) {
this.cameraConfiguration = cameraConfiguration;
frameProvider = new FileFrameProvider(cameraConfiguration.path, cameraConfiguration.FOV);
frameProvider =
new FileFrameProvider(
Path.of(cameraConfiguration.path),
cameraConfiguration.FOV,
FileFrameProvider.MAX_FPS,
cameraConfiguration.camPitch,
cameraConfiguration.calibrations.get(0));
settables =
new FileSourceSettables(cameraConfiguration, frameProvider.get().frameStaticProperties);
}
@@ -91,7 +98,9 @@ public class FileVisionSource implements VisionSource {
}
@Override
public void setCurrentVideoMode(VideoMode videoMode) {}
protected void setVideoModeInternal(VideoMode videoMode) {
// Do nothing
}
@Override
public HashMap<Integer, VideoMode> getAllVideoModes() {

8
photon-server/src/main/java/org/photonvision/vision/camera/USBCameraSource.java
View File

@@ -27,7 +27,6 @@ import org.photonvision.common.configuration.CameraConfiguration;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.vision.frame.FrameProvider;
import org.photonvision.vision.frame.FrameStaticProperties;
import org.photonvision.vision.frame.provider.USBFrameProvider;
import org.photonvision.vision.processes.VisionSource;
import org.photonvision.vision.processes.VisionSourceSettables;
@@ -68,8 +67,8 @@ public class USBCameraSource implements VisionSource {
protected USBCameraSettables(CameraConfiguration configuration) {
super(configuration);
getAllVideoModes();
setCurrentVideoMode(videoModes.get(0));
frameStaticProperties = new FrameStaticProperties(getCurrentVideoMode(), getFOV());
setVideoMode(videoModes.get(0));
calculateFrameStaticProps();
}
@Override
@@ -110,14 +109,13 @@ public class USBCameraSource implements VisionSource {
}
@Override
public void setCurrentVideoMode(VideoMode videoMode) {
public void setVideoModeInternal(VideoMode videoMode) {
try {
if (videoMode == null) {
logger.error("Got a null video mode! Doing nothing...");
return;
}
camera.setVideoMode(videoMode);
this.frameStaticProperties = new FrameStaticProperties(getCurrentVideoMode(), getFOV());
} catch (Exception e) {
logger.error("Failed to set video mode!", e);
}

18
photon-server/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java
View File

@@ -18,9 +18,11 @@
package org.photonvision.vision.frame;
import edu.wpi.cscore.VideoMode;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import org.apache.commons.math3.fraction.Fraction;
import org.apache.commons.math3.util.FastMath;
import org.opencv.core.Point;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
/** Represents the properties of a frame. */
public class FrameStaticProperties {
@@ -33,6 +35,8 @@ public class FrameStaticProperties {
public final Point centerPoint;
public final double horizontalFocalLength;
public final double verticalFocalLength;
public final Rotation2d cameraPitch;
public CameraCalibrationCoefficients cameraCalibration;
/**
* Instantiates a new Frame static properties.
@@ -40,8 +44,9 @@ public class FrameStaticProperties {
* @param mode The Video Mode of the camera.
* @param fov The fov of the image.
*/
public FrameStaticProperties(VideoMode mode, double fov) {
this(mode != null ? mode.width : 1, mode != null ? mode.height : 1, fov);
public FrameStaticProperties(
VideoMode mode, double fov, Rotation2d cameraPitch, CameraCalibrationCoefficients cal) {
this(mode != null ? mode.width : 1, mode != null ? mode.height : 1, fov, cameraPitch, cal);
}
/**
@@ -51,10 +56,17 @@ public class FrameStaticProperties {
* @param imageHeight The width of the image.
* @param fov The fov of the image.
*/
public FrameStaticProperties(int imageWidth, int imageHeight, double fov) {
public FrameStaticProperties(
int imageWidth,
int imageHeight,
double fov,
Rotation2d cameraPitch,
CameraCalibrationCoefficients cal) {
this.imageWidth = imageWidth;
this.imageHeight = imageHeight;
this.fov = fov;
this.cameraPitch = cameraPitch;
this.cameraCalibration = cal;
imageArea = this.imageWidth * this.imageHeight;

20
photon-server/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java
View File

@@ -17,11 +17,13 @@
package org.photonvision.vision.frame.provider;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import java.nio.file.Files;
import java.nio.file.Path;
import java.nio.file.Paths;
import org.opencv.core.Mat;
import org.opencv.imgcodecs.Imgcodecs;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.frame.FrameProvider;
import org.photonvision.vision.frame.FrameStaticProperties;
@@ -32,7 +34,7 @@ import org.photonvision.vision.opencv.CVMat;
* path}.
*/
public class FileFrameProvider implements FrameProvider {
private static final int MAX_FPS = 120;
public static final int MAX_FPS = 120;
private static int count = 0;
private final int thisIndex = count++;
@@ -51,6 +53,20 @@ public class FileFrameProvider implements FrameProvider {
* @param maxFPS The max framerate to provide the image at.
*/
public FileFrameProvider(Path path, double fov, int maxFPS) {
this(path, fov, maxFPS, null, null);
}
public FileFrameProvider(
Path path, double fov, Rotation2d pitch, CameraCalibrationCoefficients calibration) {
this(path, fov, MAX_FPS, pitch, calibration);
}
public FileFrameProvider(
Path path,
double fov,
int maxFPS,
Rotation2d pitch,
CameraCalibrationCoefficients calibration) {
if (!Files.exists(path))
throw new RuntimeException("Invalid path for image: " + path.toAbsolutePath().toString());
this.path = path;
@@ -59,7 +75,7 @@ public class FileFrameProvider implements FrameProvider {
Mat rawImage = Imgcodecs.imread(path.toString());
if (rawImage.cols() > 0 && rawImage.rows() > 0) {
FrameStaticProperties m_properties =
new FrameStaticProperties(rawImage.width(), rawImage.height(), fov);
new FrameStaticProperties(rawImage.width(), rawImage.height(), fov, pitch, calibration);
Mat originalImage = new Mat();
rawImage.copyTo(originalImage);
originalFrame = new Frame(new CVMat(rawImage), m_properties);

44
photon-server/src/main/java/org/photonvision/vision/pipe/impl/CalculateFPSPipe.java Normal file
View File

@@ -0,0 +1,44 @@
/*
* 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.vision.pipe.impl;
import edu.wpi.first.wpilibj.LinearFilter;
import org.apache.commons.lang3.time.StopWatch;
import org.photonvision.vision.pipe.CVPipe;
public class CalculateFPSPipe
extends CVPipe<Void, Integer, CalculateFPSPipe.CalculateFPSPipeParams> {
private LinearFilter fpsFilter = LinearFilter.movingAverage(5);
StopWatch clock = new StopWatch();
@Override
protected Integer process(Void in) {
if (!clock.isStarted()) {
clock.reset();
clock.start();
}
clock.stop();
var fps = (int) fpsFilter.calculate(1000.0 / clock.getTime());
clock.reset();
clock.start();
return fps;
}
public static class CalculateFPSPipeParams {}
}

32
photon-server/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java
View File

@@ -21,6 +21,8 @@ import com.fasterxml.jackson.core.JsonProcessingException;
import com.fasterxml.jackson.databind.ObjectMapper;
import java.util.ArrayList;
import java.util.List;
import java.util.stream.Collectors;
import org.apache.commons.lang3.tuple.Triple;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.*;
import org.photonvision.common.logging.LogGroup;
@@ -31,7 +33,9 @@ import org.photonvision.vision.pipe.CVPipe;
public class Calibrate3dPipe
extends CVPipe<
List<List<Mat>>, CameraCalibrationCoefficients, Calibrate3dPipe.CalibratePipeParams> {
List<Triple<Size, Mat, Mat>>,
CameraCalibrationCoefficients,
Calibrate3dPipe.CalibratePipeParams> {
// Camera matrix stores the center of the image and focal length across the x and y-axis in a 3x3
// matrix
@@ -60,19 +64,28 @@ public class Calibrate3dPipe
/**
* Runs the process for the pipe.
*
* @param in Input for pipe processing.
* @param in Input for pipe processing. In the format (Input image, object points, image points)
* @return Result of processing.
*/
@Override
protected CameraCalibrationCoefficients process(List<List<Mat>> in) {
protected CameraCalibrationCoefficients process(List<Triple<Size, Mat, Mat>> in) {
in =
in.stream()
.filter(
it ->
it != null
&& it.getLeft() != null
&& it.getMiddle() != null
&& it.getRight() != null)
.collect(Collectors.toList());
try {
// FindBoardCorners pipe outputs all the image points, object points, and frames to calculate
// imageSize from, other parameters are output Mats
calibrationAccuracy =
Calib3d.calibrateCameraExtended(
in.get(1),
in.get(2),
new Size(in.get(0).get(0).width(), in.get(0).get(0).height()),
in.stream().map(Triple::getMiddle).collect(Collectors.toList()),
in.stream().map(Triple::getRight).collect(Collectors.toList()),
new Size(in.get(0).getLeft().width, in.get(0).getLeft().height),
cameraMatrix,
distortionCoefficients,
rvecs,
@@ -82,6 +95,8 @@ public class Calibrate3dPipe
perViewErrors);
} catch (Exception e) {
logger.error("Calibration failed!", e);
e.printStackTrace();
return null;
}
JsonMat cameraMatrixMat = JsonMat.fromMat(cameraMatrix);
JsonMat distortionCoefficientsMat = JsonMat.fromMat(distortionCoefficients);
@@ -95,7 +110,9 @@ public class Calibrate3dPipe
try {
// Print calibration successful
logger.info(
"CALIBRATION SUCCESS (with accuracy "
"CALIBRATION SUCCESS for res "
+ params.resolution
+ " (with accuracy "
+ calibrationAccuracy
+ ")! camMatrix: \n"
+ new ObjectMapper().writeValueAsString(cameraMatrixMat)
@@ -134,6 +151,7 @@ public class Calibrate3dPipe
private final Size resolution;
public CalibratePipeParams(Size resolution) {
// logger.info("res: " + resolution.toString());
this.resolution = resolution;
}
}

1
photon-server/src/main/java/org/photonvision/vision/pipe/impl/CornerDetectionPipe.java
View File

@@ -196,7 +196,6 @@ public class CornerDetectionPipe
rightList.sort(distanceProvider);
var bl = leftList.get(leftList.size() - 1);
var br = rightList.get(rightList.size() - 1);
System.out.printf("Found points: TL (%s) BL (%s) BR (%s) TR (%s)\n", tl, bl, br, tr);
return List.of(tl, bl, br, tr);
}

87
photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw2dTargetsPipe.java
View File

@@ -17,35 +17,40 @@
package org.photonvision.vision.pipe.impl;
import java.awt.Color;
import java.awt.*;
import java.util.ArrayList;
import java.util.List;
import org.apache.commons.lang3.tuple.Pair;
import org.apache.commons.lang3.tuple.Triple;
import org.opencv.core.*;
import org.opencv.core.Point;
import org.opencv.imgproc.Imgproc;
import org.photonvision.common.util.ColorHelper;
import org.photonvision.vision.pipe.MutatingPipe;
import org.photonvision.vision.target.TrackedTarget;
public class Draw2dTargetsPipe
extends MutatingPipe<Pair<Mat, List<TrackedTarget>>, Draw2dTargetsPipe.Draw2dContoursParams> {
extends MutatingPipe<
Triple<Mat, List<TrackedTarget>, Integer>, Draw2dTargetsPipe.Draw2dContoursParams> {
private List<MatOfPoint> m_drawnContours = new ArrayList<>();
@Override
protected Void process(Pair<Mat, List<TrackedTarget>> in) {
if (!in.getRight().isEmpty()
protected Void process(Triple<Mat, List<TrackedTarget>, Integer> in) {
if (!in.getMiddle().isEmpty()
&& (params.showCentroid
|| params.showMaximumBox
|| params.showRotatedBox
|| params.showShape)) {
var fps = in.getRight();
var imageSize = Math.sqrt(in.getLeft().rows() * in.getLeft().cols());
var centroidColour = ColorHelper.colorToScalar(params.centroidColor);
var maximumBoxColour = ColorHelper.colorToScalar(params.maximumBoxColor);
var rotatedBoxColour = ColorHelper.colorToScalar(params.rotatedBoxColor);
var shapeColour = ColorHelper.colorToScalar(params.shapeOutlineColour);
for (int i = 0; i < (params.showMultiple ? in.getRight().size() : 1); i++) {
for (int i = 0; i < (params.showMultiple ? in.getMiddle().size() : 1); i++) {
Point[] vertices = new Point[4];
MatOfPoint contour = new MatOfPoint();
@@ -53,7 +58,7 @@ public class Draw2dTargetsPipe
break;
}
TrackedTarget target = in.getRight().get(i);
TrackedTarget target = in.getMiddle().get(i);
RotatedRect r = target.getMinAreaRect();
if (r == null) continue;
@@ -68,7 +73,11 @@ public class Draw2dTargetsPipe
if (params.showRotatedBox) {
Imgproc.drawContours(
in.getLeft(), m_drawnContours, 0, rotatedBoxColour, params.boxOutlineSize);
in.getLeft(),
m_drawnContours,
0,
rotatedBoxColour,
(int) Math.ceil(imageSize * params.kPixelsToBoxThickness));
}
if (params.showMaximumBox) {
@@ -78,7 +87,7 @@ public class Draw2dTargetsPipe
new Point(box.x, box.y),
new Point(box.x + box.width, box.y + box.height),
maximumBoxColour,
params.boxOutlineSize);
(int) Math.ceil(imageSize * params.kPixelsToBoxThickness));
}
if (params.showShape) {
@@ -87,21 +96,17 @@ public class Draw2dTargetsPipe
List.of(target.m_mainContour.mat),
-1,
shapeColour,
params.boxOutlineSize);
}
if (params.showCentroid) {
Imgproc.circle(in.getLeft(), target.getTargetOffsetPoint(), 3, centroidColour, 2);
(int) Math.ceil(imageSize * params.kPixelsToBoxThickness));
}
if (params.showContourNumber) {
var textSize = params.kPixelsToText * in.getLeft().rows();
var thickness = params.kPixelsToThickness * in.getLeft().rows();
var textSize = params.kPixelsToText * imageSize;
var thickness = params.kPixelsToThickness * imageSize;
var center = target.m_mainContour.getCenterPoint();
var textPos =
new Point(
center.x + params.kPixelsToOffset * in.getLeft().rows(),
center.y - params.kPixelsToOffset * in.getLeft().rows());
center.x + params.kPixelsToOffset * imageSize,
center.y - params.kPixelsToOffset * imageSize);
Imgproc.putText(
in.getLeft(),
@@ -112,6 +117,43 @@ public class Draw2dTargetsPipe
ColorHelper.colorToScalar(params.textColor),
(int) thickness);
}
if (params.showCentroid) {
Point centroid = target.getTargetOffsetPoint();
var crosshairRadius = (int) (imageSize * params.kPixelsToCentroidRadius);
var x = centroid.x;
var y = centroid.y;
Point xMax = new Point(x + crosshairRadius, y);
Point xMin = new Point(x - crosshairRadius, y);
Point yMax = new Point(x, y + crosshairRadius);
Point yMin = new Point(x, y - crosshairRadius);
Imgproc.line(
in.getLeft(),
xMax,
xMin,
centroidColour,
(int) Math.ceil(imageSize * params.kPixelsToBoxThickness));
Imgproc.line(
in.getLeft(),
yMax,
yMin,
centroidColour,
(int) Math.ceil(imageSize * params.kPixelsToBoxThickness));
}
// Draw FPS
var textSize = params.kPixelsToText * imageSize;
var thickness = params.kPixelsToThickness * imageSize;
Imgproc.putText(
in.getLeft(),
fps.toString(),
new Point(10, 10 + textSize * 25),
0,
textSize,
ColorHelper.colorToScalar(params.textColor),
(int) thickness);
}
}
@@ -119,17 +161,20 @@ public class Draw2dTargetsPipe
}
public static class Draw2dContoursParams {
public final double kPixelsToText = 0.003;
public final double kPixelsToText = 0.0025;
public final double kPixelsToThickness = 0.008;
public final double kPixelsToOffset = 0.02;
public final double kPixelsToBoxThickness = 0.007;
public final double kPixelsToCentroidRadius = 0.03;
public boolean showCentroid = true;
public boolean showMultiple;
public int boxOutlineSize = 1;
public boolean showRotatedBox = true;
public boolean showShape = false;
public boolean showMaximumBox = true;
public boolean showContourNumber = true;
public Color centroidColor = Color.GREEN;
public Color centroidColor = Color.green; // Color.decode("#ff5ebf");
public Color rotatedBoxColor = Color.BLUE;
public Color maximumBoxColor = Color.RED;
public Color shapeOutlineColour = Color.MAGENTA;

87
photon-server/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java
View File

@@ -17,19 +17,16 @@
package org.photonvision.vision.pipe.impl;
import java.util.ArrayList;
import java.util.List;
import org.apache.commons.lang3.tuple.Pair;
import org.apache.commons.lang3.tuple.Triple;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.*;
import org.opencv.imgproc.Imgproc;
import org.photonvision.vision.pipe.CVPipe;
import org.photonvision.vision.pipeline.UICalibrationData;
public class FindBoardCornersPipe
extends CVPipe<List<Mat>, List<List<Mat>>, FindBoardCornersPipe.FindCornersPipeParams> {
extends CVPipe<Mat, Triple<Size, Mat, Mat>, FindBoardCornersPipe.FindCornersPipeParams> {
MatOfPoint3f objectPoints = new MatOfPoint3f();
private final List<Mat> listOfObjectPoints = new ArrayList<>();
private final List<Mat> listOfImagePoints = new ArrayList<>();
Size imageSize;
Size patternSize;
@@ -43,8 +40,17 @@ public class FindBoardCornersPipe
private boolean objectPointsCreated = false;
@Override
public void setParams(FindCornersPipeParams params) {
super.setParams(params);
if (new Size(params.boardWidth, params.boardHeight).equals(patternSize)) return;
objectPointsCreated = false;
}
public void createObjectPoints() {
if (objectPointsCreated) return;
if (objectPointsCreated) return; // TODO reinstantiate on settings change
/*If using a chessboard, then the pattern size if the inner corners of the board. For example, the pattern size of a 9x9 chessboard would be 8x8
If using a dot board, then the pattern size width is the sum of the bottom 2 rows and the height is the left or right most column
@@ -54,14 +60,17 @@ public class FindBoardCornersPipe
// Chessboard and dot board have different 3D points to project as a dot board has alternating
// dots per column
if (params.isUsingChessboard) {
if (params.type == UICalibrationData.BoardType.CHESSBOARD) {
// Here we can create an NxN grid since a chessboard is rectangular
for (int i = 0; i < patternSize.height * patternSize.width; i++) {
objectPoints.push_back(
new MatOfPoint3f(
new Point3((double) i / patternSize.width, i % patternSize.width, 0.0f)));
new Point3(
(double) i / patternSize.width * params.gridSize,
i % patternSize.width * params.gridSize,
0.0f)));
}
} else {
} else if (params.type == UICalibrationData.BoardType.DOTBOARD) {
// Here we need to alternate the amount of dots per column since a dot board is not
// rectangular and also by taking in account the grid size which should be in mm
for (int i = 0; i < patternSize.height; i++) {
@@ -71,47 +80,38 @@ public class FindBoardCornersPipe
new Point3((2 * j + i % 2) * params.gridSize, i * params.gridSize, 0.0d)));
}
}
} else {
// TOOD log
}
objectPointsCreated = true;
}
/**
* Runs the process for the pipe.
* Finds the corners in a given image and returns them
*
* @param in Input for pipe processing.
* @return All valid Mats for camera calibration
*/
@Override
protected List<List<Mat>> process(List<Mat> in) {
// If we have less than 20 snapshots we need to return null
if (in.size() < 20) return null;
// Contains all valid Mats where a chessboard or dot board have been found
List<Mat> outputMats = new ArrayList<>();
protected Triple<Size, Mat, Mat> process(Mat in) {
// Create the object points
createObjectPoints();
for (Mat board : in) {
if (findBoardCorners(board).getLeft()) {
outputMats.add(board);
}
}
// Contains the list of valid Mats, object points and images points where objectPoints.size() =
// imagePoints.size()
return List.of(outputMats, listOfObjectPoints, listOfImagePoints);
return findBoardCorners(in);
}
public Pair<Boolean, Mat> findBoardCorners(Mat frame) {
private Triple<Size, Mat, Mat> findBoardCorners(Mat frame) {
createObjectPoints();
// Convert the frame to grayscale to increase contrast
Imgproc.cvtColor(frame, frame, Imgproc.COLOR_BGR2GRAY);
boolean boardFound;
boolean boardFound = false;
if (params.isUsingChessboard) {
if (params.type == UICalibrationData.BoardType.CHESSBOARD) {
// This is for chessboards
boardFound = Calib3d.findChessboardCorners(frame, patternSize, boardCorners);
} else {
} else if (params.type == UICalibrationData.BoardType.DOTBOARD) {
// For dot boards
boardFound =
Calib3d.findCirclesGrid(
@@ -122,41 +122,44 @@ public class FindBoardCornersPipe
// If we can't find a chessboard/dot board, convert the frame back to BGR and return false.
Imgproc.cvtColor(frame, frame, Imgproc.COLOR_GRAY2BGR);
return Pair.of(false, null);
return null;
}
var outBoardCorners = new MatOfPoint2f();
boardCorners.copyTo(outBoardCorners);
// Get the size of the frame
this.imageSize = new Size(frame.width(), frame.height());
// Add the 3D points and the points of the corners found
this.listOfObjectPoints.add(objectPoints);
this.listOfImagePoints.add(boardCorners);
// Do sub corner pix for drawing chessboard
Imgproc.cornerSubPix(frame, boardCorners, windowSize, zeroZone, criteria);
Imgproc.cornerSubPix(frame, outBoardCorners, windowSize, zeroZone, criteria);
// convert back to BGR
Imgproc.cvtColor(frame, frame, Imgproc.COLOR_GRAY2BGR);
// draw the chessboard, doesn't have to be different for a dot board since it just re projects
// the corners we found
Mat chessboardDrawn = new Mat();
frame.copyTo(chessboardDrawn);
Calib3d.drawChessboardCorners(chessboardDrawn, patternSize, boardCorners, true);
boardCorners = new MatOfPoint2f();
return Pair.of(true, chessboardDrawn);
Calib3d.drawChessboardCorners(frame, patternSize, outBoardCorners, true);
// // Add the 3D points and the points of the corners found
// if (addToSnapList) {
// this.listOfObjectPoints.add(objectPoints);
// this.listOfImagePoints.add(boardCorners);
// }
return Triple.of(frame.size(), objectPoints, outBoardCorners);
}
public static class FindCornersPipeParams {
private final int boardHeight;
private final int boardWidth;
private final boolean isUsingChessboard;
private final UICalibrationData.BoardType type;
private final double gridSize;
public FindCornersPipeParams(
int boardHeight, int boardWidth, boolean isUsingChessboard, double gridSize) {
int boardHeight, int boardWidth, UICalibrationData.BoardType type, double gridSize) {
this.boardHeight = boardHeight;
this.boardWidth = boardWidth;
this.isUsingChessboard = isUsingChessboard;
this.type = type;
this.gridSize = gridSize; // mm
}
}

11
photon-server/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java
View File

@@ -17,8 +17,8 @@
package org.photonvision.vision.pipe.impl;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Transform2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import java.util.List;
import org.apache.commons.math3.util.FastMath;
@@ -50,7 +50,7 @@ public class SolvePNPPipe
}
private void calculateTargetPose(TrackedTarget target) {
Pose2d targetPose;
Transform2d targetPose;
var corners = target.getTargetCorners();
if (corners == null
@@ -81,7 +81,7 @@ public class SolvePNPPipe
targetPose = correctLocationForCameraPitch(tVec, rVec, params.cameraPitchAngle);
target.setRobotRelativePose(targetPose);
target.setCameraToTarget(targetPose);
}
Mat rotationMatrix = new Mat();
@@ -91,7 +91,8 @@ public class SolvePNPPipe
Mat scaledTvec;
@SuppressWarnings("DuplicatedCode") // yes I know we have another solvePNP pipe
private Pose2d correctLocationForCameraPitch(Mat tVec, Mat rVec, Rotation2d cameraPitchAngle) {
private Transform2d correctLocationForCameraPitch(
Mat tVec, Mat rVec, Rotation2d cameraPitchAngle) {
// Algorithm from team 5190 Green Hope Falcons. Can also be found in Ligerbot's vision
// whitepaper
var tiltAngle = cameraPitchAngle.getRadians();
@@ -124,7 +125,7 @@ public class SolvePNPPipe
// so Z_field becomes X, and X becomes Y
var targetLocation = new Translation2d(zField, -x);
return new Pose2d(targetLocation, new Rotation2d(targetRotation));
return new Transform2d(targetLocation, new Rotation2d(targetRotation));
}
/**

12
photon-server/src/main/java/org/photonvision/vision/pipeline/CVPipelineSettings.java
View File

@@ -33,7 +33,7 @@ import org.photonvision.vision.opencv.ImageRotationMode;
@JsonSubTypes.Type(value = ReflectivePipelineSettings.class),
@JsonSubTypes.Type(value = DriverModePipelineSettings.class)
})
public class CVPipelineSettings {
public class CVPipelineSettings implements Cloneable {
public int pipelineIndex = 0;
public PipelineType pipelineType = PipelineType.DriverMode;
public ImageFlipMode inputImageFlipMode = ImageFlipMode.NONE;
@@ -79,4 +79,14 @@ public class CVPipelineSettings {
streamingFrameDivisor,
ledMode);
}
@Override
public CVPipelineSettings clone() {
try {
return (CVPipelineSettings) super.clone();
} catch (CloneNotSupportedException e) {
e.printStackTrace();
return null;
}
}
}

127
photon-server/src/main/java/org/photonvision/vision/pipeline/Calibration3dPipeline.java → photon-server/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java
View File

@@ -17,12 +17,19 @@
package org.photonvision.vision.pipeline;
import com.fasterxml.jackson.core.JsonProcessingException;
import edu.wpi.first.wpilibj.util.Units;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
import org.apache.commons.lang3.tuple.Triple;
import org.opencv.core.Mat;
import org.opencv.core.Size;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.SerializationUtils;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.server.SocketHandler;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.frame.FrameStaticProperties;
@@ -32,33 +39,35 @@ import org.photonvision.vision.pipe.impl.Calibrate3dPipe;
import org.photonvision.vision.pipe.impl.FindBoardCornersPipe;
import org.photonvision.vision.pipeline.result.CVPipelineResult;
public class Calibration3dPipeline
public class Calibrate3dPipeline
extends CVPipeline<CVPipelineResult, Calibration3dPipelineSettings> {
// For loggging
private static final Logger logger = new Logger(Calibration3dPipeline.class, LogGroup.General);
private static final Logger logger = new Logger(Calibrate3dPipeline.class, LogGroup.General);
// Only 2 pipes needed, one for finding the board corners and one for actually calibrating
private final FindBoardCornersPipe findBoardCornersPipe = new FindBoardCornersPipe();
private final Calibrate3dPipe calibrate3dPipe = new Calibrate3dPipe();
// Getter methods have been set for calibrate and takeSnapshot
private int numSnapshots = 0;
private boolean calibrate = false;
private boolean takeSnapshot = false;
// BoardSnapshots is a list of all valid snapshots taken
private ArrayList<Mat> boardSnapshots;
// Output of the corners
private CVPipeResult<List<List<Mat>>> findCornersPipeOutput;
final List<Triple<Size, Mat, Mat>> foundCornersList;
/// Output of the calibration, getter method is set for this.
private CVPipeResult<CameraCalibrationCoefficients> calibrationOutput;
public Calibration3dPipeline() {
private int minSnapshots;
public Calibrate3dPipeline() {
this(25);
}
public Calibrate3dPipeline(int minSnapshots) {
this.settings = new Calibration3dPipelineSettings();
this.boardSnapshots = new ArrayList<>();
this.foundCornersList = new ArrayList<>();
this.minSnapshots = minSnapshots;
}
@Override
@@ -66,14 +75,12 @@ public class Calibration3dPipeline
FrameStaticProperties frameStaticProperties, Calibration3dPipelineSettings settings) {
FindBoardCornersPipe.FindCornersPipeParams findCornersPipeParams =
new FindBoardCornersPipe.FindCornersPipeParams(
settings.boardHeight,
settings.boardWidth,
settings.isUsingChessboard,
settings.gridSize);
settings.boardHeight, settings.boardWidth, settings.boardType, settings.gridSize);
findBoardCornersPipe.setParams(findCornersPipeParams);
Calibrate3dPipe.CalibratePipeParams calibratePipeParams =
new Calibrate3dPipe.CalibratePipeParams(settings.resolution);
new Calibrate3dPipe.CalibratePipeParams(
new Size(frameStaticProperties.imageWidth, frameStaticProperties.imageHeight));
calibrate3dPipe.setParams(calibratePipeParams);
}
@@ -85,33 +92,20 @@ public class Calibration3dPipeline
long sumPipeNanosElapsed = 0L;
// Check if the frame has chessboard corners
var hasBoard = findBoardCornersPipe.findBoardCorners(frame.image.getMat());
var findBoardResult = findBoardCornersPipe.run(frame.image.getMat()).output;
// hasEnough() is a getter method for numSnapshots that checks if there are more than 25
// snapshots
// calibrate will be true when it is get by it's putter method
if (hasEnough() && calibrate) {
if (takeSnapshot) {
// Set snapshot to false even if we don't find a board
takeSnapshot = false;
/*Pass the board corners to the pipe, which will check again to see if all boards are valid
and returns the corresponding image and object points*/
findCornersPipeOutput = findBoardCornersPipe.run(boardSnapshots);
// Increment the time it took to process all board pics to total elapsed time
sumPipeNanosElapsed += findCornersPipeOutput.nanosElapsed;
if (findBoardResult != null) {
foundCornersList.add(findBoardResult);
calibrationOutput = calibrate3dPipe.run(findCornersPipeOutput.output);
sumPipeNanosElapsed += calibrationOutput.nanosElapsed;
// update the UI
broadcastState();
calibrate = false;
} else if (takeSnapshot) {
if (hasBoard.getLeft()) {
Mat board = new Mat();
frame.image.getMat().copyTo(board);
// Add board to snapshots
boardSnapshots.add(board);
// Set snapshot to false and increment number of snapshots taken
takeSnapshot = false;
numSnapshots++;
return new CVPipelineResult(
MathUtils.nanosToMillis(sumPipeNanosElapsed), Collections.emptyList(), frame);
}
}
@@ -119,17 +113,29 @@ public class Calibration3dPipeline
return new CVPipelineResult(
MathUtils.nanosToMillis(sumPipeNanosElapsed),
null,
new Frame(
new CVMat(hasBoard.getLeft() ? hasBoard.getRight() : frame.image.getMat()),
frame.frameStaticProperties));
new Frame(new CVMat(frame.image.getMat()), frame.frameStaticProperties));
}
public boolean hasEnough() {
return numSnapshots >= 25;
return foundCornersList.size() >= minSnapshots;
}
public void startCalibration() {
calibrate = true;
public CameraCalibrationCoefficients tryCalibration() {
if (!hasEnough()) {
logger.info(
"Not enough snapshots! Only got "
+ foundCornersList.size()
+ " of "
+ minSnapshots
+ " -- returning null..");
return null;
}
/*Pass the board corners to the pipe, which will check again to see if all boards are valid
and returns the corresponding image and object points*/
calibrationOutput = calibrate3dPipe.run(foundCornersList);
return calibrationOutput.output;
}
public void takeSnapshot() {
@@ -141,13 +147,40 @@ public class Calibration3dPipeline
}
public void finishCalibration() {
numSnapshots = 0;
boardSnapshots.clear();
foundCornersList.forEach(
it -> {
it.getMiddle().release();
it.getRight().release();
});
foundCornersList.clear();
broadcastState();
}
private void broadcastState() {
var state =
SerializationUtils.objectToHashMap(
new UICalibrationData(
foundCornersList.size(),
settings.cameraVideoModeIndex,
minSnapshots,
hasEnough(),
Units.metersToInches(settings.gridSize),
settings.boardWidth,
settings.boardHeight,
settings.boardType));
var map = new SocketHandler.UIMap();
map.put("calibrationData", state);
try {
SocketHandler.getInstance().broadcastMessage(map, null);
} catch (JsonProcessingException e) {
logger.error("Unable to send cal data!", e);
}
}
public boolean removeSnapshot(int index) {
try {
boardSnapshots.remove(index);
foundCornersList.remove(index);
return true;
} catch (ArrayIndexOutOfBoundsException e) {
logger.error("Could not remove snapshot at index " + index, e);

9
photon-server/src/main/java/org/photonvision/vision/pipeline/Calibration3dPipelineSettings.java
View File

@@ -17,13 +17,14 @@
package org.photonvision.vision.pipeline;
import edu.wpi.first.wpilibj.util.Units;
import org.opencv.core.Size;
public class Calibration3dPipelineSettings extends AdvancedPipelineSettings {
public int boardHeight = 0;
public int boardWidth = 0;
public boolean isUsingChessboard = true;
public double gridSize = 0;
public int boardHeight = 7;
public int boardWidth = 7;
public UICalibrationData.BoardType boardType = UICalibrationData.BoardType.CHESSBOARD;
public double gridSize = Units.inchesToMeters(1.0);
public Size resolution = new Size(640, 480);
}

7
photon-server/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java
View File

@@ -21,6 +21,7 @@ import java.util.Arrays;
import java.util.List;
import java.util.stream.Collectors;
import org.apache.commons.lang3.tuple.Pair;
import org.apache.commons.lang3.tuple.Triple;
import org.opencv.core.Mat;
import org.opencv.core.Point;
import org.photonvision.common.util.math.MathUtils;
@@ -159,7 +160,6 @@ public class ColoredShapePipeline
draw2dContoursParams.showShape = true;
draw2dContoursParams.showMaximumBox = false;
draw2dContoursParams.showRotatedBox = false;
draw2dContoursParams.boxOutlineSize = 2;
draw2DTargetsPipe.setParams(draw2dContoursParams);
Draw2dCrosshairPipe.Draw2dCrosshairParams draw2dCrosshairParams =
@@ -258,11 +258,12 @@ public class ColoredShapePipeline
// Draw 2D contours on input and output
var draw2dContoursResultOnInput =
draw2DTargetsPipe.run(Pair.of(rawInputMat, collect2dTargetsResult.output));
draw2DTargetsPipe.run(Triple.of(rawInputMat, collect2dTargetsResult.output, -12345));
sumPipeNanosElapsed += draw2dContoursResultOnInput.nanosElapsed;
var draw2dContoursResultOnOutput =
draw2DTargetsPipe.run(Pair.of(hsvPipeResult.output, collect2dTargetsResult.output));
draw2DTargetsPipe.run(
Triple.of(hsvPipeResult.output, collect2dTargetsResult.output, -12345));
sumPipeNanosElapsed += draw2dContoursResultOnOutput.nanosElapsed;
if (settings.solvePNPEnabled && settings.desiredShape == ContourShape.Circle) {

2
photon-server/src/main/java/org/photonvision/vision/pipeline/PipelineType.java
View File

@@ -19,7 +19,7 @@ package org.photonvision.vision.pipeline;
@SuppressWarnings("rawtypes")
public enum PipelineType {
Calib3d(-2, Calibration3dPipeline.class),
Calib3d(-2, Calibrate3dPipeline.class),
DriverMode(-1, DriverModePipeline.class),
Reflective(0, ReflectivePipeline.class),
ColoredShape(0, ColoredShapePipeline.class);

33
photon-server/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java
View File

@@ -19,6 +19,7 @@ package org.photonvision.vision.pipeline;
import java.util.List;
import org.apache.commons.lang3.tuple.Pair;
import org.apache.commons.lang3.tuple.Triple;
import org.opencv.core.Mat;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.vision.frame.Frame;
@@ -26,21 +27,7 @@ import org.photonvision.vision.frame.FrameStaticProperties;
import org.photonvision.vision.opencv.CVMat;
import org.photonvision.vision.opencv.Contour;
import org.photonvision.vision.pipe.CVPipe.CVPipeResult;
import org.photonvision.vision.pipe.impl.Collect2dTargetsPipe;
import org.photonvision.vision.pipe.impl.CornerDetectionPipe;
import org.photonvision.vision.pipe.impl.Draw2dCrosshairPipe;
import org.photonvision.vision.pipe.impl.Draw2dTargetsPipe;
import org.photonvision.vision.pipe.impl.Draw3dTargetsPipe;
import org.photonvision.vision.pipe.impl.ErodeDilatePipe;
import org.photonvision.vision.pipe.impl.FilterContoursPipe;
import org.photonvision.vision.pipe.impl.FindContoursPipe;
import org.photonvision.vision.pipe.impl.GroupContoursPipe;
import org.photonvision.vision.pipe.impl.HSVPipe;
import org.photonvision.vision.pipe.impl.OutputMatPipe;
import org.photonvision.vision.pipe.impl.RotateImagePipe;
import org.photonvision.vision.pipe.impl.SolvePNPPipe;
import org.photonvision.vision.pipe.impl.SortContoursPipe;
import org.photonvision.vision.pipe.impl.SpeckleRejectPipe;
import org.photonvision.vision.pipe.impl.*;
import org.photonvision.vision.pipeline.result.CVPipelineResult;
import org.photonvision.vision.target.PotentialTarget;
import org.photonvision.vision.target.TrackedTarget;
@@ -64,6 +51,7 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
private final Draw2dCrosshairPipe draw2dCrosshairPipe = new Draw2dCrosshairPipe();
private final Draw2dTargetsPipe draw2dTargetsPipe = new Draw2dTargetsPipe();
private final Draw3dTargetsPipe draw3dTargetsPipe = new Draw3dTargetsPipe();
private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
private final Mat rawInputMat = new Mat();
private final long[] pipeProfileNanos = new long[PipelineProfiler.ReflectivePipeCount];
@@ -79,6 +67,7 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
@Override
protected void setPipeParams(
FrameStaticProperties frameStaticProperties, ReflectivePipelineSettings settings) {
RotateImagePipe.RotateImageParams rotateImageParams =
new RotateImagePipe.RotateImageParams(settings.inputImageRotationMode);
rotateImagePipe.setParams(rotateImageParams);
@@ -151,12 +140,14 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
var draw3dContoursParams =
new Draw3dTargetsPipe.Draw3dContoursParams(
settings.cameraCalibration, settings.targetModel);
frameStaticProperties.cameraCalibration, settings.targetModel);
draw3dTargetsPipe.setParams(draw3dContoursParams);
var solvePNPParams =
new SolvePNPPipe.SolvePNPPipeParams(
settings.cameraCalibration, settings.cameraPitch, settings.targetModel);
frameStaticProperties.cameraCalibration,
frameStaticProperties.cameraPitch,
settings.targetModel);
solvePNPPipe.setParams(solvePNPParams);
}
@@ -223,6 +214,10 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
targetList = collect2dTargetsResult.output;
}
var fpsResult = calculateFPSPipe.run(null);
var fps = fpsResult.output;
sumPipeNanosElapsed += fpsResult.nanosElapsed;
// Convert single-channel HSV output mat to 3-channel BGR in preparation for streaming
var outputMatPipeResult = outputMatPipe.run(hsvPipeResult.output);
sumPipeNanosElapsed += pipeProfileNanos[12] = outputMatPipeResult.nanosElapsed;
@@ -237,11 +232,11 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
// Draw 2D contours on input and output
var draw2dTargetsOnInput =
draw2dTargetsPipe.run(Pair.of(rawInputMat, collect2dTargetsResult.output));
draw2dTargetsPipe.run(Triple.of(rawInputMat, collect2dTargetsResult.output, fps));
sumPipeNanosElapsed += pipeProfileNanos[15] = draw2dTargetsOnInput.nanosElapsed;
var draw2dTargetsOnOutput =
draw2dTargetsPipe.run(Pair.of(hsvPipeResult.output, collect2dTargetsResult.output));
draw2dTargetsPipe.run(Triple.of(hsvPipeResult.output, collect2dTargetsResult.output, fps));
sumPipeNanosElapsed += pipeProfileNanos[16] = draw2dTargetsOnOutput.nanosElapsed;
// Draw 3D Targets on input and output if necessary

10
photon-server/src/main/java/org/photonvision/vision/pipeline/ReflectivePipelineSettings.java
View File

@@ -18,9 +18,7 @@
package org.photonvision.vision.pipeline;
import com.fasterxml.jackson.annotation.JsonTypeName;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import java.util.Objects;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.opencv.ContourGroupingMode;
import org.photonvision.vision.opencv.ContourIntersectionDirection;
import org.photonvision.vision.pipe.impl.CornerDetectionPipe;
@@ -36,9 +34,7 @@ public class ReflectivePipelineSettings extends AdvancedPipelineSettings {
// 3d settings
public boolean solvePNPEnabled = false;
public CameraCalibrationCoefficients cameraCalibration;
public TargetModel targetModel;
public Rotation2d cameraPitch = Rotation2d.fromDegrees(0.0);
public TargetModel targetModel = TargetModel.get2020Target();
// Corner detection settings
public CornerDetectionPipe.DetectionStrategy cornerDetectionStrategy =
@@ -67,9 +63,7 @@ public class ReflectivePipelineSettings extends AdvancedPipelineSettings {
== 0
&& contourGroupingMode == that.contourGroupingMode
&& contourIntersection == that.contourIntersection
&& Objects.equals(cameraCalibration, that.cameraCalibration)
&& targetModel.equals(that.targetModel)
&& cameraPitch.equals(that.cameraPitch)
&& cornerDetectionStrategy == that.cornerDetectionStrategy;
}
@@ -80,9 +74,7 @@ public class ReflectivePipelineSettings extends AdvancedPipelineSettings {
contourGroupingMode,
contourIntersection,
solvePNPEnabled,
cameraCalibration,
targetModel,
cameraPitch,
cornerDetectionStrategy,
cornerDetectionUseConvexHulls,
cornerDetectionExactSideCount,

89
photon-server/src/main/java/org/photonvision/vision/pipeline/UICalibrationData.java Normal file
View File

@@ -0,0 +1,89 @@
/*
* 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.vision.pipeline;
import java.util.Map;
public class UICalibrationData {
public final int videoModeIndex;
public int count;
public final int minCount;
public final boolean hasEnough;
public final double squareSizeIn;
public final int patternWidth;
public final int patternHeight;
public final BoardType boardType; //
public UICalibrationData(
int count,
int videoModeIndex,
int minCount,
boolean hasEnough,
double squareSizeIn,
int patternWidth,
int patternHeight,
BoardType boardType) {
this.count = count;
this.minCount = minCount;
this.videoModeIndex = videoModeIndex;
this.hasEnough = hasEnough;
this.squareSizeIn = squareSizeIn;
this.patternWidth = patternWidth;
this.patternHeight = patternHeight;
this.boardType = boardType;
}
public enum BoardType {
CHESSBOARD,
DOTBOARD
}
public static UICalibrationData fromMap(Map<String, Object> map) {
return new UICalibrationData(
((Number) map.get("count")).intValue(),
((Number) map.get("videoModeIndex")).intValue(),
((Number) map.get("minCount")).intValue(),
(boolean) map.get("hasEnough"),
((Number) map.get("squareSizeIn")).doubleValue(),
((Number) map.get("patternWidth")).intValue(),
((Number) map.get("patternHeight")).intValue(),
BoardType.values()[(int) map.get("boardType")]);
}
@Override
public String toString() {
return "UICalibrationData{"
+ "videoModeIndex="
+ videoModeIndex
+ ", count="
+ count
+ ", minCount="
+ minCount
+ ", hasEnough="
+ hasEnough
+ ", squareSizeIn="
+ squareSizeIn
+ ", patternWidth="
+ patternWidth
+ ", patternHeight="
+ patternHeight
+ ", boardType="
+ boardType
+ '}';
}
}

3
photon-server/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java
View File

@@ -17,6 +17,7 @@
package org.photonvision.vision.pipeline.result;
import java.util.Collections;
import java.util.List;
import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.opencv.Releasable;
@@ -32,7 +33,7 @@ public class CVPipelineResult implements Releasable {
public CVPipelineResult(
double processingMillis, List<TrackedTarget> targets, Frame outputFrame, Frame inputFrame) {
this.processingMillis = processingMillis;
this.targets = targets;
this.targets = targets != null ? targets : Collections.emptyList();
this.outputFrame = Frame.copyFromAndRelease(outputFrame);
this.inputFrame = inputFrame != null ? Frame.copyFromAndRelease(inputFrame) : null;

27
photon-server/src/main/java/org/photonvision/vision/pipeline/result/SimpleTrackedTarget.java
View File

@@ -17,8 +17,9 @@
package org.photonvision.vision.pipeline.result;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Transform2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import java.util.Objects;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.vision.target.TrackedTarget;
@@ -30,20 +31,20 @@ public class SimpleTrackedTarget {
private double pitch;
private double area;
private double skew;
private Pose2d robotRelativePose = new Pose2d();
private Transform2d cameraToTarget = new Transform2d();
public SimpleTrackedTarget() {}
public SimpleTrackedTarget(double yaw, double pitch, double area, double skew, Pose2d pose) {
public SimpleTrackedTarget(double yaw, double pitch, double area, double skew, Transform2d pose) {
this.yaw = yaw;
this.pitch = pitch;
this.area = area;
this.skew = skew;
robotRelativePose = pose;
cameraToTarget = pose;
}
public SimpleTrackedTarget(TrackedTarget t) {
this(t.getYaw(), t.getPitch(), t.getArea(), t.getSkew(), t.getRobotRelativePose());
this(t.getYaw(), t.getPitch(), t.getArea(), t.getSkew(), t.getCameraToTarget());
}
public double getYaw() {
@@ -58,8 +59,8 @@ public class SimpleTrackedTarget {
return area;
}
public Pose2d getRobotRelativePose() {
return robotRelativePose;
public Transform2d getCameraToTarget() {
return cameraToTarget;
}
@Override
@@ -70,12 +71,12 @@ public class SimpleTrackedTarget {
return Double.compare(that.yaw, yaw) == 0
&& Double.compare(that.pitch, pitch) == 0
&& Double.compare(that.area, area) == 0
&& Objects.equals(robotRelativePose, that.robotRelativePose);
&& Objects.equals(cameraToTarget, that.cameraToTarget);
}
@Override
public int hashCode() {
return Objects.hash(yaw, pitch, area, robotRelativePose);
return Objects.hash(yaw, pitch, area, cameraToTarget);
}
/**
@@ -94,7 +95,7 @@ public class SimpleTrackedTarget {
double y = packet.decodeDouble();
double r = packet.decodeDouble();
robotRelativePose = new Pose2d(x, y, Rotation2d.fromDegrees(r));
cameraToTarget = new Transform2d(new Translation2d(x, y), Rotation2d.fromDegrees(r));
return packet;
}
@@ -110,9 +111,9 @@ public class SimpleTrackedTarget {
packet.encode(pitch);
packet.encode(area);
packet.encode(skew);
packet.encode(robotRelativePose.getTranslation().getX());
packet.encode(robotRelativePose.getTranslation().getY());
packet.encode(robotRelativePose.getRotation().getDegrees());
packet.encode(cameraToTarget.getTranslation().getX());
packet.encode(cameraToTarget.getTranslation().getY());
packet.encode(cameraToTarget.getRotation().getDegrees());
return packet;
}

108
photon-server/src/main/java/org/photonvision/vision/processes/PipelineManager.java
View File

@@ -32,14 +32,14 @@ public class PipelineManager {
public static final int CAL_3D_INDEX = -2;
protected final List<CVPipelineSettings> userPipelineSettings;
protected final Calibration3dPipeline calibration3dPipeline = new Calibration3dPipeline();
protected final Calibrate3dPipeline calibration3dPipeline = new Calibrate3dPipeline();
protected final DriverModePipeline driverModePipeline = new DriverModePipeline();
/** Index of the currently active pipeline. */
private int currentPipelineIndex = DRIVERMODE_INDEX;
/** Index of the currently active pipeline. Defaults to 0. */
private int currentPipelineIndex = 0;
/** The currently active pipeline. */
private CVPipeline currentPipeline = driverModePipeline;
private CVPipeline currentUserPipeline = driverModePipeline;
/**
* Index of the last active user-created pipeline. <br>
@@ -109,7 +109,7 @@ public class PipelineManager {
*
* @return The currently active pipeline.
*/
public CVPipeline getCurrentPipeline() {
public CVPipeline getCurrentUserPipeline() {
if (currentPipelineIndex < 0) {
switch (currentPipelineIndex) {
case CAL_3D_INDEX:
@@ -120,20 +120,23 @@ public class PipelineManager {
}
var desiredPipelineSettings = userPipelineSettings.get(currentPipelineIndex);
if (currentPipeline.getSettings().pipelineIndex != desiredPipelineSettings.pipelineIndex) {
switch (desiredPipelineSettings.pipelineType) {
case Reflective:
currentPipeline =
new ReflectivePipeline((ReflectivePipelineSettings) desiredPipelineSettings);
break;
case ColoredShape:
currentPipeline =
new ColoredShapePipeline((ColoredShapePipelineSettings) desiredPipelineSettings);
break;
}
}
// if (currentPipeline.getSettings().pipelineIndex !=
// desiredPipelineSettings.pipelineIndex) {
// switch (desiredPipelineSettings.pipelineType) {
// case Reflective:
// currentPipeline =
// new ReflectivePipeline((ReflectivePipelineSettings)
// desiredPipelineSettings);
// break;
// case ColoredShape:
// currentPipeline =
// new ColoredShapePipeline((ColoredShapePipelineSettings)
// desiredPipelineSettings);
// break;
// }
// }
return currentPipeline;
return currentUserPipeline;
}
/**
@@ -164,6 +167,31 @@ public class PipelineManager {
}
currentPipelineIndex = index;
if (index >= 0) {
var desiredPipelineSettings = userPipelineSettings.get(currentPipelineIndex);
switch (desiredPipelineSettings.pipelineType) {
case Reflective:
currentUserPipeline =
new ReflectivePipeline((ReflectivePipelineSettings) desiredPipelineSettings);
break;
case ColoredShape:
currentUserPipeline =
new ColoredShapePipeline((ColoredShapePipelineSettings) desiredPipelineSettings);
break;
}
}
}
/**
* Enters or exits calibration mode based on the parameter. <br>
* <br>
* Exiting returns to the last used user pipeline.
*
* @param wantsCalibration True to enter calibration mode, false to exit calibration mode.
*/
public void setCalibrationMode(boolean wantsCalibration) {
if (!wantsCalibration) calibration3dPipeline.finishCalibration();
setPipelineInternal(wantsCalibration ? CAL_3D_INDEX : lastPipelineIndex);
}
/**
@@ -197,15 +225,20 @@ public class PipelineManager {
private void reassignIndexes() {
userPipelineSettings.sort(PipelineSettingsIndexComparator);
for (int i = 0; i < userPipelineSettings.size(); i++) {
getPipelineSettings(i).pipelineIndex = i;
userPipelineSettings.get(i).pipelineIndex = i;
}
}
public CVPipelineSettings addPipeline(PipelineType type) {
return addPipeline(type, "New Pipeline");
}
public CVPipelineSettings addPipeline(PipelineType type, String nickname) {
switch (type) {
case Reflective:
{
var added = new ReflectivePipelineSettings();
added.pipelineNickname = nickname;
addPipelineInternal(added);
return added;
}
@@ -228,6 +261,7 @@ public class PipelineManager {
private void removePipelineInternal(int index) {
userPipelineSettings.remove(index);
currentPipelineIndex = Math.min(index, userPipelineSettings.size() - 1);
reassignIndexes();
}
@@ -241,6 +275,40 @@ public class PipelineManager {
}
// TODO should we block/lock on a mutex?
removePipelineInternal(index);
currentPipelineIndex = Math.max(userPipelineSettings.size() - 1, currentPipelineIndex);
setIndex(currentPipelineIndex);
}
public void renameCurrentPipeline(String newName) {
getCurrentPipelineSettings().pipelineNickname = newName;
}
public void duplicatePipeline(int index) {
var settings = userPipelineSettings.get(index);
var newSettings = settings.clone();
newSettings.pipelineNickname =
createUniqueName(settings.pipelineNickname, userPipelineSettings);
newSettings.pipelineIndex = Integer.MAX_VALUE;
logger.debug("Duplicating pipe " + index + " to " + newSettings.pipelineNickname);
userPipelineSettings.add(newSettings);
reassignIndexes();
}
private static String createUniqueName(
String nickname, List<CVPipelineSettings> existingSettings) {
int index = 0;
String uniqueName = nickname;
while (true) {
String finalUniqueName = uniqueName;
var conflictingName =
existingSettings.stream().anyMatch(it -> it.pipelineNickname.equals(finalUniqueName));
if (!conflictingName) return uniqueName;
index++;
uniqueName = nickname + " (" + index + ")";
if (index == 6
&& existingSettings.stream()
.noneMatch(it -> it.pipelineNickname.equals(nickname + "( dQw4w9WgXcQ )")))
return nickname + "( dQw4w9WgXcQ )";
}
}
}

129
photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java
View File

@@ -17,6 +17,8 @@
package org.photonvision.vision.processes;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.util.Units;
import io.javalin.websocket.WsContext;
import java.util.*;
import org.apache.commons.lang3.tuple.Pair;
@@ -38,6 +40,7 @@ import org.photonvision.common.util.SerializationUtils;
import org.photonvision.common.util.numbers.DoubleCouple;
import org.photonvision.common.util.numbers.IntegerCouple;
import org.photonvision.server.UIUpdateType;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.camera.CameraQuirk;
import org.photonvision.vision.camera.QuirkyCamera;
import org.photonvision.vision.camera.USBCameraSource;
@@ -45,6 +48,7 @@ import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.frame.FrameConsumer;
import org.photonvision.vision.frame.consumer.MJPGFrameConsumer;
import org.photonvision.vision.pipeline.PipelineType;
import org.photonvision.vision.pipeline.UICalibrationData;
import org.photonvision.vision.pipeline.result.CVPipelineResult;
/**
@@ -85,7 +89,7 @@ public class VisionModule {
this.visionRunner =
new VisionRunner(
this.visionSource.getFrameProvider(),
this.pipelineManager::getCurrentPipeline,
this.pipelineManager::getCurrentUserPipeline,
this::consumeResult);
this.moduleIndex = index;
@@ -124,6 +128,13 @@ public class VisionModule {
pipelineManager.getCurrentPipelineSettings().streamingFrameDivisor);
dashboardOutputStreamer.setFrameDivisor(
pipelineManager.getCurrentPipelineSettings().streamingFrameDivisor);
// Set vendor FOV
if (isVendorCamera()) {
var fov = ConfigManager.getInstance().getConfig().getHardwareConfig().vendorFOV;
logger.info("Setting FOV of vendor camera to " + fov);
visionSource.getSettables().setFOV(fov);
}
}
private void setDriverMode(boolean isDriverMode) {
@@ -135,6 +146,68 @@ public class VisionModule {
visionRunner.startProcess();
}
public void setFovAndPitch(double fov, Rotation2d pitch) {
var settables = visionSource.getSettables();
logger.trace(
() ->
"Setting "
+ settables.getConfiguration().nickname
+ ": pitch ("
+ pitch.getDegrees()
+ ") FOV ("
+ fov
+ ")");
settables.setCameraPitch(pitch);
// Only set FOV if we have no vendor JSON and we aren't using a PiCAM
if (isVendorCamera()) {
logger.info("Cannot set FOV on a vendor device! Ignoring...");
} else {
settables.setFOV(fov);
}
}
// TODO improve robustness of this detection
private boolean isVendorCamera() {
return ConfigManager.getInstance().getConfig().getHardwareConfig().hasPresetFOV()
&& cameraQuirks.hasQuirk(CameraQuirk.PiCam);
}
public void startCalibration(UICalibrationData data) {
var settings = pipelineManager.calibration3dPipeline.getSettings();
settings.cameraVideoModeIndex = data.videoModeIndex;
visionSource.getSettables().setVideoModeIndex(data.videoModeIndex);
logger.info(
"Starting calibration at resolution index "
+ data.videoModeIndex
+ " and settings "
+ data);
settings.gridSize = Units.inchesToMeters(data.squareSizeIn);
settings.boardHeight = data.patternHeight;
settings.boardWidth = data.patternWidth;
settings.boardType = data.boardType;
pipelineManager.setCalibrationMode(true);
}
public void takeCalibrationSnapshot() {
pipelineManager.calibration3dPipeline.takeSnapshot();
}
public CameraCalibrationCoefficients endCalibration() {
var ret = pipelineManager.calibration3dPipeline.tryCalibration();
pipelineManager.setCalibrationMode(false);
if (ret != null) {
logger.debug("Saving calibration...");
visionSource.getSettables().getConfiguration().addCalibration(ret);
visionSource.getSettables().calculateFrameStaticProps();
} else {
logger.error("Calibration failed...");
}
saveAndBroadcastAll();
return ret;
}
private class VisionSettingChangeSubscriber extends DataChangeSubscriber {
private VisionSettingChangeSubscriber() {
@@ -152,7 +225,7 @@ public class VisionModule {
var propName = wsEvent.propertyName;
var newPropValue = wsEvent.data;
var currentSettings = pipelineManager.getCurrentPipeline().getSettings();
var currentSettings = pipelineManager.getCurrentUserPipeline().getSettings();
// special case for non-PipelineSetting changes
switch (propName) {
@@ -164,7 +237,7 @@ public class VisionModule {
return;
case "pipelineName": // rename current pipeline
logger.info("Changing nick to " + newPropValue);
pipelineManager.getCurrentPipelineSettings().pipelineNickname = (String) newPropValue;
pipelineManager.renameCurrentPipeline((String) newPropValue);
saveAndBroadcastAll();
return;
case "newPipelineInfo": // add new pipeline
@@ -174,8 +247,11 @@ public class VisionModule {
logger.info("Adding a " + type + " pipeline with name " + name);
var addedSettings = pipelineManager.addPipeline(type);
var addedSettings = pipelineManager.addPipeline(type, name);
addedSettings.pipelineNickname = name;
var newIndex = pipelineManager.userPipelineSettings.indexOf(addedSettings);
setPipeline(newIndex);
saveAndBroadcastAll();
return;
case "deleteCurrPipeline":
@@ -184,6 +260,11 @@ public class VisionModule {
pipelineManager.removePipeline(indexToDelete);
saveAndBroadcastAll();
return;
case "duplicatePipeline":
logger.info("Duplicating pipe " + newPropValue);
pipelineManager.duplicatePipeline((Integer) newPropValue);
saveAndBroadcastAll();
return;
case "changePipeline": // change active pipeline
var index = (Integer) newPropValue;
if (index == pipelineManager.getCurrentPipelineIndex()) {
@@ -222,6 +303,14 @@ public class VisionModule {
HardwareManager.getInstance().shutdown();
}
return;
case "startcalibration":
var data = UICalibrationData.fromMap((Map<String, Object>) newPropValue);
startCalibration(data);
saveAndBroadcastAll();
return;
case "takeCalSnapshot":
takeCalibrationSnapshot();
return;
}
// special case for camera settables
@@ -306,7 +395,7 @@ public class VisionModule {
return;
}
visionSource.getSettables().setCurrentVideoMode(config.cameraVideoModeIndex);
visionSource.getSettables().setVideoModeInternal(config.cameraVideoModeIndex);
visionSource.getSettables().setBrightness(config.cameraBrightness);
visionSource.getSettables().setExposure(config.cameraExposure);
@@ -320,7 +409,7 @@ public class VisionModule {
pipelineManager.getCurrentPipelineIndex();
}
private void saveModule() {
public void saveModule() {
ConfigManager.getInstance()
.saveModule(
getStateAsCameraConfig(), visionSource.getSettables().getConfiguration().uniqueName);
@@ -391,7 +480,25 @@ public class VisionModule {
ret.videoFormatList = temp;
ret.outputStreamPort = dashboardOutputStreamer.getCurrentStreamPort();
ret.inputStreamPort = dashboardInputStreamer.getCurrentStreamPort();
// ret.uiStreamPort = uiStreamer.getCurrentStreamPort();
var calList = new ArrayList<HashMap<String, Object>>();
for (var c : visionSource.getSettables().getConfiguration().calibrations) {
var internalMap = new HashMap<String, Object>();
internalMap.put("perViewErrors", c.perViewErrors);
internalMap.put("standardDeviation", c.standardDeviation);
internalMap.put("width", c.resolution.width);
internalMap.put("height", c.resolution.height);
internalMap.put("intrinsics", c.cameraIntrinsics.data);
internalMap.put("extrinsics", c.cameraExtrinsics.data);
calList.add(internalMap);
}
ret.calibrations = calList;
ret.isFovConfigurable =
!(HardwareManager.getInstance().getConfig().hasPresetFOV()
&& cameraQuirks.hasQuirk(CameraQuirk.PiCam));
return ret;
}
@@ -400,7 +507,13 @@ public class VisionModule {
var config = visionSource.getSettables().getConfiguration();
config.setPipelineSettings(pipelineManager.userPipelineSettings);
config.driveModeSettings = pipelineManager.driverModePipeline.getSettings();
config.currentPipelineIndex = pipelineManager.getCurrentPipelineIndex();
config.currentPipelineIndex = Math.max(pipelineManager.getCurrentPipelineIndex(), -1);
logger.info(
"Saving state with "
+ config.calibrations.size()
+ " calibrated resolutions and index "
+ config.currentPipelineIndex);
return config;
}

7
photon-server/src/main/java/org/photonvision/vision/processes/VisionModuleManager.java
View File

@@ -41,6 +41,13 @@ public class VisionModuleManager {
return visionModules;
}
public VisionModule getModule(String nickname) {
for (var module : visionModules) {
if (module.getStateAsCameraConfig().nickname.equals(nickname)) return module;
}
return null;
}
public VisionModule getModule(int i) {
return visionModules.get(i);
}

41
photon-server/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java
View File

@@ -18,11 +18,17 @@
package org.photonvision.vision.processes;
import edu.wpi.cscore.VideoMode;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import java.util.HashMap;
import org.photonvision.common.configuration.CameraConfiguration;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.vision.frame.FrameStaticProperties;
public abstract class VisionSourceSettables {
private static final Logger logger =
new Logger(VisionSourceSettables.class, LogGroup.VisionModule);
private final CameraConfiguration configuration;
protected VisionSourceSettables(CameraConfiguration configuration) {
@@ -44,15 +50,25 @@ public abstract class VisionSourceSettables {
public abstract VideoMode getCurrentVideoMode();
public void setCurrentVideoMode(int index) {
setCurrentVideoMode(getAllVideoModes().get(index));
public void setVideoModeInternal(int index) {
setVideoMode(getAllVideoModes().get(index));
}
public abstract void setCurrentVideoMode(VideoMode videoMode);
public void setVideoMode(VideoMode mode) {
setVideoModeInternal(mode);
calculateFrameStaticProps();
}
protected abstract void setVideoModeInternal(VideoMode videoMode);
public void setCameraPitch(Rotation2d pitch) {
configuration.camPitch = pitch;
calculateFrameStaticProps();
}
@SuppressWarnings("unused")
public void setVideoModeIndex(int index) {
setCurrentVideoMode(videoModes.get(index));
setVideoMode(videoModes.get(index));
}
public abstract HashMap<Integer, VideoMode> getAllVideoModes();
@@ -63,6 +79,23 @@ public abstract class VisionSourceSettables {
public void setFOV(double fov) {
configuration.FOV = fov;
calculateFrameStaticProps();
}
public void calculateFrameStaticProps() {
var videoMode = getCurrentVideoMode();
this.frameStaticProperties =
new FrameStaticProperties(
videoMode,
getFOV(),
configuration.camPitch,
configuration.calibrations.stream()
.filter(
it ->
it.resolution.width == videoMode.width
&& it.resolution.height == videoMode.height)
.findFirst()
.orElse(null));
}
public FrameStaticProperties getFrameStaticProperties() {

26
photon-server/src/main/java/org/photonvision/vision/target/TargetModel.java
View File

@@ -20,6 +20,7 @@ package org.photonvision.vision.target;
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnore;
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.wpilibj.util.Units;
import java.util.ArrayList;
import java.util.List;
import java.util.Objects;
@@ -81,27 +82,28 @@ public class TargetModel implements Releasable {
}
public static TargetModel get2020TargetInnerPort() {
return get2020Target(2d * 12d + 5.25); // Inches, TODO switch to meters
// Per the game manual, the inner port is 2ft 5.25in behind the outer port
return get2020Target(Units.inchesToMeters(2d * 12d + 5.25));
}
public static TargetModel get2020Target(double offset) {
public static TargetModel get2020Target(double offsetMeters) {
var corners =
List.of(
new Point3(-19.625, 0, offset),
new Point3(-9.819867, -17, offset),
new Point3(9.819867, -17, offset),
new Point3(19.625, 0, offset));
return new TargetModel(corners, 12); // TODO switch to meters
new Point3(Units.inchesToMeters(-19.625), 0, offsetMeters),
new Point3(Units.inchesToMeters(-9.819867), Units.inchesToMeters(-17), offsetMeters),
new Point3(Units.inchesToMeters(9.819867), Units.inchesToMeters(-17), offsetMeters),
new Point3(Units.inchesToMeters(19.625), 0, offsetMeters));
return new TargetModel(corners, Units.inchesToMeters(12));
}
public static TargetModel get2019Target() {
var corners =
List.of(
new Point3(-5.936, 2.662, 0),
new Point3(-7.313, -2.662, 0),
new Point3(7.313, -2.662, 0),
new Point3(5.936, 2.662, 0));
return new TargetModel(corners, 4);
new Point3(Units.inchesToMeters(-5.936), Units.inchesToMeters(2.662), 0),
new Point3(Units.inchesToMeters(-7.313), Units.inchesToMeters(-2.662), 0),
new Point3(Units.inchesToMeters(7.313), Units.inchesToMeters(-2.662), 0),
new Point3(Units.inchesToMeters(5.936), Units.inchesToMeters(2.662), 0));
return new TargetModel(corners, 0.1);
}
public static TargetModel getCircleTarget(double radius) {

16
photon-server/src/main/java/org/photonvision/vision/target/TrackedTarget.java
View File

@@ -17,7 +17,7 @@
package org.photonvision.vision.target;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Transform2d;
import java.util.HashMap;
import java.util.List;
import org.opencv.core.Mat;
@@ -44,7 +44,7 @@ public class TrackedTarget implements Releasable {
private double m_area;
private double m_skew;
private Pose2d m_robotRelativePose = new Pose2d();
private Transform2d m_cameraToTarget = new Transform2d();
private Mat m_cameraRelativeTvec, m_cameraRelativeRvec;
@@ -143,12 +143,12 @@ public class TrackedTarget implements Releasable {
return !m_subContours.isEmpty();
}
public Pose2d getRobotRelativePose() {
return m_robotRelativePose;
public Transform2d getCameraToTarget() {
return m_cameraToTarget;
}
public void setRobotRelativePose(Pose2d robotRelativePose) {
this.m_robotRelativePose = robotRelativePose;
public void setCameraToTarget(Transform2d pose) {
this.m_cameraToTarget = pose;
}
public Mat getCameraRelativeTvec() {
@@ -181,8 +181,8 @@ public class TrackedTarget implements Releasable {
ret.put("yaw", getYaw());
ret.put("skew", getSkew());
ret.put("area", getArea());
if (getRobotRelativePose() != null) {
ret.put("pose", getRobotRelativePose().toHashMap());
if (getCameraToTarget() != null) {
ret.put("pose", getCameraToTarget().toHashMap());
}
return ret;
}