Filters are of the form: y[n] = (b0*x[n] + b1*x[n-1] + ... + bP*x[n-P]) - (a0*y[n-1] +
+ * a2*y[n-2] + ... + aQ*y[n-Q])
+ *
+ * Where: y[n] is the output at time "n" x[n] is the input at time "n" y[n-1] is the output from
+ * the LAST time step ("n-1") x[n-1] is the input from the LAST time step ("n-1") b0...bP are the
+ * "feedforward" (FIR) gains a0...aQ are the "feedback" (IIR) gains IMPORTANT! Note the "-" sign in
+ * front of the feedback term! This is a common convention in signal processing.
+ *
+ *
What can linear filters do? Basically, they can filter, or diminish, the effects of
+ * undesirable input frequencies. High frequencies, or rapid changes, can be indicative of sensor
+ * noise or be otherwise undesirable. A "low pass" filter smooths out the signal, reducing the
+ * impact of these high frequency components. Likewise, a "high pass" filter gets rid of
+ * slow-moving signal components, letting you detect large changes more easily.
+ *
+ *
Example FRC applications of filters: - Getting rid of noise from an analog sensor input (note:
+ * the roboRIO's FPGA can do this faster in hardware) - Smoothing out joystick input to prevent the
+ * wheels from slipping or the robot from tipping - Smoothing motor commands so that unnecessary
+ * strain isn't put on electrical or mechanical components - If you use clever gains, you can make a
+ * PID controller out of this class!
+ *
+ *
For more on filters, we highly recommend the following articles:
+ * https://en.wikipedia.org/wiki/Linear_filter
+ * https://en.wikipedia.org/wiki/Iir_filter
+ * https://en.wikipedia.org/wiki/Fir_filter
+ *
+ *
Note 1: calculate() should be called by the user on a known, regular period. You can use a
+ * Notifier for this or do it "inline" with code in a periodic function.
+ *
+ *
Note 2: For ALL filters, gains are necessarily a function of frequency. If you make a filter
+ * that works well for you at, say, 100Hz, you will most definitely need to adjust the gains if you
+ * then want to run it at 200Hz! Combining this with Note 1 - the impetus is on YOU as a developer
+ * to make sure calculate() gets called at the desired, constant frequency!
+ */
+public class LinearFilter {
+ private final CircularBuffer m_inputs;
+ private final CircularBuffer m_outputs;
+ private final double[] m_inputGains;
+ private final double[] m_outputGains;
+
+ /**
+ * Create a linear FIR or IIR filter.
+ *
+ * @param ffGains The "feed forward" or FIR gains.
+ * @param fbGains The "feed back" or IIR gains.
+ */
+ public LinearFilter(double[] ffGains, double[] fbGains) {
+ m_inputs = new CircularBuffer(ffGains.length);
+ m_outputs = new CircularBuffer(fbGains.length);
+ m_inputGains = Arrays.copyOf(ffGains, ffGains.length);
+ m_outputGains = Arrays.copyOf(fbGains, fbGains.length);
+
+ }
+
+ /**
+ * Creates a one-pole IIR low-pass filter of the form: y[n] = (1-gain)*x[n] + gain*y[n-1] where
+ * gain = e^(-dt / T), T is the time constant in seconds.
+ *
+ *
This filter is stable for time constants greater than zero.
+ *
+ * @param timeConstant The discrete-time time constant in seconds.
+ * @param period The period in seconds between samples taken by the user.
+ */
+ public static LinearFilter singlePoleIIR(double timeConstant,
+ double period) {
+ double gain = Math.exp(-period / timeConstant);
+ double[] ffGains = {1.0 - gain};
+ double[] fbGains = {-gain};
+
+ return new LinearFilter(ffGains, fbGains);
+ }
+
+ /**
+ * Creates a first-order high-pass filter of the form: y[n] = gain*x[n] + (-gain)*x[n-1] +
+ * gain*y[n-1] where gain = e^(-dt / T), T is the time constant in seconds.
+ *
+ *
This filter is stable for time constants greater than zero.
+ *
+ * @param timeConstant The discrete-time time constant in seconds.
+ * @param period The period in seconds between samples taken by the user.
+ */
+ public static LinearFilter highPass(double timeConstant,
+ double period) {
+ double gain = Math.exp(-period / timeConstant);
+ double[] ffGains = {gain, -gain};
+ double[] fbGains = {-gain};
+
+ return new LinearFilter(ffGains, fbGains);
+ }
+
+ /**
+ * Creates a K-tap FIR moving average filter of the form: y[n] = 1/k * (x[k] + x[k-1] + ... +
+ * x[0]).
+ *
+ *
This filter is always stable.
+ *
+ * @param taps The number of samples to average over. Higher = smoother but slower.
+ * @throws IllegalArgumentException if number of taps is less than 1.
+ */
+ public static LinearFilter movingAverage(int taps) {
+ if (taps <= 0) {
+ throw new IllegalArgumentException("Number of taps was not at least 1");
+ }
+
+ double[] ffGains = new double[taps];
+ for (int i = 0; i < ffGains.length; i++) {
+ ffGains[i] = 1.0 / taps;
+ }
+
+ double[] fbGains = new double[0];
+
+ return new LinearFilter(ffGains, fbGains);
+ }
+
+ /**
+ * Reset the filter state.
+ */
+ public void reset() {
+ m_inputs.clear();
+ m_outputs.clear();
+ }
+
+ /**
+ * Calculates the next value of the filter.
+ *
+ * @param input Current input value.
+ *
+ * @return The filtered value at this step
+ */
+ public double calculate(double input) {
+ double retVal = 0.0;
+
+ // Rotate the inputs
+ m_inputs.addFirst(input);
+
+ // Calculate the new value
+ for (int i = 0; i < m_inputGains.length; i++) {
+ retVal += m_inputs.get(i) * m_inputGains[i];
+ }
+ for (int i = 0; i < m_outputGains.length; i++) {
+ retVal -= m_outputs.get(i) * m_outputGains[i];
+ }
+
+ // Rotate the outputs
+ m_outputs.addFirst(retVal);
+
+ return retVal;
+ }
+}
diff --git a/photon-server/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java b/photon-server/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java
deleted file mode 100644
index 332959fe7..000000000
--- a/photon-server/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java
+++ /dev/null
@@ -1,86 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import java.util.ArrayList;
-import java.util.Collections;
-import java.util.List;
-
-import edu.wpi.first.wpiutil.CircularBuffer;
-
-/**
- * A class that implements a moving-window median filter. Useful for reducing measurement noise,
- * especially with processes that generate occasional, extreme outliers (such as values from
- * vision processing, LIDAR, or ultrasonic sensors).
- */
-public class MedianFilter {
- private final CircularBuffer m_valueBuffer;
- private final List m_orderedValues;
- private final int m_size;
-
- /**
- * Creates a new MedianFilter.
- *
- * @param size The number of samples in the moving window.
- */
- public MedianFilter(int size) {
- // Circular buffer of values currently in the window, ordered by time
- m_valueBuffer = new CircularBuffer(size);
- // List of values currently in the window, ordered by value
- m_orderedValues = new ArrayList<>(size);
- // Size of rolling window
- m_size = size;
- }
-
- /**
- * Calculates the moving-window median for the next value of the input stream.
- *
- * @param next The next input value.
- * @return The median of the moving window, updated to include the next value.
- */
- public double calculate(double next) {
- // Find insertion point for next value
- int index = Collections.binarySearch(m_orderedValues, next);
-
- // Deal with binarySearch behavior for element not found
- if (index < 0) {
- index = Math.abs(index + 1);
- }
-
- // Place value at proper insertion point
- m_orderedValues.add(index, next);
-
- int curSize = m_orderedValues.size();
-
- // If buffer is at max size, pop element off of end of circular buffer
- // and remove from ordered list
- if (curSize > m_size) {
- m_orderedValues.remove(m_valueBuffer.removeLast());
- curSize = curSize - 1;
- }
-
- // Add next value to circular buffer
- m_valueBuffer.addFirst(next);
-
- if (curSize % 2 == 1) {
- // If size is odd, return middle element of sorted list
- return m_orderedValues.get(curSize / 2);
- } else {
- // If size is even, return average of middle elements
- return (m_orderedValues.get(curSize / 2 - 1) + m_orderedValues.get(curSize / 2)) / 2.0;
- }
- }
-
- /**
- * Resets the filter, clearing the window of all elements.
- */
- public void reset() {
- m_orderedValues.clear();
- m_valueBuffer.clear();
- }
-}
diff --git a/photon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java b/photon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java
index 9ec6ef2bf..ab33a1cc5 100644
--- a/photon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java
+++ b/photon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java
@@ -7,6 +7,7 @@
package edu.wpi.first.wpilibj.geometry;
+import java.util.HashMap;
import java.util.Objects;
/** Represents a transformation for a Pose2d. */
@@ -115,4 +116,12 @@ public class Transform2d {
public int hashCode() {
return Objects.hash(m_translation, m_rotation);
}
+
+ public HashMap toHashMap() {
+ var ret = new HashMap();
+ ret.put("x", getTranslation().getX());
+ ret.put("y", getTranslation().getY());
+ ret.put("rot", getRotation().getDegrees());
+ return ret;
+ }
}
diff --git a/photon-server/src/main/java/org/photonvision/Main.java b/photon-server/src/main/java/org/photonvision/Main.java
index b71fe8320..e8d17244d 100644
--- a/photon-server/src/main/java/org/photonvision/Main.java
+++ b/photon-server/src/main/java/org/photonvision/Main.java
@@ -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();
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();
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);
}
diff --git a/photon-server/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java b/photon-server/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java
index 63b8eaabb..544c70a91 100644
--- a/photon-server/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java
+++ b/photon-server/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java
@@ -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 calibrations;
public List 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 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 calibrations,
@JsonProperty("cameraLEDs") List 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 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);
+ }
}
diff --git a/photon-server/src/main/java/org/photonvision/common/configuration/ConfigManager.java b/photon-server/src/main/java/org/photonvision/common/configuration/ConfigManager.java
index 6b4db3670..76c090498 100644
--- a/photon-server/src/main/java/org/photonvision/common/configuration/ConfigManager.java
+++ b/photon-server/src/main/java/org/photonvision/common/configuration/ConfigManager.java
@@ -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();
+ }
}
diff --git a/photon-server/src/main/java/org/photonvision/common/configuration/HardwareConfig.java b/photon-server/src/main/java/org/photonvision/common/configuration/HardwareConfig.java
index abe00db84..fc4ebc71f 100644
--- a/photon-server/src/main/java/org/photonvision/common/configuration/HardwareConfig.java
+++ b/photon-server/src/main/java/org/photonvision/common/configuration/HardwareConfig.java
@@ -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 hardware,
- @JsonProperty("metrics") Map metrics) {
+ String deviceName,
+ String deviceLogoPath,
+ String supportURL,
+ ArrayList ledPins,
+ String ledSetCommand,
+ boolean ledsCanDim,
+ ArrayList 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) hardware.get("leds");
- this.ledSetCommand = (String) hardware.get("ledSetCommand");
- this.ledsCanDim = (Boolean) hardware.get("ledsCanDim");
- this.ledPWMRange = (ArrayList) 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;
}
}
diff --git a/photon-server/src/main/java/org/photonvision/common/configuration/NetworkConfig.java b/photon-server/src/main/java/org/photonvision/common/configuration/NetworkConfig.java
index 07c2dd96b..a3094e889 100644
--- a/photon-server/src/main/java/org/photonvision/common/configuration/NetworkConfig.java
+++ b/photon-server/src/main/java/org/photonvision/common/configuration/NetworkConfig.java
@@ -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 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 toHashMap() {
HashMap 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;
}
}
diff --git a/photon-server/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java b/photon-server/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java
index da38d23b0..20444008f 100644
--- a/photon-server/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java
+++ b/photon-server/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java
@@ -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 getCameraConfigurations() {
return cameraConfigurations;
}
@@ -54,6 +59,7 @@ public class PhotonConfiguration {
}
private HardwareConfig hardwareConfig;
+
private NetworkConfig networkConfig;
private HashMap cameraConfigurations;
@@ -73,8 +79,9 @@ public class PhotonConfiguration {
public Map toHashMap() {
Map map = new HashMap<>();
+ var settingsSubmap = new HashMap();
- 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();
+ 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> videoFormatList;
public int outputStreamPort;
public int inputStreamPort;
+ public List> calibrations;
+ public boolean isFovConfigurable = true;
}
}
diff --git a/photon-server/src/main/java/org/photonvision/common/dataflow/DataChangeService.java b/photon-server/src/main/java/org/photonvision/common/dataflow/DataChangeService.java
index 65148319e..eadcba327 100644
--- a/photon-server/src/main/java/org/photonvision/common/dataflow/DataChangeService.java
+++ b/photon-server/src/main/java/org/photonvision/common/dataflow/DataChangeService.java
@@ -68,6 +68,7 @@ public class DataChangeService {
}
} catch (Exception e) {
logger.error("Exception when dispatching event!", e);
+ e.printStackTrace();
}
}
}
diff --git a/photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
index 05c256fef..d150be39a 100644
--- a/photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
+++ b/photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
@@ -52,8 +52,6 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
private final Supplier pipelineIndexSupplier;
private final BooleanSupplier driverModeSupplier;
- private String currentCameraNickname;
-
public NTDataPublisher(
String cameraNickname,
Supplier 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);
diff --git a/photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java b/photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java
index 0fe991176..384af5188 100644
--- a/photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java
+++ b/photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java
@@ -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() {}
diff --git a/photon-server/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java b/photon-server/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java
index f41de278c..cd06340d1 100644
--- a/photon-server/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java
+++ b/photon-server/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java
@@ -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>();
var dataMap = new HashMap();
- dataMap.put("fps", fps);
dataMap.put("latency", result.getLatencyMillis());
var targets = result.targets;
diff --git a/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/GPIOBase.java b/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/GPIOBase.java
index 28e519380..ed40701e1 100644
--- a/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/GPIOBase.java
+++ b/photon-server/src/main/java/org/photonvision/common/hardware/GPIO/GPIOBase.java
@@ -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 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);
diff --git a/photon-server/src/main/java/org/photonvision/common/hardware/HardwareManager.java b/photon-server/src/main/java/org/photonvision/common/hardware/HardwareManager.java
index db57715e8..905584a42 100644
--- a/photon-server/src/main/java/org/photonvision/common/hardware/HardwareManager.java
+++ b/photon-server/src/main/java/org/photonvision/common/hardware/HardwareManager.java
@@ -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 LEDs = new HashMap<>();
+ private final HashMap 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;
}
}
diff --git a/photon-server/src/main/java/org/photonvision/common/hardware/Platform.java b/photon-server/src/main/java/org/photonvision/common/hardware/Platform.java
index 76d3baa28..4a4f199be 100644
--- a/photon-server/src/main/java/org/photonvision/common/hardware/Platform.java
+++ b/photon-server/src/main/java/org/photonvision/common/hardware/Platform.java
@@ -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();
}
diff --git a/photon-server/src/main/java/org/photonvision/common/hardware/metrics/CPU.java b/photon-server/src/main/java/org/photonvision/common/hardware/metrics/CPUMetrics.java
similarity index 81%
rename from photon-server/src/main/java/org/photonvision/common/hardware/metrics/CPU.java
rename to photon-server/src/main/java/org/photonvision/common/hardware/metrics/CPUMetrics.java
index 3e8d97c5f..3c69ba961 100644
--- a/photon-server/src/main/java/org/photonvision/common/hardware/metrics/CPU.java
+++ b/photon-server/src/main/java/org/photonvision/common/hardware/metrics/CPUMetrics.java
@@ -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();
- }
}
diff --git a/photon-server/src/main/java/org/photonvision/common/hardware/metrics/GPU.java b/photon-server/src/main/java/org/photonvision/common/hardware/metrics/GPUMetrics.java
similarity index 79%
rename from photon-server/src/main/java/org/photonvision/common/hardware/metrics/GPU.java
rename to photon-server/src/main/java/org/photonvision/common/hardware/metrics/GPUMetrics.java
index e7188ce51..3bff35f23 100644
--- a/photon-server/src/main/java/org/photonvision/common/hardware/metrics/GPU.java
+++ b/photon-server/src/main/java/org/photonvision/common/hardware/metrics/GPUMetrics.java
@@ -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();
- }
}
diff --git a/photon-server/src/main/java/org/photonvision/common/hardware/metrics/MetricsPublisher.java b/photon-server/src/main/java/org/photonvision/common/hardware/metrics/MetricsPublisher.java
index e213e581c..e3ca2654d 100644
--- a/photon-server/src/main/java/org/photonvision/common/hardware/metrics/MetricsPublisher.java
+++ b/photon-server/src/main/java/org/photonvision/common/hardware/metrics/MetricsPublisher.java
@@ -28,18 +28,18 @@ import org.photonvision.server.UIUpdateType;
public class MetricsPublisher {
private final HashMap 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(
diff --git a/photon-server/src/main/java/org/photonvision/common/hardware/metrics/RAM.java b/photon-server/src/main/java/org/photonvision/common/hardware/metrics/RAMMetrics.java
similarity index 78%
rename from photon-server/src/main/java/org/photonvision/common/hardware/metrics/RAM.java
rename to photon-server/src/main/java/org/photonvision/common/hardware/metrics/RAMMetrics.java
index 61c0eb35e..2c3b2d527 100644
--- a/photon-server/src/main/java/org/photonvision/common/hardware/metrics/RAM.java
+++ b/photon-server/src/main/java/org/photonvision/common/hardware/metrics/RAMMetrics.java
@@ -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();
- }
}
diff --git a/photon-server/src/main/java/org/photonvision/common/logging/Logger.java b/photon-server/src/main/java/org/photonvision/common/logging/Logger.java
index 8fbda8ba5..22bb238a3 100644
--- a/photon-server/src/main/java/org/photonvision/common/logging/Logger.java
+++ b/photon-server/src/main/java/org/photonvision/common/logging/Logger.java
@@ -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 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();
}
diff --git a/photon-server/src/main/java/org/photonvision/common/networking/LinuxNetworking.java b/photon-server/src/main/java/org/photonvision/common/networking/LinuxNetworking.java
index 1082f0b79..1b0d90c7a 100644
--- a/photon-server/src/main/java/org/photonvision/common/networking/LinuxNetworking.java
+++ b/photon-server/src/main/java/org/photonvision/common/networking/LinuxNetworking.java
@@ -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) {
diff --git a/photon-server/src/main/java/org/photonvision/common/networking/NetworkInterface.java b/photon-server/src/main/java/org/photonvision/common/networking/NetworkInterface.java
index f3e58177d..2f1f3a35f 100644
--- a/photon-server/src/main/java/org/photonvision/common/networking/NetworkInterface.java
+++ b/photon-server/src/main/java/org/photonvision/common/networking/NetworkInterface.java
@@ -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) {
diff --git a/photon-server/src/main/java/org/photonvision/common/networking/NetworkManager.java b/photon-server/src/main/java/org/photonvision/common/networking/NetworkManager.java
index 5de5dd3ef..fa8e70bd0 100644
--- a/photon-server/src/main/java/org/photonvision/common/networking/NetworkManager.java
+++ b/photon-server/src/main/java/org/photonvision/common/networking/NetworkManager.java
@@ -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);
}
}
diff --git a/photon-server/src/main/java/org/photonvision/common/networking/SysNetworking.java b/photon-server/src/main/java/org/photonvision/common/networking/SysNetworking.java
index d4e8cf51c..15ae8a5e4 100644
--- a/photon-server/src/main/java/org/photonvision/common/networking/SysNetworking.java
+++ b/photon-server/src/main/java/org/photonvision/common/networking/SysNetworking.java
@@ -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 getNetworkInterfaces() throws SocketException;
}
diff --git a/photon-server/src/main/java/org/photonvision/common/scripting/ScriptManager.java b/photon-server/src/main/java/org/photonvision/common/scripting/ScriptManager.java
index 270a87800..34a0cd420 100644
--- a/photon-server/src/main/java/org/photonvision/common/scripting/ScriptManager.java
+++ b/photon-server/src/main/java/org/photonvision/common/scripting/ScriptManager.java
@@ -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);
}
diff --git a/photon-server/src/main/java/org/photonvision/common/util/file/FileUtils.java b/photon-server/src/main/java/org/photonvision/common/util/file/FileUtils.java
index 3f1ef7c66..69e373f7f 100644
--- a/photon-server/src/main/java/org/photonvision/common/util/file/FileUtils.java
+++ b/photon-server/src/main/java/org/photonvision/common/util/file/FileUtils.java
@@ -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 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();
diff --git a/photon-server/src/main/java/org/photonvision/common/util/file/JacksonUtils.java b/photon-server/src/main/java/org/photonvision/common/util/file/JacksonUtils.java
index 96480a538..9b49a0002 100644
--- a/photon-server/src/main/java/org/photonvision/common/util/file/JacksonUtils.java
+++ b/photon-server/src/main/java/org/photonvision/common/util/file/JacksonUtils.java
@@ -32,11 +32,11 @@ import java.io.IOException;
import java.nio.file.Path;
public class JacksonUtils {
- public static void serializer(Path path, T object) throws IOException {
- serializer(path, object, false);
+ public static void serialize(Path path, T object) throws IOException {
+ serialize(path, object, false);
}
- public static void serializer(Path path, T object, boolean forceSync) throws IOException {
+ public static 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) {
diff --git a/photon-server/src/main/java/org/photonvision/server/RequestHandler.java b/photon-server/src/main/java/org/photonvision/server/RequestHandler.java
index 7715d82d7..af83461eb 100644
--- a/photon-server/src/main/java/org/photonvision/server/RequestHandler.java
+++ b/photon-server/src/main/java/org/photonvision/server/RequestHandler.java
@@ -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 map =
+ (Map) kObjectMapper.readValue(context.body(), Map.class);
+ var networking =
+ (Map)
+ map.get("networkSettings"); // teamNumber (int), supported (bool), connectionType (int),
+ // staticIp (str), netmask (str), gateway (str), hostname (str)
+ var lighting =
+ (Map) 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) 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);
}
}
diff --git a/photon-server/src/main/java/org/photonvision/server/Server.java b/photon-server/src/main/java/org/photonvision/server/Server.java
index 753718aeb..b664a74e6 100644
--- a/photon-server/src/main/java/org/photonvision/server/Server.java
+++ b/photon-server/src/main/java/org/photonvision/server/Server.java
@@ -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);
}
}
diff --git a/photon-server/src/main/java/org/photonvision/server/SocketHandler.java b/photon-server/src/main/java/org/photonvision/server/SocketHandler.java
index ba2535b57..fa02fe8b4 100644
--- a/photon-server/src/main/java/org/photonvision/server/SocketHandler.java
+++ b/photon-server/src/main/java/org/photonvision/server/SocketHandler.java
@@ -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);
diff --git a/photon-server/src/main/java/org/photonvision/server/SocketMessageType.java b/photon-server/src/main/java/org/photonvision/server/SocketMessageType.java
index 05d9c9aa8..748fc5ec1 100644
--- a/photon-server/src/main/java/org/photonvision/server/SocketMessageType.java
+++ b/photon-server/src/main/java/org/photonvision/server/SocketMessageType.java
@@ -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;
diff --git a/photon-server/src/main/java/org/photonvision/vision/camera/FileVisionSource.java b/photon-server/src/main/java/org/photonvision/vision/camera/FileVisionSource.java
index f2da07b00..ca8f02994 100644
--- a/photon-server/src/main/java/org/photonvision/vision/camera/FileVisionSource.java
+++ b/photon-server/src/main/java/org/photonvision/vision/camera/FileVisionSource.java
@@ -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 getAllVideoModes() {
diff --git a/photon-server/src/main/java/org/photonvision/vision/camera/USBCameraSource.java b/photon-server/src/main/java/org/photonvision/vision/camera/USBCameraSource.java
index d55468979..aba98ab38 100644
--- a/photon-server/src/main/java/org/photonvision/vision/camera/USBCameraSource.java
+++ b/photon-server/src/main/java/org/photonvision/vision/camera/USBCameraSource.java
@@ -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);
}
diff --git a/photon-server/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java b/photon-server/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java
index 3e8000368..e41f8d214 100644
--- a/photon-server/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java
+++ b/photon-server/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java
@@ -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;
diff --git a/photon-server/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java b/photon-server/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java
index 5af806d04..9bc12b8e3 100644
--- a/photon-server/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java
+++ b/photon-server/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java
@@ -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);
diff --git a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/CalculateFPSPipe.java b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/CalculateFPSPipe.java
new file mode 100644
index 000000000..9bbeecb93
--- /dev/null
+++ b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/CalculateFPSPipe.java
@@ -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 .
+ */
+
+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 {
+
+ 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 {}
+}
diff --git a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java
index 35d6e1432..4405a136d 100644
--- a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java
+++ b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java
@@ -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>, CameraCalibrationCoefficients, Calibrate3dPipe.CalibratePipeParams> {
+ List>,
+ 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> in) {
+ protected CameraCalibrationCoefficients process(List> 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;
}
}
diff --git a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/CornerDetectionPipe.java b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/CornerDetectionPipe.java
index 2a528ae27..52327ba04 100644
--- a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/CornerDetectionPipe.java
+++ b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/CornerDetectionPipe.java
@@ -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);
}
diff --git a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw2dTargetsPipe.java b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw2dTargetsPipe.java
index a427f6452..9bd306a35 100644
--- a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw2dTargetsPipe.java
+++ b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw2dTargetsPipe.java
@@ -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>, Draw2dTargetsPipe.Draw2dContoursParams> {
+ extends MutatingPipe<
+ Triple, Integer>, Draw2dTargetsPipe.Draw2dContoursParams> {
private List m_drawnContours = new ArrayList<>();
@Override
- protected Void process(Pair> in) {
- if (!in.getRight().isEmpty()
+ protected Void process(Triple, 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;
diff --git a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java
index 18a304fd8..628f786e5 100644
--- a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java
+++ b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java
@@ -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>, FindBoardCornersPipe.FindCornersPipeParams> {
+ extends CVPipe, FindBoardCornersPipe.FindCornersPipeParams> {
MatOfPoint3f objectPoints = new MatOfPoint3f();
- private final List listOfObjectPoints = new ArrayList<>();
- private final List 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> process(List 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 outputMats = new ArrayList<>();
+ protected Triple 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 findBoardCorners(Mat frame) {
+ private Triple 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
}
}
diff --git a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java
index e63e0dbde..2611f0878 100644
--- a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java
+++ b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java
@@ -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));
}
/**
diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/CVPipelineSettings.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/CVPipelineSettings.java
index 1b0339543..e59407023 100644
--- a/photon-server/src/main/java/org/photonvision/vision/pipeline/CVPipelineSettings.java
+++ b/photon-server/src/main/java/org/photonvision/vision/pipeline/CVPipelineSettings.java
@@ -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;
+ }
+ }
}
diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/Calibration3dPipeline.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java
similarity index 52%
rename from photon-server/src/main/java/org/photonvision/vision/pipeline/Calibration3dPipeline.java
rename to photon-server/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java
index f2513e553..40adfbb01 100644
--- a/photon-server/src/main/java/org/photonvision/vision/pipeline/Calibration3dPipeline.java
+++ b/photon-server/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java
@@ -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 {
// 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 boardSnapshots;
-
// Output of the corners
- private CVPipeResult>> findCornersPipeOutput;
+ final List> foundCornersList;
/// Output of the calibration, getter method is set for this.
private CVPipeResult 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);
diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/Calibration3dPipelineSettings.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/Calibration3dPipelineSettings.java
index 7feace93f..546dd6f3b 100644
--- a/photon-server/src/main/java/org/photonvision/vision/pipeline/Calibration3dPipelineSettings.java
+++ b/photon-server/src/main/java/org/photonvision/vision/pipeline/Calibration3dPipelineSettings.java
@@ -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);
}
diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java
index 073795ab5..f85ff9800 100644
--- a/photon-server/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java
+++ b/photon-server/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java
@@ -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) {
diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/PipelineType.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/PipelineType.java
index 4e741fd1e..b59c15cca 100644
--- a/photon-server/src/main/java/org/photonvision/vision/pipeline/PipelineType.java
+++ b/photon-server/src/main/java/org/photonvision/vision/pipeline/PipelineType.java
@@ -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);
diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java
index 7b3f06325..dc7474c9c 100644
--- a/photon-server/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java
+++ b/photon-server/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java
@@ -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.
+ */
+
+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 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
+ + '}';
+ }
+}
diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java
index 2312726ca..2ec0997ac 100644
--- a/photon-server/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java
+++ b/photon-server/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java
@@ -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 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;
diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/result/SimpleTrackedTarget.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/result/SimpleTrackedTarget.java
index bad4a1372..de4585f4d 100644
--- a/photon-server/src/main/java/org/photonvision/vision/pipeline/result/SimpleTrackedTarget.java
+++ b/photon-server/src/main/java/org/photonvision/vision/pipeline/result/SimpleTrackedTarget.java
@@ -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;
}
diff --git a/photon-server/src/main/java/org/photonvision/vision/processes/PipelineManager.java b/photon-server/src/main/java/org/photonvision/vision/processes/PipelineManager.java
index 950db0ac9..d820a1ca2 100644
--- a/photon-server/src/main/java/org/photonvision/vision/processes/PipelineManager.java
+++ b/photon-server/src/main/java/org/photonvision/vision/processes/PipelineManager.java
@@ -32,14 +32,14 @@ public class PipelineManager {
public static final int CAL_3D_INDEX = -2;
protected final List 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.
@@ -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.
+ *
+ * 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 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 )";
+ }
}
}
diff --git a/photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java
index 495454efe..2abfab960 100644
--- a/photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java
+++ b/photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java
@@ -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) 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>();
+ for (var c : visionSource.getSettables().getConfiguration().calibrations) {
+ var internalMap = new HashMap();
+
+ 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;
}
diff --git a/photon-server/src/main/java/org/photonvision/vision/processes/VisionModuleManager.java b/photon-server/src/main/java/org/photonvision/vision/processes/VisionModuleManager.java
index 1950984b6..52f21ec38 100644
--- a/photon-server/src/main/java/org/photonvision/vision/processes/VisionModuleManager.java
+++ b/photon-server/src/main/java/org/photonvision/vision/processes/VisionModuleManager.java
@@ -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);
}
diff --git a/photon-server/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java b/photon-server/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java
index 3f03fc65e..7181a11b8 100644
--- a/photon-server/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java
+++ b/photon-server/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java
@@ -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 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() {
diff --git a/photon-server/src/main/java/org/photonvision/vision/target/TargetModel.java b/photon-server/src/main/java/org/photonvision/vision/target/TargetModel.java
index 44202d0d6..2d87ec9aa 100644
--- a/photon-server/src/main/java/org/photonvision/vision/target/TargetModel.java
+++ b/photon-server/src/main/java/org/photonvision/vision/target/TargetModel.java
@@ -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) {
diff --git a/photon-server/src/main/java/org/photonvision/vision/target/TrackedTarget.java b/photon-server/src/main/java/org/photonvision/vision/target/TrackedTarget.java
index 56235113e..99c0aa4e5 100644
--- a/photon-server/src/main/java/org/photonvision/vision/target/TrackedTarget.java
+++ b/photon-server/src/main/java/org/photonvision/vision/target/TrackedTarget.java
@@ -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;
}
diff --git a/photon-server/src/main/resources/web/index.html b/photon-server/src/main/resources/web/index.html
index 988f55e6a..f008dfd12 100644
--- a/photon-server/src/main/resources/web/index.html
+++ b/photon-server/src/main/resources/web/index.html
@@ -1 +1 @@
-UI has not been copied!
+UI has not been copied!
\ No newline at end of file
diff --git a/photon-server/src/test/java/org/photonvision/common/configuration/ConfigTest.java b/photon-server/src/test/java/org/photonvision/common/configuration/ConfigTest.java
index 6b5d1a54e..2e135fdb1 100644
--- a/photon-server/src/test/java/org/photonvision/common/configuration/ConfigTest.java
+++ b/photon-server/src/test/java/org/photonvision/common/configuration/ConfigTest.java
@@ -28,31 +28,31 @@ import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.LogLevel;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.TestUtils;
-import org.photonvision.common.util.file.JacksonUtils;
import org.photonvision.vision.pipeline.ColoredShapePipelineSettings;
import org.photonvision.vision.pipeline.ReflectivePipelineSettings;
import org.photonvision.vision.target.TargetModel;
public class ConfigTest {
- private static final ConfigManager configMgr;
- private static final CameraConfiguration cameraConfig =
- new CameraConfiguration("TestCamera", "/dev/video420");
- private static final ReflectivePipelineSettings REFLECTIVE_PIPELINE_SETTINGS =
- new ReflectivePipelineSettings();
- private static final ColoredShapePipelineSettings COLORED_SHAPE_PIPELINE_SETTINGS =
- new ColoredShapePipelineSettings();
-
static {
TestUtils.loadLibraries();
- configMgr = new ConfigManager(Path.of("testconfigdir"));
}
+ private static ConfigManager configMgr;
+ private static final CameraConfiguration cameraConfig =
+ new CameraConfiguration("TestCamera", "/dev/video420");
+ private static ReflectivePipelineSettings REFLECTIVE_PIPELINE_SETTINGS;
+ private static ColoredShapePipelineSettings COLORED_SHAPE_PIPELINE_SETTINGS;
+
@BeforeAll
public static void init() {
TestUtils.loadLibraries();
+ configMgr = new ConfigManager(Path.of("testconfigdir"));
Logger.setLevel(LogGroup.General, LogLevel.TRACE);
+ REFLECTIVE_PIPELINE_SETTINGS = new ReflectivePipelineSettings();
+ COLORED_SHAPE_PIPELINE_SETTINGS = new ColoredShapePipelineSettings();
+
REFLECTIVE_PIPELINE_SETTINGS.pipelineNickname = "2019Tape";
REFLECTIVE_PIPELINE_SETTINGS.targetModel = TargetModel.get2019Target();
@@ -67,7 +67,6 @@ public class ConfigTest {
@Order(1)
public void serializeConfig() throws IOException {
TestUtils.loadLibraries();
- JacksonUtils.serializer(Path.of("settings.json"), REFLECTIVE_PIPELINE_SETTINGS);
Logger.setLevel(LogGroup.General, LogLevel.TRACE);
configMgr.getConfig().addCameraConfig(cameraConfig);
@@ -75,16 +74,16 @@ public class ConfigTest {
var camConfDir =
new File(
- Path.of(configMgr.rootFolder.toString(), "cameras", "TestCamera")
+ Path.of(configMgr.configDirectoryFile.toString(), "cameras", "TestCamera")
.toAbsolutePath()
.toString());
Assertions.assertTrue(camConfDir.exists(), "TestCamera config folder not found!");
Assertions.assertTrue(
- Files.exists(Path.of(configMgr.rootFolder.toString(), "hardwareConfig.json")),
+ Files.exists(Path.of(configMgr.configDirectoryFile.toString(), "hardwareConfig.json")),
"hardwareConfig.json file not found!");
Assertions.assertTrue(
- Files.exists(Path.of(configMgr.rootFolder.toString(), "networkSettings.json")),
+ Files.exists(Path.of(configMgr.configDirectoryFile.toString(), "networkSettings.json")),
"networkSettings.json file not found!");
}
@@ -116,7 +115,7 @@ public class ConfigTest {
e.printStackTrace();
}
- FileUtils.cleanDirectory(configMgr.rootFolder);
- configMgr.rootFolder.delete();
+ FileUtils.cleanDirectory(configMgr.configDirectoryFile);
+ configMgr.configDirectoryFile.delete();
}
}
diff --git a/photon-server/src/test/java/org/photonvision/hardware/HardwareManagerTest.java b/photon-server/src/test/java/org/photonvision/hardware/HardwareManagerTest.java
index 63d1f0184..8c6b81765 100644
--- a/photon-server/src/test/java/org/photonvision/hardware/HardwareManagerTest.java
+++ b/photon-server/src/test/java/org/photonvision/hardware/HardwareManagerTest.java
@@ -29,7 +29,7 @@ import org.photonvision.common.util.TestUtils;
public class HardwareManagerTest {
@Test
- public void ManagementTest() throws IOException {
+ public void managementTest() throws IOException {
var config =
new ObjectMapper().readValue(TestUtils.getHardwareConfigJson(), HardwareConfig.class);
diff --git a/photon-server/src/test/java/org/photonvision/hardware/HardwareTest.java b/photon-server/src/test/java/org/photonvision/hardware/HardwareTest.java
index 1b29a6880..f502f2ce0 100644
--- a/photon-server/src/test/java/org/photonvision/hardware/HardwareTest.java
+++ b/photon-server/src/test/java/org/photonvision/hardware/HardwareTest.java
@@ -24,33 +24,33 @@ import org.photonvision.common.hardware.GPIO.CustomGPIO;
import org.photonvision.common.hardware.GPIO.GPIOBase;
import org.photonvision.common.hardware.GPIO.PiGPIO;
import org.photonvision.common.hardware.Platform;
-import org.photonvision.common.hardware.metrics.CPU;
-import org.photonvision.common.hardware.metrics.GPU;
-import org.photonvision.common.hardware.metrics.RAM;
+import org.photonvision.common.hardware.metrics.CPUMetrics;
+import org.photonvision.common.hardware.metrics.GPUMetrics;
+import org.photonvision.common.hardware.metrics.RAMMetrics;
public class HardwareTest {
@Test
public void testHardware() {
- CPU cpu = CPU.getInstance();
- RAM ram = RAM.getInstance();
- GPU gpu = GPU.getInstance();
+ CPUMetrics cpuMetrics = new CPUMetrics();
+ RAMMetrics ramMetrics = new RAMMetrics();
+ GPUMetrics gpuMetrics = new GPUMetrics();
if (!Platform.isRaspberryPi()) return;
System.out.println("Testing on platform: " + Platform.CurrentPlatform);
System.out.println("Printing CPU Info:");
- System.out.println("Memory: " + cpu.getMemory() + "MB");
- System.out.println("Temperature: " + cpu.getTemp() + "C");
- System.out.println("Utilization: : " + cpu.getUtilization() + "%");
+ System.out.println("Memory: " + cpuMetrics.getMemory() + "MB");
+ System.out.println("Temperature: " + cpuMetrics.getTemp() + "C");
+ System.out.println("Utilization: : " + cpuMetrics.getUtilization() + "%");
System.out.println("Printing GPU Info:");
- System.out.println("Memory: " + gpu.getMemory() + "MB");
- System.out.println("Temperature: " + gpu.getTemp() + "C");
+ System.out.println("Memory: " + gpuMetrics.getMemory() + "MB");
+ System.out.println("Temperature: " + gpuMetrics.getTemp() + "C");
System.out.println("Printing RAM Info: ");
- System.out.println("Used RAM: : " + ram.getUsedRam() + "MB");
+ System.out.println("Used RAM: : " + ramMetrics.getUsedRam() + "MB");
}
@Test
diff --git a/photon-server/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java b/photon-server/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java
index 59111daa2..9543f8400 100644
--- a/photon-server/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java
+++ b/photon-server/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java
@@ -17,12 +17,15 @@
package org.photonvision.vision.pipeline;
+import static org.junit.jupiter.api.Assertions.assertNotNull;
import static org.junit.jupiter.api.Assertions.assertTrue;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
import java.io.File;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
+import org.apache.commons.lang3.tuple.Triple;
import org.junit.jupiter.api.BeforeEach;
import org.junit.jupiter.api.Test;
import org.opencv.core.Mat;
@@ -53,13 +56,19 @@ public class Calibrate3dPipeTest {
FindBoardCornersPipe findBoardCornersPipe = new FindBoardCornersPipe();
findBoardCornersPipe.setParams(
- new FindBoardCornersPipe.FindCornersPipeParams(11, 4, false, 15));
- var findBoardCornersPipeOutput = findBoardCornersPipe.run(frames);
+ new FindBoardCornersPipe.FindCornersPipeParams(
+ 11, 4, UICalibrationData.BoardType.DOTBOARD, 15));
+
+ List> foundCornersList = new ArrayList<>();
+
+ for (var f : frames) {
+ foundCornersList.add(findBoardCornersPipe.run(f).output);
+ }
Calibrate3dPipe calibrate3dPipe = new Calibrate3dPipe();
calibrate3dPipe.setParams(new Calibrate3dPipe.CalibratePipeParams(new Size(640, 480)));
- var calibrate3dPipeOutput = calibrate3dPipe.run(findBoardCornersPipeOutput.output);
+ var calibrate3dPipeOutput = calibrate3dPipe.run(foundCornersList);
assertTrue(calibrate3dPipeOutput.output.perViewErrors.length > 0);
System.out.println(
"Per View Errors: " + Arrays.toString(calibrate3dPipeOutput.output.perViewErrors));
@@ -71,10 +80,10 @@ public class Calibrate3dPipeTest {
File dir = new File(TestUtils.getDotBoardImagesPath().toAbsolutePath().toString());
File[] directoryListing = dir.listFiles();
- Calibration3dPipeline calibration3dPipeline = new Calibration3dPipeline();
+ Calibrate3dPipeline calibration3dPipeline = new Calibrate3dPipeline(20);
calibration3dPipeline.getSettings().boardHeight = 11;
calibration3dPipeline.getSettings().boardWidth = 4;
- calibration3dPipeline.getSettings().isUsingChessboard = false;
+ calibration3dPipeline.getSettings().boardType = UICalibrationData.BoardType.DOTBOARD;
calibration3dPipeline.getSettings().gridSize = 15;
calibration3dPipeline.getSettings().resolution = new Size(640, 480);
@@ -84,28 +93,35 @@ public class Calibrate3dPipeTest {
calibration3dPipeline.run(
new Frame(
new CVMat(Imgcodecs.imread(file.getAbsolutePath())),
- new FrameStaticProperties(640, 480, 60)));
- TestUtils.showImage(output.outputFrame.image.getMat());
+ new FrameStaticProperties(640, 480, 60, new Rotation2d(), null)));
+ // TestUtils.showImage(output.outputFrame.image.getMat());
}
+ assertTrue(
+ calibration3dPipeline.foundCornersList.stream()
+ .map(Triple::getRight)
+ .allMatch(it -> it.width() > 0 && it.height() > 0));
+
calibration3dPipeline.removeSnapshot(0);
- calibration3dPipeline.startCalibration();
calibration3dPipeline.run(
new Frame(
new CVMat(Imgcodecs.imread(directoryListing[0].getAbsolutePath())),
- new FrameStaticProperties(640, 480, 60)));
+ new FrameStaticProperties(640, 480, 60, new Rotation2d(), null)));
+
+ assertTrue(
+ calibration3dPipeline.foundCornersList.stream()
+ .map(Triple::getRight)
+ .allMatch(it -> it.width() > 0 && it.height() > 0));
+
+ var cal = calibration3dPipeline.tryCalibration();
calibration3dPipeline.finishCalibration();
- System.out.println(
- "Per View Errors: " + Arrays.toString(calibration3dPipeline.perViewErrors()));
- System.out.println(
- "Camera Intrinsics : "
- + calibration3dPipeline.cameraCalibrationCoefficients().cameraIntrinsics.toString());
- System.out.println(
- "Camera Extrinsics : "
- + calibration3dPipeline.cameraCalibrationCoefficients().cameraExtrinsics.toString());
- System.out.println(
- "Standard Deviation: "
- + calibration3dPipeline.cameraCalibrationCoefficients().standardDeviation);
+
+ assertNotNull(cal);
+ assertNotNull(cal.perViewErrors);
+ System.out.println("Per View Errors: " + Arrays.toString(cal.perViewErrors));
+ System.out.println("Camera Intrinsics : " + cal.cameraIntrinsics.toString());
+ System.out.println("Camera Extrinsics : " + cal.cameraExtrinsics.toString());
+ System.out.println("Standard Deviation: " + cal.standardDeviation);
System.out.println(
"Mean: " + Arrays.stream(calibration3dPipeline.perViewErrors()).average().toString());
}
diff --git a/photon-server/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java b/photon-server/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java
index 8ae2aded9..317ec3c1b 100644
--- a/photon-server/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java
+++ b/photon-server/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java
@@ -57,7 +57,7 @@ public class CirclePNPTest {
}
private CameraCalibrationCoefficients getCoeffs(String filename) {
- var cameraCalibration = TestUtils.getCoeffs(filename, false);
+ var cameraCalibration = TestUtils.getCoeffs(filename, true);
checkCameraCoefficients(cameraCalibration);
return cameraCalibration;
}
@@ -162,7 +162,7 @@ public class CirclePNPTest {
System.out.println(
"Found targets at "
+ pipelineResult.targets.stream()
- .map(TrackedTarget::getRobotRelativePose)
+ .map(TrackedTarget::getCameraToTarget)
.collect(Collectors.toList()));
}
}
diff --git a/photon-server/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java b/photon-server/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java
index e06516e20..22551f60f 100644
--- a/photon-server/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java
+++ b/photon-server/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java
@@ -21,10 +21,12 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertNotNull;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.util.Units;
import java.util.stream.Collectors;
import org.junit.jupiter.api.Assertions;
import org.junit.jupiter.api.BeforeEach;
import org.junit.jupiter.api.Test;
+import org.opencv.imgcodecs.Imgcodecs;
import org.photonvision.common.util.TestUtils;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.frame.Frame;
@@ -58,7 +60,7 @@ public class SolvePNPTest {
}
private CameraCalibrationCoefficients getCoeffs(String filename) {
- var cameraCalibration = TestUtils.getCoeffs(filename, false);
+ var cameraCalibration = TestUtils.getCoeffs(filename, true);
checkCameraCoefficients(cameraCalibration);
return cameraCalibration;
}
@@ -97,12 +99,13 @@ public class SolvePNPTest {
pipeline.getSettings().contourIntersection = ContourIntersectionDirection.Up;
pipeline.getSettings().cornerDetectionUseConvexHulls = true;
pipeline.getSettings().targetModel = TargetModel.get2019Target();
- pipeline.getSettings().cameraCalibration = getCoeffs(LIFECAM_240P_CAL_FILE);
var frameProvider =
new FileFrameProvider(
TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark48in, false),
- TestUtils.WPI2019Image.FOV);
+ TestUtils.WPI2019Image.FOV,
+ new Rotation2d(),
+ TestUtils.get2019LifeCamCoeffs(true));
CVPipelineResult pipelineResult;
@@ -110,12 +113,13 @@ public class SolvePNPTest {
printTestResults(pipelineResult);
// these numbers are not *accurate*, but they are known and expected
- var pose = pipelineResult.targets.get(0).getRobotRelativePose();
- Assertions.assertEquals(41.96, pose.getTranslation().getX(), 0.05);
- Assertions.assertEquals(-1.03, pose.getTranslation().getY(), 0.05);
- Assertions.assertEquals(1.46, pose.getRotation().getDegrees(), 0.05);
+ var pose = pipelineResult.targets.get(0).getCameraToTarget();
+ Assertions.assertEquals(1.1, pose.getTranslation().getX(), 0.05);
+ Assertions.assertEquals(0.0, pose.getTranslation().getY(), 0.05);
+ Assertions.assertEquals(1, pose.getRotation().getDegrees(), 1);
- TestUtils.showImage(pipelineResult.outputFrame.image.getMat(), "Pipeline output", 1000 * 90);
+ Imgcodecs.imwrite("D:\\out.jpg", pipelineResult.outputFrame.image.getMat());
+ TestUtils.showImage(pipelineResult.outputFrame.image.getMat(), "Pipeline output", 999999);
}
@Test
@@ -129,23 +133,23 @@ public class SolvePNPTest {
pipeline.getSettings().solvePNPEnabled = true;
pipeline.getSettings().cornerDetectionAccuracyPercentage = 4;
pipeline.getSettings().cornerDetectionUseConvexHulls = true;
- pipeline.getSettings().cameraCalibration = getCoeffs(LIFECAM_480P_CAL_FILE);
- pipeline.getSettings().targetModel = TargetModel.get2020Target(36);
- pipeline.getSettings().cameraPitch = Rotation2d.fromDegrees(0.0);
+ pipeline.getSettings().targetModel = TargetModel.get2020Target();
var frameProvider =
new FileFrameProvider(
TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_224in_Left, false),
- TestUtils.WPI2020Image.FOV);
+ TestUtils.WPI2020Image.FOV,
+ new Rotation2d(),
+ TestUtils.get2020LifeCamCoeffs(true));
CVPipelineResult pipelineResult = pipeline.run(frameProvider.get());
printTestResults(pipelineResult);
// these numbers are not *accurate*, but they are known and expected
- var pose = pipelineResult.targets.get(0).getRobotRelativePose();
- Assertions.assertEquals(260.26, pose.getTranslation().getX(), 0.05);
- Assertions.assertEquals(64.26, pose.getTranslation().getY(), 0.05);
- Assertions.assertEquals(36.88, pose.getRotation().getDegrees(), 0.05);
+ var pose = pipelineResult.targets.get(0).getCameraToTarget();
+ Assertions.assertEquals(Units.inchesToMeters(240.26), pose.getTranslation().getX(), 0.05);
+ Assertions.assertEquals(Units.inchesToMeters(35), pose.getTranslation().getY(), 0.05);
+ Assertions.assertEquals(42, pose.getRotation().getDegrees(), 1);
TestUtils.showImage(pipelineResult.outputFrame.image.getMat(), "Pipeline output", 999999);
}
@@ -193,7 +197,7 @@ public class SolvePNPTest {
System.out.println(
"Found targets at "
+ pipelineResult.targets.stream()
- .map(TrackedTarget::getRobotRelativePose)
+ .map(TrackedTarget::getCameraToTarget)
.collect(Collectors.toList()));
}
}
diff --git a/photon-server/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java b/photon-server/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java
index e45ab1b6b..98a6ea7b8 100644
--- a/photon-server/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java
+++ b/photon-server/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java
@@ -18,6 +18,7 @@
package org.photonvision.vision.processes;
import edu.wpi.cscore.VideoMode;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
import java.util.HashMap;
import java.util.List;
import org.junit.jupiter.api.*;
@@ -78,8 +79,9 @@ public class VisionModuleManagerTest {
}
@Override
- public void setCurrentVideoMode(VideoMode videoMode) {
- this.frameStaticProperties = new FrameStaticProperties(getCurrentVideoMode(), getFOV());
+ public void setVideoModeInternal(VideoMode videoMode) {
+ this.frameStaticProperties =
+ new FrameStaticProperties(getCurrentVideoMode(), getFOV(), new Rotation2d(), null);
}
@Override
diff --git a/photon-server/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java b/photon-server/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java
index 809aba903..c79b1d4d5 100644
--- a/photon-server/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java
+++ b/photon-server/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java
@@ -19,6 +19,7 @@ package org.photonvision.vision.target;
import static org.junit.jupiter.api.Assertions.assertEquals;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
import org.apache.commons.math3.util.FastMath;
import org.junit.jupiter.api.BeforeEach;
import org.junit.jupiter.api.Test;
@@ -35,7 +36,8 @@ public class TargetCalculationsTest {
private static final double diagFOV = Math.toRadians(70.0);
private static final FrameStaticProperties props =
- new FrameStaticProperties((int) imageSize.width, (int) imageSize.height, diagFOV);
+ new FrameStaticProperties(
+ (int) imageSize.width, (int) imageSize.height, diagFOV, new Rotation2d(), null);
private static final TrackedTarget.TargetCalculationParameters params =
new TrackedTarget.TargetCalculationParameters(
true,
diff --git a/photon-server/src/test/resources/calibration/lifecam240p.json b/photon-server/src/test/resources/calibration/lifecam240p.json
deleted file mode 100644
index 349c450c8..000000000
--- a/photon-server/src/test/resources/calibration/lifecam240p.json
+++ /dev/null
@@ -1,34 +0,0 @@
-{
- "resolution": {
- "width": 320.0,
- "height": 240.0
- },
- "cameraIntrinsics": {
- "rows": 3,
- "cols": 3,
- "type": 6,
- "data": [
- 353.74653217742724,
- 0.0,
- 163.55407989211918,
- 0.0,
- 340.77624878700817,
- 119.8945718300403,
- 0.0,
- 0.0,
- 1.0
- ]
- },
- "cameraExtrinsics": {
- "rows": 1,
- "cols": 5,
- "type": 6,
- "data": [
- 0.10322037759535845,
- -0.2890556437050186,
- 0.00406400648501475,
- 2.5573586808275763E-4,
- -1.462385758978924
- ]
- }
-}
\ No newline at end of file
diff --git a/photon-server/src/test/resources/calibration/lifecam480p.json b/photon-server/src/test/resources/calibration/lifecam480p.json
deleted file mode 100644
index fff629bf6..000000000
--- a/photon-server/src/test/resources/calibration/lifecam480p.json
+++ /dev/null
@@ -1,34 +0,0 @@
-{
- "resolution": {
- "width": 640.0,
- "height": 480.0
- },
- "cameraIntrinsics": {
- "rows": 3,
- "cols": 3,
- "type": 6,
- "data": [
- 699.3778103158814,
- 0.0,
- 345.6059345433618,
- 0.0,
- 677.7161226393544,
- 207.12741326228522,
- 0.0,
- 0.0,
- 1.0
- ]
- },
- "cameraExtrinsics": {
- "rows": 1,
- "cols": 5,
- "type": 6,
- "data": [
- 0.14382207979312617,
- -0.9851192814987014,
- -0.018168751047242335,
- 0.011034504043795105,
- 1.9833437176538498
- ]
- }
-}
\ No newline at end of file
diff --git a/photon-server/src/test/resources/hardware/HardwareConfig.json b/photon-server/src/test/resources/hardware/HardwareConfig.json
index 70ee11600..90dd50739 100644
--- a/photon-server/src/test/resources/hardware/HardwareConfig.json
+++ b/photon-server/src/test/resources/hardware/HardwareConfig.json
@@ -2,23 +2,20 @@
"deviceName": "PhotonVision",
"deviceLogoPath": "photonvision.png",
"supportURL": "https://support.photonvision.com",
- "hardware": {
- "leds": [2,13],
- "ledSetCommand": "",
- "ledsCanDim": true,
- "ledPWMRange": [0, 100],
- "ledPWMFrequency" : 800,
- "ledPWMSetRange": "",
- "ledDimCommand": "",
- "ledBlinkCommand": ""
- },
-
- "metrics": {
- "cpuTemp": "echo 10",
- "cpuMemory": "echo 10",
- "cpuUtil": "echo 10",
- "gpuMemory": "echo 10",
- "gpuUtil": "echo 10",
- "ramUtil": "echo 10"
- }
-}
+ "ledPins" : [2, 13],
+ "ledSetCommand" : "",
+ "ledsCanDim" : true,
+ "ledPWMRange" : [0, 100],
+ "ledPWMSetRange" : "",
+ "ledPWMFrequency" : 800,
+ "ledDimCommand" : "echo 10",
+ "ledBlinkCommand" : "echo 10",
+ "cpuTempCommand" : "echo 10",
+ "cpuMemoryCommand" : "echo 10",
+ "cpuUtilCommand" : "echo 10",
+ "gpuMemoryCommand" : "echo 10",
+ "gpuTempCommand" : "echo 10",
+ "ramUtilCommand" : "echo 10",
+ "restartHardwareCommand" : "echo 10",
+ "vendorFOV" : 40.0
+}
\ No newline at end of file
diff --git a/photon-server/versioningHelper.gradle b/photon-server/versioningHelper.gradle
index 2cebdebae..8ce7ae47e 100644
--- a/photon-server/versioningHelper.gradle
+++ b/photon-server/versioningHelper.gradle
@@ -32,7 +32,7 @@ task writeCurrentVersionJava {
"public final class PhotonVersion {\n" +
" public static final String versionString = \"${versionString}\";\n" +
" public static final String buildDate = \"${date}\";\n" +
- " public static final boolean isRelease = versionString.startsWith(\"dev\");\n" +
+ " public static final boolean isRelease = !versionString.startsWith(\"dev\");\n" +
"}"
}