mirror of
https://github.com/PhotonVision/photonvision
synced 2026-07-04 03:11:40 +00:00
@@ -1,6 +1,6 @@
|
|||||||
plugins {
|
plugins {
|
||||||
id "com.diffplug.spotless" version "6.0.5"
|
id "com.diffplug.spotless" version "6.1.2"
|
||||||
id "com.github.johnrengelman.shadow" version "7.1.1"
|
id "com.github.johnrengelman.shadow" version "7.1.2"
|
||||||
id "com.github.node-gradle.node" version "3.1.1" apply false
|
id "com.github.node-gradle.node" version "3.1.1" apply false
|
||||||
id "edu.wpi.first.GradleJni" version "1.0.0"
|
id "edu.wpi.first.GradleJni" version "1.0.0"
|
||||||
id "edu.wpi.first.GradleVsCode" version "1.1.0"
|
id "edu.wpi.first.GradleVsCode" version "1.1.0"
|
||||||
|
|||||||
@@ -19,11 +19,11 @@ package org.photonvision.common.hardware.GPIO.pi;
|
|||||||
import java.util.HashMap;
|
import java.util.HashMap;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* A class that defines the exceptions that can be thrown by Pigpio.
|
* A class that defines the exceptions that can be thrown by Pigpio.
|
||||||
*
|
*
|
||||||
* <p>Credit to nkolban
|
* <p>Credit to nkolban
|
||||||
* https://github.com/nkolban/jpigpio/blob/master/JPigpio/src/jpigpio/PigpioException.java
|
* https://github.com/nkolban/jpigpio/blob/master/JPigpio/src/jpigpio/PigpioException.java
|
||||||
*/
|
*/
|
||||||
@SuppressWarnings({"SpellCheckingInspection", "unused", "RedundantSuppression"})
|
@SuppressWarnings({"SpellCheckingInspection", "unused", "RedundantSuppression"})
|
||||||
public class PigpioException extends Exception {
|
public class PigpioException extends Exception {
|
||||||
private int rc = -99999999;
|
private int rc = -99999999;
|
||||||
@@ -65,10 +65,10 @@ public class PigpioException extends Exception {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Retrieve the error code that was returned by the underlying Pigpio call.
|
* Retrieve the error code that was returned by the underlying Pigpio call.
|
||||||
*
|
*
|
||||||
* @return The error code that was returned by the underlying Pigpio call.
|
* @return The error code that was returned by the underlying Pigpio call.
|
||||||
*/
|
*/
|
||||||
public int getErrorCode() {
|
public int getErrorCode() {
|
||||||
return rc;
|
return rc;
|
||||||
} // End of getErrorCode
|
} // End of getErrorCode
|
||||||
|
|||||||
@@ -22,14 +22,14 @@ public class PigpioPulse {
|
|||||||
int delayMicros;
|
int delayMicros;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Initialises a pulse.
|
* Initialises a pulse.
|
||||||
*
|
*
|
||||||
* @param gpioOn GPIO number to switch on at the start of the pulse. If zero, then no GPIO will be
|
* @param gpioOn GPIO number to switch on at the start of the pulse. If zero, then no GPIO will be
|
||||||
* switched on.
|
* switched on.
|
||||||
* @param gpioOff GPIO number to switch off at the start of the pulse. If zero, then no GPIO will
|
* @param gpioOff GPIO number to switch off at the start of the pulse. If zero, then no GPIO will
|
||||||
* be switched off.
|
* be switched off.
|
||||||
* @param delayMicros the delay in microseconds before the next pulse.
|
* @param delayMicros the delay in microseconds before the next pulse.
|
||||||
*/
|
*/
|
||||||
public PigpioPulse(int gpioOn, int gpioOff, int delayMicros) {
|
public PigpioPulse(int gpioOn, int gpioOff, int delayMicros) {
|
||||||
this.gpioOn = gpioOn != 0 ? 1 << gpioOn : 0;
|
this.gpioOn = gpioOn != 0 ? 1 << gpioOn : 0;
|
||||||
this.gpioOff = gpioOff != 0 ? 1 << gpioOff : 0;
|
this.gpioOff = gpioOff != 0 ? 1 << gpioOff : 0;
|
||||||
|
|||||||
@@ -40,12 +40,12 @@ public class PigpioSocket {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Creates and starts a socket connection to a pigpio daemon on a remote host with the specified
|
* Creates and starts a socket connection to a pigpio daemon on a remote host with the specified
|
||||||
* address and port
|
* address and port
|
||||||
*
|
*
|
||||||
* @param addr Address of remote pigpio daemon
|
* @param addr Address of remote pigpio daemon
|
||||||
* @param port Port of remote pigpio daemon
|
* @param port Port of remote pigpio daemon
|
||||||
*/
|
*/
|
||||||
public PigpioSocket(String addr, int port) {
|
public PigpioSocket(String addr, int port) {
|
||||||
try {
|
try {
|
||||||
commandSocket = new PigpioSocketLock(addr, port);
|
commandSocket = new PigpioSocketLock(addr, port);
|
||||||
@@ -55,10 +55,10 @@ public class PigpioSocket {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Reconnects to the pigpio daemon
|
* Reconnects to the pigpio daemon
|
||||||
*
|
*
|
||||||
* @throws PigpioException on failure
|
* @throws PigpioException on failure
|
||||||
*/
|
*/
|
||||||
public void reconnect() throws PigpioException {
|
public void reconnect() throws PigpioException {
|
||||||
try {
|
try {
|
||||||
commandSocket.reconnect();
|
commandSocket.reconnect();
|
||||||
@@ -69,10 +69,10 @@ public class PigpioSocket {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Terminates the connection to the pigpio daemon
|
* Terminates the connection to the pigpio daemon
|
||||||
*
|
*
|
||||||
* @throws PigpioException on failure
|
* @throws PigpioException on failure
|
||||||
*/
|
*/
|
||||||
public void gpioTerminate() throws PigpioException {
|
public void gpioTerminate() throws PigpioException {
|
||||||
try {
|
try {
|
||||||
commandSocket.terminate();
|
commandSocket.terminate();
|
||||||
@@ -83,12 +83,12 @@ public class PigpioSocket {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Read the GPIO level
|
* Read the GPIO level
|
||||||
*
|
*
|
||||||
* @param pin Pin to read from
|
* @param pin Pin to read from
|
||||||
* @return Value of the pin
|
* @return Value of the pin
|
||||||
* @throws PigpioException on failure
|
* @throws PigpioException on failure
|
||||||
*/
|
*/
|
||||||
public boolean gpioRead(int pin) throws PigpioException {
|
public boolean gpioRead(int pin) throws PigpioException {
|
||||||
try {
|
try {
|
||||||
int retCode = commandSocket.sendCmd(PigpioCommand.PCMD_READ.value, pin);
|
int retCode = commandSocket.sendCmd(PigpioCommand.PCMD_READ.value, pin);
|
||||||
@@ -101,12 +101,12 @@ public class PigpioSocket {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Write the GPIO level
|
* Write the GPIO level
|
||||||
*
|
*
|
||||||
* @param pin Pin to write to
|
* @param pin Pin to write to
|
||||||
* @param value Value to write
|
* @param value Value to write
|
||||||
* @throws PigpioException on failure
|
* @throws PigpioException on failure
|
||||||
*/
|
*/
|
||||||
public void gpioWrite(int pin, boolean value) throws PigpioException {
|
public void gpioWrite(int pin, boolean value) throws PigpioException {
|
||||||
try {
|
try {
|
||||||
int retCode = commandSocket.sendCmd(PigpioCommand.PCMD_WRITE.value, pin, value ? 1 : 0);
|
int retCode = commandSocket.sendCmd(PigpioCommand.PCMD_WRITE.value, pin, value ? 1 : 0);
|
||||||
@@ -118,10 +118,10 @@ public class PigpioSocket {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Clears all waveforms and any data added by calls to {@link #waveAddGeneric(ArrayList)}
|
* Clears all waveforms and any data added by calls to {@link #waveAddGeneric(ArrayList)}
|
||||||
*
|
*
|
||||||
* @throws PigpioException on failure
|
* @throws PigpioException on failure
|
||||||
*/
|
*/
|
||||||
public void waveClear() throws PigpioException {
|
public void waveClear() throws PigpioException {
|
||||||
try {
|
try {
|
||||||
int retCode = commandSocket.sendCmd(PigpioCommand.PCMD_WVCLR.value);
|
int retCode = commandSocket.sendCmd(PigpioCommand.PCMD_WVCLR.value);
|
||||||
@@ -133,12 +133,12 @@ public class PigpioSocket {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Adds a number of pulses to the current waveform
|
* Adds a number of pulses to the current waveform
|
||||||
*
|
*
|
||||||
* @param pulses ArrayList of pulses to add
|
* @param pulses ArrayList of pulses to add
|
||||||
* @return the new total number of pulses in the current waveform
|
* @return the new total number of pulses in the current waveform
|
||||||
* @throws PigpioException on failure
|
* @throws PigpioException on failure
|
||||||
*/
|
*/
|
||||||
private int waveAddGeneric(ArrayList<PigpioPulse> pulses) throws PigpioException {
|
private int waveAddGeneric(ArrayList<PigpioPulse> pulses) throws PigpioException {
|
||||||
// pigpio wave message format
|
// pigpio wave message format
|
||||||
|
|
||||||
@@ -174,12 +174,12 @@ public class PigpioSocket {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Creates pulses and adds them to the current waveform
|
* Creates pulses and adds them to the current waveform
|
||||||
*
|
*
|
||||||
* @param pulseTimeMillis Pulse length in milliseconds
|
* @param pulseTimeMillis Pulse length in milliseconds
|
||||||
* @param blinks Number of times to pulse. -1 for repeat
|
* @param blinks Number of times to pulse. -1 for repeat
|
||||||
* @param pinNo Pin to pulse
|
* @param pinNo Pin to pulse
|
||||||
*/
|
*/
|
||||||
private void addBlinkPulsesToWaveform(int pulseTimeMillis, int blinks, int pinNo) {
|
private void addBlinkPulsesToWaveform(int pulseTimeMillis, int blinks, int pinNo) {
|
||||||
boolean repeat = blinks == -1;
|
boolean repeat = blinks == -1;
|
||||||
|
|
||||||
@@ -207,13 +207,13 @@ public class PigpioSocket {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Generates and sends a waveform to the given pins with the specified parameters.
|
* Generates and sends a waveform to the given pins with the specified parameters.
|
||||||
*
|
*
|
||||||
* @param pulseTimeMillis Pulse length in milliseconds
|
* @param pulseTimeMillis Pulse length in milliseconds
|
||||||
* @param blinks Number of times to pulse. -1 for repeat
|
* @param blinks Number of times to pulse. -1 for repeat
|
||||||
* @param pins Pins to pulse
|
* @param pins Pins to pulse
|
||||||
* @throws PigpioException on failure
|
* @throws PigpioException on failure
|
||||||
*/
|
*/
|
||||||
public void generateAndSendWaveform(int pulseTimeMillis, int blinks, int... pins)
|
public void generateAndSendWaveform(int pulseTimeMillis, int blinks, int... pins)
|
||||||
throws PigpioException {
|
throws PigpioException {
|
||||||
if (pins.length == 0) return;
|
if (pins.length == 0) return;
|
||||||
@@ -262,11 +262,11 @@ public class PigpioSocket {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Stops the transmission of the current waveform
|
* Stops the transmission of the current waveform
|
||||||
*
|
*
|
||||||
* @return success
|
* @return success
|
||||||
* @throws PigpioException on failure
|
* @throws PigpioException on failure
|
||||||
*/
|
*/
|
||||||
public boolean waveTxStop() throws PigpioException {
|
public boolean waveTxStop() throws PigpioException {
|
||||||
try {
|
try {
|
||||||
int retCode = commandSocket.sendCmd(PigpioCommand.PCMD_WVHLT.value);
|
int retCode = commandSocket.sendCmd(PigpioCommand.PCMD_WVHLT.value);
|
||||||
@@ -279,12 +279,12 @@ public class PigpioSocket {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Creates a waveform from the data provided by the prior calls to {@link
|
* Creates a waveform from the data provided by the prior calls to {@link
|
||||||
* #waveAddGeneric(ArrayList)} Upon success a wave ID greater than or equal to 0 is returned
|
* #waveAddGeneric(ArrayList)} Upon success a wave ID greater than or equal to 0 is returned
|
||||||
*
|
*
|
||||||
* @return ID of the created waveform
|
* @return ID of the created waveform
|
||||||
* @throws PigpioException on failure
|
* @throws PigpioException on failure
|
||||||
*/
|
*/
|
||||||
public int waveCreate() throws PigpioException {
|
public int waveCreate() throws PigpioException {
|
||||||
try {
|
try {
|
||||||
int retCode = commandSocket.sendCmd(PigpioCommand.PCMD_WVCRE.value);
|
int retCode = commandSocket.sendCmd(PigpioCommand.PCMD_WVCRE.value);
|
||||||
@@ -297,11 +297,11 @@ public class PigpioSocket {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Deletes the waveform with specified wave ID
|
* Deletes the waveform with specified wave ID
|
||||||
*
|
*
|
||||||
* @param waveId ID of the waveform to delete
|
* @param waveId ID of the waveform to delete
|
||||||
* @throws PigpioException on failure
|
* @throws PigpioException on failure
|
||||||
*/
|
*/
|
||||||
public void waveDelete(int waveId) throws PigpioException {
|
public void waveDelete(int waveId) throws PigpioException {
|
||||||
try {
|
try {
|
||||||
int retCode = commandSocket.sendCmd(PigpioCommand.PCMD_WVDEL.value, waveId);
|
int retCode = commandSocket.sendCmd(PigpioCommand.PCMD_WVDEL.value, waveId);
|
||||||
@@ -313,12 +313,12 @@ public class PigpioSocket {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Transmits the waveform with specified wave ID. The waveform is sent once
|
* Transmits the waveform with specified wave ID. The waveform is sent once
|
||||||
*
|
*
|
||||||
* @param waveId ID of the waveform to transmit
|
* @param waveId ID of the waveform to transmit
|
||||||
* @return The number of DMA control blocks in the waveform
|
* @return The number of DMA control blocks in the waveform
|
||||||
* @throws PigpioException on failure
|
* @throws PigpioException on failure
|
||||||
*/
|
*/
|
||||||
public int waveSendOnce(int waveId) throws PigpioException {
|
public int waveSendOnce(int waveId) throws PigpioException {
|
||||||
try {
|
try {
|
||||||
int retCode = commandSocket.sendCmd(PigpioCommand.PCMD_WVTX.value, waveId);
|
int retCode = commandSocket.sendCmd(PigpioCommand.PCMD_WVTX.value, waveId);
|
||||||
@@ -330,13 +330,13 @@ public class PigpioSocket {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Transmits the waveform with specified wave ID. The waveform cycles until cancelled (either by
|
* Transmits the waveform with specified wave ID. The waveform cycles until cancelled (either by
|
||||||
* the sending of a new waveform or {@link #waveTxStop()}
|
* the sending of a new waveform or {@link #waveTxStop()}
|
||||||
*
|
*
|
||||||
* @param waveId ID of the waveform to transmit
|
* @param waveId ID of the waveform to transmit
|
||||||
* @return The number of DMA control blocks in the waveform
|
* @return The number of DMA control blocks in the waveform
|
||||||
* @throws PigpioException on failure
|
* @throws PigpioException on failure
|
||||||
*/
|
*/
|
||||||
public int waveSendRepeat(int waveId) throws PigpioException {
|
public int waveSendRepeat(int waveId) throws PigpioException {
|
||||||
try {
|
try {
|
||||||
int retCode = commandSocket.sendCmd(PigpioCommand.PCMD_WVTXR.value, waveId);
|
int retCode = commandSocket.sendCmd(PigpioCommand.PCMD_WVTXR.value, waveId);
|
||||||
@@ -348,14 +348,14 @@ public class PigpioSocket {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Starts hardware PWM on a GPIO at the specified frequency and dutycycle
|
* Starts hardware PWM on a GPIO at the specified frequency and dutycycle
|
||||||
*
|
*
|
||||||
* @param pin GPIO pin to start PWM on
|
* @param pin GPIO pin to start PWM on
|
||||||
* @param pwmFrequency Frequency to run at (1Hz-125MHz). Frequencies above 30MHz are unlikely to
|
* @param pwmFrequency Frequency to run at (1Hz-125MHz). Frequencies above 30MHz are unlikely to
|
||||||
* work
|
* work
|
||||||
* @param pwmDuty Duty cycle to run at (0-1,000,000)
|
* @param pwmDuty Duty cycle to run at (0-1,000,000)
|
||||||
* @throws PigpioException on failure
|
* @throws PigpioException on failure
|
||||||
*/
|
*/
|
||||||
public void hardwarePWM(int pin, int pwmFrequency, int pwmDuty) throws PigpioException {
|
public void hardwarePWM(int pin, int pwmFrequency, int pwmDuty) throws PigpioException {
|
||||||
try {
|
try {
|
||||||
ByteBuffer bb = ByteBuffer.allocate(4);
|
ByteBuffer bb = ByteBuffer.allocate(4);
|
||||||
|
|||||||
@@ -23,9 +23,9 @@ import java.net.Socket;
|
|||||||
import java.nio.ByteBuffer;
|
import java.nio.ByteBuffer;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Credit to nkolban
|
* Credit to nkolban
|
||||||
* https://github.com/nkolban/jpigpio/blob/master/JPigpio/src/jpigpio/SocketLock.java
|
* https://github.com/nkolban/jpigpio/blob/master/JPigpio/src/jpigpio/SocketLock.java
|
||||||
*/
|
*/
|
||||||
final class PigpioSocketLock {
|
final class PigpioSocketLock {
|
||||||
private static final int replyTimeoutMillis = 1000;
|
private static final int replyTimeoutMillis = 1000;
|
||||||
|
|
||||||
@@ -81,16 +81,16 @@ final class PigpioSocketLock {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Send extended command to pigpiod and return result code
|
* Send extended command to pigpiod and return result code
|
||||||
*
|
*
|
||||||
* @param cmd Command to send
|
* @param cmd Command to send
|
||||||
* @param p1 Command parameter 1
|
* @param p1 Command parameter 1
|
||||||
* @param p2 Command parameter 2
|
* @param p2 Command parameter 2
|
||||||
* @param p3 Command parameter 3 (usually length of extended data - see paramater ext)
|
* @param p3 Command parameter 3 (usually length of extended data - see paramater ext)
|
||||||
* @param ext Array of bytes containing extended data
|
* @param ext Array of bytes containing extended data
|
||||||
* @return Command result code
|
* @return Command result code
|
||||||
* @throws IOException in case of network connection error
|
* @throws IOException in case of network connection error
|
||||||
*/
|
*/
|
||||||
@SuppressWarnings("UnusedAssignment")
|
@SuppressWarnings("UnusedAssignment")
|
||||||
public synchronized int sendCmd(int cmd, int p1, int p2, int p3, byte[] ext) throws IOException {
|
public synchronized int sendCmd(int cmd, int p1, int p2, int p3, byte[] ext) throws IOException {
|
||||||
ByteBuffer bb = ByteBuffer.allocate(16 + ext.length);
|
ByteBuffer bb = ByteBuffer.allocate(16 + ext.length);
|
||||||
@@ -135,11 +135,11 @@ final class PigpioSocketLock {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Read all remaining bytes coming from pigpiod
|
* Read all remaining bytes coming from pigpiod
|
||||||
*
|
*
|
||||||
* @param data Array to store read bytes.
|
* @param data Array to store read bytes.
|
||||||
* @throws IOException if unable to read from network
|
* @throws IOException if unable to read from network
|
||||||
*/
|
*/
|
||||||
public void readBytes(byte[] data) throws IOException {
|
public void readBytes(byte[] data) throws IOException {
|
||||||
in.readFully(data);
|
in.readFully(data);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -232,12 +232,12 @@ public class Logger {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Logs an error message with the stack trace of a Throwable. The stacktrace will only be printed
|
* Logs an error message with the stack trace of a Throwable. The stacktrace will only be printed
|
||||||
* if the current LogLevel is TRACE
|
* if the current LogLevel is TRACE
|
||||||
*
|
*
|
||||||
* @param message
|
* @param message
|
||||||
* @param t
|
* @param t
|
||||||
*/
|
*/
|
||||||
public void error(String message, Throwable t) {
|
public void error(String message, Throwable t) {
|
||||||
log(message, LogLevel.ERROR);
|
log(message, LogLevel.ERROR);
|
||||||
log(convertStackTraceToString(t), LogLevel.ERROR, LogLevel.DEBUG);
|
log(convertStackTraceToString(t), LogLevel.ERROR, LogLevel.DEBUG);
|
||||||
|
|||||||
@@ -43,12 +43,12 @@ public class ShellExec {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Execute a bash command. We can handle complex bash commands including multiple executions (; |
|
* Execute a bash command. We can handle complex bash commands including multiple executions (; |
|
||||||
* and ||), quotes, expansions ($), escapes (\), e.g.: "cd /abc/def; mv ghi 'older ghi '$(whoami)"
|
* and ||), quotes, expansions ($), escapes (\), e.g.: "cd /abc/def; mv ghi 'older ghi '$(whoami)"
|
||||||
*
|
*
|
||||||
* @param command Bash command to execute
|
* @param command Bash command to execute
|
||||||
* @return true if bash got started, but your command may have failed.
|
* @return true if bash got started, but your command may have failed.
|
||||||
*/
|
*/
|
||||||
public int executeBashCommand(String command, boolean wait) throws IOException {
|
public int executeBashCommand(String command, boolean wait) throws IOException {
|
||||||
logger.debug("Executing \"" + command + "\"");
|
logger.debug("Executing \"" + command + "\"");
|
||||||
|
|
||||||
@@ -71,25 +71,25 @@ public class ShellExec {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Execute a command in current folder, and wait for process to end
|
* Execute a command in current folder, and wait for process to end
|
||||||
*
|
*
|
||||||
* @param command command ("c:/some/folder/script.bat" or "some/folder/script.sh")
|
* @param command command ("c:/some/folder/script.bat" or "some/folder/script.sh")
|
||||||
* @param args 0..n command line arguments
|
* @param args 0..n command line arguments
|
||||||
* @return process exit code
|
* @return process exit code
|
||||||
*/
|
*/
|
||||||
public int execute(String command, String... args) throws IOException {
|
public int execute(String command, String... args) throws IOException {
|
||||||
return execute(command, null, true, args);
|
return execute(command, null, true, args);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Execute a command.
|
* Execute a command.
|
||||||
*
|
*
|
||||||
* @param command command ("c:/some/folder/script.bat" or "some/folder/script.sh")
|
* @param command command ("c:/some/folder/script.bat" or "some/folder/script.sh")
|
||||||
* @param workdir working directory or NULL to use command folder
|
* @param workdir working directory or NULL to use command folder
|
||||||
* @param wait wait for process to end
|
* @param wait wait for process to end
|
||||||
* @param args 0..n command line arguments
|
* @param args 0..n command line arguments
|
||||||
* @return process exit code
|
* @return process exit code
|
||||||
*/
|
*/
|
||||||
public int execute(String command, String workdir, boolean wait, String... args)
|
public int execute(String command, String workdir, boolean wait, String... args)
|
||||||
throws IOException {
|
throws IOException {
|
||||||
String[] cmdArr;
|
String[] cmdArr;
|
||||||
@@ -153,10 +153,10 @@ public class ShellExec {
|
|||||||
// ********************************************
|
// ********************************************
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* StreamGobbler reads inputstream to "gobble" it. This is used by Executor class when running a
|
* StreamGobbler reads inputstream to "gobble" it. This is used by Executor class when running a
|
||||||
* commandline applications. Gobblers must read/purge INSTR and ERRSTR process streams.
|
* commandline applications. Gobblers must read/purge INSTR and ERRSTR process streams.
|
||||||
* http://www.javaworld.com/javaworld/jw-12-2000/jw-1229-traps.html?page=4
|
* http://www.javaworld.com/javaworld/jw-12-2000/jw-1229-traps.html?page=4
|
||||||
*/
|
*/
|
||||||
@SuppressWarnings("WeakerAccess")
|
@SuppressWarnings("WeakerAccess")
|
||||||
private static class StreamGobbler extends Thread {
|
private static class StreamGobbler extends Thread {
|
||||||
private InputStream is;
|
private InputStream is;
|
||||||
@@ -186,19 +186,19 @@ public class ShellExec {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Get inputstream buffer or null if stream was not consumed.
|
* Get inputstream buffer or null if stream was not consumed.
|
||||||
*
|
*
|
||||||
* @return Output stream
|
* @return Output stream
|
||||||
*/
|
*/
|
||||||
public String getOutput() {
|
public String getOutput() {
|
||||||
return (output != null ? output.toString() : null);
|
return (output != null ? output.toString() : null);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Is input stream completed.
|
* Is input stream completed.
|
||||||
*
|
*
|
||||||
* @return if input stream is completed
|
* @return if input stream is completed
|
||||||
*/
|
*/
|
||||||
public boolean isCompleted() {
|
public boolean isCompleted() {
|
||||||
return completed;
|
return completed;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -64,13 +64,13 @@ public class MathUtils {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Linearly interpolates between two values.
|
* Linearly interpolates between two values.
|
||||||
*
|
*
|
||||||
* @param startValue The start value.
|
* @param startValue The start value.
|
||||||
* @param endValue The end value.
|
* @param endValue The end value.
|
||||||
* @param t The fraction for interpolation.
|
* @param t The fraction for interpolation.
|
||||||
* @return The interpolated value.
|
* @return The interpolated value.
|
||||||
*/
|
*/
|
||||||
@SuppressWarnings("ParameterName")
|
@SuppressWarnings("ParameterName")
|
||||||
public static double lerp(double startValue, double endValue, double t) {
|
public static double lerp(double startValue, double endValue, double t) {
|
||||||
return startValue + (endValue - startValue) * t;
|
return startValue + (endValue - startValue) * t;
|
||||||
|
|||||||
@@ -25,9 +25,9 @@ import java.util.StringJoiner;
|
|||||||
@SuppressWarnings("unused")
|
@SuppressWarnings("unused")
|
||||||
public class NumberListUtils {
|
public class NumberListUtils {
|
||||||
/**
|
/**
|
||||||
* @param collection an ArrayList of Comparable objects
|
* @param collection an ArrayList of Comparable objects
|
||||||
* @return the median of collection
|
* @return the median of collection
|
||||||
*/
|
*/
|
||||||
public static <T extends Number> double median(List<T> collection, Comparator<T> comp) {
|
public static <T extends Number> double median(List<T> collection, Comparator<T> comp) {
|
||||||
double result;
|
double result;
|
||||||
int n = collection.size() / 2;
|
int n = collection.size() / 2;
|
||||||
@@ -57,9 +57,9 @@ public class NumberListUtils {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @param collection an ArrayList of Numbers
|
* @param collection an ArrayList of Numbers
|
||||||
* @return the mean of collection
|
* @return the mean of collection
|
||||||
*/
|
*/
|
||||||
public static double mean(final List<? extends Number> collection) {
|
public static double mean(final List<? extends Number> collection) {
|
||||||
BigDecimal sum = BigDecimal.ZERO;
|
BigDecimal sum = BigDecimal.ZERO;
|
||||||
for (final Number number : collection) {
|
for (final Number number : collection) {
|
||||||
@@ -69,11 +69,11 @@ public class NumberListUtils {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @param collection a collection of Comparable objects
|
* @param collection a collection of Comparable objects
|
||||||
* @param n the position of the desired object, using the ordering defined on the collection
|
* @param n the position of the desired object, using the ordering defined on the collection
|
||||||
* elements
|
* elements
|
||||||
* @return the nth smallest object
|
* @return the nth smallest object
|
||||||
*/
|
*/
|
||||||
public static <T> T nthSmallest(List<T> collection, int n, Comparator<T> comp) {
|
public static <T> T nthSmallest(List<T> collection, int n, Comparator<T> comp) {
|
||||||
T result, pivot;
|
T result, pivot;
|
||||||
ArrayList<T> underPivot = new ArrayList<>(),
|
ArrayList<T> underPivot = new ArrayList<>(),
|
||||||
|
|||||||
@@ -116,18 +116,18 @@ public class PicamJNI {
|
|||||||
// Everything here is static because multiple picams are unsupported at the hardware level
|
// Everything here is static because multiple picams are unsupported at the hardware level
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Called once for each video mode change. Starts a native thread running MMAL that stays alive
|
* Called once for each video mode change. Starts a native thread running MMAL that stays alive
|
||||||
* until destroyCamera is called.
|
* until destroyCamera is called.
|
||||||
*
|
*
|
||||||
* @return true on error.
|
* @return true on error.
|
||||||
*/
|
*/
|
||||||
public static native boolean createCamera(int width, int height, int fps);
|
public static native boolean createCamera(int width, int height, int fps);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Destroys MMAL and EGL contexts. Called once for each video mode change *before* createCamera.
|
* Destroys MMAL and EGL contexts. Called once for each video mode change *before* createCamera.
|
||||||
*
|
*
|
||||||
* @return true on error.
|
* @return true on error.
|
||||||
*/
|
*/
|
||||||
public static native boolean destroyCamera();
|
public static native boolean destroyCamera();
|
||||||
|
|
||||||
public static native void setThresholds(
|
public static native void setThresholds(
|
||||||
|
|||||||
@@ -42,24 +42,24 @@ public class QuirkyCamera {
|
|||||||
public final HashMap<CameraQuirk, Boolean> quirks;
|
public final HashMap<CameraQuirk, Boolean> quirks;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Creates a QuirkyCamera that matches by USB VID/PID
|
* Creates a QuirkyCamera that matches by USB VID/PID
|
||||||
*
|
*
|
||||||
* @param usbVid USB VID of camera
|
* @param usbVid USB VID of camera
|
||||||
* @param usbPid USB PID of camera
|
* @param usbPid USB PID of camera
|
||||||
* @param quirks Camera quirks
|
* @param quirks Camera quirks
|
||||||
*/
|
*/
|
||||||
private QuirkyCamera(int usbVid, int usbPid, CameraQuirk... quirks) {
|
private QuirkyCamera(int usbVid, int usbPid, CameraQuirk... quirks) {
|
||||||
this(usbVid, usbPid, "", quirks);
|
this(usbVid, usbPid, "", quirks);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Creates a QuirkyCamera that matches by USB VID/PID and name
|
* Creates a QuirkyCamera that matches by USB VID/PID and name
|
||||||
*
|
*
|
||||||
* @param usbVid USB VID of camera
|
* @param usbVid USB VID of camera
|
||||||
* @param usbPid USB PID of camera
|
* @param usbPid USB PID of camera
|
||||||
* @param baseName CSCore name of camera
|
* @param baseName CSCore name of camera
|
||||||
* @param quirks Camera quirks
|
* @param quirks Camera quirks
|
||||||
*/
|
*/
|
||||||
private QuirkyCamera(int usbVid, int usbPid, String baseName, CameraQuirk... quirks) {
|
private QuirkyCamera(int usbVid, int usbPid, String baseName, CameraQuirk... quirks) {
|
||||||
this.usbVid = usbVid;
|
this.usbVid = usbVid;
|
||||||
this.usbPid = usbPid;
|
this.usbPid = usbPid;
|
||||||
|
|||||||
@@ -56,13 +56,13 @@ public class ZeroCopyPicamSource extends VisionSource {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* On the OV5649 the actual FPS we want to request from the GPU can be higher than the FPS that we
|
* On the OV5649 the actual FPS we want to request from the GPU can be higher than the FPS that we
|
||||||
* can do after processing. On the IMX219 these FPSes match pretty closely, except for the
|
* can do after processing. On the IMX219 these FPSes match pretty closely, except for the
|
||||||
* 1280x720 mode. We use this to present a rated FPS to the user that's lower than the actual FPS
|
* 1280x720 mode. We use this to present a rated FPS to the user that's lower than the actual FPS
|
||||||
* we request from the GPU. This is important for setting user expectations, and is also used by
|
* we request from the GPU. This is important for setting user expectations, and is also used by
|
||||||
* the frontend to detect and explain FPS drops. This class should ONLY be used by Picam video
|
* the frontend to detect and explain FPS drops. This class should ONLY be used by Picam video
|
||||||
* modes! This is to make sure it shows up nice in the frontend
|
* modes! This is to make sure it shows up nice in the frontend
|
||||||
*/
|
*/
|
||||||
public static class FPSRatedVideoMode extends VideoMode {
|
public static class FPSRatedVideoMode extends VideoMode {
|
||||||
public final int fpsActual;
|
public final int fpsActual;
|
||||||
public final double fovMultiplier;
|
public final double fovMultiplier;
|
||||||
|
|||||||
@@ -37,23 +37,23 @@ public class FrameStaticProperties {
|
|||||||
public CameraCalibrationCoefficients cameraCalibration;
|
public CameraCalibrationCoefficients cameraCalibration;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Instantiates a new Frame static properties.
|
* Instantiates a new Frame static properties.
|
||||||
*
|
*
|
||||||
* @param mode The Video Mode of the camera.
|
* @param mode The Video Mode of the camera.
|
||||||
* @param fov The fov of the image.
|
* @param fov The fov of the image.
|
||||||
*/
|
*/
|
||||||
public FrameStaticProperties(
|
public FrameStaticProperties(
|
||||||
VideoMode mode, double fov, Rotation2d cameraPitch, CameraCalibrationCoefficients cal) {
|
VideoMode mode, double fov, Rotation2d cameraPitch, CameraCalibrationCoefficients cal) {
|
||||||
this(mode != null ? mode.width : 1, mode != null ? mode.height : 1, fov, cameraPitch, cal);
|
this(mode != null ? mode.width : 1, mode != null ? mode.height : 1, fov, cameraPitch, cal);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Instantiates a new Frame static properties.
|
* Instantiates a new Frame static properties.
|
||||||
*
|
*
|
||||||
* @param imageWidth The width of the image.
|
* @param imageWidth The width of the image.
|
||||||
* @param imageHeight The width of the image.
|
* @param imageHeight The width of the image.
|
||||||
* @param fov The fov of the image.
|
* @param fov The fov of the image.
|
||||||
*/
|
*/
|
||||||
public FrameStaticProperties(
|
public FrameStaticProperties(
|
||||||
int imageWidth,
|
int imageWidth,
|
||||||
int imageHeight,
|
int imageHeight,
|
||||||
|
|||||||
@@ -29,9 +29,9 @@ import org.photonvision.vision.frame.FrameStaticProperties;
|
|||||||
import org.photonvision.vision.opencv.CVMat;
|
import org.photonvision.vision.opencv.CVMat;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* A {@link FrameProvider} that will read and provide an image from a {@link java.nio.file.Path
|
* A {@link FrameProvider} that will read and provide an image from a {@link java.nio.file.Path
|
||||||
* path}.
|
* path}.
|
||||||
*/
|
*/
|
||||||
public class FileFrameProvider implements FrameProvider {
|
public class FileFrameProvider implements FrameProvider {
|
||||||
public static final int MAX_FPS = 120;
|
public static final int MAX_FPS = 120;
|
||||||
private static int count = 0;
|
private static int count = 0;
|
||||||
@@ -46,12 +46,12 @@ public class FileFrameProvider implements FrameProvider {
|
|||||||
private long lastGetMillis = System.currentTimeMillis();
|
private long lastGetMillis = System.currentTimeMillis();
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Instantiates a new FileFrameProvider.
|
* Instantiates a new FileFrameProvider.
|
||||||
*
|
*
|
||||||
* @param path The path of the image to read from.
|
* @param path The path of the image to read from.
|
||||||
* @param fov The fov of the image.
|
* @param fov The fov of the image.
|
||||||
* @param maxFPS The max framerate to provide the image at.
|
* @param maxFPS The max framerate to provide the image at.
|
||||||
*/
|
*/
|
||||||
public FileFrameProvider(Path path, double fov, int maxFPS) {
|
public FileFrameProvider(Path path, double fov, int maxFPS) {
|
||||||
this(path, fov, maxFPS, null, null);
|
this(path, fov, maxFPS, null, null);
|
||||||
}
|
}
|
||||||
@@ -83,21 +83,21 @@ public class FileFrameProvider implements FrameProvider {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Instantiates a new File frame provider.
|
* Instantiates a new File frame provider.
|
||||||
*
|
*
|
||||||
* @param pathAsString The path of the image to read from as a string.
|
* @param pathAsString The path of the image to read from as a string.
|
||||||
* @param fov The fov of the image.
|
* @param fov The fov of the image.
|
||||||
*/
|
*/
|
||||||
public FileFrameProvider(String pathAsString, double fov) {
|
public FileFrameProvider(String pathAsString, double fov) {
|
||||||
this(Paths.get(pathAsString), fov, MAX_FPS);
|
this(Paths.get(pathAsString), fov, MAX_FPS);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Instantiates a new File frame provider.
|
* Instantiates a new File frame provider.
|
||||||
*
|
*
|
||||||
* @param path The path of the image to read from.
|
* @param path The path of the image to read from.
|
||||||
* @param fov The fov of the image.
|
* @param fov The fov of the image.
|
||||||
*/
|
*/
|
||||||
public FileFrameProvider(Path path, double fov) {
|
public FileFrameProvider(Path path, double fov) {
|
||||||
this(path, fov, MAX_FPS);
|
this(path, fov, MAX_FPS);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -17,13 +17,13 @@
|
|||||||
package org.photonvision.vision.pipe;
|
package org.photonvision.vision.pipe;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Defines a pipe. A pipe is a single step in a pipeline. This class is to be extended, never used
|
* Defines a pipe. A pipe is a single step in a pipeline. This class is to be extended, never used
|
||||||
* on its own.
|
* on its own.
|
||||||
*
|
*
|
||||||
* @param <I> Input type for the pipe
|
* @param <I> Input type for the pipe
|
||||||
* @param <O> Output type for the pipe
|
* @param <O> Output type for the pipe
|
||||||
* @param <P> Parameters type for the pipe
|
* @param <P> Parameters type for the pipe
|
||||||
*/
|
*/
|
||||||
public abstract class CVPipe<I, O, P> {
|
public abstract class CVPipe<I, O, P> {
|
||||||
protected CVPipeResult<O> result = new CVPipeResult<>();
|
protected CVPipeResult<O> result = new CVPipeResult<>();
|
||||||
protected P params;
|
protected P params;
|
||||||
@@ -33,17 +33,17 @@ public abstract class CVPipe<I, O, P> {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Runs the process for the pipe.
|
* Runs the process for the pipe.
|
||||||
*
|
*
|
||||||
* @param in Input for pipe processing.
|
* @param in Input for pipe processing.
|
||||||
* @return Result of processing.
|
* @return Result of processing.
|
||||||
*/
|
*/
|
||||||
protected abstract O process(I in);
|
protected abstract O process(I in);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @param in Input for pipe processing.
|
* @param in Input for pipe processing.
|
||||||
* @return Result of processing.
|
* @return Result of processing.
|
||||||
*/
|
*/
|
||||||
public CVPipeResult<O> run(I in) {
|
public CVPipeResult<O> run(I in) {
|
||||||
long pipeStartNanos = System.nanoTime();
|
long pipeStartNanos = System.nanoTime();
|
||||||
result.output = process(in);
|
result.output = process(in);
|
||||||
|
|||||||
@@ -24,11 +24,11 @@ import org.photonvision.vision.pipe.MutatingPipe;
|
|||||||
/** Represents a pipeline that blurs the image. */
|
/** Represents a pipeline that blurs the image. */
|
||||||
public class BlurPipe extends MutatingPipe<Mat, BlurPipe.BlurParams> {
|
public class BlurPipe extends MutatingPipe<Mat, BlurPipe.BlurParams> {
|
||||||
/**
|
/**
|
||||||
* Processes this pipe.
|
* Processes this pipe.
|
||||||
*
|
*
|
||||||
* @param in Input for pipe processing.
|
* @param in Input for pipe processing.
|
||||||
* @return The processed frame.
|
* @return The processed frame.
|
||||||
*/
|
*/
|
||||||
@Override
|
@Override
|
||||||
protected Void process(Mat in) {
|
protected Void process(Mat in) {
|
||||||
Imgproc.blur(in, in, params.getBlurSize());
|
Imgproc.blur(in, in, params.getBlurSize());
|
||||||
@@ -43,19 +43,19 @@ public class BlurPipe extends MutatingPipe<Mat, BlurPipe.BlurParams> {
|
|||||||
private final int m_blurSize;
|
private final int m_blurSize;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Constructs a new BlurImageParams.
|
* Constructs a new BlurImageParams.
|
||||||
*
|
*
|
||||||
* @param blurSize The blur size.
|
* @param blurSize The blur size.
|
||||||
*/
|
*/
|
||||||
public BlurParams(int blurSize) {
|
public BlurParams(int blurSize) {
|
||||||
m_blurSize = blurSize;
|
m_blurSize = blurSize;
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Returns the blur size.
|
* Returns the blur size.
|
||||||
*
|
*
|
||||||
* @return The blur size.
|
* @return The blur size.
|
||||||
*/
|
*/
|
||||||
public Size getBlurSize() {
|
public Size getBlurSize() {
|
||||||
return new Size(m_blurSize, m_blurSize);
|
return new Size(m_blurSize, m_blurSize);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -60,11 +60,11 @@ public class Calibrate3dPipe
|
|||||||
private double calibrationAccuracy;
|
private double calibrationAccuracy;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Runs the process for the pipe.
|
* Runs the process for the pipe.
|
||||||
*
|
*
|
||||||
* @param in Input for pipe processing. In the format (Input image, object points, image points)
|
* @param in Input for pipe processing. In the format (Input image, object points, image points)
|
||||||
* @return Result of processing.
|
* @return Result of processing.
|
||||||
*/
|
*/
|
||||||
@Override
|
@Override
|
||||||
protected CameraCalibrationCoefficients process(List<Triple<Size, Mat, Mat>> in) {
|
protected CameraCalibrationCoefficients process(List<Triple<Size, Mat, Mat>> in) {
|
||||||
in =
|
in =
|
||||||
|
|||||||
@@ -29,11 +29,11 @@ public class Collect2dTargetsPipe
|
|||||||
extends CVPipe<
|
extends CVPipe<
|
||||||
List<PotentialTarget>, List<TrackedTarget>, Collect2dTargetsPipe.Collect2dTargetsParams> {
|
List<PotentialTarget>, List<TrackedTarget>, Collect2dTargetsPipe.Collect2dTargetsParams> {
|
||||||
/**
|
/**
|
||||||
* Processes this pipeline.
|
* Processes this pipeline.
|
||||||
*
|
*
|
||||||
* @param in Input for pipe processing.
|
* @param in Input for pipe processing.
|
||||||
* @return A list of tracked targets.
|
* @return A list of tracked targets.
|
||||||
*/
|
*/
|
||||||
@Override
|
@Override
|
||||||
protected List<TrackedTarget> process(List<PotentialTarget> in) {
|
protected List<TrackedTarget> process(List<PotentialTarget> in) {
|
||||||
List<TrackedTarget> targets = new ArrayList<>();
|
List<TrackedTarget> targets = new ArrayList<>();
|
||||||
|
|||||||
@@ -59,9 +59,9 @@ public class CornerDetectionPipe
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @param target the target to find the corners of.
|
* @param target the target to find the corners of.
|
||||||
* @return the corners. left top, left bottom, right bottom, right top
|
* @return the corners. left top, left bottom, right bottom, right top
|
||||||
*/
|
*/
|
||||||
private List<Point> findBoundingBoxCorners(TrackedTarget target) {
|
private List<Point> findBoundingBoxCorners(TrackedTarget target) {
|
||||||
// extract the corners
|
// extract the corners
|
||||||
var points = new Point[4];
|
var points = new Point[4];
|
||||||
@@ -88,30 +88,30 @@ public class CornerDetectionPipe
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @param a First point.
|
* @param a First point.
|
||||||
* @param b Second point.
|
* @param b Second point.
|
||||||
* @return The straight line distance between them.
|
* @return The straight line distance between them.
|
||||||
*/
|
*/
|
||||||
private static double distanceBetween(Point a, Point b) {
|
private static double distanceBetween(Point a, Point b) {
|
||||||
return Math.sqrt(Math.pow(a.x - b.x, 2) + Math.pow(a.y - b.y, 2));
|
return Math.sqrt(Math.pow(a.x - b.x, 2) + Math.pow(a.y - b.y, 2));
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @param a First point.
|
* @param a First point.
|
||||||
* @param b Second point.
|
* @param b Second point.
|
||||||
* @return The straight line distance between them.
|
* @return The straight line distance between them.
|
||||||
*/
|
*/
|
||||||
private static double distanceBetween(Translation2d a, Translation2d b) {
|
private static double distanceBetween(Translation2d a, Translation2d b) {
|
||||||
return Math.sqrt(Math.pow(a.getX() - b.getX(), 2) + Math.pow(a.getY() - b.getY(), 2));
|
return Math.sqrt(Math.pow(a.getX() - b.getX(), 2) + Math.pow(a.getY() - b.getY(), 2));
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Find the 4 most extreme corners,
|
* Find the 4 most extreme corners,
|
||||||
*
|
*
|
||||||
* @param target the target to track.
|
* @param target the target to track.
|
||||||
* @param convexHull weather to use the convex hull of the target.
|
* @param convexHull weather to use the convex hull of the target.
|
||||||
* @return the 4 extreme corners of the contour.
|
* @return the 4 extreme corners of the contour.
|
||||||
*/
|
*/
|
||||||
private List<Point> detectExtremeCornersByApproxPolyDp(TrackedTarget target, boolean convexHull) {
|
private List<Point> detectExtremeCornersByApproxPolyDp(TrackedTarget target, boolean convexHull) {
|
||||||
var centroid = target.getMinAreaRect().center;
|
var centroid = target.getMinAreaRect().center;
|
||||||
Comparator<Point> distanceProvider =
|
Comparator<Point> distanceProvider =
|
||||||
|
|||||||
@@ -28,11 +28,11 @@ public class FilterShapesPipe
|
|||||||
List<CVShape> outputList = new ArrayList<>();
|
List<CVShape> outputList = new ArrayList<>();
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Runs the process for the pipe.
|
* Runs the process for the pipe.
|
||||||
*
|
*
|
||||||
* @param in Input for pipe processing.
|
* @param in Input for pipe processing.
|
||||||
* @return Result of processing.
|
* @return Result of processing.
|
||||||
*/
|
*/
|
||||||
@Override
|
@Override
|
||||||
protected List<CVShape> process(List<CVShape> in) {
|
protected List<CVShape> process(List<CVShape> in) {
|
||||||
outputList.forEach(CVShape::release);
|
outputList.forEach(CVShape::release);
|
||||||
|
|||||||
@@ -109,11 +109,11 @@ public class FindBoardCornersPipe
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Finds the corners in a given image and returns them
|
* Finds the corners in a given image and returns them
|
||||||
*
|
*
|
||||||
* @param in Input for pipe processing. Pair of input and output mat
|
* @param in Input for pipe processing. Pair of input and output mat
|
||||||
* @return All valid Mats for camera calibration
|
* @return All valid Mats for camera calibration
|
||||||
*/
|
*/
|
||||||
@Override
|
@Override
|
||||||
protected Triple<Size, Mat, Mat> process(Pair<Mat, Mat> in) {
|
protected Triple<Size, Mat, Mat> process(Pair<Mat, Mat> in) {
|
||||||
// Create the object points
|
// Create the object points
|
||||||
@@ -123,12 +123,12 @@ public class FindBoardCornersPipe
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Figures out how much a frame or point cloud must be scaled down by to match the desired size at
|
* Figures out how much a frame or point cloud must be scaled down by to match the desired size at
|
||||||
* which to run FindCorners
|
* which to run FindCorners
|
||||||
*
|
*
|
||||||
* @param inFrame
|
* @param inFrame
|
||||||
* @return
|
* @return
|
||||||
*/
|
*/
|
||||||
private double getFindCornersScaleFactor(Mat inFrame) {
|
private double getFindCornersScaleFactor(Mat inFrame) {
|
||||||
if (inFrame.width() > FIND_CORNERS_WIDTH_PX) {
|
if (inFrame.width() > FIND_CORNERS_WIDTH_PX) {
|
||||||
return ((double) FIND_CORNERS_WIDTH_PX) / inFrame.width();
|
return ((double) FIND_CORNERS_WIDTH_PX) / inFrame.width();
|
||||||
@@ -138,21 +138,21 @@ public class FindBoardCornersPipe
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Finds the minimum spacing between a set of x/y points Currently only considers points whose
|
* Finds the minimum spacing between a set of x/y points Currently only considers points whose
|
||||||
* index is next to each other Which, currently, means it traverses one dimension. This is a rough
|
* index is next to each other Which, currently, means it traverses one dimension. This is a rough
|
||||||
* heuristic approach which could be refined in the future.
|
* heuristic approach which could be refined in the future.
|
||||||
*
|
*
|
||||||
* <p>Note that the current implementation can be fooled under the following conditions: (1) The
|
* <p>Note that the current implementation can be fooled under the following conditions: (1) The
|
||||||
* width of the image is an odd number, and the smallest distance was actually on the between the
|
* width of the image is an odd number, and the smallest distance was actually on the between the
|
||||||
* last two points in a given row and (2) The smallest distance was actually in the direction
|
* last two points in a given row and (2) The smallest distance was actually in the direction
|
||||||
* orthogonal to that which was getting traversed by iterating through the MatOfPoint2f in order.
|
* orthogonal to that which was getting traversed by iterating through the MatOfPoint2f in order.
|
||||||
*
|
*
|
||||||
* <p>I've chosen not to handle these for speed's sake, and because, really, you don't need the
|
* <p>I've chosen not to handle these for speed's sake, and because, really, you don't need the
|
||||||
* exact answer for "min distance". you just need something fairly reasonable.
|
* exact answer for "min distance". you just need something fairly reasonable.
|
||||||
*
|
*
|
||||||
* @param inPoints point set to analyze. Must be a "tall" matrix.
|
* @param inPoints point set to analyze. Must be a "tall" matrix.
|
||||||
* @return min spacing between neighbors
|
* @return min spacing between neighbors
|
||||||
*/
|
*/
|
||||||
private double getApproxMinSpacing(MatOfPoint2f inPoints) {
|
private double getApproxMinSpacing(MatOfPoint2f inPoints) {
|
||||||
double minSpacing = Double.MAX_VALUE;
|
double minSpacing = Double.MAX_VALUE;
|
||||||
for (int pointIdx = 0; pointIdx < inPoints.height() - 1; pointIdx += 2) {
|
for (int pointIdx = 0; pointIdx < inPoints.height() - 1; pointIdx += 2) {
|
||||||
@@ -169,24 +169,24 @@ public class FindBoardCornersPipe
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @param inFrame Full-size mat that is going to get scaled down before passing to
|
* @param inFrame Full-size mat that is going to get scaled down before passing to
|
||||||
* findBoardCorners
|
* findBoardCorners
|
||||||
* @return the size to scale the input mat to
|
* @return the size to scale the input mat to
|
||||||
*/
|
*/
|
||||||
private Size getFindCornersImgSize(Mat inFrame) {
|
private Size getFindCornersImgSize(Mat inFrame) {
|
||||||
var findcorners_height = Math.round(inFrame.height() * getFindCornersScaleFactor(inFrame));
|
var findcorners_height = Math.round(inFrame.height() * getFindCornersScaleFactor(inFrame));
|
||||||
return new Size(FIND_CORNERS_WIDTH_PX, findcorners_height);
|
return new Size(FIND_CORNERS_WIDTH_PX, findcorners_height);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Given an input frame and a set of points from the "smaller" findChessboardCorner analysis,
|
* Given an input frame and a set of points from the "smaller" findChessboardCorner analysis,
|
||||||
* re-scale the points back to where they would have been in the input frame
|
* re-scale the points back to where they would have been in the input frame
|
||||||
*
|
*
|
||||||
* @param inPoints set of points derived from a call to findChessboardCorner on a shrunken mat.
|
* @param inPoints set of points derived from a call to findChessboardCorner on a shrunken mat.
|
||||||
* Must be a "tall" matrix.
|
* Must be a "tall" matrix.
|
||||||
* @param origFrame Original frame we're rescaling points back to
|
* @param origFrame Original frame we're rescaling points back to
|
||||||
* @param outPoints mat into which the output rescaled points get placed
|
* @param outPoints mat into which the output rescaled points get placed
|
||||||
*/
|
*/
|
||||||
private void rescalePointsToOrigFrame(
|
private void rescalePointsToOrigFrame(
|
||||||
MatOfPoint2f inPoints, Mat origFrame, MatOfPoint2f outPoints) {
|
MatOfPoint2f inPoints, Mat origFrame, MatOfPoint2f outPoints) {
|
||||||
// Rescale boardCorners back up to the inproc image size
|
// Rescale boardCorners back up to the inproc image size
|
||||||
@@ -202,12 +202,12 @@ public class FindBoardCornersPipe
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Picks a window size for doing subpixel optimization based on the board type and spacing
|
* Picks a window size for doing subpixel optimization based on the board type and spacing
|
||||||
* observed between the corners or points in the image
|
* observed between the corners or points in the image
|
||||||
*
|
*
|
||||||
* @param inPoints
|
* @param inPoints
|
||||||
* @return
|
* @return
|
||||||
*/
|
*/
|
||||||
private Size getWindowSize(MatOfPoint2f inPoints) {
|
private Size getWindowSize(MatOfPoint2f inPoints) {
|
||||||
double windowHalfWidth = 11; // Dot board uses fixed-size window half-width
|
double windowHalfWidth = 11; // Dot board uses fixed-size window half-width
|
||||||
if (params.type == UICalibrationData.BoardType.CHESSBOARD) {
|
if (params.type == UICalibrationData.BoardType.CHESSBOARD) {
|
||||||
@@ -219,10 +219,10 @@ public class FindBoardCornersPipe
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Find chessboard corners given a input mat and output mat to draw on
|
* Find chessboard corners given a input mat and output mat to draw on
|
||||||
*
|
*
|
||||||
* @return Frame resolution, object points, board corners
|
* @return Frame resolution, object points, board corners
|
||||||
*/
|
*/
|
||||||
private Triple<Size, Mat, Mat> findBoardCorners(Pair<Mat, Mat> in) {
|
private Triple<Size, Mat, Mat> findBoardCorners(Pair<Mat, Mat> in) {
|
||||||
createObjectPoints();
|
createObjectPoints();
|
||||||
|
|
||||||
|
|||||||
@@ -33,15 +33,15 @@ public class FindCirclesPipe
|
|||||||
// (x,y,radius) or (x,y,radius,votes) .
|
// (x,y,radius) or (x,y,radius,votes) .
|
||||||
private final Mat circles = new Mat();
|
private final Mat circles = new Mat();
|
||||||
/**
|
/**
|
||||||
* Runs the process for the pipe. The reason we need a separate pipe for circles is because if we
|
* Runs the process for the pipe. The reason we need a separate pipe for circles is because if we
|
||||||
* were to use the FindShapes pipe, we would have to assume that any shape more than 10-20+ sides
|
* were to use the FindShapes pipe, we would have to assume that any shape more than 10-20+ sides
|
||||||
* is a circle. Only issue with such approximation is that the user would no longer be able to
|
* is a circle. Only issue with such approximation is that the user would no longer be able to
|
||||||
* track shapes with 10-20+ sides. And hence, in order to overcome this edge case, we can use
|
* track shapes with 10-20+ sides. And hence, in order to overcome this edge case, we can use
|
||||||
* HoughCircles which is more flexible and accurate for finding circles.
|
* HoughCircles which is more flexible and accurate for finding circles.
|
||||||
*
|
*
|
||||||
* @param in Input for pipe processing. 8-bit, single-channel, grayscale input image.
|
* @param in Input for pipe processing. 8-bit, single-channel, grayscale input image.
|
||||||
* @return Result of processing.
|
* @return Result of processing.
|
||||||
*/
|
*/
|
||||||
@Override
|
@Override
|
||||||
protected List<CVShape> process(Pair<Mat, List<Contour>> in) {
|
protected List<CVShape> process(Pair<Mat, List<Contour>> in) {
|
||||||
circles.release();
|
circles.release();
|
||||||
@@ -115,16 +115,16 @@ public class FindCirclesPipe
|
|||||||
private final double diagonalLengthPx;
|
private final double diagonalLengthPx;
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* @params minDist - Minimum distance between the centers of the detected circles.
|
* @params minDist - Minimum distance between the centers of the detected circles.
|
||||||
* If the parameter is too small, multiple neighbor circles may be falsely detected in addition to a true one. If it is too large, some circles may be missed.
|
* If the parameter is too small, multiple neighbor circles may be falsely detected in addition to a true one. If it is too large, some circles may be missed.
|
||||||
*
|
*
|
||||||
* @param maxCannyThresh -First method-specific parameter. In case of #HOUGH_GRADIENT and #HOUGH_GRADIENT_ALT, it is the higher threshold of the two passed to the Canny edge detector (the lower one is twice smaller).
|
* @param maxCannyThresh -First method-specific parameter. In case of #HOUGH_GRADIENT and #HOUGH_GRADIENT_ALT, it is the higher threshold of the two passed to the Canny edge detector (the lower one is twice smaller).
|
||||||
* Note that #HOUGH_GRADIENT_ALT uses #Scharr algorithm to compute image derivatives, so the threshold value shough normally be higher, such as 300 or normally exposed and contrasty images.
|
* Note that #HOUGH_GRADIENT_ALT uses #Scharr algorithm to compute image derivatives, so the threshold value shough normally be higher, such as 300 or normally exposed and contrasty images.
|
||||||
*
|
*
|
||||||
*
|
*
|
||||||
* @param allowableThreshold - When finding the corresponding contour, this is used to see how close a center should be to a contour for it to be considered THAT contour.
|
* @param allowableThreshold - When finding the corresponding contour, this is used to see how close a center should be to a contour for it to be considered THAT contour.
|
||||||
* Should be increased with lower resolutions and decreased with higher resolution
|
* Should be increased with lower resolutions and decreased with higher resolution
|
||||||
* */
|
* */
|
||||||
public FindCirclePipeParams(
|
public FindCirclePipeParams(
|
||||||
int allowableThreshold,
|
int allowableThreshold,
|
||||||
int minRadius,
|
int minRadius,
|
||||||
|
|||||||
@@ -30,11 +30,11 @@ public class FindPolygonPipe
|
|||||||
List<CVShape> shapeList = new ArrayList<>();
|
List<CVShape> shapeList = new ArrayList<>();
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Runs the process for the pipe.
|
* Runs the process for the pipe.
|
||||||
*
|
*
|
||||||
* @param in Input for pipe processing.
|
* @param in Input for pipe processing.
|
||||||
* @return Result of processing.
|
* @return Result of processing.
|
||||||
*/
|
*/
|
||||||
@Override
|
@Override
|
||||||
protected List<CVShape> process(List<Contour> in) {
|
protected List<CVShape> process(List<Contour> in) {
|
||||||
shapeList.forEach(CVShape::release);
|
shapeList.forEach(CVShape::release);
|
||||||
|
|||||||
@@ -25,10 +25,10 @@ import org.photonvision.vision.pipe.MutatingPipe;
|
|||||||
/** Pipe that resizes an image to a given resolution */
|
/** Pipe that resizes an image to a given resolution */
|
||||||
public class ResizeImagePipe extends MutatingPipe<Mat, ResizeImagePipe.ResizeImageParams> {
|
public class ResizeImagePipe extends MutatingPipe<Mat, ResizeImagePipe.ResizeImageParams> {
|
||||||
/**
|
/**
|
||||||
* Process this pipe
|
* Process this pipe
|
||||||
*
|
*
|
||||||
* @param in {@link Mat} to be resized
|
* @param in {@link Mat} to be resized
|
||||||
*/
|
*/
|
||||||
@Override
|
@Override
|
||||||
protected Void process(Mat in) {
|
protected Void process(Mat in) {
|
||||||
int width = in.cols() / params.getDivisor().value;
|
int width = in.cols() / params.getDivisor().value;
|
||||||
|
|||||||
@@ -32,11 +32,11 @@ public class RotateImagePipe extends MutatingPipe<Mat, RotateImagePipe.RotateIma
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Process this pipe
|
* Process this pipe
|
||||||
*
|
*
|
||||||
* @param in {@link Mat} to be rotated
|
* @param in {@link Mat} to be rotated
|
||||||
* @return Rotated {@link Mat}
|
* @return Rotated {@link Mat}
|
||||||
*/
|
*/
|
||||||
@Override
|
@Override
|
||||||
protected Void process(Mat in) {
|
protected Void process(Mat in) {
|
||||||
Core.rotate(in, in, params.rotation.value);
|
Core.rotate(in, in, params.rotation.value);
|
||||||
|
|||||||
@@ -139,12 +139,12 @@ public class SolvePNPPipe
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Element-wise scale a matrix by a given factor
|
* Element-wise scale a matrix by a given factor
|
||||||
*
|
*
|
||||||
* @param src the source matrix
|
* @param src the source matrix
|
||||||
* @param factor by how much to scale each element
|
* @param factor by how much to scale each element
|
||||||
* @return the scaled matrix
|
* @return the scaled matrix
|
||||||
*/
|
*/
|
||||||
@SuppressWarnings("SameParameterValue")
|
@SuppressWarnings("SameParameterValue")
|
||||||
private static Mat matScale(Mat src, double factor) {
|
private static Mat matScale(Mat src, double factor) {
|
||||||
Mat dst = new Mat(src.rows(), src.cols(), src.type());
|
Mat dst = new Mat(src.rows(), src.cols(), src.type());
|
||||||
|
|||||||
@@ -28,9 +28,9 @@ import org.photonvision.vision.pipeline.result.CVPipelineResult;
|
|||||||
import org.photonvision.vision.target.TrackedTarget;
|
import org.photonvision.vision.target.TrackedTarget;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* This is a "fake" pipeline that is just used to move identical pipe sets out of real pipelines. It
|
* This is a "fake" pipeline that is just used to move identical pipe sets out of real pipelines. It
|
||||||
* shall not get its settings saved, nor shall it be managed by PipelineManager
|
* shall not get its settings saved, nor shall it be managed by PipelineManager
|
||||||
*/
|
*/
|
||||||
public class OutputStreamPipeline {
|
public class OutputStreamPipeline {
|
||||||
private final OutputMatPipe outputMatPipe = new OutputMatPipe();
|
private final OutputMatPipe outputMatPipe = new OutputMatPipe();
|
||||||
private final Draw2dCrosshairPipe draw2dCrosshairPipe = new Draw2dCrosshairPipe();
|
private final Draw2dCrosshairPipe draw2dCrosshairPipe = new Draw2dCrosshairPipe();
|
||||||
|
|||||||
@@ -29,13 +29,13 @@ public class PipelineProfiler {
|
|||||||
new Logger(ColoredShapePipeline.class, LogGroup.VisionModule);
|
new Logger(ColoredShapePipeline.class, LogGroup.VisionModule);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Indices for Reflective profiling 0 - rotateImagePipe 1 - inputCopy (not a pipe) 2 - hsvPipe 3 -
|
* Indices for Reflective profiling 0 - rotateImagePipe 1 - inputCopy (not a pipe) 2 - hsvPipe 3 -
|
||||||
* findContoursPipe 4 - speckleRejectPipe 5 - filterContoursPipe 6 - groupContoursPipe 7 -
|
* findContoursPipe 4 - speckleRejectPipe 5 - filterContoursPipe 6 - groupContoursPipe 7 -
|
||||||
* sortContoursPipe 8 - collect2dTargetsPipe 9 - cornerDetectionPipe 10 - solvePNPPipe (OPTIONAL)
|
* sortContoursPipe 8 - collect2dTargetsPipe 9 - cornerDetectionPipe 10 - solvePNPPipe (OPTIONAL)
|
||||||
* 11 - outputMatPipe (OPTIONAL) 12 - draw2dCrosshairPipe (on input) 13 - draw2dCrosshairPipe (on
|
* 11 - outputMatPipe (OPTIONAL) 12 - draw2dCrosshairPipe (on input) 13 - draw2dCrosshairPipe (on
|
||||||
* output) 14 - draw2dTargetsPipe (on input) 15 - draw2dTargetsPipe (on output) 16 -
|
* output) 14 - draw2dTargetsPipe (on input) 15 - draw2dTargetsPipe (on output) 16 -
|
||||||
* draw3dTargetsPipe (OPTIONAL, on input) 17 - draw3dTargetsPipe (OPTIONAL, on output)
|
* draw3dTargetsPipe (OPTIONAL, on input) 17 - draw3dTargetsPipe (OPTIONAL, on output)
|
||||||
*/
|
*/
|
||||||
private static final String[] ReflectivePipeNames =
|
private static final String[] ReflectivePipeNames =
|
||||||
new String[] {
|
new String[] {
|
||||||
"RotateImage",
|
"RotateImage",
|
||||||
|
|||||||
@@ -63,10 +63,10 @@ public class CVPipelineResult implements Releasable {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Get the latency between now (wpi::Now) and the time at which the image was captured. FOOTGUN:
|
* Get the latency between now (wpi::Now) and the time at which the image was captured. FOOTGUN:
|
||||||
* the latency is relative to the time at which this method is called. Waiting to call this method
|
* the latency is relative to the time at which this method is called. Waiting to call this method
|
||||||
* will change the latency this method returns.
|
* will change the latency this method returns.
|
||||||
*/
|
*/
|
||||||
public double getLatencyMillis() {
|
public double getLatencyMillis() {
|
||||||
var now = MathUtils.wpiNanoTime();
|
var now = MathUtils.wpiNanoTime();
|
||||||
return MathUtils.nanosToMillis(now - imageCaptureTimestampNanos);
|
return MathUtils.nanosToMillis(now - imageCaptureTimestampNanos);
|
||||||
|
|||||||
@@ -43,18 +43,18 @@ public class PipelineManager {
|
|||||||
private CVPipeline currentUserPipeline = driverModePipeline;
|
private CVPipeline currentUserPipeline = driverModePipeline;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Index of the last active user-created pipeline. <br>
|
* Index of the last active user-created pipeline. <br>
|
||||||
* <br>
|
* <br>
|
||||||
* Used only when switching from any of the built-in pipelines back to a user-created pipeline.
|
* Used only when switching from any of the built-in pipelines back to a user-created pipeline.
|
||||||
*/
|
*/
|
||||||
private int lastPipelineIndex;
|
private int lastPipelineIndex;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Creates a PipelineManager with a DriverModePipeline, a Calibration3dPipeline, and all provided
|
* Creates a PipelineManager with a DriverModePipeline, a Calibration3dPipeline, and all provided
|
||||||
* pipelines.
|
* pipelines.
|
||||||
*
|
*
|
||||||
* @param userPipelines Pipelines to add to the manager.
|
* @param userPipelines Pipelines to add to the manager.
|
||||||
*/
|
*/
|
||||||
public PipelineManager(
|
public PipelineManager(
|
||||||
DriverModePipelineSettings driverSettings, List<CVPipelineSettings> userPipelines) {
|
DriverModePipelineSettings driverSettings, List<CVPipelineSettings> userPipelines) {
|
||||||
this.userPipelineSettings = new ArrayList<>(userPipelines);
|
this.userPipelineSettings = new ArrayList<>(userPipelines);
|
||||||
@@ -70,11 +70,11 @@ public class PipelineManager {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Get the settings for a pipeline by index.
|
* Get the settings for a pipeline by index.
|
||||||
*
|
*
|
||||||
* @param index Index of pipeline whose settings need getting.
|
* @param index Index of pipeline whose settings need getting.
|
||||||
* @return The gotten settings of the pipeline whose index was provided.
|
* @return The gotten settings of the pipeline whose index was provided.
|
||||||
*/
|
*/
|
||||||
public CVPipelineSettings getPipelineSettings(int index) {
|
public CVPipelineSettings getPipelineSettings(int index) {
|
||||||
if (index < 0) {
|
if (index < 0) {
|
||||||
switch (index) {
|
switch (index) {
|
||||||
@@ -92,10 +92,10 @@ public class PipelineManager {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Gets a list of nicknames for all user pipelines
|
* Gets a list of nicknames for all user pipelines
|
||||||
*
|
*
|
||||||
* @return The list of nicknames for all user pipelines
|
* @return The list of nicknames for all user pipelines
|
||||||
*/
|
*/
|
||||||
public List<String> getPipelineNicknames() {
|
public List<String> getPipelineNicknames() {
|
||||||
List<String> ret = new ArrayList<>();
|
List<String> ret = new ArrayList<>();
|
||||||
for (var p : userPipelineSettings) {
|
for (var p : userPipelineSettings) {
|
||||||
@@ -105,19 +105,19 @@ public class PipelineManager {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Gets the index of the currently active pipeline
|
* Gets the index of the currently active pipeline
|
||||||
*
|
*
|
||||||
* @return The index of the currently active pipeline
|
* @return The index of the currently active pipeline
|
||||||
*/
|
*/
|
||||||
public int getCurrentPipelineIndex() {
|
public int getCurrentPipelineIndex() {
|
||||||
return currentPipelineIndex;
|
return currentPipelineIndex;
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Get the currently active pipeline.
|
* Get the currently active pipeline.
|
||||||
*
|
*
|
||||||
* @return The currently active pipeline.
|
* @return The currently active pipeline.
|
||||||
*/
|
*/
|
||||||
public CVPipeline getCurrentUserPipeline() {
|
public CVPipeline getCurrentUserPipeline() {
|
||||||
if (currentPipelineIndex < 0) {
|
if (currentPipelineIndex < 0) {
|
||||||
switch (currentPipelineIndex) {
|
switch (currentPipelineIndex) {
|
||||||
@@ -149,22 +149,22 @@ public class PipelineManager {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Get the currently active pipelines settings
|
* Get the currently active pipelines settings
|
||||||
*
|
*
|
||||||
* @return The currently active pipelines settings
|
* @return The currently active pipelines settings
|
||||||
*/
|
*/
|
||||||
public CVPipelineSettings getCurrentPipelineSettings() {
|
public CVPipelineSettings getCurrentPipelineSettings() {
|
||||||
return getPipelineSettings(currentPipelineIndex);
|
return getPipelineSettings(currentPipelineIndex);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Internal method for setting the active pipeline. <br>
|
* Internal method for setting the active pipeline. <br>
|
||||||
* <br>
|
* <br>
|
||||||
* All externally accessible methods that intend to change the active pipeline MUST go through
|
* All externally accessible methods that intend to change the active pipeline MUST go through
|
||||||
* here to ensure all proper steps are taken.
|
* here to ensure all proper steps are taken.
|
||||||
*
|
*
|
||||||
* @param index Index of pipeline to be active
|
* @param index Index of pipeline to be active
|
||||||
*/
|
*/
|
||||||
private void setPipelineInternal(int index) {
|
private void setPipelineInternal(int index) {
|
||||||
if (index < 0) {
|
if (index < 0) {
|
||||||
lastPipelineIndex = currentPipelineIndex;
|
lastPipelineIndex = currentPipelineIndex;
|
||||||
@@ -192,33 +192,33 @@ public class PipelineManager {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Enters or exits calibration mode based on the parameter. <br>
|
* Enters or exits calibration mode based on the parameter. <br>
|
||||||
* <br>
|
* <br>
|
||||||
* Exiting returns to the last used user pipeline.
|
* Exiting returns to the last used user pipeline.
|
||||||
*
|
*
|
||||||
* @param wantsCalibration True to enter calibration mode, false to exit calibration mode.
|
* @param wantsCalibration True to enter calibration mode, false to exit calibration mode.
|
||||||
*/
|
*/
|
||||||
public void setCalibrationMode(boolean wantsCalibration) {
|
public void setCalibrationMode(boolean wantsCalibration) {
|
||||||
if (!wantsCalibration) calibration3dPipeline.finishCalibration();
|
if (!wantsCalibration) calibration3dPipeline.finishCalibration();
|
||||||
setPipelineInternal(wantsCalibration ? CAL_3D_INDEX : lastPipelineIndex);
|
setPipelineInternal(wantsCalibration ? CAL_3D_INDEX : lastPipelineIndex);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Enters or exits driver mode based on the parameter. <br>
|
* Enters or exits driver mode based on the parameter. <br>
|
||||||
* <br>
|
* <br>
|
||||||
* Exiting returns to the last used user pipeline.
|
* Exiting returns to the last used user pipeline.
|
||||||
*
|
*
|
||||||
* @param state True to enter driver mode, false to exit driver mode.
|
* @param state True to enter driver mode, false to exit driver mode.
|
||||||
*/
|
*/
|
||||||
public void setDriverMode(boolean state) {
|
public void setDriverMode(boolean state) {
|
||||||
setPipelineInternal(state ? DRIVERMODE_INDEX : lastPipelineIndex);
|
setPipelineInternal(state ? DRIVERMODE_INDEX : lastPipelineIndex);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Returns whether or not driver mode is active.
|
* Returns whether or not driver mode is active.
|
||||||
*
|
*
|
||||||
* @return Whether or not driver mode is active.
|
* @return Whether or not driver mode is active.
|
||||||
*/
|
*/
|
||||||
public boolean getDriverMode() {
|
public boolean getDriverMode() {
|
||||||
return currentPipelineIndex == DRIVERMODE_INDEX;
|
return currentPipelineIndex == DRIVERMODE_INDEX;
|
||||||
}
|
}
|
||||||
@@ -227,10 +227,10 @@ public class PipelineManager {
|
|||||||
Comparator.comparingInt(o -> o.pipelineIndex);
|
Comparator.comparingInt(o -> o.pipelineIndex);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Sorts the pipeline list by index, and reassigns their indexes to match the new order. <br>
|
* Sorts the pipeline list by index, and reassigns their indexes to match the new order. <br>
|
||||||
* <br>
|
* <br>
|
||||||
* I don't like this but I have no other ideas, and it works so ¯\_(ツ)_/¯
|
* I don't like this but I have no other ideas, and it works so ¯\_(ツ)_/¯
|
||||||
*/
|
*/
|
||||||
private void reassignIndexes() {
|
private void reassignIndexes() {
|
||||||
userPipelineSettings.sort(PipelineSettingsIndexComparator);
|
userPipelineSettings.sort(PipelineSettingsIndexComparator);
|
||||||
for (int i = 0; i < userPipelineSettings.size(); i++) {
|
for (int i = 0; i < userPipelineSettings.size(); i++) {
|
||||||
@@ -283,10 +283,10 @@ public class PipelineManager {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Remove a pipeline settings at the given index and return the new current index
|
* Remove a pipeline settings at the given index and return the new current index
|
||||||
*
|
*
|
||||||
* @param index The idx to remove
|
* @param index The idx to remove
|
||||||
*/
|
*/
|
||||||
private int removePipelineInternal(int index) {
|
private int removePipelineInternal(int index) {
|
||||||
userPipelineSettings.remove(index);
|
userPipelineSettings.remove(index);
|
||||||
currentPipelineIndex = Math.min(index, userPipelineSettings.size() - 1);
|
currentPipelineIndex = Math.min(index, userPipelineSettings.size() - 1);
|
||||||
@@ -311,11 +311,11 @@ public class PipelineManager {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Duplicate a pipeline at a given index
|
* Duplicate a pipeline at a given index
|
||||||
*
|
*
|
||||||
* @param index the index of the target pipeline
|
* @param index the index of the target pipeline
|
||||||
* @return The new index
|
* @return The new index
|
||||||
*/
|
*/
|
||||||
public int duplicatePipeline(int index) {
|
public int duplicatePipeline(int index) {
|
||||||
var settings = userPipelineSettings.get(index);
|
var settings = userPipelineSettings.get(index);
|
||||||
var newSettings = settings.clone();
|
var newSettings = settings.clone();
|
||||||
|
|||||||
@@ -50,11 +50,11 @@ import org.photonvision.vision.target.TargetModel;
|
|||||||
import org.photonvision.vision.target.TrackedTarget;
|
import org.photonvision.vision.target.TrackedTarget;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* This is the God Class
|
* This is the God Class
|
||||||
*
|
*
|
||||||
* <p>VisionModule has a pipeline manager, vision runner, and data providers. The data providers
|
* <p>VisionModule has a pipeline manager, vision runner, and data providers. The data providers
|
||||||
* provide info on settings changes. VisionModuleManager holds a list of all current vision modules.
|
* provide info on settings changes. VisionModuleManager holds a list of all current vision modules.
|
||||||
*/
|
*/
|
||||||
public class VisionModule {
|
public class VisionModule {
|
||||||
private static final int streamFPSCap = 30;
|
private static final int streamFPSCap = 30;
|
||||||
|
|
||||||
|
|||||||
@@ -39,13 +39,13 @@ public class VisionRunner {
|
|||||||
private long loopCount;
|
private long loopCount;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* VisionRunner contains a thread to run a pipeline, given a frame, and will give the result to
|
* VisionRunner contains a thread to run a pipeline, given a frame, and will give the result to
|
||||||
* the consumer.
|
* the consumer.
|
||||||
*
|
*
|
||||||
* @param frameSupplier The supplier of the latest frame.
|
* @param frameSupplier The supplier of the latest frame.
|
||||||
* @param pipelineSupplier The supplier of the current pipeline.
|
* @param pipelineSupplier The supplier of the current pipeline.
|
||||||
* @param pipelineResultConsumer The consumer of the latest result.
|
* @param pipelineResultConsumer The consumer of the latest result.
|
||||||
*/
|
*/
|
||||||
public VisionRunner(
|
public VisionRunner(
|
||||||
FrameProvider frameSupplier,
|
FrameProvider frameSupplier,
|
||||||
Supplier<CVPipeline> pipelineSupplier,
|
Supplier<CVPipeline> pipelineSupplier,
|
||||||
|
|||||||
@@ -61,11 +61,11 @@ public class VisionSourceManager {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Register new camera configs loaded from disk. This will add them to the list of configs to try
|
* Register new camera configs loaded from disk. This will add them to the list of configs to try
|
||||||
* to match, and also automatically spawn new vision processes as necessary.
|
* to match, and also automatically spawn new vision processes as necessary.
|
||||||
*
|
*
|
||||||
* @param configs The loaded camera configs.
|
* @param configs The loaded camera configs.
|
||||||
*/
|
*/
|
||||||
public void registerLoadedConfigs(Collection<CameraConfiguration> configs) {
|
public void registerLoadedConfigs(Collection<CameraConfiguration> configs) {
|
||||||
unmatchedLoadedConfigs.addAll(configs);
|
unmatchedLoadedConfigs.addAll(configs);
|
||||||
}
|
}
|
||||||
@@ -182,13 +182,13 @@ public class VisionSourceManager {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Create {@link CameraConfiguration}s based on a list of detected USB cameras and the configs on
|
* Create {@link CameraConfiguration}s based on a list of detected USB cameras and the configs on
|
||||||
* disk.
|
* disk.
|
||||||
*
|
*
|
||||||
* @param detectedCamInfos Information about currently connected USB cameras.
|
* @param detectedCamInfos Information about currently connected USB cameras.
|
||||||
* @param loadedUsbCamConfigs The USB {@link CameraConfiguration}s loaded from disk.
|
* @param loadedUsbCamConfigs The USB {@link CameraConfiguration}s loaded from disk.
|
||||||
* @return the matched configurations.
|
* @return the matched configurations.
|
||||||
*/
|
*/
|
||||||
private List<CameraConfiguration> matchUSBCameras(
|
private List<CameraConfiguration> matchUSBCameras(
|
||||||
List<UsbCameraInfo> detectedCamInfos, List<CameraConfiguration> loadedUsbCamConfigs) {
|
List<UsbCameraInfo> detectedCamInfos, List<CameraConfiguration> loadedUsbCamConfigs) {
|
||||||
var detectedCameraList = new ArrayList<>(detectedCamInfos);
|
var detectedCameraList = new ArrayList<>(detectedCamInfos);
|
||||||
@@ -319,12 +319,12 @@ public class VisionSourceManager {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Check if a given config list contains the given unique name.
|
* Check if a given config list contains the given unique name.
|
||||||
*
|
*
|
||||||
* @param configList A list of camera configs.
|
* @param configList A list of camera configs.
|
||||||
* @param uniqueName The unique name.
|
* @param uniqueName The unique name.
|
||||||
* @return If the list of configs contains the unique name.
|
* @return If the list of configs contains the unique name.
|
||||||
*/
|
*/
|
||||||
private boolean containsName(
|
private boolean containsName(
|
||||||
final List<CameraConfiguration> configList, final String uniqueName) {
|
final List<CameraConfiguration> configList, final String uniqueName) {
|
||||||
return configList.stream()
|
return configList.stream()
|
||||||
|
|||||||
@@ -57,10 +57,10 @@ public class TrackedTarget implements Releasable {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Set the approximate bouding polygon.
|
* Set the approximate bouding polygon.
|
||||||
*
|
*
|
||||||
* @param boundingPolygon List of points to copy. Not modified.
|
* @param boundingPolygon List of points to copy. Not modified.
|
||||||
*/
|
*/
|
||||||
public void setApproximateBoundingPolygon(MatOfPoint2f boundingPolygon) {
|
public void setApproximateBoundingPolygon(MatOfPoint2f boundingPolygon) {
|
||||||
if (m_approximateBoundingPolygon == null) m_approximateBoundingPolygon = new MatOfPoint2f();
|
if (m_approximateBoundingPolygon == null) m_approximateBoundingPolygon = new MatOfPoint2f();
|
||||||
boundingPolygon.copyTo(m_approximateBoundingPolygon);
|
boundingPolygon.copyTo(m_approximateBoundingPolygon);
|
||||||
|
|||||||
@@ -16,21 +16,21 @@
|
|||||||
*/
|
*/
|
||||||
package org.photonvision.common;
|
package org.photonvision.common;
|
||||||
/*
|
/*
|
||||||
* Copyright (C) 2020 Photon Vision.
|
* Copyright (C) 2020 Photon Vision.
|
||||||
*
|
*
|
||||||
* This program is free software: you can redistribute it and/or modify
|
* 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
|
* it under the terms of the GNU General Public License as published by
|
||||||
* the Free Software Foundation, either version 3 of the License, or
|
* the Free Software Foundation, either version 3 of the License, or
|
||||||
* (at your option) any later version.
|
* (at your option) any later version.
|
||||||
*
|
*
|
||||||
* This program is distributed in the hope that it will be useful,
|
* This program is distributed in the hope that it will be useful,
|
||||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
* GNU General Public License for more details.
|
* GNU General Public License for more details.
|
||||||
*
|
*
|
||||||
* You should have received a copy of the GNU General Public License
|
* You should have received a copy of the GNU General Public License
|
||||||
* along with this program. If not, see <https://www.gnu.org/licenses/>.
|
* along with this program. If not, see <https://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
import java.util.ArrayList;
|
import java.util.ArrayList;
|
||||||
import java.util.Collections;
|
import java.util.Collections;
|
||||||
import java.util.List;
|
import java.util.List;
|
||||||
|
|||||||
@@ -314,15 +314,15 @@ public class Calibrate3dPipeTest {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Uses a given camera coefficents matrix set to "undistort" every image file found in a given
|
* Uses a given camera coefficents matrix set to "undistort" every image file found in a given
|
||||||
* directory and display them. Provides an easy way to visually debug the results of the
|
* directory and display them. Provides an easy way to visually debug the results of the
|
||||||
* calibration routine. Seems to play havoc with CI and takes a chunk of time, so shouldn't
|
* calibration routine. Seems to play havoc with CI and takes a chunk of time, so shouldn't
|
||||||
* usually be left active in tests.
|
* usually be left active in tests.
|
||||||
*
|
*
|
||||||
* @param directoryListing
|
* @param directoryListing
|
||||||
* @param imgRes
|
* @param imgRes
|
||||||
* @param cal
|
* @param cal
|
||||||
*/
|
*/
|
||||||
@SuppressWarnings("unused")
|
@SuppressWarnings("unused")
|
||||||
private void visuallyDebugDistortion(
|
private void visuallyDebugDistortion(
|
||||||
File[] directoryListing, Size imgRes, CameraCalibrationCoefficients cal) {
|
File[] directoryListing, Size imgRes, CameraCalibrationCoefficients cal) {
|
||||||
|
|||||||
@@ -40,10 +40,10 @@ public class PhotonCamera {
|
|||||||
Packet packet = new Packet(1);
|
Packet packet = new Packet(1);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Constructs a PhotonCamera from a root table.
|
* Constructs a PhotonCamera from a root table.
|
||||||
*
|
*
|
||||||
* @param rootTable The root table that the camera is broadcasting information over.
|
* @param rootTable The root table that the camera is broadcasting information over.
|
||||||
*/
|
*/
|
||||||
public PhotonCamera(NetworkTable rootTable) {
|
public PhotonCamera(NetworkTable rootTable) {
|
||||||
path = rootTable.getPath();
|
path = rootTable.getPath();
|
||||||
rawBytesEntry = rootTable.getEntry("rawBytes");
|
rawBytesEntry = rootTable.getEntry("rawBytes");
|
||||||
@@ -56,19 +56,19 @@ public class PhotonCamera {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Constructs a PhotonCamera from the name of the camera.
|
* Constructs a PhotonCamera from the name of the camera.
|
||||||
*
|
*
|
||||||
* @param cameraName The nickname of the camera (found in the PhotonVision UI).
|
* @param cameraName The nickname of the camera (found in the PhotonVision UI).
|
||||||
*/
|
*/
|
||||||
public PhotonCamera(String cameraName) {
|
public PhotonCamera(String cameraName) {
|
||||||
this(NetworkTableInstance.getDefault().getTable("photonvision").getSubTable(cameraName));
|
this(NetworkTableInstance.getDefault().getTable("photonvision").getSubTable(cameraName));
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Returns the latest pipeline result.
|
* Returns the latest pipeline result.
|
||||||
*
|
*
|
||||||
* @return The latest pipeline result.
|
* @return The latest pipeline result.
|
||||||
*/
|
*/
|
||||||
public PhotonPipelineResult getLatestResult() {
|
public PhotonPipelineResult getLatestResult() {
|
||||||
verifyVersion();
|
verifyVersion();
|
||||||
|
|
||||||
@@ -88,66 +88,66 @@ public class PhotonCamera {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Returns whether the camera is in driver mode.
|
* Returns whether the camera is in driver mode.
|
||||||
*
|
*
|
||||||
* @return Whether the camera is in driver mode.
|
* @return Whether the camera is in driver mode.
|
||||||
*/
|
*/
|
||||||
public boolean getDriverMode() {
|
public boolean getDriverMode() {
|
||||||
return driverModeEntry.getBoolean(false);
|
return driverModeEntry.getBoolean(false);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Toggles driver mode.
|
* Toggles driver mode.
|
||||||
*
|
*
|
||||||
* @param driverMode Whether to set driver mode.
|
* @param driverMode Whether to set driver mode.
|
||||||
*/
|
*/
|
||||||
public void setDriverMode(boolean driverMode) {
|
public void setDriverMode(boolean driverMode) {
|
||||||
driverModeEntry.setBoolean(driverMode);
|
driverModeEntry.setBoolean(driverMode);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Request the camera to save a new image file from the input camera stream with overlays. Images
|
* Request the camera to save a new image file from the input camera stream with overlays. Images
|
||||||
* take up space in the filesystem of the PhotonCamera. Calling it frequently will fill up disk
|
* take up space in the filesystem of the PhotonCamera. Calling it frequently will fill up disk
|
||||||
* space and eventually cause the system to stop working. Clear out images in
|
* space and eventually cause the system to stop working. Clear out images in
|
||||||
* /opt/photonvision/photonvision_config/imgSaves frequently to prevent issues.
|
* /opt/photonvision/photonvision_config/imgSaves frequently to prevent issues.
|
||||||
*/
|
*/
|
||||||
public void takeInputSnapshot() {
|
public void takeInputSnapshot() {
|
||||||
inputSaveImgEntry.setBoolean(true);
|
inputSaveImgEntry.setBoolean(true);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Request the camera to save a new image file from the output stream with overlays. Images take
|
* Request the camera to save a new image file from the output stream with overlays. Images take
|
||||||
* up space in the filesystem of the PhotonCamera. Calling it frequently will fill up disk space
|
* up space in the filesystem of the PhotonCamera. Calling it frequently will fill up disk space
|
||||||
* and eventually cause the system to stop working. Clear out images in
|
* and eventually cause the system to stop working. Clear out images in
|
||||||
* /opt/photonvision/photonvision_config/imgSaves frequently to prevent issues.
|
* /opt/photonvision/photonvision_config/imgSaves frequently to prevent issues.
|
||||||
*/
|
*/
|
||||||
public void takeOutputSnapshot() {
|
public void takeOutputSnapshot() {
|
||||||
outputSaveImgEntry.setBoolean(true);
|
outputSaveImgEntry.setBoolean(true);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Returns the active pipeline index.
|
* Returns the active pipeline index.
|
||||||
*
|
*
|
||||||
* @return The active pipeline index.
|
* @return The active pipeline index.
|
||||||
*/
|
*/
|
||||||
public int getPipelineIndex() {
|
public int getPipelineIndex() {
|
||||||
return pipelineIndexEntry.getNumber(0).intValue();
|
return pipelineIndexEntry.getNumber(0).intValue();
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Allows the user to select the active pipeline index.
|
* Allows the user to select the active pipeline index.
|
||||||
*
|
*
|
||||||
* @param index The active pipeline index.
|
* @param index The active pipeline index.
|
||||||
*/
|
*/
|
||||||
public void setPipelineIndex(int index) {
|
public void setPipelineIndex(int index) {
|
||||||
pipelineIndexEntry.setNumber(index);
|
pipelineIndexEntry.setNumber(index);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Returns the current LED mode.
|
* Returns the current LED mode.
|
||||||
*
|
*
|
||||||
* @return The current LED mode.
|
* @return The current LED mode.
|
||||||
*/
|
*/
|
||||||
public VisionLEDMode getLEDMode() {
|
public VisionLEDMode getLEDMode() {
|
||||||
int value = ledModeEntry.getNumber(-1).intValue();
|
int value = ledModeEntry.getNumber(-1).intValue();
|
||||||
switch (value) {
|
switch (value) {
|
||||||
@@ -164,22 +164,22 @@ public class PhotonCamera {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Sets the LED mode.
|
* Sets the LED mode.
|
||||||
*
|
*
|
||||||
* @param led The mode to set to.
|
* @param led The mode to set to.
|
||||||
*/
|
*/
|
||||||
public void setLED(VisionLEDMode led) {
|
public void setLED(VisionLEDMode led) {
|
||||||
ledModeEntry.setNumber(led.value);
|
ledModeEntry.setNumber(led.value);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Returns whether the latest target result has targets.
|
* Returns whether the latest target result has targets.
|
||||||
*
|
*
|
||||||
* <p>This method is deprecated; {@link PhotonPipelineResult#hasTargets()} should be used instead.
|
* <p>This method is deprecated; {@link PhotonPipelineResult#hasTargets()} should be used instead.
|
||||||
*
|
*
|
||||||
* @deprecated This method should be replaced with {@link PhotonPipelineResult#hasTargets()}
|
* @deprecated This method should be replaced with {@link PhotonPipelineResult#hasTargets()}
|
||||||
* @return Whether the latest target result has targets.
|
* @return Whether the latest target result has targets.
|
||||||
*/
|
*/
|
||||||
@Deprecated
|
@Deprecated
|
||||||
public boolean hasTargets() {
|
public boolean hasTargets() {
|
||||||
return getLatestResult().hasTargets();
|
return getLatestResult().hasTargets();
|
||||||
|
|||||||
@@ -27,25 +27,25 @@ public final class PhotonUtils {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Algorithm from https://docs.limelightvision.io/en/latest/cs_estimating_distance.html Estimates
|
* Algorithm from https://docs.limelightvision.io/en/latest/cs_estimating_distance.html Estimates
|
||||||
* range to a target using the target's elevation. This method can produce more stable results
|
* range to a target using the target's elevation. This method can produce more stable results
|
||||||
* than SolvePNP when well tuned, if the full 6d robot pose is not required. Note that this method
|
* than SolvePNP when well tuned, if the full 6d robot pose is not required. Note that this method
|
||||||
* requires the camera to have 0 roll (not be skewed clockwise or CCW relative to the floor), and
|
* requires the camera to have 0 roll (not be skewed clockwise or CCW relative to the floor), and
|
||||||
* for there to exist a height differential between goal and camera. The larger this differential,
|
* for there to exist a height differential between goal and camera. The larger this differential,
|
||||||
* the more accurate the distance estimate will be.
|
* the more accurate the distance estimate will be.
|
||||||
*
|
*
|
||||||
* <p>Units can be converted using the {@link edu.wpi.first.math.util.Units} class.
|
* <p>Units can be converted using the {@link edu.wpi.first.math.util.Units} class.
|
||||||
*
|
*
|
||||||
* @param cameraHeightMeters The physical height of the camera off the floor in meters.
|
* @param cameraHeightMeters The physical height of the camera off the floor in meters.
|
||||||
* @param targetHeightMeters The physical height of the target off the floor in meters. This
|
* @param targetHeightMeters The physical height of the target off the floor in meters. This
|
||||||
* should be the height of whatever is being targeted (i.e. if the targeting region is set to
|
* should be the height of whatever is being targeted (i.e. if the targeting region is set to
|
||||||
* top, this should be the height of the top of the target).
|
* top, this should be the height of the top of the target).
|
||||||
* @param cameraPitchRadians The pitch of the camera from the horizontal plane in radians.
|
* @param cameraPitchRadians The pitch of the camera from the horizontal plane in radians.
|
||||||
* Positive values up.
|
* Positive values up.
|
||||||
* @param targetPitchRadians The pitch of the target in the camera's lens in radians. Positive
|
* @param targetPitchRadians The pitch of the target in the camera's lens in radians. Positive
|
||||||
* values up.
|
* values up.
|
||||||
* @return The estimated distance to the target in meters.
|
* @return The estimated distance to the target in meters.
|
||||||
*/
|
*/
|
||||||
public static double calculateDistanceToTargetMeters(
|
public static double calculateDistanceToTargetMeters(
|
||||||
double cameraHeightMeters,
|
double cameraHeightMeters,
|
||||||
double targetHeightMeters,
|
double targetHeightMeters,
|
||||||
@@ -56,12 +56,12 @@ public final class PhotonUtils {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Estimate the {@link Translation2d} of the target relative to the camera.
|
* Estimate the {@link Translation2d} of the target relative to the camera.
|
||||||
*
|
*
|
||||||
* @param targetDistanceMeters The distance to the target in meters.
|
* @param targetDistanceMeters The distance to the target in meters.
|
||||||
* @param yaw The observed yaw of the target.
|
* @param yaw The observed yaw of the target.
|
||||||
* @return The target's camera-relative translation.
|
* @return The target's camera-relative translation.
|
||||||
*/
|
*/
|
||||||
public static Translation2d estimateCameraToTargetTranslation(
|
public static Translation2d estimateCameraToTargetTranslation(
|
||||||
double targetDistanceMeters, Rotation2d yaw) {
|
double targetDistanceMeters, Rotation2d yaw) {
|
||||||
return new Translation2d(
|
return new Translation2d(
|
||||||
@@ -69,25 +69,25 @@ public final class PhotonUtils {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Estimate the position of the robot in the field.
|
* Estimate the position of the robot in the field.
|
||||||
*
|
*
|
||||||
* @param cameraHeightMeters The physical height of the camera off the floor in meters.
|
* @param cameraHeightMeters The physical height of the camera off the floor in meters.
|
||||||
* @param targetHeightMeters The physical height of the target off the floor in meters. This
|
* @param targetHeightMeters The physical height of the target off the floor in meters. This
|
||||||
* should be the height of whatever is being targeted (i.e. if the targeting region is set to
|
* should be the height of whatever is being targeted (i.e. if the targeting region is set to
|
||||||
* top, this should be the height of the top of the target).
|
* top, this should be the height of the top of the target).
|
||||||
* @param cameraPitchRadians The pitch of the camera from the horizontal plane in radians.
|
* @param cameraPitchRadians The pitch of the camera from the horizontal plane in radians.
|
||||||
* Positive values up.
|
* Positive values up.
|
||||||
* @param targetPitchRadians The pitch of the target in the camera's lens in radians. Positive
|
* @param targetPitchRadians The pitch of the target in the camera's lens in radians. Positive
|
||||||
* values up.
|
* values up.
|
||||||
* @param targetYaw The observed yaw of the target. Note that this *must* be CCW-positive, and
|
* @param targetYaw The observed yaw of the target. Note that this *must* be CCW-positive, and
|
||||||
* Photon returns CW-positive.
|
* Photon returns CW-positive.
|
||||||
* @param gyroAngle The current robot gyro angle, likely from odometry.
|
* @param gyroAngle The current robot gyro angle, likely from odometry.
|
||||||
* @param fieldToTarget A Pose2d representing the target position in the field coordinate system.
|
* @param fieldToTarget A Pose2d representing the target position in the field coordinate system.
|
||||||
* @param cameraToRobot The position of the robot relative to the camera. If the camera was
|
* @param cameraToRobot The position of the robot relative to the camera. If the camera was
|
||||||
* mounted 3 inches behind the "origin" (usually physical center) of the robot, this would be
|
* mounted 3 inches behind the "origin" (usually physical center) of the robot, this would be
|
||||||
* Transform2d(3 inches, 0 inches, 0 degrees).
|
* Transform2d(3 inches, 0 inches, 0 degrees).
|
||||||
* @return The position of the robot in the field.
|
* @return The position of the robot in the field.
|
||||||
*/
|
*/
|
||||||
public static Pose2d estimateFieldToRobot(
|
public static Pose2d estimateFieldToRobot(
|
||||||
double cameraHeightMeters,
|
double cameraHeightMeters,
|
||||||
double targetHeightMeters,
|
double targetHeightMeters,
|
||||||
@@ -110,17 +110,17 @@ public final class PhotonUtils {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Estimates a {@link Transform2d} that maps the camera position to the target position, using the
|
* Estimates a {@link Transform2d} that maps the camera position to the target position, using the
|
||||||
* robot's gyro. Note that the gyro angle provided *must* line up with the field coordinate system
|
* robot's gyro. Note that the gyro angle provided *must* line up with the field coordinate system
|
||||||
* -- that is, it should read zero degrees when pointed towards the opposing alliance station, and
|
* -- that is, it should read zero degrees when pointed towards the opposing alliance station, and
|
||||||
* increase as the robot rotates CCW.
|
* increase as the robot rotates CCW.
|
||||||
*
|
*
|
||||||
* @param cameraToTargetTranslation A Translation2d that encodes the x/y position of the target
|
* @param cameraToTargetTranslation A Translation2d that encodes the x/y position of the target
|
||||||
* relative to the camera.
|
* relative to the camera.
|
||||||
* @param fieldToTarget A Pose2d representing the target position in the field coordinate system.
|
* @param fieldToTarget A Pose2d representing the target position in the field coordinate system.
|
||||||
* @param gyroAngle The current robot gyro angle, likely from odometry.
|
* @param gyroAngle The current robot gyro angle, likely from odometry.
|
||||||
* @return A Transform2d that takes us from the camera to the target.
|
* @return A Transform2d that takes us from the camera to the target.
|
||||||
*/
|
*/
|
||||||
public static Transform2d estimateCameraToTarget(
|
public static Transform2d estimateCameraToTarget(
|
||||||
Translation2d cameraToTargetTranslation, Pose2d fieldToTarget, Rotation2d gyroAngle) {
|
Translation2d cameraToTargetTranslation, Pose2d fieldToTarget, Rotation2d gyroAngle) {
|
||||||
// This pose maps our camera at the origin out to our target, in the robot
|
// This pose maps our camera at the origin out to our target, in the robot
|
||||||
@@ -133,31 +133,31 @@ public final class PhotonUtils {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Estimates the pose of the robot in the field coordinate system, given the position of the
|
* Estimates the pose of the robot in the field coordinate system, given the position of the
|
||||||
* target relative to the camera, the target relative to the field, and the robot relative to the
|
* target relative to the camera, the target relative to the field, and the robot relative to the
|
||||||
* camera.
|
* camera.
|
||||||
*
|
*
|
||||||
* @param cameraToTarget The position of the target relative to the camera.
|
* @param cameraToTarget The position of the target relative to the camera.
|
||||||
* @param fieldToTarget The position of the target in the field.
|
* @param fieldToTarget The position of the target in the field.
|
||||||
* @param cameraToRobot The position of the robot relative to the camera. If the camera was
|
* @param cameraToRobot The position of the robot relative to the camera. If the camera was
|
||||||
* mounted 3 inches behind the "origin" (usually physical center) of the robot, this would be
|
* mounted 3 inches behind the "origin" (usually physical center) of the robot, this would be
|
||||||
* Transform2d(3 inches, 0 inches, 0 degrees).
|
* Transform2d(3 inches, 0 inches, 0 degrees).
|
||||||
* @return The position of the robot in the field.
|
* @return The position of the robot in the field.
|
||||||
*/
|
*/
|
||||||
public static Pose2d estimateFieldToRobot(
|
public static Pose2d estimateFieldToRobot(
|
||||||
Transform2d cameraToTarget, Pose2d fieldToTarget, Transform2d cameraToRobot) {
|
Transform2d cameraToTarget, Pose2d fieldToTarget, Transform2d cameraToRobot) {
|
||||||
return estimateFieldToCamera(cameraToTarget, fieldToTarget).transformBy(cameraToRobot);
|
return estimateFieldToCamera(cameraToTarget, fieldToTarget).transformBy(cameraToRobot);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Estimates the pose of the camera in the field coordinate system, given the position of the
|
* Estimates the pose of the camera in the field coordinate system, given the position of the
|
||||||
* target relative to the camera, and the target relative to the field. This *only* tracks the
|
* target relative to the camera, and the target relative to the field. This *only* tracks the
|
||||||
* position of the camera, not the position of the robot itself.
|
* position of the camera, not the position of the robot itself.
|
||||||
*
|
*
|
||||||
* @param cameraToTarget The position of the target relative to the camera.
|
* @param cameraToTarget The position of the target relative to the camera.
|
||||||
* @param fieldToTarget The position of the target in the field.
|
* @param fieldToTarget The position of the target in the field.
|
||||||
* @return The position of the camera in the field.
|
* @return The position of the camera in the field.
|
||||||
*/
|
*/
|
||||||
public static Pose2d estimateFieldToCamera(Transform2d cameraToTarget, Pose2d fieldToTarget) {
|
public static Pose2d estimateFieldToCamera(Transform2d cameraToTarget, Pose2d fieldToTarget) {
|
||||||
var targetToCamera = cameraToTarget.inverse();
|
var targetToCamera = cameraToTarget.inverse();
|
||||||
return fieldToTarget.transformBy(targetToCamera);
|
return fieldToTarget.transformBy(targetToCamera);
|
||||||
|
|||||||
@@ -36,10 +36,10 @@ public class SimPhotonCamera extends PhotonCamera {
|
|||||||
private final NetworkTableEntry targetPoseEntry;
|
private final NetworkTableEntry targetPoseEntry;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Constructs a Simulated PhotonCamera from a root table.
|
* Constructs a Simulated PhotonCamera from a root table.
|
||||||
*
|
*
|
||||||
* @param rootTable The root table that the camera is broadcasting information over.
|
* @param rootTable The root table that the camera is broadcasting information over.
|
||||||
*/
|
*/
|
||||||
public SimPhotonCamera(NetworkTable rootTable) {
|
public SimPhotonCamera(NetworkTable rootTable) {
|
||||||
super(rootTable);
|
super(rootTable);
|
||||||
|
|
||||||
@@ -53,53 +53,53 @@ public class SimPhotonCamera extends PhotonCamera {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Constructs a Simulated PhotonCamera from the name of the camera.
|
* Constructs a Simulated PhotonCamera from the name of the camera.
|
||||||
*
|
*
|
||||||
* @param cameraName The nickname of the camera (found in the PhotonVision UI).
|
* @param cameraName The nickname of the camera (found in the PhotonVision UI).
|
||||||
*/
|
*/
|
||||||
public SimPhotonCamera(String cameraName) {
|
public SimPhotonCamera(String cameraName) {
|
||||||
this(NetworkTableInstance.getDefault().getTable("photonvision").getSubTable(cameraName));
|
this(NetworkTableInstance.getDefault().getTable("photonvision").getSubTable(cameraName));
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Simulate one processed frame of vision data, putting one result to NT.
|
* Simulate one processed frame of vision data, putting one result to NT.
|
||||||
*
|
*
|
||||||
* @param latencyMillis Latency of the provided frame
|
* @param latencyMillis Latency of the provided frame
|
||||||
* @param targets Each target detected
|
* @param targets Each target detected
|
||||||
*/
|
*/
|
||||||
public void submitProcessedFrame(double latencyMillis, PhotonTrackedTarget... targets) {
|
public void submitProcessedFrame(double latencyMillis, PhotonTrackedTarget... targets) {
|
||||||
submitProcessedFrame(latencyMillis, Arrays.asList(targets));
|
submitProcessedFrame(latencyMillis, Arrays.asList(targets));
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Simulate one processed frame of vision data, putting one result to NT.
|
* Simulate one processed frame of vision data, putting one result to NT.
|
||||||
*
|
*
|
||||||
* @param latencyMillis Latency of the provided frame
|
* @param latencyMillis Latency of the provided frame
|
||||||
* @param sortMode Order in which to sort targets
|
* @param sortMode Order in which to sort targets
|
||||||
* @param targets Each target detected
|
* @param targets Each target detected
|
||||||
*/
|
*/
|
||||||
public void submitProcessedFrame(
|
public void submitProcessedFrame(
|
||||||
double latencyMillis, PhotonTargetSortMode sortMode, PhotonTrackedTarget... targets) {
|
double latencyMillis, PhotonTargetSortMode sortMode, PhotonTrackedTarget... targets) {
|
||||||
submitProcessedFrame(latencyMillis, sortMode, Arrays.asList(targets));
|
submitProcessedFrame(latencyMillis, sortMode, Arrays.asList(targets));
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Simulate one processed frame of vision data, putting one result to NT.
|
* Simulate one processed frame of vision data, putting one result to NT.
|
||||||
*
|
*
|
||||||
* @param latencyMillis Latency of the provided frame
|
* @param latencyMillis Latency of the provided frame
|
||||||
* @param targetList List of targets detected
|
* @param targetList List of targets detected
|
||||||
*/
|
*/
|
||||||
public void submitProcessedFrame(double latencyMillis, List<PhotonTrackedTarget> targetList) {
|
public void submitProcessedFrame(double latencyMillis, List<PhotonTrackedTarget> targetList) {
|
||||||
submitProcessedFrame(latencyMillis, null, targetList);
|
submitProcessedFrame(latencyMillis, null, targetList);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Simulate one processed frame of vision data, putting one result to NT.
|
* Simulate one processed frame of vision data, putting one result to NT.
|
||||||
*
|
*
|
||||||
* @param latencyMillis Latency of the provided frame
|
* @param latencyMillis Latency of the provided frame
|
||||||
* @param sortMode Order in which to sort targets
|
* @param sortMode Order in which to sort targets
|
||||||
* @param targetList List of targets detected
|
* @param targetList List of targets detected
|
||||||
*/
|
*/
|
||||||
public void submitProcessedFrame(
|
public void submitProcessedFrame(
|
||||||
double latencyMillis, PhotonTargetSortMode sortMode, List<PhotonTrackedTarget> targetList) {
|
double latencyMillis, PhotonTargetSortMode sortMode, List<PhotonTrackedTarget> targetList) {
|
||||||
latencyMillisEntry.setDouble(latencyMillis);
|
latencyMillisEntry.setDouble(latencyMillis);
|
||||||
|
|||||||
@@ -38,28 +38,28 @@ public class SimVisionSystem {
|
|||||||
ArrayList<SimVisionTarget> tgtList;
|
ArrayList<SimVisionTarget> tgtList;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Create a simulated vision system involving a camera and coprocessor mounted on a mobile robot
|
* Create a simulated vision system involving a camera and coprocessor mounted on a mobile robot
|
||||||
* running PhotonVision, detecting one or more targets scattered around the field. This assumes a
|
* running PhotonVision, detecting one or more targets scattered around the field. This assumes a
|
||||||
* fairly simple and distortion-less pinhole camera model.
|
* fairly simple and distortion-less pinhole camera model.
|
||||||
*
|
*
|
||||||
* @param camName Name of the PhotonVision camera to create. Align it with the settings you use in
|
* @param camName Name of the PhotonVision camera to create. Align it with the settings you use in
|
||||||
* the PhotonVision GUI.
|
* the PhotonVision GUI.
|
||||||
* @param camDiagFOVDegrees Diagonal Field of View of the camera used. Align it with the
|
* @param camDiagFOVDegrees Diagonal Field of View of the camera used. Align it with the
|
||||||
* manufacturer specifications, and/or whatever is configured in the PhotonVision Setting
|
* manufacturer specifications, and/or whatever is configured in the PhotonVision Setting
|
||||||
* page.
|
* page.
|
||||||
* @param camPitchDegrees pitch of the camera's view axis back from horizontal. Make this the same
|
* @param camPitchDegrees pitch of the camera's view axis back from horizontal. Make this the same
|
||||||
* as whatever is configured in the PhotonVision Setting page.
|
* as whatever is configured in the PhotonVision Setting page.
|
||||||
* @param cameraToRobot Pose Transform to move from the camera's mount position to the robot's
|
* @param cameraToRobot Pose Transform to move from the camera's mount position to the robot's
|
||||||
* position
|
* position
|
||||||
* @param cameraHeightOffGroundMeters Height of the camera off the ground in meters
|
* @param cameraHeightOffGroundMeters Height of the camera off the ground in meters
|
||||||
* @param maxLEDRangeMeters Maximum distance at which your camera can illuminate the target and
|
* @param maxLEDRangeMeters Maximum distance at which your camera can illuminate the target and
|
||||||
* make it visible. Set to 9000 or more if your vision system does not rely on LED's.
|
* make it visible. Set to 9000 or more if your vision system does not rely on LED's.
|
||||||
* @param cameraResWidth Width of your camera's image sensor in pixels
|
* @param cameraResWidth Width of your camera's image sensor in pixels
|
||||||
* @param cameraResHeight Height of your camera's image sensor in pixels
|
* @param cameraResHeight Height of your camera's image sensor in pixels
|
||||||
* @param minTargetArea Minimum area that that the target should be before it's recognized as a
|
* @param minTargetArea Minimum area that that the target should be before it's recognized as a
|
||||||
* target by the camera. Match this with your contour filtering settings in the PhotonVision
|
* target by the camera. Match this with your contour filtering settings in the PhotonVision
|
||||||
* GUI.
|
* GUI.
|
||||||
*/
|
*/
|
||||||
public SimVisionSystem(
|
public SimVisionSystem(
|
||||||
String camName,
|
String camName,
|
||||||
double camDiagFOVDegrees,
|
double camDiagFOVDegrees,
|
||||||
@@ -88,24 +88,24 @@ public class SimVisionSystem {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Add a target on the field which your vision system is designed to detect. The PhotonCamera from
|
* Add a target on the field which your vision system is designed to detect. The PhotonCamera from
|
||||||
* this system will report the location of the robot relative to the subset of these targets which
|
* this system will report the location of the robot relative to the subset of these targets which
|
||||||
* are visible from the given robot position.
|
* are visible from the given robot position.
|
||||||
*
|
*
|
||||||
* @param target Target to add to the simulated field
|
* @param target Target to add to the simulated field
|
||||||
*/
|
*/
|
||||||
public void addSimVisionTarget(SimVisionTarget target) {
|
public void addSimVisionTarget(SimVisionTarget target) {
|
||||||
tgtList.add(target);
|
tgtList.add(target);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Adjust the camera position relative to the robot. Use this if your camera is on a gimbal or
|
* Adjust the camera position relative to the robot. Use this if your camera is on a gimbal or
|
||||||
* turret or some other mobile platform.
|
* turret or some other mobile platform.
|
||||||
*
|
*
|
||||||
* @param newCameraToRobot New Transform from the robot to the camera
|
* @param newCameraToRobot New Transform from the robot to the camera
|
||||||
* @param newCamHeightMeters New height of the camera off the floor
|
* @param newCamHeightMeters New height of the camera off the floor
|
||||||
* @param newCamPitchDegrees New pitch of the camera axis back from horizontal
|
* @param newCamPitchDegrees New pitch of the camera axis back from horizontal
|
||||||
*/
|
*/
|
||||||
public void moveCamera(
|
public void moveCamera(
|
||||||
Transform2d newCameraToRobot, double newCamHeightMeters, double newCamPitchDegrees) {
|
Transform2d newCameraToRobot, double newCamHeightMeters, double newCamPitchDegrees) {
|
||||||
this.cameraToRobot = newCameraToRobot;
|
this.cameraToRobot = newCameraToRobot;
|
||||||
@@ -114,13 +114,13 @@ public class SimVisionSystem {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Periodic update. Call this once per frame of image data you wish to process and send to
|
* Periodic update. Call this once per frame of image data you wish to process and send to
|
||||||
* NetworkTables
|
* NetworkTables
|
||||||
*
|
*
|
||||||
* @param robotPoseMeters current pose of the robot on the field. Will be used to calculate which
|
* @param robotPoseMeters current pose of the robot on the field. Will be used to calculate which
|
||||||
* targets are actually in view, where they are at relative to the robot, and relevant
|
* targets are actually in view, where they are at relative to the robot, and relevant
|
||||||
* PhotonVision parameters.
|
* PhotonVision parameters.
|
||||||
*/
|
*/
|
||||||
public void processFrame(Pose2d robotPoseMeters) {
|
public void processFrame(Pose2d robotPoseMeters) {
|
||||||
Pose2d cameraPos = robotPoseMeters.transformBy(cameraToRobot.inverse());
|
Pose2d cameraPos = robotPoseMeters.transformBy(cameraToRobot.inverse());
|
||||||
|
|
||||||
|
|||||||
@@ -26,15 +26,15 @@ public class SimVisionTarget {
|
|||||||
double tgtAreaMeters2;
|
double tgtAreaMeters2;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Describes a vision target located somewhere on the field that your SimVisionSystem can detect.
|
* Describes a vision target located somewhere on the field that your SimVisionSystem can detect.
|
||||||
*
|
*
|
||||||
* @param targetPos Pose2d of the target on the field. Define it such that, if you are standing on
|
* @param targetPos Pose2d of the target on the field. Define it such that, if you are standing on
|
||||||
* the middle of the field facing the target, the Y axis points to your left, and the X axis
|
* the middle of the field facing the target, the Y axis points to your left, and the X axis
|
||||||
* points away from you.
|
* points away from you.
|
||||||
* @param targetHeightAboveGroundMeters Height of the target above the field plane, in meters.
|
* @param targetHeightAboveGroundMeters Height of the target above the field plane, in meters.
|
||||||
* @param targetWidthMeters Width of the outer bounding box of the target in meters.
|
* @param targetWidthMeters Width of the outer bounding box of the target in meters.
|
||||||
* @param targetHeightMeters Pair Height of the outer bounding box of the target in meters.
|
* @param targetHeightMeters Pair Height of the outer bounding box of the target in meters.
|
||||||
*/
|
*/
|
||||||
public SimVisionTarget(
|
public SimVisionTarget(
|
||||||
Pose2d targetPos,
|
Pose2d targetPos,
|
||||||
double targetHeightAboveGroundMeters,
|
double targetHeightAboveGroundMeters,
|
||||||
|
|||||||
@@ -233,9 +233,9 @@ public class RequestHandler {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Note that this doesn't actually restart the program itself -- instead, it relies on systemd or
|
* Note that this doesn't actually restart the program itself -- instead, it relies on systemd or
|
||||||
* an equivalent.
|
* an equivalent.
|
||||||
*/
|
*/
|
||||||
public static void restartProgramInternal() {
|
public static void restartProgramInternal() {
|
||||||
if (Platform.isRaspberryPi()) {
|
if (Platform.isRaspberryPi()) {
|
||||||
try {
|
try {
|
||||||
|
|||||||
@@ -29,8 +29,8 @@ import org.photonvision.common.logging.Logger;
|
|||||||
|
|
||||||
@SuppressWarnings("rawtypes")
|
@SuppressWarnings("rawtypes")
|
||||||
/*
|
/*
|
||||||
* DO NOT use logging in this class. If you do, the logs will recurse forever!
|
* DO NOT use logging in this class. If you do, the logs will recurse forever!
|
||||||
*/
|
*/
|
||||||
class UIOutboundSubscriber extends DataChangeSubscriber {
|
class UIOutboundSubscriber extends DataChangeSubscriber {
|
||||||
Logger logger = new Logger(UIOutboundSubscriber.class, LogGroup.WebServer);
|
Logger logger = new Logger(UIOutboundSubscriber.class, LogGroup.WebServer);
|
||||||
|
|
||||||
|
|||||||
@@ -26,20 +26,20 @@ public class Packet {
|
|||||||
int readPos, writePos;
|
int readPos, writePos;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Constructs an empty packet.
|
* Constructs an empty packet.
|
||||||
*
|
*
|
||||||
* @param size The size of the packet buffer.
|
* @param size The size of the packet buffer.
|
||||||
*/
|
*/
|
||||||
public Packet(int size) {
|
public Packet(int size) {
|
||||||
this.size = size;
|
this.size = size;
|
||||||
packetData = new byte[size];
|
packetData = new byte[size];
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Constructs a packet with the given data.
|
* Constructs a packet with the given data.
|
||||||
*
|
*
|
||||||
* @param data The packet data.
|
* @param data The packet data.
|
||||||
*/
|
*/
|
||||||
public Packet(byte[] data) {
|
public Packet(byte[] data) {
|
||||||
packetData = data;
|
packetData = data;
|
||||||
size = packetData.length;
|
size = packetData.length;
|
||||||
@@ -57,38 +57,38 @@ public class Packet {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Returns the packet data.
|
* Returns the packet data.
|
||||||
*
|
*
|
||||||
* @return The packet data.
|
* @return The packet data.
|
||||||
*/
|
*/
|
||||||
public byte[] getData() {
|
public byte[] getData() {
|
||||||
return packetData;
|
return packetData;
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Sets the packet data.
|
* Sets the packet data.
|
||||||
*
|
*
|
||||||
* @param data The packet data.
|
* @param data The packet data.
|
||||||
*/
|
*/
|
||||||
public void setData(byte[] data) {
|
public void setData(byte[] data) {
|
||||||
packetData = data;
|
packetData = data;
|
||||||
size = data.length;
|
size = data.length;
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Encodes the byte into the packet.
|
* Encodes the byte into the packet.
|
||||||
*
|
*
|
||||||
* @param src The byte to encode.
|
* @param src The byte to encode.
|
||||||
*/
|
*/
|
||||||
public void encode(byte src) {
|
public void encode(byte src) {
|
||||||
packetData[writePos++] = src;
|
packetData[writePos++] = src;
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Encodes the integer into the packet.
|
* Encodes the integer into the packet.
|
||||||
*
|
*
|
||||||
* @param src The integer to encode.
|
* @param src The integer to encode.
|
||||||
*/
|
*/
|
||||||
public void encode(int src) {
|
public void encode(int src) {
|
||||||
packetData[writePos++] = (byte) (src >>> 24);
|
packetData[writePos++] = (byte) (src >>> 24);
|
||||||
packetData[writePos++] = (byte) (src >>> 16);
|
packetData[writePos++] = (byte) (src >>> 16);
|
||||||
@@ -97,10 +97,10 @@ public class Packet {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Encodes the double into the packet.
|
* Encodes the double into the packet.
|
||||||
*
|
*
|
||||||
* @param src The double to encode.
|
* @param src The double to encode.
|
||||||
*/
|
*/
|
||||||
public void encode(double src) {
|
public void encode(double src) {
|
||||||
long data = Double.doubleToRawLongBits(src);
|
long data = Double.doubleToRawLongBits(src);
|
||||||
packetData[writePos++] = (byte) ((data >> 56) & 0xff);
|
packetData[writePos++] = (byte) ((data >> 56) & 0xff);
|
||||||
@@ -114,28 +114,28 @@ public class Packet {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Encodes the boolean into the packet.
|
* Encodes the boolean into the packet.
|
||||||
*
|
*
|
||||||
* @param src The boolean to encode.
|
* @param src The boolean to encode.
|
||||||
*/
|
*/
|
||||||
public void encode(boolean src) {
|
public void encode(boolean src) {
|
||||||
packetData[writePos++] = src ? (byte) 1 : (byte) 0;
|
packetData[writePos++] = src ? (byte) 1 : (byte) 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Returns a decoded byte from the packet.
|
* Returns a decoded byte from the packet.
|
||||||
*
|
*
|
||||||
* @return A decoded byte from the packet.
|
* @return A decoded byte from the packet.
|
||||||
*/
|
*/
|
||||||
public byte decodeByte() {
|
public byte decodeByte() {
|
||||||
return packetData[readPos++];
|
return packetData[readPos++];
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Returns a decoded int from the packet.
|
* Returns a decoded int from the packet.
|
||||||
*
|
*
|
||||||
* @return A decoded int from the packet.
|
* @return A decoded int from the packet.
|
||||||
*/
|
*/
|
||||||
public int decodeInt() {
|
public int decodeInt() {
|
||||||
return (0xff & packetData[readPos++]) << 24
|
return (0xff & packetData[readPos++]) << 24
|
||||||
| (0xff & packetData[readPos++]) << 16
|
| (0xff & packetData[readPos++]) << 16
|
||||||
@@ -144,10 +144,10 @@ public class Packet {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Returns a decoded double from the packet.
|
* Returns a decoded double from the packet.
|
||||||
*
|
*
|
||||||
* @return A decoded double from the packet.
|
* @return A decoded double from the packet.
|
||||||
*/
|
*/
|
||||||
public double decodeDouble() {
|
public double decodeDouble() {
|
||||||
long data =
|
long data =
|
||||||
(long) (0xff & packetData[readPos++]) << 56
|
(long) (0xff & packetData[readPos++]) << 56
|
||||||
@@ -162,10 +162,10 @@ public class Packet {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Returns a decoded boolean from the packet.
|
* Returns a decoded boolean from the packet.
|
||||||
*
|
*
|
||||||
* @return A decoded boolean from the packet.
|
* @return A decoded boolean from the packet.
|
||||||
*/
|
*/
|
||||||
public boolean decodeBoolean() {
|
public boolean decodeBoolean() {
|
||||||
return packetData[readPos++] == 1;
|
return packetData[readPos++] == 1;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -35,31 +35,31 @@ public class PhotonPipelineResult {
|
|||||||
public PhotonPipelineResult() {}
|
public PhotonPipelineResult() {}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Constructs a pipeline result.
|
* Constructs a pipeline result.
|
||||||
*
|
*
|
||||||
* @param latencyMillis The latency in the pipeline.
|
* @param latencyMillis The latency in the pipeline.
|
||||||
* @param targets The list of targets identified by the pipeline.
|
* @param targets The list of targets identified by the pipeline.
|
||||||
*/
|
*/
|
||||||
public PhotonPipelineResult(double latencyMillis, List<PhotonTrackedTarget> targets) {
|
public PhotonPipelineResult(double latencyMillis, List<PhotonTrackedTarget> targets) {
|
||||||
this.latencyMillis = latencyMillis;
|
this.latencyMillis = latencyMillis;
|
||||||
this.targets.addAll(targets);
|
this.targets.addAll(targets);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Returns the size of the packet needed to store this pipeline result.
|
* Returns the size of the packet needed to store this pipeline result.
|
||||||
*
|
*
|
||||||
* @return The size of the packet needed to store this pipeline result.
|
* @return The size of the packet needed to store this pipeline result.
|
||||||
*/
|
*/
|
||||||
public int getPacketSize() {
|
public int getPacketSize() {
|
||||||
return targets.size() * PhotonTrackedTarget.PACK_SIZE_BYTES + 8 + 2;
|
return targets.size() * PhotonTrackedTarget.PACK_SIZE_BYTES + 8 + 2;
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Returns the best target in this pipeline result. If there are no targets, this method will
|
* Returns the best target in this pipeline result. If there are no targets, this method will
|
||||||
* return null. The best target is determined by the target sort mode in the PhotonVision UI.
|
* return null. The best target is determined by the target sort mode in the PhotonVision UI.
|
||||||
*
|
*
|
||||||
* @return The best target of the pipeline result.
|
* @return The best target of the pipeline result.
|
||||||
*/
|
*/
|
||||||
public PhotonTrackedTarget getBestTarget() {
|
public PhotonTrackedTarget getBestTarget() {
|
||||||
if (!hasTargets() && !HAS_WARNED) {
|
if (!hasTargets() && !HAS_WARNED) {
|
||||||
String errStr =
|
String errStr =
|
||||||
@@ -74,28 +74,28 @@ public class PhotonPipelineResult {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Returns the latency in the pipeline.
|
* Returns the latency in the pipeline.
|
||||||
*
|
*
|
||||||
* @return The latency in the pipeline.
|
* @return The latency in the pipeline.
|
||||||
*/
|
*/
|
||||||
public double getLatencyMillis() {
|
public double getLatencyMillis() {
|
||||||
return latencyMillis;
|
return latencyMillis;
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Returns whether the pipeline has targets.
|
* Returns whether the pipeline has targets.
|
||||||
*
|
*
|
||||||
* @return Whether the pipeline has targets.
|
* @return Whether the pipeline has targets.
|
||||||
*/
|
*/
|
||||||
public boolean hasTargets() {
|
public boolean hasTargets() {
|
||||||
return targets.size() > 0;
|
return targets.size() > 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Returns a copy of the vector of targets.
|
* Returns a copy of the vector of targets.
|
||||||
*
|
*
|
||||||
* @return A copy of the vector of targets.
|
* @return A copy of the vector of targets.
|
||||||
*/
|
*/
|
||||||
public List<PhotonTrackedTarget> getTargets() {
|
public List<PhotonTrackedTarget> getTargets() {
|
||||||
return new ArrayList<>(targets);
|
return new ArrayList<>(targets);
|
||||||
}
|
}
|
||||||
@@ -116,11 +116,11 @@ public class PhotonPipelineResult {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Populates the fields of the pipeline result from the packet.
|
* Populates the fields of the pipeline result from the packet.
|
||||||
*
|
*
|
||||||
* @param packet The incoming packet.
|
* @param packet The incoming packet.
|
||||||
* @return The incoming packet.
|
* @return The incoming packet.
|
||||||
*/
|
*/
|
||||||
public Packet createFromPacket(Packet packet) {
|
public Packet createFromPacket(Packet packet) {
|
||||||
// Decode latency, existence of targets, and number of targets.
|
// Decode latency, existence of targets, and number of targets.
|
||||||
latencyMillis = packet.decodeDouble();
|
latencyMillis = packet.decodeDouble();
|
||||||
@@ -139,11 +139,11 @@ public class PhotonPipelineResult {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Populates the outgoing packet with information from this pipeline result.
|
* Populates the outgoing packet with information from this pipeline result.
|
||||||
*
|
*
|
||||||
* @param packet The outgoing packet.
|
* @param packet The outgoing packet.
|
||||||
* @return The outgoing packet.
|
* @return The outgoing packet.
|
||||||
*/
|
*/
|
||||||
public Packet populatePacket(Packet packet) {
|
public Packet populatePacket(Packet packet) {
|
||||||
// Encode latency, existence of targets, and number of targets.
|
// Encode latency, existence of targets, and number of targets.
|
||||||
packet.encode(latencyMillis);
|
packet.encode(latencyMillis);
|
||||||
|
|||||||
@@ -78,11 +78,11 @@ public class PhotonTrackedTarget {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Populates the fields of this class with information from the incoming packet.
|
* Populates the fields of this class with information from the incoming packet.
|
||||||
*
|
*
|
||||||
* @param packet The incoming packet.
|
* @param packet The incoming packet.
|
||||||
* @return The incoming packet.
|
* @return The incoming packet.
|
||||||
*/
|
*/
|
||||||
public Packet createFromPacket(Packet packet) {
|
public Packet createFromPacket(Packet packet) {
|
||||||
yaw = packet.decodeDouble();
|
yaw = packet.decodeDouble();
|
||||||
pitch = packet.decodeDouble();
|
pitch = packet.decodeDouble();
|
||||||
@@ -99,11 +99,11 @@ public class PhotonTrackedTarget {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Populates the outgoing packet with information from the current target.
|
* Populates the outgoing packet with information from the current target.
|
||||||
*
|
*
|
||||||
* @param packet The outgoing packet.
|
* @param packet The outgoing packet.
|
||||||
* @return The outgoing packet.
|
* @return The outgoing packet.
|
||||||
*/
|
*/
|
||||||
public Packet populatePacket(Packet packet) {
|
public Packet populatePacket(Packet packet) {
|
||||||
packet.encode(yaw);
|
packet.encode(yaw);
|
||||||
packet.encode(pitch);
|
packet.encode(pitch);
|
||||||
|
|||||||
@@ -19,18 +19,18 @@ package org.photonlib.examples.aimandrange;
|
|||||||
import edu.wpi.first.wpilibj.RobotBase;
|
import edu.wpi.first.wpilibj.RobotBase;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
|
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
|
||||||
* you are doing, do not modify this file except to change the parameter class to the startRobot
|
* you are doing, do not modify this file except to change the parameter class to the startRobot
|
||||||
* call.
|
* call.
|
||||||
*/
|
*/
|
||||||
public final class Main {
|
public final class Main {
|
||||||
private Main() {}
|
private Main() {}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Main initialization function. Do not perform any initialization here.
|
* Main initialization function. Do not perform any initialization here.
|
||||||
*
|
*
|
||||||
* <p>If you change your main robot class, change the parameter type.
|
* <p>If you change your main robot class, change the parameter type.
|
||||||
*/
|
*/
|
||||||
public static void main(String... args) {
|
public static void main(String... args) {
|
||||||
RobotBase.startRobot(Robot::new);
|
RobotBase.startRobot(Robot::new);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -26,11 +26,11 @@ import org.photonvision.PhotonCamera;
|
|||||||
import org.photonvision.PhotonUtils;
|
import org.photonvision.PhotonUtils;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* The VM is configured to automatically run this class, and to call the functions corresponding to
|
* The VM is configured to automatically run this class, and to call the functions corresponding to
|
||||||
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
|
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
|
||||||
* the package after creating this project, you must also update the build.gradle file in the
|
* the package after creating this project, you must also update the build.gradle file in the
|
||||||
* project.
|
* project.
|
||||||
*/
|
*/
|
||||||
public class Robot extends TimedRobot {
|
public class Robot extends TimedRobot {
|
||||||
// Constants such as camera and target height stored. Change per robot and goal!
|
// Constants such as camera and target height stored. Change per robot and goal!
|
||||||
final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24);
|
final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24);
|
||||||
|
|||||||
@@ -19,18 +19,18 @@ package org.photonlib.examples.aimattarget;
|
|||||||
import edu.wpi.first.wpilibj.RobotBase;
|
import edu.wpi.first.wpilibj.RobotBase;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
|
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
|
||||||
* you are doing, do not modify this file except to change the parameter class to the startRobot
|
* you are doing, do not modify this file except to change the parameter class to the startRobot
|
||||||
* call.
|
* call.
|
||||||
*/
|
*/
|
||||||
public final class Main {
|
public final class Main {
|
||||||
private Main() {}
|
private Main() {}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Main initialization function. Do not perform any initialization here.
|
* Main initialization function. Do not perform any initialization here.
|
||||||
*
|
*
|
||||||
* <p>If you change your main robot class, change the parameter type.
|
* <p>If you change your main robot class, change the parameter type.
|
||||||
*/
|
*/
|
||||||
public static void main(String... args) {
|
public static void main(String... args) {
|
||||||
RobotBase.startRobot(Robot::new);
|
RobotBase.startRobot(Robot::new);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -25,11 +25,11 @@ import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX;
|
|||||||
import org.photonvision.PhotonCamera;
|
import org.photonvision.PhotonCamera;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* The VM is configured to automatically run this class, and to call the functions corresponding to
|
* The VM is configured to automatically run this class, and to call the functions corresponding to
|
||||||
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
|
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
|
||||||
* the package after creating this project, you must also update the build.gradle file in the
|
* the package after creating this project, you must also update the build.gradle file in the
|
||||||
* project.
|
* project.
|
||||||
*/
|
*/
|
||||||
public class Robot extends TimedRobot {
|
public class Robot extends TimedRobot {
|
||||||
// Constants such as camera and target height stored. Change per robot and goal!
|
// Constants such as camera and target height stored. Change per robot and goal!
|
||||||
final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24);
|
final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24);
|
||||||
|
|||||||
@@ -19,18 +19,18 @@ package org.photonlib.examples.getinrange;
|
|||||||
import edu.wpi.first.wpilibj.RobotBase;
|
import edu.wpi.first.wpilibj.RobotBase;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
|
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
|
||||||
* you are doing, do not modify this file except to change the parameter class to the startRobot
|
* you are doing, do not modify this file except to change the parameter class to the startRobot
|
||||||
* call.
|
* call.
|
||||||
*/
|
*/
|
||||||
public final class Main {
|
public final class Main {
|
||||||
private Main() {}
|
private Main() {}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Main initialization function. Do not perform any initialization here.
|
* Main initialization function. Do not perform any initialization here.
|
||||||
*
|
*
|
||||||
* <p>If you change your main robot class, change the parameter type.
|
* <p>If you change your main robot class, change the parameter type.
|
||||||
*/
|
*/
|
||||||
public static void main(String... args) {
|
public static void main(String... args) {
|
||||||
RobotBase.startRobot(Robot::new);
|
RobotBase.startRobot(Robot::new);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -26,11 +26,11 @@ import org.photonvision.PhotonCamera;
|
|||||||
import org.photonvision.PhotonUtils;
|
import org.photonvision.PhotonUtils;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* The VM is configured to automatically run this class, and to call the functions corresponding to
|
* The VM is configured to automatically run this class, and to call the functions corresponding to
|
||||||
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
|
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
|
||||||
* the package after creating this project, you must also update the build.gradle file in the
|
* the package after creating this project, you must also update the build.gradle file in the
|
||||||
* project.
|
* project.
|
||||||
*/
|
*/
|
||||||
public class Robot extends TimedRobot {
|
public class Robot extends TimedRobot {
|
||||||
// Constants such as camera and target height stored. Change per robot and goal!
|
// Constants such as camera and target height stored. Change per robot and goal!
|
||||||
final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24);
|
final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24);
|
||||||
|
|||||||
@@ -19,18 +19,18 @@ package org.photonlib.examples.simaimandrange;
|
|||||||
import edu.wpi.first.wpilibj.RobotBase;
|
import edu.wpi.first.wpilibj.RobotBase;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
|
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
|
||||||
* you are doing, do not modify this file except to change the parameter class to the startRobot
|
* you are doing, do not modify this file except to change the parameter class to the startRobot
|
||||||
* call.
|
* call.
|
||||||
*/
|
*/
|
||||||
public final class Main {
|
public final class Main {
|
||||||
private Main() {}
|
private Main() {}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Main initialization function. Do not perform any initialization here.
|
* Main initialization function. Do not perform any initialization here.
|
||||||
*
|
*
|
||||||
* <p>If you change your main robot class, change the parameter type.
|
* <p>If you change your main robot class, change the parameter type.
|
||||||
*/
|
*/
|
||||||
public static void main(String... args) {
|
public static void main(String... args) {
|
||||||
RobotBase.startRobot(Robot::new);
|
RobotBase.startRobot(Robot::new);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -27,11 +27,11 @@ import org.photonvision.PhotonCamera;
|
|||||||
import org.photonvision.PhotonUtils;
|
import org.photonvision.PhotonUtils;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* The VM is configured to automatically run this class, and to call the functions corresponding to
|
* The VM is configured to automatically run this class, and to call the functions corresponding to
|
||||||
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
|
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
|
||||||
* the package after creating this project, you must also update the build.gradle file in the
|
* the package after creating this project, you must also update the build.gradle file in the
|
||||||
* project.
|
* project.
|
||||||
*/
|
*/
|
||||||
public class Robot extends TimedRobot {
|
public class Robot extends TimedRobot {
|
||||||
// 2020 High goal target height above ground
|
// 2020 High goal target height above ground
|
||||||
public static final double TARGET_HEIGHT_METERS = Units.inchesToMeters(81.19);
|
public static final double TARGET_HEIGHT_METERS = Units.inchesToMeters(81.19);
|
||||||
|
|||||||
@@ -36,12 +36,12 @@ import org.photonvision.SimVisionSystem;
|
|||||||
import org.photonvision.SimVisionTarget;
|
import org.photonvision.SimVisionTarget;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Implementation of a simulation of robot physics, sensors, motor controllers Includes a Simulated
|
* Implementation of a simulation of robot physics, sensors, motor controllers Includes a Simulated
|
||||||
* PhotonVision system and one vision target.
|
* PhotonVision system and one vision target.
|
||||||
*
|
*
|
||||||
* <p>This class and its methods are only relevant during simulation. While on the real robot, the
|
* <p>This class and its methods are only relevant during simulation. While on the real robot, the
|
||||||
* real motors/sensors/physics are used instead.
|
* real motors/sensors/physics are used instead.
|
||||||
*/
|
*/
|
||||||
public class DrivetrainSim {
|
public class DrivetrainSim {
|
||||||
// Simulated Motor Controllers
|
// Simulated Motor Controllers
|
||||||
PWMSim leftLeader = new PWMSim(0);
|
PWMSim leftLeader = new PWMSim(0);
|
||||||
@@ -108,9 +108,9 @@ public class DrivetrainSim {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Perform all periodic drivetrain simulation related tasks to advance our simulation of robot
|
* Perform all periodic drivetrain simulation related tasks to advance our simulation of robot
|
||||||
* physics forward by a single 20ms step.
|
* physics forward by a single 20ms step.
|
||||||
*/
|
*/
|
||||||
public void update() {
|
public void update() {
|
||||||
double leftMotorCmd = 0;
|
double leftMotorCmd = 0;
|
||||||
double rightMotorCmd = 0;
|
double rightMotorCmd = 0;
|
||||||
@@ -132,11 +132,11 @@ public class DrivetrainSim {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Resets the simulation back to a pre-defined pose Useful to simulate the action of placing the
|
* Resets the simulation back to a pre-defined pose Useful to simulate the action of placing the
|
||||||
* robot onto a specific spot in the field (IE, at the start of each match).
|
* robot onto a specific spot in the field (IE, at the start of each match).
|
||||||
*
|
*
|
||||||
* @param pose
|
* @param pose
|
||||||
*/
|
*/
|
||||||
public void resetPose(Pose2d pose) {
|
public void resetPose(Pose2d pose) {
|
||||||
drivetrainSimulator.setPose(pose);
|
drivetrainSimulator.setPose(pose);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -20,18 +20,18 @@ import edu.wpi.first.wpilibj.RobotBase;
|
|||||||
import org.photonlib.examples.simposeest.robot.Robot;
|
import org.photonlib.examples.simposeest.robot.Robot;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
|
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
|
||||||
* you are doing, do not modify this file except to change the parameter class to the startRobot
|
* you are doing, do not modify this file except to change the parameter class to the startRobot
|
||||||
* call.
|
* call.
|
||||||
*/
|
*/
|
||||||
public final class Main {
|
public final class Main {
|
||||||
private Main() {}
|
private Main() {}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Main initialization function. Do not perform any initialization here.
|
* Main initialization function. Do not perform any initialization here.
|
||||||
*
|
*
|
||||||
* <p>If you change your main robot class, change the parameter type.
|
* <p>If you change your main robot class, change the parameter type.
|
||||||
*/
|
*/
|
||||||
public static void main(String... args) {
|
public static void main(String... args) {
|
||||||
RobotBase.startRobot(Robot::new);
|
RobotBase.startRobot(Robot::new);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -27,11 +27,11 @@ import edu.wpi.first.wpilibj.Timer;
|
|||||||
import java.util.List;
|
import java.util.List;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Implements logic to convert a set of desired waypoints (ie, a trajectory) and the current
|
* Implements logic to convert a set of desired waypoints (ie, a trajectory) and the current
|
||||||
* estimate of where the robot is at (ie, the estimated Pose) into motion commands for a drivetrain.
|
* estimate of where the robot is at (ie, the estimated Pose) into motion commands for a drivetrain.
|
||||||
* The Ramaste controller is used to smoothly move the robot from where it thinks it is to where it
|
* The Ramaste controller is used to smoothly move the robot from where it thinks it is to where it
|
||||||
* thinks it ought to be.
|
* thinks it ought to be.
|
||||||
*/
|
*/
|
||||||
public class AutoController {
|
public class AutoController {
|
||||||
private Trajectory trajectory;
|
private Trajectory trajectory;
|
||||||
|
|
||||||
@@ -72,13 +72,13 @@ public class AutoController {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Given the current estimate of the robot's position, calculate drivetrain speed commands which
|
* Given the current estimate of the robot's position, calculate drivetrain speed commands which
|
||||||
* will best-execute the active trajectory. Be sure to call `startPath()` prior to calling this
|
* will best-execute the active trajectory. Be sure to call `startPath()` prior to calling this
|
||||||
* method.
|
* method.
|
||||||
*
|
*
|
||||||
* @param curEstPose Current estimate of drivetrain pose on the field
|
* @param curEstPose Current estimate of drivetrain pose on the field
|
||||||
* @return The commanded drivetrain motion
|
* @return The commanded drivetrain motion
|
||||||
*/
|
*/
|
||||||
public ChassisSpeeds getCurMotorCmds(Pose2d curEstPose) {
|
public ChassisSpeeds getCurMotorCmds(Pose2d curEstPose) {
|
||||||
if (isRunning) {
|
if (isRunning) {
|
||||||
double elapsed = timer.get();
|
double elapsed = timer.get();
|
||||||
@@ -91,9 +91,9 @@ public class AutoController {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @return The position which the auto controller is attempting to move the drivetrain to right
|
* @return The position which the auto controller is attempting to move the drivetrain to right
|
||||||
* now.
|
* now.
|
||||||
*/
|
*/
|
||||||
public Pose2d getCurPose2d() {
|
public Pose2d getCurPose2d() {
|
||||||
return desiredDtState.poseMeters;
|
return desiredDtState.poseMeters;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -24,11 +24,11 @@ import edu.wpi.first.math.util.Units;
|
|||||||
import org.photonvision.SimVisionTarget;
|
import org.photonvision.SimVisionTarget;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Holding class for all physical constants that must be used throughout the codebase. These values
|
* Holding class for all physical constants that must be used throughout the codebase. These values
|
||||||
* should be set by one of a few methods: 1) Talk to your mechanical and electrical teams and
|
* should be set by one of a few methods: 1) Talk to your mechanical and electrical teams and
|
||||||
* determine how the physical robot is being built and configured. 2) Read the game manual and look
|
* determine how the physical robot is being built and configured. 2) Read the game manual and look
|
||||||
* at the field drawings 3) Match with how your vision coprocessor is configured.
|
* at the field drawings 3) Match with how your vision coprocessor is configured.
|
||||||
*/
|
*/
|
||||||
public class Constants {
|
public class Constants {
|
||||||
//////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////
|
||||||
// Drivetrain Physical
|
// Drivetrain Physical
|
||||||
|
|||||||
@@ -27,10 +27,10 @@ import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
|
|||||||
import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX;
|
import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Implements a controller for the drivetrain. Converts a set of chassis motion commands into motor
|
* Implements a controller for the drivetrain. Converts a set of chassis motion commands into motor
|
||||||
* controller PWM values which attempt to speed up or slow down the wheels to match the desired
|
* controller PWM values which attempt to speed up or slow down the wheels to match the desired
|
||||||
* speed.
|
* speed.
|
||||||
*/
|
*/
|
||||||
public class Drivetrain {
|
public class Drivetrain {
|
||||||
// PWM motor controller output definitions
|
// PWM motor controller output definitions
|
||||||
PWMVictorSPX leftLeader = new PWMVictorSPX(Constants.kDtLeftLeaderPin);
|
PWMVictorSPX leftLeader = new PWMVictorSPX(Constants.kDtLeftLeaderPin);
|
||||||
@@ -77,12 +77,12 @@ public class Drivetrain {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Given a set of chassis (fwd/rev + rotate) speed commands, perform all periodic tasks to assign
|
* Given a set of chassis (fwd/rev + rotate) speed commands, perform all periodic tasks to assign
|
||||||
* new outputs to the motor controllers.
|
* new outputs to the motor controllers.
|
||||||
*
|
*
|
||||||
* @param xSpeed Desired chassis Forward or Reverse speed (in meters/sec). Positive is forward.
|
* @param xSpeed Desired chassis Forward or Reverse speed (in meters/sec). Positive is forward.
|
||||||
* @param rot Desired chassis rotation speed in radians/sec. Positive is counter-clockwise.
|
* @param rot Desired chassis rotation speed in radians/sec. Positive is counter-clockwise.
|
||||||
*/
|
*/
|
||||||
public void drive(double xSpeed, double rot) {
|
public void drive(double xSpeed, double rot) {
|
||||||
// Convert our fwd/rev and rotate commands to wheel speed commands
|
// Convert our fwd/rev and rotate commands to wheel speed commands
|
||||||
DifferentialDriveWheelSpeeds speeds =
|
DifferentialDriveWheelSpeeds speeds =
|
||||||
@@ -111,12 +111,12 @@ public class Drivetrain {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Force the pose estimator and all sensors to a particular pose. This is useful for indicating to
|
* Force the pose estimator and all sensors to a particular pose. This is useful for indicating to
|
||||||
* the software when you have manually moved your robot in a particular position on the field (EX:
|
* the software when you have manually moved your robot in a particular position on the field (EX:
|
||||||
* when you place it on the field at the start of the match).
|
* when you place it on the field at the start of the match).
|
||||||
*
|
*
|
||||||
* @param pose
|
* @param pose
|
||||||
*/
|
*/
|
||||||
public void resetOdometry(Pose2d pose) {
|
public void resetOdometry(Pose2d pose) {
|
||||||
leftEncoder.reset();
|
leftEncoder.reset();
|
||||||
rightEncoder.reset();
|
rightEncoder.reset();
|
||||||
|
|||||||
@@ -31,10 +31,10 @@ import edu.wpi.first.wpilibj.Timer;
|
|||||||
import org.photonvision.PhotonCamera;
|
import org.photonvision.PhotonCamera;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Performs estimation of the drivetrain's current position on the field, using a vision system,
|
* Performs estimation of the drivetrain's current position on the field, using a vision system,
|
||||||
* drivetrain encoders, and a gyroscope. These sensor readings are fused together using a Kalman
|
* drivetrain encoders, and a gyroscope. These sensor readings are fused together using a Kalman
|
||||||
* filter. This in turn creates a best-guess at a Pose2d of where our drivetrain is currently at.
|
* filter. This in turn creates a best-guess at a Pose2d of where our drivetrain is currently at.
|
||||||
*/
|
*/
|
||||||
public class DrivetrainPoseEstimator {
|
public class DrivetrainPoseEstimator {
|
||||||
// Sensors used as part of the Pose Estimation
|
// Sensors used as part of the Pose Estimation
|
||||||
private final AnalogGyro gyro = new AnalogGyro(Constants.kGyroPin);
|
private final AnalogGyro gyro = new AnalogGyro(Constants.kGyroPin);
|
||||||
@@ -65,12 +65,12 @@ public class DrivetrainPoseEstimator {
|
|||||||
public DrivetrainPoseEstimator() {}
|
public DrivetrainPoseEstimator() {}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Perform all periodic pose estimation tasks.
|
* Perform all periodic pose estimation tasks.
|
||||||
*
|
*
|
||||||
* @param actWheelSpeeds Current Speeds (in m/s) of the drivetrain wheels
|
* @param actWheelSpeeds Current Speeds (in m/s) of the drivetrain wheels
|
||||||
* @param leftDist Distance (in m) the left wheel has traveled
|
* @param leftDist Distance (in m) the left wheel has traveled
|
||||||
* @param rightDist Distance (in m) the right wheel has traveled
|
* @param rightDist Distance (in m) the right wheel has traveled
|
||||||
*/
|
*/
|
||||||
public void update(
|
public void update(
|
||||||
DifferentialDriveWheelSpeeds actWheelSpeeds, double leftDist, double rightDist) {
|
DifferentialDriveWheelSpeeds actWheelSpeeds, double leftDist, double rightDist) {
|
||||||
m_poseEstimator.update(gyro.getRotation2d(), actWheelSpeeds, leftDist, rightDist);
|
m_poseEstimator.update(gyro.getRotation2d(), actWheelSpeeds, leftDist, rightDist);
|
||||||
@@ -86,12 +86,12 @@ public class DrivetrainPoseEstimator {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Force the pose estimator to a particular pose. This is useful for indicating to the software
|
* Force the pose estimator to a particular pose. This is useful for indicating to the software
|
||||||
* when you have manually moved your robot in a particular position on the field (EX: when you
|
* when you have manually moved your robot in a particular position on the field (EX: when you
|
||||||
* place it on the field at the start of the match).
|
* place it on the field at the start of the match).
|
||||||
*
|
*
|
||||||
* @param pose
|
* @param pose
|
||||||
*/
|
*/
|
||||||
public void resetToPose(Pose2d pose) {
|
public void resetToPose(Pose2d pose) {
|
||||||
m_poseEstimator.resetPosition(pose, gyro.getRotation2d());
|
m_poseEstimator.resetPosition(pose, gyro.getRotation2d());
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -34,12 +34,12 @@ import org.photonlib.examples.simposeest.robot.Constants;
|
|||||||
import org.photonvision.SimVisionSystem;
|
import org.photonvision.SimVisionSystem;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Implementation of a simulation of robot physics, sensors, motor controllers Includes a Simulated
|
* Implementation of a simulation of robot physics, sensors, motor controllers Includes a Simulated
|
||||||
* PhotonVision system and one vision target.
|
* PhotonVision system and one vision target.
|
||||||
*
|
*
|
||||||
* <p>This class and its methods are only relevant during simulation. While on the real robot, the
|
* <p>This class and its methods are only relevant during simulation. While on the real robot, the
|
||||||
* real motors/sensors/physics are used instead.
|
* real motors/sensors/physics are used instead.
|
||||||
*/
|
*/
|
||||||
public class DrivetrainSim {
|
public class DrivetrainSim {
|
||||||
// Simulated Sensors
|
// Simulated Sensors
|
||||||
AnalogGyroSim gyroSim = new AnalogGyroSim(Constants.kGyroPin);
|
AnalogGyroSim gyroSim = new AnalogGyroSim(Constants.kGyroPin);
|
||||||
@@ -94,9 +94,9 @@ public class DrivetrainSim {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Perform all periodic drivetrain simulation related tasks to advance our simulation of robot
|
* Perform all periodic drivetrain simulation related tasks to advance our simulation of robot
|
||||||
* physics forward by a single 20ms step.
|
* physics forward by a single 20ms step.
|
||||||
*/
|
*/
|
||||||
public void update() {
|
public void update() {
|
||||||
double leftMotorCmd = 0;
|
double leftMotorCmd = 0;
|
||||||
double rightMotorCmd = 0;
|
double rightMotorCmd = 0;
|
||||||
@@ -135,11 +135,11 @@ public class DrivetrainSim {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Resets the simulation back to a pre-defined pose Useful to simulate the action of placing the
|
* Resets the simulation back to a pre-defined pose Useful to simulate the action of placing the
|
||||||
* robot onto a specific spot in the field (IE, at the start of each match).
|
* robot onto a specific spot in the field (IE, at the start of each match).
|
||||||
*
|
*
|
||||||
* @param pose
|
* @param pose
|
||||||
*/
|
*/
|
||||||
public void resetPose(Pose2d pose) {
|
public void resetPose(Pose2d pose) {
|
||||||
drivetrainSimulator.setPose(pose);
|
drivetrainSimulator.setPose(pose);
|
||||||
}
|
}
|
||||||
@@ -150,11 +150,11 @@ public class DrivetrainSim {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* For testing purposes only! Applies an unmodeled, undetected offset to the pose Similar to if
|
* For testing purposes only! Applies an unmodeled, undetected offset to the pose Similar to if
|
||||||
* you magically kicked your robot to the side in a way the encoders and gyro didn't measure.
|
* you magically kicked your robot to the side in a way the encoders and gyro didn't measure.
|
||||||
*
|
*
|
||||||
* <p>This distrubance should be corrected for once a vision target is in view.
|
* <p>This distrubance should be corrected for once a vision target is in view.
|
||||||
*/
|
*/
|
||||||
public void applyKick() {
|
public void applyKick() {
|
||||||
Pose2d newPose =
|
Pose2d newPose =
|
||||||
drivetrainSimulator
|
drivetrainSimulator
|
||||||
|
|||||||
@@ -17,9 +17,9 @@
|
|||||||
package org.photonvision;
|
package org.photonvision;
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Autogenerated file! Do not manually edit this file. This version is regenerated
|
* Autogenerated file! Do not manually edit this file. This version is regenerated
|
||||||
* any time the publish task is run, or when this file is deleted.
|
* any time the publish task is run, or when this file is deleted.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
import java.util.regex.Matcher;
|
import java.util.regex.Matcher;
|
||||||
import java.util.regex.Pattern;
|
import java.util.regex.Pattern;
|
||||||
|
|||||||
Reference in New Issue
Block a user