Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-28 02:11:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add and apply spotless. (#85)

Browse Source 
* Adding spotless dependency.

* Applying spotless.

* Changing tab size to 4 spaces.
This commit is contained in:
Claudius Tewari
2020-03-28 18:38:50 -07:00
committed by GitHub
parent b236d20fd6
commit 7406ba7549
53 changed files with 2032 additions and 2068 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
chameleon-server/build.gradle
View File

@@ -1,6 +1,7 @@
plugins {
id 'java'
id 'com.github.johnrengelman.shadow' version '5.2.0'
id "com.diffplug.gradle.spotless" version "3.28.0"
}
group 'com.chameleonvision'
@@ -116,4 +117,12 @@ test {
events "passed", "skipped", "failed"
}
}
}
spotless {
java {
googleJavaFormat()
paddedCell()
indentWithTabs(2)
}
}

13
chameleon-server/src/main/java/com/chameleonvision/common/configuration/ConfigFile.java
View File

@@ -2,11 +2,10 @@ package com.chameleonvision.common.configuration;
public class ConfigFile {
/**
* Represents a config file at a fixed path
* @param path Path to config file
*/
public ConfigFile(String path) {
}
/**
* Represents a config file at a fixed path
*
* @param path Path to config file
*/
public ConfigFile(String path) {}
}

13
chameleon-server/src/main/java/com/chameleonvision/common/configuration/ConfigFolder.java
View File

@@ -2,11 +2,10 @@ package com.chameleonvision.common.configuration;
public class ConfigFolder {
/**
* Represents a folder of config files
* @param path path to config file
*/
public ConfigFolder(String path) {
}
/**
* Represents a folder of config files
*
* @param path path to config file
*/
public ConfigFolder(String path) {}
}

20
chameleon-server/src/main/java/com/chameleonvision/common/configuration/ConfigManager.java
View File

@@ -2,18 +2,18 @@ package com.chameleonvision.common.configuration;
public class ConfigManager {
private final ConfigFolder rootFolder;
private final ConfigFolder rootFolder;
protected ConfigManager() {
protected ConfigManager() {
rootFolder = new ConfigFolder("");
}
rootFolder = new ConfigFolder("");
}
private static class SingletonHolder {
private static final ConfigManager INSTANCE = new ConfigManager();
}
private static class SingletonHolder {
private static final ConfigManager INSTANCE = new ConfigManager();
}
public static ConfigManager getInstance() {
return SingletonHolder.INSTANCE;
}
public static ConfigManager getInstance() {
return SingletonHolder.INSTANCE;
}
}

3
chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/DataConsumer.java
View File

@@ -1,4 +1,3 @@
package com.chameleonvision.common.datatransfer;
public interface DataConsumer {
}
public interface DataConsumer {}

3
chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/DataProvider.java
View File

@@ -1,4 +1,3 @@
package com.chameleonvision.common.datatransfer;
public interface DataProvider {
}
public interface DataProvider {}

105
chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/networktables/NetworkTablesManager.java
View File

@@ -5,71 +5,72 @@ import com.chameleonvision.common.scripting.ScriptManager;
import edu.wpi.first.networktables.LogMessage;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import java.util.function.Consumer;
public class NetworkTablesManager {
private NetworkTablesManager() {}
private NetworkTablesManager() {}
private static final NetworkTableInstance ntInstance = NetworkTableInstance.getDefault();
private static final NetworkTableInstance ntInstance = NetworkTableInstance.getDefault();
public static final String kRootTableName = "/chameleon-vision";
public static final NetworkTable kRootTable = NetworkTableInstance.getDefault().getTable(kRootTableName);
public static final String kRootTableName = "/chameleon-vision";
public static final NetworkTable kRootTable =
NetworkTableInstance.getDefault().getTable(kRootTableName);
public static boolean isServer = false;
public static boolean isServer = false;
private static int getTeamNumber() {
// TODO: FIX
return 0;
// return ConfigManager.settings.teamNumber;
}
private static int getTeamNumber() {
// TODO: FIX
return 0;
// return ConfigManager.settings.teamNumber;
}
private static class NTLogger implements Consumer<LogMessage> {
private static class NTLogger implements Consumer<LogMessage> {
private boolean hasReportedConnectionFailure = false;
private boolean hasReportedConnectionFailure = false;
@Override
public void accept(LogMessage logMessage) {
if (!hasReportedConnectionFailure && logMessage.message.contains("timed out")) {
System.err.println("NT Connection has failed! Will retry in background.");
hasReportedConnectionFailure = true;
} else if (logMessage.message.contains("connected")) {
System.out.println("NT Connected!");
hasReportedConnectionFailure = false;
ScriptManager.queueEvent(ScriptEventType.kNTConnected);
}
}
}
@Override
public void accept(LogMessage logMessage) {
if (!hasReportedConnectionFailure && logMessage.message.contains("timed out")) {
System.err.println("NT Connection has failed! Will retry in background.");
hasReportedConnectionFailure = true;
} else if (logMessage.message.contains("connected")) {
System.out.println("NT Connected!");
hasReportedConnectionFailure = false;
ScriptManager.queueEvent(ScriptEventType.kNTConnected);
}
}
}
static {
NetworkTableInstance.getDefault().addLogger(new NTLogger(), 0, 255); // to hide error messages
}
static {
NetworkTableInstance.getDefault().addLogger(new NTLogger(), 0, 255); // to hide error messages
}
public static void setClientMode(String host) {
isServer = false;
System.out.println("Starting NT Client");
ntInstance.stopServer();
if (host != null) {
ntInstance.startClient(host);
} else {
ntInstance.startClientTeam(getTeamNumber());
if(ntInstance.isConnected()) {
System.out.println("[NetworkTablesManager] Connected to the robot!");
} else {
System.out.println("[NetworkTablesManager] Could NOT to the robot! Will retry in the background...");
}
}
}
public static void setClientMode(String host) {
isServer = false;
System.out.println("Starting NT Client");
ntInstance.stopServer();
if (host != null) {
ntInstance.startClient(host);
} else {
ntInstance.startClientTeam(getTeamNumber());
if (ntInstance.isConnected()) {
System.out.println("[NetworkTablesManager] Connected to the robot!");
} else {
System.out.println(
"[NetworkTablesManager] Could NOT to the robot! Will retry in the background...");
}
}
}
public static void setTeamClientMode() {
setClientMode(null);
}
public static void setTeamClientMode() {
setClientMode(null);
}
public static void setServerMode() {
isServer = true;
System.out.println("Starting NT Server");
ntInstance.stopClient();
ntInstance.startServer();
}
public static void setServerMode() {
isServer = true;
System.out.println("Starting NT Server");
ntInstance.stopClient();
ntInstance.startServer();
}
}

25
chameleon-server/src/main/java/com/chameleonvision/common/logging/DebugLogger.java
View File

@@ -2,20 +2,19 @@ package com.chameleonvision.common.logging;
public class DebugLogger {
private final boolean verbose;
private final boolean verbose;
public DebugLogger(boolean verbose) {
this.verbose = verbose;
}
public DebugLogger(boolean verbose) {
this.verbose = verbose;
}
public void printInfo(String infoMessage) {
if (verbose) {
System.out.println(infoMessage);
}
}
public void printInfo(String smallInfo, String largeInfo) {
System.out.println(verbose ? String.format("%s - %s" , smallInfo, largeInfo) : smallInfo);
}
public void printInfo(String infoMessage) {
if (verbose) {
System.out.println(infoMessage);
}
}
public void printInfo(String smallInfo, String largeInfo) {
System.out.println(verbose ? String.format("%s - %s", smallInfo, largeInfo) : smallInfo);
}
}

163
chameleon-server/src/main/java/com/chameleonvision/common/networking/LinuxNetworking.java
View File

@@ -1,7 +1,5 @@
package com.chameleonvision.common.networking;
import org.apache.commons.io.FileUtils;
import java.io.File;
import java.io.IOException;
import java.net.InetAddress;
@@ -10,92 +8,93 @@ import java.nio.charset.StandardCharsets;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
import org.apache.commons.io.FileUtils;
public class LinuxNetworking extends SysNetworking {
private static final String PATH = "/etc/dhcpcd.conf";
private static final String PATH = "/etc/dhcpcd.conf";
@Override
public boolean setDHCP() {
File dhcpConf = new File(PATH);
if (dhcpConf.exists()) {
try {
List<String> lines = FileUtils.readLines(dhcpConf, StandardCharsets.UTF_8);
for (int i = 0; i < lines.size(); i++) {
String line = lines.get(i);
if (line.startsWith("interface " + networkInterface.name)) {
lines.remove(i);
for (int j = i; j < lines.size(); j++) {
String subInterface = lines.get(j);
if (subInterface.contains("static ip_address") || subInterface.contains("static routers")) {
lines.remove(j);
j--;
}
if (subInterface.contains("interface")) {
break;
}
}
FileUtils.writeLines(dhcpConf, lines);
return true;
}
}
} catch (IOException e) {
e.printStackTrace();
return false;
}
@Override
public boolean setDHCP() {
File dhcpConf = new File(PATH);
if (dhcpConf.exists()) {
try {
List<String> lines = FileUtils.readLines(dhcpConf, StandardCharsets.UTF_8);
for (int i = 0; i < lines.size(); i++) {
String line = lines.get(i);
if (line.startsWith("interface " + networkInterface.name)) {
lines.remove(i);
for (int j = i; j < lines.size(); j++) {
String subInterface = lines.get(j);
if (subInterface.contains("static ip_address")
|| subInterface.contains("static routers")) {
lines.remove(j);
j--;
}
if (subInterface.contains("interface")) {
break;
}
}
FileUtils.writeLines(dhcpConf, lines);
return true;
}
}
} catch (IOException e) {
e.printStackTrace();
return false;
}
} else {
System.err.println("dhcpcd5 is not installed, unable to set IP.");
return false;
}
return true;
}
} else {
System.err.println("dhcpcd5 is not installed, unable to set IP.");
return false;
}
return true;
}
@Override
public boolean setHostname(String newHostname) {
try {
var setHostnameRetCode = shell.execute("hostnamectl", "set-hostname", newHostname);
return setHostnameRetCode == 0;
} catch (Exception e) {
e.printStackTrace();
return false;
}
}
@Override
public boolean setHostname(String newHostname) {
try {
var setHostnameRetCode = shell.execute("hostnamectl", "set-hostname", newHostname);
return setHostnameRetCode == 0;
} catch (Exception e) {
e.printStackTrace();
return false;
}
}
@Override
public boolean setStatic(String ipAddress, String netmask, String gateway) {
setDHCP(); // clean up old static interface
File dhcpConf = new File(PATH);
try {
List<String> lines = FileUtils.readLines(dhcpConf, StandardCharsets.UTF_8);
lines.add("interface " + networkInterface.name);
InetAddress iNetMask = InetAddress.getByName(netmask);
int prefix = convertNetmaskToCIDR(iNetMask);
lines.add("static ip_address=" + ipAddress + "/" + prefix);
lines.add("static routers=" + gateway);
FileUtils.writeLines(dhcpConf, lines);
return true;
} catch (IOException e) {
e.printStackTrace();
}
return false;
}
@Override
public boolean setStatic(String ipAddress, String netmask, String gateway) {
setDHCP(); // clean up old static interface
File dhcpConf = new File(PATH);
try {
List<String> lines = FileUtils.readLines(dhcpConf, StandardCharsets.UTF_8);
lines.add("interface " + networkInterface.name);
InetAddress iNetMask = InetAddress.getByName(netmask);
int prefix = convertNetmaskToCIDR(iNetMask);
lines.add("static ip_address=" + ipAddress + "/" + prefix);
lines.add("static routers=" + gateway);
FileUtils.writeLines(dhcpConf, lines);
return true;
} catch (IOException e) {
e.printStackTrace();
}
return false;
}
@Override
public List<java.net.NetworkInterface> getNetworkInterfaces() throws SocketException {
List<java.net.NetworkInterface> netInterfaces;
try {
netInterfaces = Collections.list(java.net.NetworkInterface.getNetworkInterfaces());
} catch (SocketException e) {
return null;
}
List<java.net.NetworkInterface> goodInterfaces = new ArrayList<>();
@Override
public List<java.net.NetworkInterface> getNetworkInterfaces() throws SocketException {
List<java.net.NetworkInterface> netInterfaces;
try {
netInterfaces = Collections.list(java.net.NetworkInterface.getNetworkInterfaces());
} catch (SocketException e) {
return null;
}
List<java.net.NetworkInterface> goodInterfaces = new ArrayList<>();
for (var netInterface : netInterfaces) {
if (netInterface.getDisplayName().contains("lo")) continue;
if (!netInterface.isUp()) continue;
goodInterfaces.add(netInterface);
}
return goodInterfaces;
}
for (var netInterface : netInterfaces) {
if (netInterface.getDisplayName().contains("lo")) continue;
if (!netInterface.isUp()) continue;
goodInterfaces.add(netInterface);
}
return goodInterfaces;
}
}

91
chameleon-server/src/main/java/com/chameleonvision/common/networking/NetworkInterface.java
View File

@@ -4,51 +4,56 @@ import java.net.InterfaceAddress;
@SuppressWarnings("WeakerAccess")
public class NetworkInterface {
public final String name;
public final String displayName;
public final String IPAddress;
public final String Netmask;
public final String Gateway;
public final String Broadcast;
public final String name;
public final String displayName;
public final String IPAddress;
public final String Netmask;
public final String Gateway;
public final String Broadcast;
public NetworkInterface(java.net.NetworkInterface inetface, InterfaceAddress ifaceAddress) {
name = inetface.getName();
displayName = inetface.getDisplayName();
public NetworkInterface(java.net.NetworkInterface inetface, InterfaceAddress ifaceAddress) {
name = inetface.getName();
displayName = inetface.getDisplayName();
var inetAddress = ifaceAddress.getAddress();
IPAddress = inetAddress.getHostAddress();
Netmask = getIPv4LocalNetMask(ifaceAddress);
var inetAddress = ifaceAddress.getAddress();
IPAddress = inetAddress.getHostAddress();
Netmask = getIPv4LocalNetMask(ifaceAddress);
// TODO: (low) hack to "get" gateway, this is gross and bad, pls fix
var splitIPAddr = IPAddress.split("\\.");
splitIPAddr[3] = "1";
Gateway = String.join(".", splitIPAddr);
splitIPAddr[3] = "255";
Broadcast = String.join(".", splitIPAddr);
}
// TODO: (low) hack to "get" gateway, this is gross and bad, pls fix
var splitIPAddr = IPAddress.split("\\.");
splitIPAddr[3] = "1";
Gateway = String.join(".", splitIPAddr);
splitIPAddr[3] = "255";
Broadcast = String.join(".", splitIPAddr);
}
private static String getIPv4LocalNetMask(InterfaceAddress interfaceAddress) {
var netPrefix = interfaceAddress.getNetworkPrefixLength();
try {
// Since this is for IPv4, it's 32 bits, so set the sign value of
// the int to "negative"...
int shiftby = (1<<31);
// For the number of bits of the prefix -1 (we already set the sign bit)
for (int i = netPrefix - 1; i > 0; i--) {
// Shift the sign right... Java makes the sign bit sticky on a shift...
// So no need to "set it back up"...
shiftby = (shiftby >> 1);
}
// Transform the resulting value in xxx.xxx.xxx.xxx format, like if
/// it was a standard address...
// Return the address thus created...
return ((shiftby >> 24) & 255) + "." + ((shiftby >> 16) & 255) + "." + ((shiftby >> 8) & 255) + "." + (shiftby & 255);
// return InetAddress.getByName(maskString);
}
catch(Exception e) {
e.printStackTrace();
}
// Something went wrong here...
return null;
}
private static String getIPv4LocalNetMask(InterfaceAddress interfaceAddress) {
var netPrefix = interfaceAddress.getNetworkPrefixLength();
try {
// Since this is for IPv4, it's 32 bits, so set the sign value of
// the int to "negative"...
int shiftby = (1 << 31);
// For the number of bits of the prefix -1 (we already set the sign bit)
for (int i = netPrefix - 1; i > 0; i--) {
// Shift the sign right... Java makes the sign bit sticky on a shift...
// So no need to "set it back up"...
shiftby = (shiftby >> 1);
}
// Transform the resulting value in xxx.xxx.xxx.xxx format, like if
/// it was a standard address...
// Return the address thus created...
return ((shiftby >> 24) & 255)
+ "."
+ ((shiftby >> 16) & 255)
+ "."
+ ((shiftby >> 8) & 255)
+ "."
+ (shiftby & 255);
// return InetAddress.getByName(maskString);
} catch (Exception e) {
e.printStackTrace();
}
// Something went wrong here...
return null;
}
}

29
chameleon-server/src/main/java/com/chameleonvision/common/networking/NetworkManager.java
View File

@@ -1,23 +1,22 @@
package com.chameleonvision.common.networking;
public class NetworkManager {
private NetworkManager() {}
private NetworkManager() {}
private static class SingletonHolder {
private static final NetworkManager INSTANCE = new NetworkManager();
}
private static class SingletonHolder {
private static final NetworkManager INSTANCE = new NetworkManager();
}
public static NetworkManager getInstance() {
return SingletonHolder.INSTANCE;
}
public static NetworkManager getInstance() {
return SingletonHolder.INSTANCE;
}
private boolean isManaged = false;
private boolean isManaged = false;
public void initialize(boolean shouldManage) {
isManaged = shouldManage;
if (!isManaged) {
return;
}
}
public void initialize(boolean shouldManage) {
isManaged = shouldManage;
if (!isManaged) {
return;
}
}
}

4
chameleon-server/src/main/java/com/chameleonvision/common/networking/NetworkMode.java
View File

@@ -1,6 +1,6 @@
package com.chameleonvision.common.networking;
public enum NetworkMode {
DHCP,
STATIC
DHCP,
STATIC
}

98
chameleon-server/src/main/java/com/chameleonvision/common/networking/SysNetworking.java
View File

@@ -1,63 +1,67 @@
package com.chameleonvision.common.networking;
import com.chameleonvision.common.util.ShellExec;
import java.io.IOException;
import java.net.InetAddress;
import java.net.SocketException;
import java.util.List;
public abstract class SysNetworking {
NetworkInterface networkInterface;
ShellExec shell = new ShellExec(true, true);
NetworkInterface networkInterface;
ShellExec shell = new ShellExec(true, true);
private String hostname = getHostname();
private String hostname = getHostname();
public String getHostname() {
if (hostname == null) {
try {
var retCode = shell.execute("hostname", null, true);
if (retCode == 0) {
while(!shell.isOutputCompleted()) {}
return shell.getOutput();
} else {
return null;
}
} catch (IOException e) {
return null;
}
} else return hostname;
}
public String getHostname() {
if (hostname == null) {
try {
var retCode = shell.execute("hostname", null, true);
if (retCode == 0) {
while (!shell.isOutputCompleted()) {}
return shell.getOutput();
} else {
return null;
}
} catch (IOException e) {
return null;
}
} else return hostname;
}
//code belongs to https://stackoverflow.com/questions/19531411/calculate-cidr-from-a-given-netmask-java
public static int convertNetmaskToCIDR(InetAddress netmask) {
// code belongs to
// https://stackoverflow.com/questions/19531411/calculate-cidr-from-a-given-netmask-java
public static int convertNetmaskToCIDR(InetAddress netmask) {
byte[] netmaskBytes = netmask.getAddress();
int cidr = 0;
boolean zero = false;
for (byte b : netmaskBytes) {
int mask = 0x80;
byte[] netmaskBytes = netmask.getAddress();
int cidr = 0;
boolean zero = false;
for (byte b : netmaskBytes) {
int mask = 0x80;
for (int i = 0; i < 8; i++) {
int result = b & mask;
if (result == 0) {
zero = true;
} else if (zero) {
throw new IllegalArgumentException("Invalid netmask.");
} else {
cidr++;
}
mask >>>= 1;
}
}
return cidr;
}
for (int i = 0; i < 8; i++) {
int result = b & mask;
if (result == 0) {
zero = true;
} else if (zero) {
throw new IllegalArgumentException("Invalid netmask.");
} else {
cidr++;
}
mask >>>= 1;
}
}
return cidr;
}
public void setNetworkInterface(NetworkInterface networkInterface) {
this.networkInterface = networkInterface;
}
public abstract boolean setDHCP();
public abstract boolean setHostname(String hostname);
public abstract boolean setStatic(String ipAddress, String netmask, String gateway);
public abstract List<java.net.NetworkInterface> getNetworkInterfaces() throws SocketException;
public void setNetworkInterface(NetworkInterface networkInterface) {
this.networkInterface = networkInterface;
}
public abstract boolean setDHCP();
public abstract boolean setHostname(String hostname);
public abstract boolean setStatic(String ipAddress, String netmask, String gateway);
public abstract List<java.net.NetworkInterface> getNetworkInterfaces() throws SocketException;
}

16
chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptCommandType.java
View File

@@ -1,14 +1,14 @@
package com.chameleonvision.common.scripting;
public enum ScriptCommandType {
kDefault(""),
kBashScript("bash"),
kPythonScript("python"),
kPython3Script("python3");
kDefault(""),
kBashScript("bash"),
kPythonScript("python"),
kPython3Script("python3");
public final String value;
public final String value;
ScriptCommandType(String value) {
this.value = value;
}
ScriptCommandType(String value) {
this.value = value;
}
}

27
chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptConfig.java
View File

@@ -4,20 +4,19 @@ import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonProperty;
public class ScriptConfig {
public final ScriptEventType eventType;
public final String command;
public final ScriptEventType eventType;
public final String command;
public ScriptConfig(ScriptEventType eventType) {
this.eventType = eventType;
this.command = "";
}
public ScriptConfig(ScriptEventType eventType) {
this.eventType = eventType;
this.command = "";
}
@JsonCreator
public ScriptConfig(
@JsonProperty("eventType") ScriptEventType eventType,
@JsonProperty("command") String command
) {
this.eventType = eventType;
this.command = command;
}
@JsonCreator
public ScriptConfig(
@JsonProperty("eventType") ScriptEventType eventType,
@JsonProperty("command") String command) {
this.eventType = eventType;
this.command = command;
}
}

42
chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptEvent.java
View File

@@ -1,33 +1,35 @@
package com.chameleonvision.common.scripting;
import com.chameleonvision.common.logging.DebugLogger;
import com.chameleonvision.common.util.ShellExec;
import java.io.IOException;
public class ScriptEvent {
private static final DebugLogger logger = new DebugLogger(true);
private static final ShellExec executor = new ShellExec(true, true);
private static final DebugLogger logger = new DebugLogger(true);
private static final ShellExec executor = new ShellExec(true, true);
public final ScriptConfig config;
public final ScriptConfig config;
public ScriptEvent(ScriptConfig config) {
this.config = config;
}
public ScriptEvent(ScriptConfig config) {
this.config = config;
}
public int run() throws IOException {
int retVal = executor.executeBashCommand(config.command);
public int run() throws IOException {
int retVal = executor.executeBashCommand(config.command);
String output = executor.getOutput();
String error = executor.getError();
String output = executor.getOutput();
String error = executor.getError();
if (!error.isEmpty()) {
System.err.printf("Error when running \"%s\" script: %s\n", config.eventType.name(), error);
} else if (!output.isEmpty()) {
logger.printInfo(String.format("Output from \"%s\" script: %s\n", config.eventType.name(), output));
}
logger.printInfo(String.format("Script for %s ran with command line: \"%s\", exit code: %d, output: %s, error: %s\n", config.eventType.name(), config.command, retVal, output, error));
return retVal;
}
if (!error.isEmpty()) {
System.err.printf("Error when running \"%s\" script: %s\n", config.eventType.name(), error);
} else if (!output.isEmpty()) {
logger.printInfo(
String.format("Output from \"%s\" script: %s\n", config.eventType.name(), output));
}
logger.printInfo(
String.format(
"Script for %s ran with command line: \"%s\", exit code: %d, output: %s, error: %s\n",
config.eventType.name(), config.command, retVal, output, error));
return retVal;
}
}

30
chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptEventType.java
View File

@@ -1,21 +1,21 @@
package com.chameleonvision.common.scripting;
public enum ScriptEventType {
kProgramInit("Program Init"),
kProgramExit("Program Exit"),
kNTConnected("NT Connected"),
kLEDOn("LED On"),
kLEDOff("LED Off"),
kEnterDriverMode("Enter Driver Mode"),
kExitDriverMode("Exit Driver Mode"),
kFoundTarget("Found Target"),
kFoundMultipleTarget("Found Multiple Target"),
kLostTarget("Lost Target"),
kPipelineLag("Pipeline Lag");
kProgramInit("Program Init"),
kProgramExit("Program Exit"),
kNTConnected("NT Connected"),
kLEDOn("LED On"),
kLEDOff("LED Off"),
kEnterDriverMode("Enter Driver Mode"),
kExitDriverMode("Exit Driver Mode"),
kFoundTarget("Found Target"),
kFoundMultipleTarget("Found Multiple Target"),
kLostTarget("Lost Target"),
kPipelineLag("Pipeline Lag");
public final String value;
public final String value;
ScriptEventType(String value) {
this.value = value;
}
ScriptEventType(String value) {
this.value = value;
}
}

189
chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptManager.java
View File

@@ -4,7 +4,6 @@ import com.chameleonvision.common.logging.DebugLogger;
import com.chameleonvision.common.util.LoopingRunnable;
import com.chameleonvision.common.util.Platform;
import com.chameleonvision.common.util.file.JacksonUtils;
import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.Path;
@@ -15,112 +14,120 @@ import java.util.concurrent.LinkedBlockingDeque;
public class ScriptManager {
private static DebugLogger logger = new DebugLogger(true);
private static DebugLogger logger = new DebugLogger(true);
private ScriptManager() {
}
private ScriptManager() {}
private static final List<ScriptEvent> events = new ArrayList<>();
private static final LinkedBlockingDeque<ScriptEventType> queuedEvents = new LinkedBlockingDeque<>(25);
private static final List<ScriptEvent> events = new ArrayList<>();
private static final LinkedBlockingDeque<ScriptEventType> queuedEvents =
new LinkedBlockingDeque<>(25);
public static void initialize() {
ScriptConfigManager.initialize();
if (ScriptConfigManager.fileExists()) {
for (ScriptConfig scriptConfig : ScriptConfigManager.loadConfig()) {
ScriptEvent scriptEvent = new ScriptEvent(scriptConfig);
events.add(scriptEvent);
}
public static void initialize() {
ScriptConfigManager.initialize();
if (ScriptConfigManager.fileExists()) {
for (ScriptConfig scriptConfig : ScriptConfigManager.loadConfig()) {
ScriptEvent scriptEvent = new ScriptEvent(scriptConfig);
events.add(scriptEvent);
}
new Thread(new ScriptRunner(10L)).start();
} else {
System.err.println("Something went wrong initializing scripts! Events will not run.");
}
}
new Thread(new ScriptRunner(10L)).start();
} else {
System.err.println("Something went wrong initializing scripts! Events will not run.");
}
}
private static class ScriptRunner extends LoopingRunnable {
private static class ScriptRunner extends LoopingRunnable {
ScriptRunner(Long loopTimeMs) {
super(loopTimeMs);
}
ScriptRunner(Long loopTimeMs) {
super(loopTimeMs);
}
@Override
protected void process() {
try {
@Override
protected void process() {
try {
handleEvent(queuedEvents.takeFirst());
} catch (InterruptedException e) {
e.printStackTrace();
}
}
handleEvent(queuedEvents.takeFirst());
} catch (InterruptedException e) {
e.printStackTrace();
}
}
private void handleEvent(ScriptEventType eventType) {
var toRun = events.parallelStream().filter(e -> e.config.eventType == eventType).findFirst().orElse(null);
if (toRun != null) {
try {
toRun.run();
} catch (IOException e) {
System.err.printf("Failed to run script for event: %s, exception below.\n%s\n", eventType.name(), e.getMessage());
}
}
}
}
private void handleEvent(ScriptEventType eventType) {
var toRun =
events
.parallelStream()
.filter(e -> e.config.eventType == eventType)
.findFirst()
.orElse(null);
if (toRun != null) {
try {
toRun.run();
} catch (IOException e) {
System.err.printf(
"Failed to run script for event: %s, exception below.\n%s\n",
eventType.name(), e.getMessage());
}
}
}
}
protected static class ScriptConfigManager {
protected static class ScriptConfigManager {
// protected static final Path scriptConfigPath = Paths.get(ConfigManager.SettingsPath.toString(), "scripts.json");
static final Path scriptConfigPath = Paths.get(""); // TODO: FIX
// protected static final Path scriptConfigPath =
// Paths.get(ConfigManager.SettingsPath.toString(), "scripts.json");
static final Path scriptConfigPath = Paths.get(""); // TODO: FIX
private ScriptConfigManager() {
}
private ScriptConfigManager() {}
static boolean fileExists() {
return Files.exists(scriptConfigPath);
}
static boolean fileExists() {
return Files.exists(scriptConfigPath);
}
public static void initialize() {
if (!fileExists()) {
List<ScriptConfig> eventsConfig = new ArrayList<>();
for (var eventType : ScriptEventType.values()) {
eventsConfig.add(new ScriptConfig(eventType));
}
public static void initialize() {
if (!fileExists()) {
List<ScriptConfig> eventsConfig = new ArrayList<>();
for (var eventType : ScriptEventType.values()) {
eventsConfig.add(new ScriptConfig(eventType));
}
try {
JacksonUtils.serializer(scriptConfigPath, eventsConfig.toArray(new ScriptConfig[0]), true);
} catch (IOException e) {
e.printStackTrace();
}
}
}
try {
JacksonUtils.serializer(
scriptConfigPath, eventsConfig.toArray(new ScriptConfig[0]), true);
} catch (IOException e) {
e.printStackTrace();
}
}
}
static List<ScriptConfig> loadConfig() {
try {
var raw = JacksonUtils.deserialize(scriptConfigPath, ScriptConfig[].class);
if (raw != null) {
return List.of(raw);
}
} catch (IOException e) {
e.printStackTrace();
}
return new ArrayList<>();
}
static List<ScriptConfig> loadConfig() {
try {
var raw = JacksonUtils.deserialize(scriptConfigPath, ScriptConfig[].class);
if (raw != null) {
return List.of(raw);
}
} catch (IOException e) {
e.printStackTrace();
}
return new ArrayList<>();
}
protected static void deleteConfig() {
try {
Files.delete(scriptConfigPath);
} catch (IOException e) {
//
}
}
}
protected static void deleteConfig() {
try {
Files.delete(scriptConfigPath);
} catch (IOException e) {
//
}
}
}
public static void queueEvent(ScriptEventType eventType) {
if (!Platform.CurrentPlatform.isWindows()) {
try {
queuedEvents.putLast(eventType);
logger.printInfo("Queued event: " + eventType.name());
} catch (InterruptedException e) {
System.err.println("Failed to add event to queue: " + eventType.name());
}
}
}
public static void queueEvent(ScriptEventType eventType) {
if (!Platform.CurrentPlatform.isWindows()) {
try {
queuedEvents.putLast(eventType);
logger.printInfo("Queued event: " + eventType.name());
} catch (InterruptedException e) {
System.err.println("Failed to add event to queue: " + eventType.name());
}
}
}
}

9
chameleon-server/src/main/java/com/chameleonvision/common/util/ColorHelper.java
View File

@@ -1,11 +1,10 @@
package com.chameleonvision.common.util;
import java.awt.*;
import org.opencv.core.Scalar;
import java.awt.*;
public class ColorHelper {
public static Scalar colorToScalar(Color color) {
return new Scalar(color.getBlue(), color.getGreen(), color.getRed());
}
public static Scalar colorToScalar(Color color) {
return new Scalar(color.getBlue(), color.getGreen(), color.getRed());
}
}

51
chameleon-server/src/main/java/com/chameleonvision/common/util/LoopingRunnable.java
View File

@@ -1,37 +1,36 @@
package com.chameleonvision.common.util;
/**
* A thread that tries to run at a specified loop time
*/
/** A thread that tries to run at a specified loop time */
public abstract class LoopingRunnable implements Runnable {
protected volatile Long loopTimeMs;
protected volatile Long loopTimeMs;
protected abstract void process();
protected abstract void process();
public LoopingRunnable(Long loopTimeMs) {
this.loopTimeMs = loopTimeMs;
}
public LoopingRunnable(Long loopTimeMs) {
this.loopTimeMs = loopTimeMs;
}
@Override
public void run() {
while(!Thread.interrupted()) {
var now = System.currentTimeMillis();
@Override
public void run() {
while (!Thread.interrupted()) {
var now = System.currentTimeMillis();
// Do the thing
process();
// Do the thing
process();
// sleep for the remaining time
var timeElapsed = System.currentTimeMillis() - now;
var delta = loopTimeMs - timeElapsed;
try {
if(delta > 0.0) {
// sleep for the remaining time
var timeElapsed = System.currentTimeMillis() - now;
var delta = loopTimeMs - timeElapsed;
try {
if (delta > 0.0) {
Thread.sleep(delta, 0);
Thread.sleep(delta, 0);
} else {
Thread.sleep(1);
}
} catch (Exception ignored) {}
}
}
} else {
Thread.sleep(1);
}
} catch (Exception ignored) {
}
}
}
}

96
chameleon-server/src/main/java/com/chameleonvision/common/util/MemoryManager.java
View File

@@ -2,65 +2,67 @@ package com.chameleonvision.common.util;
public class MemoryManager {
private static final long MEGABYTE_FACTOR = 1024L * 1024L;
private static final long MEGABYTE_FACTOR = 1024L * 1024L;
private int collectionThreshold;
private long collectionPeriodMillis = -1;
private int collectionThreshold;
private long collectionPeriodMillis = -1;
private double lastUsedMb = 0;
private long lastCollectionMillis = 0;
private double lastUsedMb = 0;
private long lastCollectionMillis = 0;
public MemoryManager(int collectionThreshold) {
this.collectionThreshold = collectionThreshold;
}
public MemoryManager(int collectionThreshold) {
this.collectionThreshold = collectionThreshold;
}
public MemoryManager(int collectionThreshold, long collectionPeriodMillis) {
this.collectionThreshold = collectionThreshold;
this.collectionPeriodMillis = collectionPeriodMillis;
}
public MemoryManager(int collectionThreshold, long collectionPeriodMillis) {
this.collectionThreshold = collectionThreshold;
this.collectionPeriodMillis = collectionPeriodMillis;
}
public void setCollectionThreshold(int collectionThreshold) {
this.collectionThreshold = collectionThreshold;
}
public void setCollectionThreshold(int collectionThreshold) {
this.collectionThreshold = collectionThreshold;
}
public void setCollectionPeriodMillis(long collectionPeriodMillis) {
this.collectionPeriodMillis = collectionPeriodMillis;
}
public void setCollectionPeriodMillis(long collectionPeriodMillis) {
this.collectionPeriodMillis = collectionPeriodMillis;
}
private static long getUsedMemory() {
return Runtime.getRuntime().totalMemory() - Runtime.getRuntime().freeMemory();
}
private static long getUsedMemory() {
return Runtime.getRuntime().totalMemory() - Runtime.getRuntime().freeMemory();
}
private static double getUsedMemoryMB() {
return ((double) getUsedMemory() / MEGABYTE_FACTOR);
}
private static double getUsedMemoryMB() {
return ((double) getUsedMemory() / MEGABYTE_FACTOR);
}
private void collect() {
System.gc();
System.runFinalization();
}
private void collect() {
System.gc();
System.runFinalization();
}
public void run() {
run(false);
}
public void run() {
run(false);
}
public void run(boolean print) {
var usedMem = getUsedMemoryMB();
public void run(boolean print) {
var usedMem = getUsedMemoryMB();
if (usedMem != lastUsedMb) {
lastUsedMb = usedMem;
if (print) System.out.printf("Memory usage: %.2fMB\n", usedMem);
}
if (usedMem != lastUsedMb) {
lastUsedMb = usedMem;
if (print) System.out.printf("Memory usage: %.2fMB\n", usedMem);
}
boolean collectionThresholdPassed = usedMem >= collectionThreshold;
boolean collectionPeriodPassed = collectionPeriodMillis != -1 && (System.currentTimeMillis() - lastCollectionMillis >= collectionPeriodMillis);
boolean collectionThresholdPassed = usedMem >= collectionThreshold;
boolean collectionPeriodPassed =
collectionPeriodMillis != -1
&& (System.currentTimeMillis() - lastCollectionMillis >= collectionPeriodMillis);
if (collectionThresholdPassed || collectionPeriodPassed) {
collect();
lastCollectionMillis = System.currentTimeMillis();
if (print) {
System.out.printf("Garbage collected at %.2fMB\n", usedMem);
}
}
}
if (collectionThresholdPassed || collectionPeriodPassed) {
collect();
lastCollectionMillis = System.currentTimeMillis();
if (print) {
System.out.printf("Garbage collected at %.2fMB\n", usedMem);
}
}
}
}

160
chameleon-server/src/main/java/com/chameleonvision/common/util/Platform.java
View File

@@ -1,109 +1,107 @@
package com.chameleonvision.common.util;
import edu.wpi.first.wpiutil.RuntimeDetector;
import java.io.IOException;
@SuppressWarnings("unused")
public enum Platform {
// WPILib Supported (JNI)
WINDOWS_32("Windows x32"),
WINDOWS_64("Windows x64"),
LINUX_64("Linux x64"),
LINUX_RASPBIAN("Linux Raspbian"), // Raspberry Pi 3/4
LINUX_AARCH64BIONIC("Linux AARCH64 Bionic"), // Jetson Nano, Jetson TX2
MACOS_64("Mac OS x64"),
// WPILib Supported (JNI)
WINDOWS_32("Windows x32"),
WINDOWS_64("Windows x64"),
LINUX_64("Linux x64"),
LINUX_RASPBIAN("Linux Raspbian"), // Raspberry Pi 3/4
LINUX_AARCH64BIONIC("Linux AARCH64 Bionic"), // Jetson Nano, Jetson TX2
MACOS_64("Mac OS x64"),
// ChameleonVision Supported (Manual install)
LINUX_ARM32("Linux ARM32"), // ODROID XU4, C1+
LINUX_ARM64("Linux ARM64"), // ODROID C2, N2
// ChameleonVision Supported (Manual install)
LINUX_ARM32("Linux ARM32"), // ODROID XU4, C1+
LINUX_ARM64("Linux ARM64"), // ODROID C2, N2
// Completely unsupported
UNSUPPORTED("Unsupported Platform");
// Completely unsupported
UNSUPPORTED("Unsupported Platform");
public final String value;
public final boolean isRoot = checkForRoot();
public final String value;
public final boolean isRoot = checkForRoot();
Platform(String value) {
this.value = value;
}
Platform(String value) {
this.value = value;
}
private static final String OS_NAME = System.getProperty("os.name");
private static final String OS_ARCH = System.getProperty("os.arch");
public static final Platform CurrentPlatform = getCurrentPlatform();
private static final String OS_NAME = System.getProperty("os.name");
private static final String OS_ARCH = System.getProperty("os.arch");
public static final Platform CurrentPlatform = getCurrentPlatform();
private static String UnknownPlatformString = String.format("Unknown Platform. OS: %s, Architecture: %s", OS_NAME, OS_ARCH);
private static String UnknownPlatformString =
String.format("Unknown Platform. OS: %s, Architecture: %s", OS_NAME, OS_ARCH);
public boolean isWindows() {
return this == WINDOWS_64 || this == WINDOWS_32;
}
public boolean isWindows() {
return this == WINDOWS_64 || this == WINDOWS_32;
}
public boolean isLinux() {
return this == LINUX_64 || this == LINUX_RASPBIAN || this == LINUX_ARM64;
}
public boolean isLinux() {
return this == LINUX_64 || this == LINUX_RASPBIAN || this == LINUX_ARM64;
}
public boolean isMac() {
return this == MACOS_64;
}
public boolean isMac() {
return this == MACOS_64;
}
public static boolean isRaspberryPi() {
return CurrentPlatform.equals(LINUX_RASPBIAN);
}
public static boolean isRaspberryPi() {
return CurrentPlatform.equals(LINUX_RASPBIAN);
}
private static ShellExec shell = new ShellExec(true, false);
private static ShellExec shell = new ShellExec(true, false);
@SuppressWarnings("StatementWithEmptyBody")
private boolean checkForRoot() {
if (isLinux() || isMac()) {
try {
shell.execute("id", null, true, "-u");
} catch (IOException e) {
e.printStackTrace();
}
@SuppressWarnings("StatementWithEmptyBody")
private boolean checkForRoot() {
if (isLinux() || isMac()) {
try {
shell.execute("id", null, true, "-u");
} catch (IOException e) {
e.printStackTrace();
}
// TODO: better way to do this?
while (!shell.isOutputCompleted()) {
// ignored
}
// TODO: better way to do this?
while (!shell.isOutputCompleted()) {
// ignored
}
if (shell.getExitCode() == 0) {
return shell.getOutput().split("\n")[0].equals("0");
}
if (shell.getExitCode() == 0) {
return shell.getOutput().split("\n")[0].equals("0");
}
} else {
return true;
}
return false;
}
} else {
return true;
}
return false;
}
private static Platform getCurrentPlatform() {
if (RuntimeDetector.isWindows()) {
if (RuntimeDetector.is32BitIntel()) return WINDOWS_32;
if (RuntimeDetector.is64BitIntel()) return WINDOWS_64;
}
private static Platform getCurrentPlatform() {
if (RuntimeDetector.isWindows()) {
if (RuntimeDetector.is32BitIntel()) return WINDOWS_32;
if (RuntimeDetector.is64BitIntel()) return WINDOWS_64;
}
if (RuntimeDetector.isMac()) {
if (RuntimeDetector.is32BitIntel()) return UNSUPPORTED;
if (RuntimeDetector.is64BitIntel()) return MACOS_64;
}
if (RuntimeDetector.isMac()) {
if (RuntimeDetector.is32BitIntel()) return UNSUPPORTED;
if (RuntimeDetector.is64BitIntel()) return MACOS_64;
}
if (RuntimeDetector.isLinux()) {
if (RuntimeDetector.is32BitIntel()) return UNSUPPORTED;
if (RuntimeDetector.is64BitIntel()) return LINUX_64;
if (RuntimeDetector.isRaspbian()) return LINUX_RASPBIAN;
if (RuntimeDetector.isLinux()) {
if (RuntimeDetector.is32BitIntel()) return UNSUPPORTED;
if (RuntimeDetector.is64BitIntel()) return LINUX_64;
if (RuntimeDetector.isRaspbian()) return LINUX_RASPBIAN;
}
}
System.out.println(UnknownPlatformString);
return Platform.UNSUPPORTED;
}
System.out.println(UnknownPlatformString);
return Platform.UNSUPPORTED;
}
public String toString() {
if (this.equals(UNSUPPORTED)) {
return UnknownPlatformString;
} else {
return this.value;
}
}
public String toString() {
if (this.equals(UNSUPPORTED)) {
return UnknownPlatformString;
} else {
return this.value;
}
}
}

298
chameleon-server/src/main/java/com/chameleonvision/common/util/ShellExec.java
View File

@@ -2,180 +2,178 @@ package com.chameleonvision.common.util;
import java.io.*;
/**
* Execute external process and optionally read output buffer.
*/
/** Execute external process and optionally read output buffer. */
@SuppressWarnings({"unused", "ConstantConditions"})
public class ShellExec {
private int exitCode;
private boolean readOutput, readError;
private StreamGobbler errorGobbler, outputGobbler;
private int exitCode;
private boolean readOutput, readError;
private StreamGobbler errorGobbler, outputGobbler;
public ShellExec() {
this(false, false);
}
public ShellExec() {
this(false, false);
}
public ShellExec(boolean readOutput, boolean readError) {
this.readOutput = readOutput;
this.readError = readError;
}
public ShellExec(boolean readOutput, boolean readError) {
this.readOutput = readOutput;
this.readError = readError;
}
/**
* Execute a bash command. We can handle complex bash commands including
* multiple executions (; | && ||), quotes, expansions ($), escapes (\), e.g.:
* "cd /abc/def; mv ghi 'older ghi '$(whoami)"
* @param command Bash command to execute
* @return true if bash got started, but your command may have failed.
*/
public int executeBashCommand(String command) throws IOException {
boolean wait = true;
boolean success = false;
Runtime r = Runtime.getRuntime();
// Use bash -c so we can handle things like multi commands separated by ; and
// things like quotes, $, |, and \. My tests show that command comes as
// one argument to bash, so we do not need to quote it to make it one thing.
// Also, exec may object if it does not have an executable file as the first thing,
// so having bash here makes it happy provided bash is installed and in path.
String[] commands = {"bash", "-c", command};
/**
* Execute a bash command. We can handle complex bash commands including multiple executions (; |
* && ||), quotes, expansions ($), escapes (\), e.g.: "cd /abc/def; mv ghi 'older ghi '$(whoami)"
*
* @param command Bash command to execute
* @return true if bash got started, but your command may have failed.
*/
public int executeBashCommand(String command) throws IOException {
boolean wait = true;
boolean success = false;
Runtime r = Runtime.getRuntime();
// Use bash -c so we can handle things like multi commands separated by ; and
// things like quotes, $, |, and \. My tests show that command comes as
// one argument to bash, so we do not need to quote it to make it one thing.
// Also, exec may object if it does not have an executable file as the first thing,
// so having bash here makes it happy provided bash is installed and in path.
String[] commands = {"bash", "-c", command};
Process process = r.exec(commands);
Process process = r.exec(commands);
// Consume streams, older jvm's had a memory leak if streams were not read,
// some other jvm+OS combinations may block unless streams are consumed.
return doProcess(wait, process);
}
// Consume streams, older jvm's had a memory leak if streams were not read,
// some other jvm+OS combinations may block unless streams are consumed.
return doProcess(wait, process);
}
/**
* 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 args 0..n command line arguments
* @return process exit code
*/
public int execute(String command, String... args) throws IOException {
return execute(command, null, true, args);
}
/**
* 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 args 0..n command line arguments
* @return process exit code
*/
public int execute(String command, String... args) throws IOException {
return execute(command, null, true, args);
}
/**
* Execute a command.
* @param command command ("c:/some/folder/script.bat" or "some/folder/script.sh")
* @param workdir working directory or NULL to use command folder
* @param wait wait for process to end
* @param args 0..n command line arguments
* @return process exit code
*/
public int execute(String command, String workdir, boolean wait, String...args) throws IOException {
String[] cmdArr;
if (args != null && args.length > 0) {
cmdArr = new String[1+args.length];
cmdArr[0] = command;
System.arraycopy(args, 0, cmdArr, 1, args.length);
} else {
cmdArr = new String[] { command };
}
/**
* Execute a command.
*
* @param command command ("c:/some/folder/script.bat" or "some/folder/script.sh")
* @param workdir working directory or NULL to use command folder
* @param wait wait for process to end
* @param args 0..n command line arguments
* @return process exit code
*/
public int execute(String command, String workdir, boolean wait, String... args)
throws IOException {
String[] cmdArr;
if (args != null && args.length > 0) {
cmdArr = new String[1 + args.length];
cmdArr[0] = command;
System.arraycopy(args, 0, cmdArr, 1, args.length);
} else {
cmdArr = new String[] {command};
}
ProcessBuilder pb = new ProcessBuilder(cmdArr);
File workingDir = (workdir==null ? new File(command).getParentFile() : new File(workdir) );
pb.directory(workingDir);
ProcessBuilder pb = new ProcessBuilder(cmdArr);
File workingDir = (workdir == null ? new File(command).getParentFile() : new File(workdir));
pb.directory(workingDir);
Process process = pb.start();
Process process = pb.start();
// Consume streams, older jvm's had a memory leak if streams were not read,
// some other jvm+OS combinations may block unless streams are consumed.
return doProcess(wait, process);
}
// Consume streams, older jvm's had a memory leak if streams were not read,
// some other jvm+OS combinations may block unless streams are consumed.
return doProcess(wait, process);
}
private int doProcess(boolean wait, Process process) {
errorGobbler = new StreamGobbler(process.getErrorStream(), readError);
outputGobbler = new StreamGobbler(process.getInputStream(), readOutput);
errorGobbler.start();
outputGobbler.start();
private int doProcess(boolean wait, Process process) {
errorGobbler = new StreamGobbler(process.getErrorStream(), readError);
outputGobbler = new StreamGobbler(process.getInputStream(), readOutput);
errorGobbler.start();
outputGobbler.start();
exitCode = 0;
if (wait) {
try {
process.waitFor();
exitCode = process.exitValue();
} catch (InterruptedException ignored) { }
}
return exitCode;
}
exitCode = 0;
if (wait) {
try {
process.waitFor();
exitCode = process.exitValue();
} catch (InterruptedException ignored) {
}
}
return exitCode;
}
public int getExitCode() {
return exitCode;
}
public int getExitCode() {
return exitCode;
}
public boolean isOutputCompleted() {
return (outputGobbler != null && outputGobbler.isCompleted());
}
public boolean isOutputCompleted() {
return (outputGobbler != null && outputGobbler.isCompleted());
}
public boolean isErrorCompleted() {
return (errorGobbler != null && errorGobbler.isCompleted());
}
public boolean isErrorCompleted() {
return (errorGobbler != null && errorGobbler.isCompleted());
}
public String getOutput() {
return (outputGobbler != null ? outputGobbler.getOutput() : null);
}
public String getOutput() {
return (outputGobbler != null ? outputGobbler.getOutput() : null);
}
public String getError() {
return (errorGobbler != null ? errorGobbler.getOutput() : null);
}
public String getError() {
return (errorGobbler != null ? errorGobbler.getOutput() : null);
}
//********************************************
//********************************************
// ********************************************
// ********************************************
/**
* 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.
* http://www.javaworld.com/javaworld/jw-12-2000/jw-1229-traps.html?page=4
*/
@SuppressWarnings("WeakerAccess")
private static class StreamGobbler extends Thread {
private InputStream is;
private StringBuilder output;
private volatile boolean completed; // mark volatile to guarantee a thread safety
/**
* 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.
* http://www.javaworld.com/javaworld/jw-12-2000/jw-1229-traps.html?page=4
*/
@SuppressWarnings("WeakerAccess")
private static class StreamGobbler extends Thread {
private InputStream is;
private StringBuilder output;
private volatile boolean completed; // mark volatile to guarantee a thread safety
public StreamGobbler(InputStream is, boolean readStream) {
this.is = is;
this.output = (readStream ? new StringBuilder(256) : null);
}
public StreamGobbler(InputStream is, boolean readStream) {
this.is = is;
this.output = (readStream ? new StringBuilder(256) : null);
}
public void run() {
completed = false;
try {
String NL = System.getProperty("line.separator", "\r\n");
public void run() {
completed = false;
try {
String NL = System.getProperty("line.separator", "\r\n");
InputStreamReader isr = new InputStreamReader(is);
BufferedReader br = new BufferedReader(isr);
String line;
while ( (line = br.readLine()) != null) {
if (output != null)
output.append(line).append(NL);
}
} catch (IOException ex) {
// ex.printStackTrace();
}
completed = true;
}
InputStreamReader isr = new InputStreamReader(is);
BufferedReader br = new BufferedReader(isr);
String line;
while ((line = br.readLine()) != null) {
if (output != null) output.append(line).append(NL);
}
} catch (IOException ex) {
// ex.printStackTrace();
}
completed = true;
}
/**
* Get inputstream buffer or null if stream
* was not consumed.
* @return Output stream
*/
public String getOutput() {
return (output != null ? output.toString() : null);
}
/**
* Is input stream completed.
* @return if input stream is completed
*/
public boolean isCompleted() {
return completed;
}
}
/**
* Get inputstream buffer or null if stream was not consumed.
*
* @return Output stream
*/
public String getOutput() {
return (output != null ? output.toString() : null);
}
/**
* Is input stream completed.
*
* @return if input stream is completed
*/
public boolean isCompleted() {
return completed;
}
}
}

66
chameleon-server/src/main/java/com/chameleonvision/common/util/file/FileUtils.java
View File

@@ -2,7 +2,6 @@ package com.chameleonvision.common.util.file;
import com.chameleonvision.common.logging.DebugLogger;
import com.chameleonvision.common.util.Platform;
import java.io.File;
import java.io.IOException;
import java.nio.file.Files;
@@ -14,38 +13,41 @@ import java.util.HashSet;
import java.util.Set;
public class FileUtils {
private static DebugLogger logger = new DebugLogger(true);
private static final Set<PosixFilePermission> allReadWriteExecutePerms = new HashSet<>(Arrays.asList(PosixFilePermission.values()));
private static DebugLogger logger = new DebugLogger(true);
private static final Set<PosixFilePermission> allReadWriteExecutePerms =
new HashSet<>(Arrays.asList(PosixFilePermission.values()));
public static void setFilePerms(Path path) throws IOException {
if (!Platform.CurrentPlatform.isWindows()) {
File thisFile = path.toFile();
Set<PosixFilePermission> perms = Files.readAttributes(path, PosixFileAttributes.class).permissions();
if (!perms.equals(allReadWriteExecutePerms)) {
logger.printInfo("Setting perms on" + path.toString());
Files.setPosixFilePermissions(path, perms);
if (thisFile.isDirectory()) {
for (File subfile : thisFile.listFiles()) {
setFilePerms(subfile.toPath());
}
}
}
}
}
public static void setFilePerms(Path path) throws IOException {
if (!Platform.CurrentPlatform.isWindows()) {
File thisFile = path.toFile();
Set<PosixFilePermission> perms =
Files.readAttributes(path, PosixFileAttributes.class).permissions();
if (!perms.equals(allReadWriteExecutePerms)) {
logger.printInfo("Setting perms on" + path.toString());
Files.setPosixFilePermissions(path, perms);
if (thisFile.isDirectory()) {
for (File subfile : thisFile.listFiles()) {
setFilePerms(subfile.toPath());
}
}
}
}
}
public static void setAllPerms(Path path) {
if (!Platform.CurrentPlatform.isWindows()) {
String command = String.format("chmod 777 -R %s", path.toString());
try {
Process p = Runtime.getRuntime().exec(command);
p.waitFor();
public static void setAllPerms(Path path) {
if (!Platform.CurrentPlatform.isWindows()) {
String command = String.format("chmod 777 -R %s", path.toString());
try {
Process p = Runtime.getRuntime().exec(command);
p.waitFor();
} catch (Exception e) {
e.printStackTrace();
}
} else {
// TODO file perms on Windows
System.out.println("File permission setting not available on Windows. Not changing file permissions.");
}
}
} catch (Exception e) {
e.printStackTrace();
}
} else {
// TODO file perms on Windows
System.out.println(
"File permission setting not available on Windows. Not changing file permissions.");
}
}
}

114
chameleon-server/src/main/java/com/chameleonvision/common/util/file/JacksonUtils.java
View File

@@ -7,7 +7,6 @@ import com.fasterxml.jackson.databind.jsontype.BasicPolymorphicTypeValidator;
import com.fasterxml.jackson.databind.jsontype.PolymorphicTypeValidator;
import com.fasterxml.jackson.databind.module.SimpleModule;
import com.fasterxml.jackson.databind.ser.std.StdSerializer;
import java.io.File;
import java.io.FileDescriptor;
import java.io.FileOutputStream;
@@ -15,60 +14,73 @@ import java.io.IOException;
import java.nio.file.Path;
public class JacksonUtils {
public static <T> void serializer(Path path, T object) throws IOException {
serializer(path, object, false);
}
public static <T> void serializer(Path path, T object) throws IOException {
serializer(path, object, false);
}
public static <T> void serializer(Path path, T object, boolean forceSync) throws IOException {
PolymorphicTypeValidator ptv = BasicPolymorphicTypeValidator.builder().allowIfBaseType(object.getClass()).build();
ObjectMapper objectMapper = JsonMapper.builder().activateDefaultTyping(ptv, ObjectMapper.DefaultTyping.JAVA_LANG_OBJECT).build();
String json = objectMapper.writerWithDefaultPrettyPrinter().writeValueAsString(object);
saveJsonString(json, path, forceSync);
}
public static <T> void serializer(Path path, T object, boolean forceSync) throws IOException {
PolymorphicTypeValidator ptv =
BasicPolymorphicTypeValidator.builder().allowIfBaseType(object.getClass()).build();
ObjectMapper objectMapper =
JsonMapper.builder()
.activateDefaultTyping(ptv, ObjectMapper.DefaultTyping.JAVA_LANG_OBJECT)
.build();
String json = objectMapper.writerWithDefaultPrettyPrinter().writeValueAsString(object);
saveJsonString(json, path, forceSync);
}
public static <T> T deserialize(Path path, Class<T> ref) throws IOException {
PolymorphicTypeValidator ptv = BasicPolymorphicTypeValidator.builder().allowIfBaseType(ref).build();
ObjectMapper objectMapper = JsonMapper.builder().activateDefaultTyping(ptv, ObjectMapper.DefaultTyping.JAVA_LANG_OBJECT).build();
File jsonFile = new File(path.toString());
if (jsonFile.exists() && jsonFile.length() > 0) {
return objectMapper.readValue(jsonFile, ref);
}
return null;
}
public static <T> T deserialize(Path path, Class<T> ref) throws IOException {
PolymorphicTypeValidator ptv =
BasicPolymorphicTypeValidator.builder().allowIfBaseType(ref).build();
ObjectMapper objectMapper =
JsonMapper.builder()
.activateDefaultTyping(ptv, ObjectMapper.DefaultTyping.JAVA_LANG_OBJECT)
.build();
File jsonFile = new File(path.toString());
if (jsonFile.exists() && jsonFile.length() > 0) {
return objectMapper.readValue(jsonFile, ref);
}
return null;
}
public static <T> T deserialize(Path path, Class<T> ref, StdDeserializer<T> deserializer) throws IOException {
ObjectMapper objectMapper = new ObjectMapper();
SimpleModule module = new SimpleModule();
module.addDeserializer(ref, deserializer);
objectMapper.registerModule(module);
public static <T> T deserialize(Path path, Class<T> ref, StdDeserializer<T> deserializer)
throws IOException {
ObjectMapper objectMapper = new ObjectMapper();
SimpleModule module = new SimpleModule();
module.addDeserializer(ref, deserializer);
objectMapper.registerModule(module);
File jsonFile = new File(path.toString());
if (jsonFile.exists() && jsonFile.length() > 0) {
return objectMapper.readValue(jsonFile, ref);
}
return null;
}
public static <T> void serialize(Path path, T object, Class<T> ref, StdSerializer<T> serializer) throws IOException {
serialize(path, object, ref, serializer, false);
}
File jsonFile = new File(path.toString());
if (jsonFile.exists() && jsonFile.length() > 0) {
return objectMapper.readValue(jsonFile, ref);
}
return null;
}
public static <T> void serialize(Path path, T object, Class<T> ref, StdSerializer<T> serializer, boolean forceSync) throws IOException {
ObjectMapper objectMapper = new ObjectMapper();
SimpleModule module = new SimpleModule();
module.addSerializer(ref, serializer);
objectMapper.registerModule(module);
String json = objectMapper.writerWithDefaultPrettyPrinter().writeValueAsString(object);
saveJsonString(json, path, forceSync);
}
public static <T> void serialize(Path path, T object, Class<T> ref, StdSerializer<T> serializer)
throws IOException {
serialize(path, object, ref, serializer, false);
}
private static void saveJsonString(String json, Path path, boolean forceSync) throws IOException {
FileOutputStream fileOutputStream = new FileOutputStream(path.toFile());
fileOutputStream.write(json.getBytes());
fileOutputStream.flush();
if (forceSync) {
FileDescriptor fileDescriptor = fileOutputStream.getFD();
fileDescriptor.sync();
}
fileOutputStream.close();
}
public static <T> void serialize(
Path path, T object, Class<T> ref, StdSerializer<T> serializer, boolean forceSync)
throws IOException {
ObjectMapper objectMapper = new ObjectMapper();
SimpleModule module = new SimpleModule();
module.addSerializer(ref, serializer);
objectMapper.registerModule(module);
String json = objectMapper.writerWithDefaultPrettyPrinter().writeValueAsString(object);
saveJsonString(json, path, forceSync);
}
private static void saveJsonString(String json, Path path, boolean forceSync) throws IOException {
FileOutputStream fileOutputStream = new FileOutputStream(path.toFile());
fileOutputStream.write(json.getBytes());
fileOutputStream.flush();
if (forceSync) {
FileDescriptor fileDescriptor = fileOutputStream.getFD();
fileDescriptor.sync();
}
fileOutputStream.close();
}
}

53
chameleon-server/src/main/java/com/chameleonvision/common/util/math/IPUtils.java
View File

@@ -4,35 +4,36 @@ import java.util.ArrayList;
import java.util.List;
public class IPUtils {
public static boolean isValidIPV4(final String ip) {
String PATTERN = "^((0|1\\d?\\d?|2[0-4]?\\d?|25[0-5]?|[3-9]\\d?)\\.){3}(0|1\\d?\\d?|2[0-4]?\\d?|25[0-5]?|[3-9]\\d?)$";
public static boolean isValidIPV4(final String ip) {
String PATTERN =
"^((0|1\\d?\\d?|2[0-4]?\\d?|25[0-5]?|[3-9]\\d?)\\.){3}(0|1\\d?\\d?|2[0-4]?\\d?|25[0-5]?|[3-9]\\d?)$";
return ip.matches(PATTERN);
}
return ip.matches(PATTERN);
}
public static List<Byte> getDigitBytes(int num) {
List<Byte> digits = new ArrayList<>();
collectDigitBytes(num, digits);
return digits;
}
public static List<Byte> getDigitBytes(int num) {
List<Byte> digits = new ArrayList<>();
collectDigitBytes(num, digits);
return digits;
}
private static void collectDigitBytes(int num, List<Byte> digits) {
if (num / 10 > 0) {
collectDigitBytes( num / 10, digits);
}
digits.add((byte) (num % 10));
}
private static void collectDigitBytes(int num, List<Byte> digits) {
if (num / 10 > 0) {
collectDigitBytes(num / 10, digits);
}
digits.add((byte) (num % 10));
}
public static List<Integer> getDigits(int num) {
List<Integer> digits = new ArrayList<>();
collectDigits(num, digits);
return digits;
}
public static List<Integer> getDigits(int num) {
List<Integer> digits = new ArrayList<>();
collectDigits(num, digits);
return digits;
}
private static void collectDigits(int num, List<Integer> digits) {
if(num / 10 > 0) {
collectDigits(num / 10, digits);
}
digits.add(num % 10);
}
private static void collectDigits(int num, List<Integer> digits) {
if (num / 10 > 0) {
collectDigits(num / 10, digits);
}
digits.add(num % 10);
}
}

37
chameleon-server/src/main/java/com/chameleonvision/common/util/math/MathUtils.java
View File

@@ -3,28 +3,27 @@ package com.chameleonvision.common.util.math;
import org.apache.commons.math3.util.FastMath;
public class MathUtils {
MathUtils() {}
MathUtils() {}
public static double sigmoid(Number x){
double bias = 0;
double a = 5;
double b = -0.05;
double k = 200;
public static double sigmoid(Number x) {
double bias = 0;
double a = 5;
double b = -0.05;
double k = 200;
if (x.doubleValue() < 50){
bias = -1.338;
}
if (x.doubleValue() < 50) {
bias = -1.338;
}
return ((k / (1 + Math.pow(Math.E,(a + (b * x.doubleValue()))))) + bias);
}
return ((k / (1 + Math.pow(Math.E, (a + (b * x.doubleValue()))))) + bias);
}
public static double toSlope(Number angle){
return FastMath.atan(FastMath.toRadians(angle.doubleValue() - 90));
}
public static double roundTo(double value, int to) {
double toMult = Math.pow(10, to);
return (double)Math.round(value * toMult) / toMult;
}
public static double toSlope(Number angle) {
return FastMath.atan(FastMath.toRadians(angle.doubleValue() - 90));
}
public static double roundTo(double value, int to) {
double toMult = Math.pow(10, to);
return (double) Math.round(value * toMult) / toMult;
}
}

12
chameleon-server/src/main/java/com/chameleonvision/common/util/numbers/DoubleCouple.java
View File

@@ -2,11 +2,11 @@ package com.chameleonvision.common.util.numbers;
public class DoubleCouple extends NumberCouple<Double> {
public DoubleCouple() {
super(0.0, 0.0);
}
public DoubleCouple() {
super(0.0, 0.0);
}
public DoubleCouple(Double first, Double second) {
super(first, second);
}
public DoubleCouple(Double first, Double second) {
super(first, second);
}
}

12
chameleon-server/src/main/java/com/chameleonvision/common/util/numbers/IntegerCouple.java
View File

@@ -2,11 +2,11 @@ package com.chameleonvision.common.util.numbers;
public class IntegerCouple extends NumberCouple<Integer> {
public IntegerCouple() {
super(0, 0);
}
public IntegerCouple() {
super(0, 0);
}
public IntegerCouple(Integer first, Integer second) {
super(first, second);
}
public IntegerCouple(Integer first, Integer second) {
super(first, second);
}
}

78
chameleon-server/src/main/java/com/chameleonvision/common/util/numbers/NumberCouple.java
View File

@@ -2,54 +2,54 @@ package com.chameleonvision.common.util.numbers;
public abstract class NumberCouple<T extends Number> {
private T first;
private T second;
private T first;
private T second;
public NumberCouple(T first, T second) {
this.first = first;
this.second = second;
}
public NumberCouple(T first, T second) {
this.first = first;
this.second = second;
}
public void setFirst(T first) {
this.first = first;
}
public void setFirst(T first) {
this.first = first;
}
public T getFirst() {
return first;
}
public T getFirst() {
return first;
}
public void setSecond(T second) {
this.second = second;
}
public void setSecond(T second) {
this.second = second;
}
public T getSecond() {
return second;
}
public T getSecond() {
return second;
}
public void set(T first, T second) {
this.first = first;
this.second = second;
}
public void set(T first, T second) {
this.first = first;
this.second = second;
}
@Override
public boolean equals(Object obj) {
if (!(obj instanceof NumberCouple)) {
return false;
}
@Override
public boolean equals(Object obj) {
if (!(obj instanceof NumberCouple)) {
return false;
}
var couple = (NumberCouple) obj;
if (!couple.first.equals(first)) {
return false;
}
var couple = (NumberCouple) obj;
if (!couple.first.equals(first)) {
return false;
}
if (!couple.second.equals(second)) {
return false;
}
if (!couple.second.equals(second)) {
return false;
}
return true;
}
return true;
}
public boolean isEmpty() {
return first.intValue() == 0 && second.intValue() == 0;
}
public boolean isEmpty() {
return first.intValue() == 0 && second.intValue() == 0;
}
}

3
chameleon-server/src/main/java/com/chameleonvision/common/vision/camera/USBCamera.java
View File

@@ -2,5 +2,4 @@ package com.chameleonvision.common.vision.camera;
import com.chameleonvision.common.vision.frame.provider.USBFrameProvider;
public class USBCamera extends USBFrameProvider {
}
public class USBCamera extends USBFrameProvider {}

21
chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/Frame.java
View File

@@ -1,19 +1,18 @@
package com.chameleonvision.common.vision.frame;
import org.opencv.core.Mat;
import org.opencv.core.Size;
public class Frame {
public long timestampNanos;
public Mat image;
public long timestampNanos;
public Mat image;
public Frame(Mat image) {
this.image = image;
timestampNanos = System.nanoTime();
}
public Frame(Mat image) {
this.image = image;
timestampNanos = System.nanoTime();
}
public Frame(Mat image, long timestampNanos) {
this.image = image;
this.timestampNanos = timestampNanos;
}
public Frame(Mat image, long timestampNanos) {
this.image = image;
this.timestampNanos = timestampNanos;
}
}

2
chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/FrameConsumer.java
View File

@@ -1,5 +1,5 @@
package com.chameleonvision.common.vision.frame;
public interface FrameConsumer {
void consume(Frame frame);
void consume(Frame frame);
}

2
chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/FrameProvider.java
View File

@@ -1,5 +1,5 @@
package com.chameleonvision.common.vision.frame;
public interface FrameProvider {
Frame getFrame();
Frame getFrame();
}

8
chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/consumer/MJPGFrameConsumer.java
View File

@@ -5,8 +5,8 @@ import com.chameleonvision.common.vision.frame.FrameConsumer;
import org.apache.commons.lang3.NotImplementedException;
public class MJPGFrameConsumer implements FrameConsumer {
@Override
public void consume(Frame frame) {
throw new NotImplementedException("");
}
@Override
public void consume(Frame frame) {
throw new NotImplementedException("");
}
}

8
chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/provider/FileFrameProvider.java
View File

@@ -5,8 +5,8 @@ import com.chameleonvision.common.vision.frame.FrameProvider;
import org.apache.commons.lang3.NotImplementedException;
public class FileFrameProvider implements FrameProvider {
@Override
public Frame getFrame() {
throw new NotImplementedException("");
}
@Override
public Frame getFrame() {
throw new NotImplementedException("");
}
}

8
chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/provider/NetworkFrameProvider.java
View File

@@ -5,8 +5,8 @@ import com.chameleonvision.common.vision.frame.FrameProvider;
import org.apache.commons.lang3.NotImplementedException;
public class NetworkFrameProvider implements FrameProvider {
@Override
public Frame getFrame() {
throw new NotImplementedException("");
}
@Override
public Frame getFrame() {
throw new NotImplementedException("");
}
}

8
chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/provider/USBFrameProvider.java
View File

@@ -5,8 +5,8 @@ import com.chameleonvision.common.vision.frame.FrameProvider;
import org.apache.commons.lang3.NotImplementedException;
public class USBFrameProvider implements FrameProvider {
@Override
public Frame getFrame() {
throw new NotImplementedException("");
}
@Override
public Frame getFrame() {
throw new NotImplementedException("");
}
}

60
chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/Contour.java
View File

@@ -5,40 +5,40 @@ import org.opencv.imgproc.Imgproc;
public class Contour {
private final MatOfPoint points;
private final MatOfPoint points;
private Double area = Double.NaN;
private RotatedRect minAreaRect = null;
private Rect boundingRect = null;
private Double area = Double.NaN;
private RotatedRect minAreaRect = null;
private Rect boundingRect = null;
public Contour(MatOfPoint points) {
this.points = points;
}
public Contour(MatOfPoint points) {
this.points = points;
}
public double getArea() {
if (Double.isNaN(area)) {
area = Imgproc.contourArea(points);
}
return area;
}
public double getArea() {
if (Double.isNaN(area)) {
area = Imgproc.contourArea(points);
}
return area;
}
public RotatedRect getMinAreaRect() {
if (minAreaRect == null) {
MatOfPoint2f temp = new MatOfPoint2f(points.toArray());
minAreaRect = Imgproc.minAreaRect(temp);
temp.release();
}
return minAreaRect;
}
public RotatedRect getMinAreaRect() {
if (minAreaRect == null) {
MatOfPoint2f temp = new MatOfPoint2f(points.toArray());
minAreaRect = Imgproc.minAreaRect(temp);
temp.release();
}
return minAreaRect;
}
public Rect getBoundingRect() {
if (boundingRect == null) {
boundingRect = Imgproc.boundingRect(points);
}
return boundingRect;
}
public Rect getBoundingRect() {
if (boundingRect == null) {
boundingRect = Imgproc.boundingRect(points);
}
return boundingRect;
}
public Point getCenterPoint() {
return getMinAreaRect().center;
}
public Point getCenterPoint() {
return getMinAreaRect().center;
}
}

62
chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/CVPipe.java
View File

@@ -3,41 +3,39 @@ package com.chameleonvision.common.vision.pipeline;
import java.util.function.Function;
/**
* Defines a pipe. A pipe is a single step in a pipeline.
* This class is to be extended, never used on its own.
*
* @param <I> Input type for the pipe
* @param <O> Output type for the pipe
* @param <P> Parameters type for the pipe
*/
* Defines a pipe. A pipe is a single step in a pipeline. This class is to be extended, never used
* on its own.
*
* @param <I> Input type for the pipe
* @param <O> Output type for the pipe
* @param <P> Parameters type for the pipe
*/
public abstract class CVPipe<I, O, P> implements Function<I, PipeResult<O>> {
protected PipeResult<O> result = new PipeResult<>();
protected P params;
protected PipeResult<O> result = new PipeResult<>();
protected P params;
public void setParams(P params) {
this.params = params;
}
public void setParams(P params) {
this.params = params;
}
/**
* Runs the process for the pipe.
*
* @param in Input for pipe processing
* @return Result of processing
*/
protected abstract O process(I in);
/**
* Runs the process for the pipe.
*
* @param in Input for pipe processing
* @return Result of processing
*/
protected abstract O process(I in);
/**
*
*
* @param in Input for pipe processing
* @return Result of processing
*/
@Override
public PipeResult<O> apply(I in) {
long pipeStartNanos = System.nanoTime();
result.result = process(in);
result.nanosElapsed = System.nanoTime() - pipeStartNanos;
return result;
}
/**
* @param in Input for pipe processing
* @return Result of processing
*/
@Override
public PipeResult<O> apply(I in) {
long pipeStartNanos = System.nanoTime();
result.result = process(in);
result.nanosElapsed = System.nanoTime() - pipeStartNanos;
return result;
}
}

38
chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/DummyPipeline.java
View File

@@ -3,33 +3,29 @@ package com.chameleonvision.common.vision.pipeline;
import com.chameleonvision.common.vision.pipeline.pipe.ResizeImagePipe;
import com.chameleonvision.common.vision.pipeline.pipe.RotateImagePipe;
import edu.wpi.cscore.CameraServerCvJNI;
import java.io.IOException;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import java.io.IOException;
/**
* This class exists for the sole purpose of showing how pipes would interact in a pipeline
*/
/** This class exists for the sole purpose of showing how pipes would interact in a pipeline */
public class DummyPipeline {
private static ResizeImagePipe resizePipe = new ResizeImagePipe();
private static RotateImagePipe rotatePipe = new RotateImagePipe();
private static ResizeImagePipe resizePipe = new ResizeImagePipe();
private static RotateImagePipe rotatePipe = new RotateImagePipe();
public static void main(String[] args) {
try {
CameraServerCvJNI.forceLoad();
} catch (UnsatisfiedLinkError | IOException e) {
throw new RuntimeException("Failed to load JNI Libraries!");
}
public static void main(String[] args) {
try {
CameraServerCvJNI.forceLoad();
} catch (UnsatisfiedLinkError | IOException e) {
throw new RuntimeException("Failed to load JNI Libraries!");
}
// obviously not a useful test, purely for example.
Mat fakeCameraMat = new Mat(640, 480, CvType.CV_8UC3);
// obviously not a useful test, purely for example.
Mat fakeCameraMat = new Mat(640, 480, CvType.CV_8UC3);
PipeResult<Mat> resizeResult = resizePipe.apply(fakeCameraMat);
PipeResult<Mat> rotateResult = rotatePipe.apply(resizeResult.result);
long fullTime = resizeResult.nanosElapsed + rotateResult.nanosElapsed;
System.out.println(fullTime / 1.0e+6 + "ms elapsed");
}
PipeResult<Mat> resizeResult = resizePipe.apply(fakeCameraMat);
PipeResult<Mat> rotateResult = rotatePipe.apply(resizeResult.result);
long fullTime = resizeResult.nanosElapsed + rotateResult.nanosElapsed;
System.out.println(fullTime / 1.0e+6 + "ms elapsed");
}
}

4
chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/PipeResult.java
View File

@@ -1,6 +1,6 @@
package com.chameleonvision.common.vision.pipeline;
public class PipeResult<O> {
O result;
long nanosElapsed;
O result;
long nanosElapsed;
}

71
chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/ResizeImagePipe.java
View File

@@ -5,49 +5,48 @@ import org.opencv.core.Mat;
import org.opencv.core.Size;
import org.opencv.imgproc.Imgproc;
/**
* Pipe that resizes an image to a given resolution
*/
/** Pipe that resizes an image to a given resolution */
public class ResizeImagePipe extends CVPipe<Mat, Mat, ResizeImagePipe.ResizeImageParams> {
public ResizeImagePipe() {
setParams(ResizeImageParams.DEFAULT);
}
public ResizeImagePipe() {
setParams(ResizeImageParams.DEFAULT);
}
public ResizeImagePipe(ResizeImageParams params) {
setParams(params);
}
public ResizeImagePipe(ResizeImageParams params) {
setParams(params);
}
/**
* Process this pipe
* @param in {@link Mat} to be resized
* @return Resized {@link Mat}
*/
@Override
protected Mat process(Mat in) {
Imgproc.resize(in, in, params.getSize());
return in;
}
/**
* Process this pipe
*
* @param in {@link Mat} to be resized
* @return Resized {@link Mat}
*/
@Override
protected Mat process(Mat in) {
Imgproc.resize(in, in, params.getSize());
return in;
}
public static class ResizeImageParams {
public static ResizeImageParams DEFAULT = new ResizeImageParams(320, 240);
public static class ResizeImageParams {
public static ResizeImageParams DEFAULT = new ResizeImageParams(320, 240);
private Size size;
public int width;
public int height;
private Size size;
public int width;
public int height;
public ResizeImageParams() {
this(DEFAULT.width, DEFAULT.height);
}
public ResizeImageParams() {
this(DEFAULT.width, DEFAULT.height);
}
public ResizeImageParams(int width, int height) {
this.width = width;
this.height = height;
size = new Size(new double[]{width, height});
}
public ResizeImageParams(int width, int height) {
this.width = width;
this.height = height;
size = new Size(new double[] {width, height});
}
public Size getSize() {
return size;
}
}
public Size getSize() {
return size;
}
}
}

83
chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/RotateImagePipe.java
View File

@@ -4,58 +4,57 @@ import com.chameleonvision.common.vision.pipeline.CVPipe;
import org.opencv.core.Core;
import org.opencv.core.Mat;
/**
* Pipe that rotates an image to a given orientation
*/
/** Pipe that rotates an image to a given orientation */
public class RotateImagePipe extends CVPipe<Mat, Mat, RotateImagePipe.RotateImageParams> {
public RotateImagePipe() {
setParams(RotateImageParams.DEFAULT);
}
public RotateImagePipe() {
setParams(RotateImageParams.DEFAULT);
}
public RotateImagePipe(RotateImageParams params) {
setParams(params);
}
public RotateImagePipe(RotateImageParams params) {
setParams(params);
}
/**
* Process this pipe
* @param in {@link Mat} to be rotated
* @return Rotated {@link Mat}
*/
@Override
protected Mat process(Mat in) {
Core.rotate(in, in, params.rotation.value);
return in;
}
/**
* Process this pipe
*
* @param in {@link Mat} to be rotated
* @return Rotated {@link Mat}
*/
@Override
protected Mat process(Mat in) {
Core.rotate(in, in, params.rotation.value);
return in;
}
public static class RotateImageParams {
public static RotateImageParams DEFAULT = new RotateImageParams(ImageRotation.DEG_0);
public static class RotateImageParams {
public static RotateImageParams DEFAULT = new RotateImageParams(ImageRotation.DEG_0);
public ImageRotation rotation;
public ImageRotation rotation;
public RotateImageParams() {
rotation = DEFAULT.rotation;
}
public RotateImageParams() {
rotation = DEFAULT.rotation;
}
public RotateImageParams(ImageRotation rotation) {
this.rotation = rotation;
}
public RotateImageParams(ImageRotation rotation) {
this.rotation = rotation;
}
public enum ImageRotation {
DEG_0(-1),
DEG_90(0),
DEG_180(1),
DEG_270(2);
public enum ImageRotation {
DEG_0(-1),
DEG_90(0),
DEG_180(1),
DEG_270(2);
public final int value;
public final int value;
ImageRotation(int value) {
this.value = value;
}
ImageRotation(int value) {
this.value = value;
}
public boolean isRotated() {
return this.value==DEG_90.value || this.value==DEG_270.value;
}
}
}
public boolean isRotated() {
return this.value == DEG_90.value || this.value == DEG_270.value;
}
}
}
}

3
chameleon-server/src/main/java/com/chameleonvision/common/vision/target/KnownTarget.java
View File

@@ -1,4 +1,3 @@
package com.chameleonvision.common.vision.target;
public class KnownTarget {
}
public class KnownTarget {}

218
chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TrackedTarget.java
View File

@@ -1,116 +1,112 @@
package com.chameleonvision.common.vision.target;
import com.chameleonvision.common.util.numbers.DoubleCouple;
import com.chameleonvision.common.vision.opencv.Contour;
import org.apache.commons.math3.util.FastMath;
import org.opencv.core.Point;
import org.opencv.core.RotatedRect;
import java.util.List;
// TODO: banks fix
public class TrackedTarget {
// final Contour externalContour;
//
// private Point targetOffsetPoint = null;
// private Point robotOffsetPoint = null;
//
// // Single Grouped
// public TrackedTarget(Contour inputContour) {
// this.externalContour = inputContour;
//
// }
//
// public TrackedTarget(List<Contour> subContours) {
//
// }
//
//
// private Point calculateOffsetPoint(boolean isLandscape, TargetOffsetPointRegion offsetRegion) {
// Point[] vertices = new Point[4];
//
// RotatedRect minRect = externalContour.getMinAreaRect();
// minRect.points(vertices);
//
// Point bl = getMiddle(vertices[0], vertices[1]);
// Point tl = getMiddle(vertices[1], vertices[2]);
// Point tr = getMiddle(vertices[2], vertices[3]);
// Point br = getMiddle(vertices[3], vertices[0]);
// boolean orientation;
// if (isLandscape) {
// orientation = minRect.size.width > minRect.size.height;
// } else {
// orientation = minRect.size.width < minRect.size.height;
// }
//
// Point result = minRect.center;
// switch (offsetRegion) {
// case Top: {
// result = orientation ? tl : tr;
// break;
// }
// case Bottom: {
// result = orientation ? br : bl;
// break;
// }
// case Left: {
// result = orientation ? bl : tl;
// break;
// }
// case Right: {
// result = orientation ? tr : br;
// break;
// }
// }
// return result;
// }
//
// public Point getTargetOffsetPoint(boolean isLandscape, TargetOffsetPointRegion offsetRegion) {
// if (targetOffsetPoint == null) {
// targetOffsetPoint = calculateOffsetPoint(isLandscape, offsetRegion);
// }
// return targetOffsetPoint;
// }
//
// private Point calculateRobotOffsetPoint(DoubleCouple offsetPoint, DoubleCouple offsetEquationValues, RobotOffsetPointMode offsetMode) {
// switch (offsetMode) {
// case Single:
// if (offsetPoint.isEmpty()) {
// offsetPoint.set(camProps.centerX, camProps.centerY);
// }
//
// t.calibratedX = offsetPoint.getFirst();
// t.calibratedY = offsetPoint.getSecond();
// break;
// case None:
// t.calibratedX = camProps.centerX;
// t.calibratedY = camProps.centerY;
// break;
// case Dual:
// t.calibratedX = (t.point.x - this.calibrationB) / this.calibrationM;
// t.calibratedY = (t.point.y * this.calibrationM) + this.calibrationB;
// break;
// }
// }
//
// private double calculatePitch(double pixelY, double centerY, double verticalFocalLength) {
// double pitch = FastMath.toDegrees(FastMath.atan((pixelY - centerY) / verticalFocalLength));
// return (pitch * -1);
// }
//
// private double calculateYaw(double pixelX, double centerX, double horizontalFocalLength) {
// return FastMath.toDegrees(FastMath.atan((pixelX - centerX) / horizontalFocalLength));
// }
//
// private Point getMiddle(Point p1, Point p2) {
// return new Point(((p1.x + p2.x) / 2), ((p1.y + p2.y) / 2));
// }
//
// public enum TargetOffsetPointRegion {
// Center, Top, Bottom, Left, Right
// }
//
// public enum RobotOffsetPointMode {
// None, Single, Dual
// }
// final Contour externalContour;
//
// private Point targetOffsetPoint = null;
// private Point robotOffsetPoint = null;
//
// // Single Grouped
// public TrackedTarget(Contour inputContour) {
// this.externalContour = inputContour;
//
// }
//
// public TrackedTarget(List<Contour> subContours) {
//
// }
//
//
// private Point calculateOffsetPoint(boolean isLandscape, TargetOffsetPointRegion
// offsetRegion) {
// Point[] vertices = new Point[4];
//
// RotatedRect minRect = externalContour.getMinAreaRect();
// minRect.points(vertices);
//
// Point bl = getMiddle(vertices[0], vertices[1]);
// Point tl = getMiddle(vertices[1], vertices[2]);
// Point tr = getMiddle(vertices[2], vertices[3]);
// Point br = getMiddle(vertices[3], vertices[0]);
// boolean orientation;
// if (isLandscape) {
// orientation = minRect.size.width > minRect.size.height;
// } else {
// orientation = minRect.size.width < minRect.size.height;
// }
//
// Point result = minRect.center;
// switch (offsetRegion) {
// case Top: {
// result = orientation ? tl : tr;
// break;
// }
// case Bottom: {
// result = orientation ? br : bl;
// break;
// }
// case Left: {
// result = orientation ? bl : tl;
// break;
// }
// case Right: {
// result = orientation ? tr : br;
// break;
// }
// }
// return result;
// }
//
// public Point getTargetOffsetPoint(boolean isLandscape, TargetOffsetPointRegion offsetRegion)
// {
// if (targetOffsetPoint == null) {
// targetOffsetPoint = calculateOffsetPoint(isLandscape, offsetRegion);
// }
// return targetOffsetPoint;
// }
//
// private Point calculateRobotOffsetPoint(DoubleCouple offsetPoint, DoubleCouple
// offsetEquationValues, RobotOffsetPointMode offsetMode) {
// switch (offsetMode) {
// case Single:
// if (offsetPoint.isEmpty()) {
// offsetPoint.set(camProps.centerX, camProps.centerY);
// }
//
// t.calibratedX = offsetPoint.getFirst();
// t.calibratedY = offsetPoint.getSecond();
// break;
// case None:
// t.calibratedX = camProps.centerX;
// t.calibratedY = camProps.centerY;
// break;
// case Dual:
// t.calibratedX = (t.point.x - this.calibrationB) / this.calibrationM;
// t.calibratedY = (t.point.y * this.calibrationM) + this.calibrationB;
// break;
// }
// }
//
// private double calculatePitch(double pixelY, double centerY, double verticalFocalLength) {
// double pitch = FastMath.toDegrees(FastMath.atan((pixelY - centerY) /
// verticalFocalLength));
// return (pitch * -1);
// }
//
// private double calculateYaw(double pixelX, double centerX, double horizontalFocalLength) {
// return FastMath.toDegrees(FastMath.atan((pixelX - centerX) / horizontalFocalLength));
// }
//
// private Point getMiddle(Point p1, Point p2) {
// return new Point(((p1.x + p2.x) / 2), ((p1.y + p2.y) / 2));
// }
//
// public enum TargetOffsetPointRegion {
// Center, Top, Bottom, Left, Right
// }
//
// public enum RobotOffsetPointMode {
// None, Single, Dual
// }
}

3
chameleon-server/src/main/java/com/chameleonvision/server/Main.java
View File

@@ -1,4 +1,3 @@
package com.chameleonvision.server;
public class Main {
}
public class Main {}

435
chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java
View File

@@ -7,9 +7,6 @@
package edu.wpi.first.wpilibj.geometry;
import java.io.IOException;
import java.util.Objects;
import com.fasterxml.jackson.core.JsonGenerator;
import com.fasterxml.jackson.core.JsonParser;
import com.fasterxml.jackson.core.JsonProcessingException;
@@ -20,252 +17,250 @@ import com.fasterxml.jackson.databind.annotation.JsonDeserialize;
import com.fasterxml.jackson.databind.annotation.JsonSerialize;
import com.fasterxml.jackson.databind.deser.std.StdDeserializer;
import com.fasterxml.jackson.databind.ser.std.StdSerializer;
import java.io.IOException;
import java.util.Objects;
/**
* Represents a 2d pose containing translational and rotational elements.
*/
/** Represents a 2d pose containing translational and rotational elements. */
@JsonSerialize(using = Pose2d.PoseSerializer.class)
@JsonDeserialize(using = Pose2d.PoseDeserializer.class)
public class Pose2d {
private final Translation2d m_translation;
private final Rotation2d m_rotation;
private final Translation2d m_translation;
private final Rotation2d m_rotation;
/**
* Constructs a pose at the origin facing toward the positive X axis.
* (Translation2d{0, 0} and Rotation{0})
*/
public Pose2d() {
m_translation = new Translation2d();
m_rotation = new Rotation2d();
}
/**
* Constructs a pose at the origin facing toward the positive X axis. (Translation2d{0, 0} and
* Rotation{0})
*/
public Pose2d() {
m_translation = new Translation2d();
m_rotation = new Rotation2d();
}
/**
* Constructs a pose with the specified translation and rotation.
*
* @param translation The translational component of the pose.
* @param rotation The rotational component of the pose.
*/
public Pose2d(Translation2d translation, Rotation2d rotation) {
m_translation = translation;
m_rotation = rotation;
}
/**
* Constructs a pose with the specified translation and rotation.
*
* @param translation The translational component of the pose.
* @param rotation The rotational component of the pose.
*/
public Pose2d(Translation2d translation, Rotation2d rotation) {
m_translation = translation;
m_rotation = rotation;
}
/**
* Convenience constructors that takes in x and y values directly instead of
* having to construct a Translation2d.
*
* @param x The x component of the translational component of the pose.
* @param y The y component of the translational component of the pose.
* @param rotation The rotational component of the pose.
*/
@SuppressWarnings("ParameterName")
public Pose2d(double x, double y, Rotation2d rotation) {
m_translation = new Translation2d(x, y);
m_rotation = rotation;
}
/**
* Convenience constructors that takes in x and y values directly instead of having to construct a
* Translation2d.
*
* @param x The x component of the translational component of the pose.
* @param y The y component of the translational component of the pose.
* @param rotation The rotational component of the pose.
*/
@SuppressWarnings("ParameterName")
public Pose2d(double x, double y, Rotation2d rotation) {
m_translation = new Translation2d(x, y);
m_rotation = rotation;
}
/**
* Transforms the pose by the given transformation and returns the new
* transformed pose.
*
* <p>The matrix multiplication is as follows
* [x_new] [cos, -sin, 0][transform.x]
* [y_new] += [sin, cos, 0][transform.y]
* [t_new] [0, 0, 1][transform.t]
*
* @param other The transform to transform the pose by.
* @return The transformed pose.
*/
public Pose2d plus(Transform2d other) {
return transformBy(other);
}
/**
* Transforms the pose by the given transformation and returns the new transformed pose.
*
* <p>The matrix multiplication is as follows [x_new] [cos, -sin, 0][transform.x] [y_new] += [sin,
* cos, 0][transform.y] [t_new] [0, 0, 1][transform.t]
*
* @param other The transform to transform the pose by.
* @return The transformed pose.
*/
public Pose2d plus(Transform2d other) {
return transformBy(other);
}
/**
* Returns the Transform2d that maps the one pose to another.
*
* @param other The initial pose of the transformation.
* @return The transform that maps the other pose to the current pose.
*/
public Transform2d minus(Pose2d other) {
final var pose = this.relativeTo(other);
return new Transform2d(pose.getTranslation(), pose.getRotation());
}
/**
* Returns the Transform2d that maps the one pose to another.
*
* @param other The initial pose of the transformation.
* @return The transform that maps the other pose to the current pose.
*/
public Transform2d minus(Pose2d other) {
final var pose = this.relativeTo(other);
return new Transform2d(pose.getTranslation(), pose.getRotation());
}
/**
* Returns the translation component of the transformation.
*
* @return The translational component of the pose.
*/
public Translation2d getTranslation() {
return m_translation;
}
/**
* Returns the translation component of the transformation.
*
* @return The translational component of the pose.
*/
public Translation2d getTranslation() {
return m_translation;
}
/**
* Returns the rotational component of the transformation.
*
* @return The rotational component of the pose.
*/
public Rotation2d getRotation() {
return m_rotation;
}
/**
* Returns the rotational component of the transformation.
*
* @return The rotational component of the pose.
*/
public Rotation2d getRotation() {
return m_rotation;
}
/**
* Transforms the pose by the given transformation and returns the new pose.
* See + operator for the matrix multiplication performed.
*
* @param other The transform to transform the pose by.
* @return The transformed pose.
*/
public Pose2d transformBy(Transform2d other) {
return new Pose2d(m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
m_rotation.plus(other.getRotation()));
}
/**
* Transforms the pose by the given transformation and returns the new pose. See + operator for
* the matrix multiplication performed.
*
* @param other The transform to transform the pose by.
* @return The transformed pose.
*/
public Pose2d transformBy(Transform2d other) {
return new Pose2d(
m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
m_rotation.plus(other.getRotation()));
}
/**
* Returns the other pose relative to the current pose.
*
* <p>This function can often be used for trajectory tracking or pose
* stabilization algorithms to get the error between the reference and the
* current pose.
*
* @param other The pose that is the origin of the new coordinate frame that
* the current pose will be converted into.
* @return The current pose relative to the new origin pose.
*/
public Pose2d relativeTo(Pose2d other) {
var transform = new Transform2d(other, this);
return new Pose2d(transform.getTranslation(), transform.getRotation());
}
/**
* Returns the other pose relative to the current pose.
*
* <p>This function can often be used for trajectory tracking or pose stabilization algorithms to
* get the error between the reference and the current pose.
*
* @param other The pose that is the origin of the new coordinate frame that the current pose will
* be converted into.
* @return The current pose relative to the new origin pose.
*/
public Pose2d relativeTo(Pose2d other) {
var transform = new Transform2d(other, this);
return new Pose2d(transform.getTranslation(), transform.getRotation());
}
/**
* Obtain a new Pose2d from a (constant curvature) velocity.
*
* <p>See <a href="https://file.tavsys.net/control/state-space-guide.pdf">
* Controls Engineering in the FIRST Robotics Competition</a>
* section on nonlinear pose estimation for derivation.
*
* <p>The twist is a change in pose in the robot's coordinate frame since the
* previous pose update. When the user runs exp() on the previous known
* field-relative pose with the argument being the twist, the user will
* receive the new field-relative pose.
*
* <p>"Exp" represents the pose exponential, which is solving a differential
* equation moving the pose forward in time.
*
* @param twist The change in pose in the robot's coordinate frame since the
* previous pose update. For example, if a non-holonomic robot moves forward
* 0.01 meters and changes angle by 0.5 degrees since the previous pose update,
* the twist would be Twist2d{0.01, 0.0, toRadians(0.5)}
* @return The new pose of the robot.
*/
@SuppressWarnings("LocalVariableName")
public Pose2d exp(Twist2d twist) {
double dx = twist.dx;
double dy = twist.dy;
double dtheta = twist.dtheta;
/**
* Obtain a new Pose2d from a (constant curvature) velocity.
*
* <p>See <a href="https://file.tavsys.net/control/state-space-guide.pdf">Controls Engineering in
* the FIRST Robotics Competition</a> section on nonlinear pose estimation for derivation.
*
* <p>The twist is a change in pose in the robot's coordinate frame since the previous pose
* update. When the user runs exp() on the previous known field-relative pose with the argument
* being the twist, the user will receive the new field-relative pose.
*
* <p>"Exp" represents the pose exponential, which is solving a differential equation moving the
* pose forward in time.
*
* @param twist The change in pose in the robot's coordinate frame since the previous pose update.
* For example, if a non-holonomic robot moves forward 0.01 meters and changes angle by 0.5
* degrees since the previous pose update, the twist would be Twist2d{0.01, 0.0,
* toRadians(0.5)}
* @return The new pose of the robot.
*/
@SuppressWarnings("LocalVariableName")
public Pose2d exp(Twist2d twist) {
double dx = twist.dx;
double dy = twist.dy;
double dtheta = twist.dtheta;
double sinTheta = Math.sin(dtheta);
double cosTheta = Math.cos(dtheta);
double sinTheta = Math.sin(dtheta);
double cosTheta = Math.cos(dtheta);
double s;
double c;
if (Math.abs(dtheta) < 1E-9) {
s = 1.0 - 1.0 / 6.0 * dtheta * dtheta;
c = 0.5 * dtheta;
} else {
s = sinTheta / dtheta;
c = (1 - cosTheta) / dtheta;
}
var transform = new Transform2d(new Translation2d(dx * s - dy * c, dx * c + dy * s),
new Rotation2d(cosTheta, sinTheta));
double s;
double c;
if (Math.abs(dtheta) < 1E-9) {
s = 1.0 - 1.0 / 6.0 * dtheta * dtheta;
c = 0.5 * dtheta;
} else {
s = sinTheta / dtheta;
c = (1 - cosTheta) / dtheta;
}
var transform =
new Transform2d(
new Translation2d(dx * s - dy * c, dx * c + dy * s),
new Rotation2d(cosTheta, sinTheta));
return this.plus(transform);
}
return this.plus(transform);
}
/**
* Returns a Twist2d that maps this pose to the end pose. If c is the output
* of a.Log(b), then a.Exp(c) would yield b.
*
* @param end The end pose for the transformation.
* @return The twist that maps this to end.
*/
public Twist2d log(Pose2d end) {
final var transform = end.relativeTo(this);
final var dtheta = transform.getRotation().getRadians();
final var halfDtheta = dtheta / 2.0;
/**
* Returns a Twist2d that maps this pose to the end pose. If c is the output of a.Log(b), then
* a.Exp(c) would yield b.
*
* @param end The end pose for the transformation.
* @return The twist that maps this to end.
*/
public Twist2d log(Pose2d end) {
final var transform = end.relativeTo(this);
final var dtheta = transform.getRotation().getRadians();
final var halfDtheta = dtheta / 2.0;
final var cosMinusOne = transform.getRotation().getCos() - 1;
final var cosMinusOne = transform.getRotation().getCos() - 1;
double halfThetaByTanOfHalfDtheta;
if (Math.abs(cosMinusOne) < 1E-9) {
halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
} else {
halfThetaByTanOfHalfDtheta = -(halfDtheta * transform.getRotation().getSin()) / cosMinusOne;
}
double halfThetaByTanOfHalfDtheta;
if (Math.abs(cosMinusOne) < 1E-9) {
halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
} else {
halfThetaByTanOfHalfDtheta = -(halfDtheta * transform.getRotation().getSin()) / cosMinusOne;
}
Translation2d translationPart = transform.getTranslation().rotateBy(
new Rotation2d(halfThetaByTanOfHalfDtheta, -halfDtheta)
).times(Math.hypot(halfThetaByTanOfHalfDtheta, halfDtheta));
Translation2d translationPart =
transform
.getTranslation()
.rotateBy(new Rotation2d(halfThetaByTanOfHalfDtheta, -halfDtheta))
.times(Math.hypot(halfThetaByTanOfHalfDtheta, halfDtheta));
return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta);
}
return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta);
}
@Override
public String toString() {
return String.format("Pose2d(%s, %s)", m_translation, m_rotation);
}
@Override
public String toString() {
return String.format("Pose2d(%s, %s)", m_translation, m_rotation);
}
/**
* Checks equality between this Pose2d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Pose2d) {
return ((Pose2d) obj).m_translation.equals(m_translation)
&& ((Pose2d) obj).m_rotation.equals(m_rotation);
}
return false;
}
/**
* Checks equality between this Pose2d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Pose2d) {
return ((Pose2d) obj).m_translation.equals(m_translation)
&& ((Pose2d) obj).m_rotation.equals(m_rotation);
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_translation, m_rotation);
}
@Override
public int hashCode() {
return Objects.hash(m_translation, m_rotation);
}
static class PoseSerializer extends StdSerializer<Pose2d> {
PoseSerializer() {
super(Pose2d.class);
}
static class PoseSerializer extends StdSerializer<Pose2d> {
PoseSerializer() {
super(Pose2d.class);
}
@Override
public void serialize(
Pose2d value, JsonGenerator jgen, SerializerProvider provider)
throws IOException, JsonProcessingException {
@Override
public void serialize(Pose2d value, JsonGenerator jgen, SerializerProvider provider)
throws IOException, JsonProcessingException {
jgen.writeStartObject();
jgen.writeObjectField("translation", value.m_translation);
jgen.writeObjectField("rotation", value.m_rotation);
jgen.writeEndObject();
}
}
jgen.writeStartObject();
jgen.writeObjectField("translation", value.m_translation);
jgen.writeObjectField("rotation", value.m_rotation);
jgen.writeEndObject();
}
}
static class PoseDeserializer extends StdDeserializer<Pose2d> {
PoseDeserializer() {
super(Pose2d.class);
}
static class PoseDeserializer extends StdDeserializer<Pose2d> {
PoseDeserializer() {
super(Pose2d.class);
}
@Override
public Pose2d deserialize(JsonParser jp, DeserializationContext ctxt)
throws IOException, JsonProcessingException {
JsonNode node = jp.getCodec().readTree(jp);
@Override
public Pose2d deserialize(JsonParser jp, DeserializationContext ctxt)
throws IOException, JsonProcessingException {
JsonNode node = jp.getCodec().readTree(jp);
Translation2d translation =
jp.getCodec().treeToValue(node.get("translation"), Translation2d.class);
Rotation2d rotation = jp.getCodec().treeToValue(node.get("rotation"), Rotation2d.class);
return new Pose2d(translation, rotation);
}
}
Translation2d translation =
jp.getCodec().treeToValue(node.get("translation"), Translation2d.class);
Rotation2d rotation = jp.getCodec().treeToValue(node.get("rotation"), Rotation2d.class);
return new Pose2d(translation, rotation);
}
}
}

393
chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java
View File

@@ -7,9 +7,6 @@
package edu.wpi.first.wpilibj.geometry;
import java.io.IOException;
import java.util.Objects;
import com.fasterxml.jackson.core.JsonGenerator;
import com.fasterxml.jackson.core.JsonParser;
import com.fasterxml.jackson.core.JsonProcessingException;
@@ -20,232 +17,218 @@ import com.fasterxml.jackson.databind.annotation.JsonDeserialize;
import com.fasterxml.jackson.databind.annotation.JsonSerialize;
import com.fasterxml.jackson.databind.deser.std.StdDeserializer;
import com.fasterxml.jackson.databind.ser.std.StdSerializer;
import java.io.IOException;
import java.util.Objects;
/**
* A rotation in a 2d coordinate frame represented a point on the unit circle
* (cosine and sine).
*/
/** A rotation in a 2d coordinate frame represented a point on the unit circle (cosine and sine). */
@JsonSerialize(using = Rotation2d.RotationSerializer.class)
@JsonDeserialize(using = Rotation2d.RotationDeserializer.class)
public class Rotation2d {
private final double m_value;
private final double m_cos;
private final double m_sin;
private final double m_value;
private final double m_cos;
private final double m_sin;
/**
* Constructs a Rotation2d with a default angle of 0 degrees.
*/
public Rotation2d() {
m_value = 0.0;
m_cos = 1.0;
m_sin = 0.0;
}
/** Constructs a Rotation2d with a default angle of 0 degrees. */
public Rotation2d() {
m_value = 0.0;
m_cos = 1.0;
m_sin = 0.0;
}
/**
* Constructs a Rotation2d with the given radian value.
* The x and y don't have to be normalized.
*
* @param value The value of the angle in radians.
*/
public Rotation2d(double value) {
m_value = value;
m_cos = Math.cos(value);
m_sin = Math.sin(value);
}
/**
* Constructs a Rotation2d with the given radian value. The x and y don't have to be normalized.
*
* @param value The value of the angle in radians.
*/
public Rotation2d(double value) {
m_value = value;
m_cos = Math.cos(value);
m_sin = Math.sin(value);
}
/**
* Constructs a Rotation2d with the given x and y (cosine and sine)
* components.
*
* @param x The x component or cosine of the rotation.
* @param y The y component or sine of the rotation.
*/
@SuppressWarnings("ParameterName")
public Rotation2d(double x, double y) {
double magnitude = Math.hypot(x, y);
if (magnitude > 1e-6) {
m_sin = y / magnitude;
m_cos = x / magnitude;
} else {
m_sin = 0.0;
m_cos = 1.0;
}
m_value = Math.atan2(m_sin, m_cos);
}
/**
* Constructs a Rotation2d with the given x and y (cosine and sine) components.
*
* @param x The x component or cosine of the rotation.
* @param y The y component or sine of the rotation.
*/
@SuppressWarnings("ParameterName")
public Rotation2d(double x, double y) {
double magnitude = Math.hypot(x, y);
if (magnitude > 1e-6) {
m_sin = y / magnitude;
m_cos = x / magnitude;
} else {
m_sin = 0.0;
m_cos = 1.0;
}
m_value = Math.atan2(m_sin, m_cos);
}
/**
* Constructs and returns a Rotation2d with the given degree value.
*
* @param degrees The value of the angle in degrees.
* @return The rotation object with the desired angle value.
*/
public static Rotation2d fromDegrees(double degrees) {
return new Rotation2d(Math.toRadians(degrees));
}
/**
* Constructs and returns a Rotation2d with the given degree value.
*
* @param degrees The value of the angle in degrees.
* @return The rotation object with the desired angle value.
*/
public static Rotation2d fromDegrees(double degrees) {
return new Rotation2d(Math.toRadians(degrees));
}
/**
* Adds two rotations together, with the result being bounded between -pi and
* pi.
*
* <p>For example, Rotation2d.fromDegrees(30) + Rotation2d.fromDegrees(60) =
* Rotation2d{-pi/2}
*
* @param other The rotation to add.
* @return The sum of the two rotations.
*/
public Rotation2d plus(Rotation2d other) {
return rotateBy(other);
}
/**
* Adds two rotations together, with the result being bounded between -pi and pi.
*
* <p>For example, Rotation2d.fromDegrees(30) + Rotation2d.fromDegrees(60) = Rotation2d{-pi/2}
*
* @param other The rotation to add.
* @return The sum of the two rotations.
*/
public Rotation2d plus(Rotation2d other) {
return rotateBy(other);
}
/**
* Subtracts the new rotation from the current rotation and returns the new
* rotation.
*
* <p>For example, Rotation2d.fromDegrees(10) - Rotation2d.fromDegrees(100) =
* Rotation2d{-pi/2}
*
* @param other The rotation to subtract.
* @return The difference between the two rotations.
*/
public Rotation2d minus(Rotation2d other) {
return rotateBy(other.unaryMinus());
}
/**
* Subtracts the new rotation from the current rotation and returns the new rotation.
*
* <p>For example, Rotation2d.fromDegrees(10) - Rotation2d.fromDegrees(100) = Rotation2d{-pi/2}
*
* @param other The rotation to subtract.
* @return The difference between the two rotations.
*/
public Rotation2d minus(Rotation2d other) {
return rotateBy(other.unaryMinus());
}
/**
* Takes the inverse of the current rotation. This is simply the negative of
* the current angular value.
*
* @return The inverse of the current rotation.
*/
public Rotation2d unaryMinus() {
return new Rotation2d(-m_value);
}
/**
* Takes the inverse of the current rotation. This is simply the negative of the current angular
* value.
*
* @return The inverse of the current rotation.
*/
public Rotation2d unaryMinus() {
return new Rotation2d(-m_value);
}
/**
* Multiplies the current rotation by a scalar.
*
* @param scalar The scalar.
* @return The new scaled Rotation2d.
*/
public Rotation2d times(double scalar) {
return new Rotation2d(m_value * scalar);
}
/**
* Multiplies the current rotation by a scalar.
*
* @param scalar The scalar.
* @return The new scaled Rotation2d.
*/
public Rotation2d times(double scalar) {
return new Rotation2d(m_value * scalar);
}
/**
* Adds the new rotation to the current rotation using a rotation matrix.
*
* <p>The matrix multiplication is as follows:
* [cos_new] [other.cos, -other.sin][cos]
* [sin_new] = [other.sin, other.cos][sin]
* value_new = atan2(cos_new, sin_new)
*
* @param other The rotation to rotate by.
* @return The new rotated Rotation2d.
*/
public Rotation2d rotateBy(Rotation2d other) {
return new Rotation2d(
m_cos * other.m_cos - m_sin * other.m_sin,
m_cos * other.m_sin + m_sin * other.m_cos
);
}
/**
* Adds the new rotation to the current rotation using a rotation matrix.
*
* <p>The matrix multiplication is as follows: [cos_new] [other.cos, -other.sin][cos] [sin_new] =
* [other.sin, other.cos][sin] value_new = atan2(cos_new, sin_new)
*
* @param other The rotation to rotate by.
* @return The new rotated Rotation2d.
*/
public Rotation2d rotateBy(Rotation2d other) {
return new Rotation2d(
m_cos * other.m_cos - m_sin * other.m_sin, m_cos * other.m_sin + m_sin * other.m_cos);
}
/*
* Returns the radian value of the rotation.
*
* @return The radian value of the rotation.
*/
public double getRadians() {
return m_value;
}
/*
* Returns the radian value of the rotation.
*
* @return The radian value of the rotation.
*/
public double getRadians() {
return m_value;
}
/**
* Returns the degree value of the rotation.
*
* @return The degree value of the rotation.
*/
public double getDegrees() {
return Math.toDegrees(m_value);
}
/**
* Returns the degree value of the rotation.
*
* @return The degree value of the rotation.
*/
public double getDegrees() {
return Math.toDegrees(m_value);
}
/**
* Returns the cosine of the rotation.
*
* @return The cosine of the rotation.
*/
public double getCos() {
return m_cos;
}
/**
* Returns the cosine of the rotation.
*
* @return The cosine of the rotation.
*/
public double getCos() {
return m_cos;
}
/**
* Returns the sine of the rotation.
*
* @return The sine of the rotation.
*/
public double getSin() {
return m_sin;
}
/**
* Returns the sine of the rotation.
*
* @return The sine of the rotation.
*/
public double getSin() {
return m_sin;
}
/**
* Returns the tangent of the rotation.
*
* @return The tangent of the rotation.
*/
public double getTan() {
return m_sin / m_cos;
}
/**
* Returns the tangent of the rotation.
*
* @return The tangent of the rotation.
*/
public double getTan() {
return m_sin / m_cos;
}
@Override
public String toString() {
return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", m_value, Math.toDegrees(m_value));
}
@Override
public String toString() {
return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", m_value, Math.toDegrees(m_value));
}
/**
* Checks equality between this Rotation2d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Rotation2d) {
return Math.abs(((Rotation2d) obj).m_value - m_value) < 1E-9;
}
return false;
}
/**
* Checks equality between this Rotation2d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Rotation2d) {
return Math.abs(((Rotation2d) obj).m_value - m_value) < 1E-9;
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_value);
}
@Override
public int hashCode() {
return Objects.hash(m_value);
}
static class RotationSerializer extends StdSerializer<Rotation2d> {
RotationSerializer() {
super(Rotation2d.class);
}
static class RotationSerializer extends StdSerializer<Rotation2d> {
RotationSerializer() {
super(Rotation2d.class);
}
@Override
public void serialize(
Rotation2d value, JsonGenerator jgen, SerializerProvider provider)
throws IOException, JsonProcessingException {
@Override
public void serialize(Rotation2d value, JsonGenerator jgen, SerializerProvider provider)
throws IOException, JsonProcessingException {
jgen.writeStartObject();
jgen.writeNumberField("radians", value.m_value);
jgen.writeEndObject();
}
}
jgen.writeStartObject();
jgen.writeNumberField("radians", value.m_value);
jgen.writeEndObject();
}
}
static class RotationDeserializer extends StdDeserializer<Rotation2d> {
RotationDeserializer() {
super(Rotation2d.class);
}
static class RotationDeserializer extends StdDeserializer<Rotation2d> {
RotationDeserializer() {
super(Rotation2d.class);
}
@Override
public Rotation2d deserialize(JsonParser jp, DeserializationContext ctxt)
throws IOException, JsonProcessingException {
JsonNode node = jp.getCodec().readTree(jp);
double radians = node.get("radians").numberValue().doubleValue();
@Override
public Rotation2d deserialize(JsonParser jp, DeserializationContext ctxt)
throws IOException, JsonProcessingException {
JsonNode node = jp.getCodec().readTree(jp);
double radians = node.get("radians").numberValue().doubleValue();
return new Rotation2d(radians);
}
}
}
return new Rotation2d(radians);
}
}
}

164
chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java
View File

@@ -9,98 +9,96 @@ package edu.wpi.first.wpilibj.geometry;
import java.util.Objects;
/**
* Represents a transformation for a Pose2d.
*/
/** Represents a transformation for a Pose2d. */
public class Transform2d {
private final Translation2d m_translation;
private final Rotation2d m_rotation;
private final Translation2d m_translation;
private final Rotation2d m_rotation;
/**
* Constructs the transform that maps the initial pose to the final pose.
*
* @param initial The initial pose for the transformation.
* @param last The final pose for the transformation.
*/
public Transform2d(Pose2d initial, Pose2d last) {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
m_translation = last.getTranslation().minus(initial.getTranslation())
.rotateBy(initial.getRotation().unaryMinus());
/**
* Constructs the transform that maps the initial pose to the final pose.
*
* @param initial The initial pose for the transformation.
* @param last The final pose for the transformation.
*/
public Transform2d(Pose2d initial, Pose2d last) {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
m_translation =
last.getTranslation()
.minus(initial.getTranslation())
.rotateBy(initial.getRotation().unaryMinus());
m_rotation = last.getRotation().minus(initial.getRotation());
}
m_rotation = last.getRotation().minus(initial.getRotation());
}
/**
* Constructs a transform with the given translation and rotation components.
*
* @param translation Translational component of the transform.
* @param rotation Rotational component of the transform.
*/
public Transform2d(Translation2d translation, Rotation2d rotation) {
m_translation = translation;
m_rotation = rotation;
}
/**
* Constructs a transform with the given translation and rotation components.
*
* @param translation Translational component of the transform.
* @param rotation Rotational component of the transform.
*/
public Transform2d(Translation2d translation, Rotation2d rotation) {
m_translation = translation;
m_rotation = rotation;
}
/**
* Constructs the identity transform -- maps an initial pose to itself.
*/
public Transform2d() {
m_translation = new Translation2d();
m_rotation = new Rotation2d();
}
/** Constructs the identity transform -- maps an initial pose to itself. */
public Transform2d() {
m_translation = new Translation2d();
m_rotation = new Rotation2d();
}
/**
* Scales the transform by the scalar.
*
* @param scalar The scalar.
* @return The scaled Transform2d.
*/
public Transform2d times(double scalar) {
return new Transform2d(m_translation.times(scalar), m_rotation.times(scalar));
}
/**
* Scales the transform by the scalar.
*
* @param scalar The scalar.
* @return The scaled Transform2d.
*/
public Transform2d times(double scalar) {
return new Transform2d(m_translation.times(scalar), m_rotation.times(scalar));
}
/**
* Returns the translation component of the transformation.
*
* @return The translational component of the transform.
*/
public Translation2d getTranslation() {
return m_translation;
}
/**
* Returns the translation component of the transformation.
*
* @return The translational component of the transform.
*/
public Translation2d getTranslation() {
return m_translation;
}
/**
* Returns the rotational component of the transformation.
*
* @return Reference to the rotational component of the transform.
*/
public Rotation2d getRotation() {
return m_rotation;
}
/**
* Returns the rotational component of the transformation.
*
* @return Reference to the rotational component of the transform.
*/
public Rotation2d getRotation() {
return m_rotation;
}
@Override
public String toString() {
return String.format("Transform2d(%s, %s)", m_translation, m_rotation);
}
@Override
public String toString() {
return String.format("Transform2d(%s, %s)", m_translation, m_rotation);
}
/**
* Checks equality between this Transform2d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Transform2d) {
return ((Transform2d) obj).m_translation.equals(m_translation)
&& ((Transform2d) obj).m_rotation.equals(m_rotation);
}
return false;
}
/**
* Checks equality between this Transform2d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Transform2d) {
return ((Transform2d) obj).m_translation.equals(m_translation)
&& ((Transform2d) obj).m_rotation.equals(m_rotation);
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_translation, m_rotation);
}
@Override
public int hashCode() {
return Objects.hash(m_translation, m_rotation);
}
}

375
chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java
View File

@@ -7,9 +7,6 @@
package edu.wpi.first.wpilibj.geometry;
import java.io.IOException;
import java.util.Objects;
import com.fasterxml.jackson.core.JsonGenerator;
import com.fasterxml.jackson.core.JsonParser;
import com.fasterxml.jackson.core.JsonProcessingException;
@@ -20,224 +17,212 @@ import com.fasterxml.jackson.databind.annotation.JsonDeserialize;
import com.fasterxml.jackson.databind.annotation.JsonSerialize;
import com.fasterxml.jackson.databind.deser.std.StdDeserializer;
import com.fasterxml.jackson.databind.ser.std.StdSerializer;
import java.io.IOException;
import java.util.Objects;
/**
* Represents a translation in 2d space.
* This object can be used to represent a point or a vector.
*
* <p>This assumes that you are using conventional mathematical axes.
* When the robot is placed on the origin, facing toward the X direction,
* moving forward increases the X, whereas moving to the left increases the Y.
*/
* Represents a translation in 2d space. This object can be used to represent a point or a vector.
*
* <p>This assumes that you are using conventional mathematical axes. When the robot is placed on
* the origin, facing toward the X direction, moving forward increases the X, whereas moving to the
* left increases the Y.
*/
@JsonSerialize(using = Translation2d.TranslationSerializer.class)
@JsonDeserialize(using = Translation2d.TranslationDeserializer.class)
@SuppressWarnings({"ParameterName", "MemberName"})
public class Translation2d {
private final double m_x;
private final double m_y;
private final double m_x;
private final double m_y;
/**
* Constructs a Translation2d with X and Y components equal to zero.
*/
public Translation2d() {
this(0.0, 0.0);
}
/** Constructs a Translation2d with X and Y components equal to zero. */
public Translation2d() {
this(0.0, 0.0);
}
/**
* Constructs a Translation2d with the X and Y components equal to the
* provided values.
*
* @param x The x component of the translation.
* @param y The y component of the translation.
*/
public Translation2d(double x, double y) {
m_x = x;
m_y = y;
}
/**
* Constructs a Translation2d with the X and Y components equal to the provided values.
*
* @param x The x component of the translation.
* @param y The y component of the translation.
*/
public Translation2d(double x, double y) {
m_x = x;
m_y = y;
}
/**
* Calculates the distance between two translations in 2d space.
*
* <p>This function uses the pythagorean theorem to calculate the distance.
* distance = sqrt((x2 - x1)^2 + (y2 - y1)^2)
*
* @param other The translation to compute the distance to.
* @return The distance between the two translations.
*/
public double getDistance(Translation2d other) {
return Math.hypot(other.m_x - m_x, other.m_y - m_y);
}
/**
* Calculates the distance between two translations in 2d space.
*
* <p>This function uses the pythagorean theorem to calculate the distance. distance = sqrt((x2 -
* x1)^2 + (y2 - y1)^2)
*
* @param other The translation to compute the distance to.
* @return The distance between the two translations.
*/
public double getDistance(Translation2d other) {
return Math.hypot(other.m_x - m_x, other.m_y - m_y);
}
/**
* Returns the X component of the translation.
*
* @return The x component of the translation.
*/
public double getX() {
return m_x;
}
/**
* Returns the X component of the translation.
*
* @return The x component of the translation.
*/
public double getX() {
return m_x;
}
/**
* Returns the Y component of the translation.
*
* @return The y component of the translation.
*/
public double getY() {
return m_y;
}
/**
* Returns the Y component of the translation.
*
* @return The y component of the translation.
*/
public double getY() {
return m_y;
}
/**
* Returns the norm, or distance from the origin to the translation.
*
* @return The norm of the translation.
*/
public double getNorm() {
return Math.hypot(m_x, m_y);
}
/**
* Returns the norm, or distance from the origin to the translation.
*
* @return The norm of the translation.
*/
public double getNorm() {
return Math.hypot(m_x, m_y);
}
/**
* Applies a rotation to the translation in 2d space.
*
* <p>This multiplies the translation vector by a counterclockwise rotation
* matrix of the given angle.
* [x_new] [other.cos, -other.sin][x]
* [y_new] = [other.sin, other.cos][y]
*
* <p>For example, rotating a Translation2d of {2, 0} by 90 degrees will return a
* Translation2d of {0, 2}.
*
* @param other The rotation to rotate the translation by.
* @return The new rotated translation.
*/
public Translation2d rotateBy(Rotation2d other) {
return new Translation2d(
m_x * other.getCos() - m_y * other.getSin(),
m_x * other.getSin() + m_y * other.getCos()
);
}
/**
* Applies a rotation to the translation in 2d space.
*
* <p>This multiplies the translation vector by a counterclockwise rotation matrix of the given
* angle. [x_new] [other.cos, -other.sin][x] [y_new] = [other.sin, other.cos][y]
*
* <p>For example, rotating a Translation2d of {2, 0} by 90 degrees will return a Translation2d of
* {0, 2}.
*
* @param other The rotation to rotate the translation by.
* @return The new rotated translation.
*/
public Translation2d rotateBy(Rotation2d other) {
return new Translation2d(
m_x * other.getCos() - m_y * other.getSin(), m_x * other.getSin() + m_y * other.getCos());
}
/**
* Adds two translations in 2d space and returns the sum. This is similar to
* vector addition.
*
* <p>For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} =
* Translation2d{3.0, 8.0}
*
* @param other The translation to add.
* @return The sum of the translations.
*/
public Translation2d plus(Translation2d other) {
return new Translation2d(m_x + other.m_x, m_y + other.m_y);
}
/**
* Adds two translations in 2d space and returns the sum. This is similar to vector addition.
*
* <p>For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} = Translation2d{3.0, 8.0}
*
* @param other The translation to add.
* @return The sum of the translations.
*/
public Translation2d plus(Translation2d other) {
return new Translation2d(m_x + other.m_x, m_y + other.m_y);
}
/**
* Subtracts the other translation from the other translation and returns the
* difference.
*
* <p>For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} =
* Translation2d{4.0, 2.0}
*
* @param other The translation to subtract.
* @return The difference between the two translations.
*/
public Translation2d minus(Translation2d other) {
return new Translation2d(m_x - other.m_x, m_y - other.m_y);
}
/**
* Subtracts the other translation from the other translation and returns the difference.
*
* <p>For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} = Translation2d{4.0, 2.0}
*
* @param other The translation to subtract.
* @return The difference between the two translations.
*/
public Translation2d minus(Translation2d other) {
return new Translation2d(m_x - other.m_x, m_y - other.m_y);
}
/**
* Returns the inverse of the current translation. This is equivalent to
* rotating by 180 degrees, flipping the point over both axes, or simply
* negating both components of the translation.
*
* @return The inverse of the current translation.
*/
public Translation2d unaryMinus() {
return new Translation2d(-m_x, -m_y);
}
/**
* Returns the inverse of the current translation. This is equivalent to rotating by 180 degrees,
* flipping the point over both axes, or simply negating both components of the translation.
*
* @return The inverse of the current translation.
*/
public Translation2d unaryMinus() {
return new Translation2d(-m_x, -m_y);
}
/**
* Multiplies the translation by a scalar and returns the new translation.
*
* <p>For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}
*
* @param scalar The scalar to multiply by.
* @return The scaled translation.
*/
public Translation2d times(double scalar) {
return new Translation2d(m_x * scalar, m_y * scalar);
}
/**
* Multiplies the translation by a scalar and returns the new translation.
*
* <p>For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}
*
* @param scalar The scalar to multiply by.
* @return The scaled translation.
*/
public Translation2d times(double scalar) {
return new Translation2d(m_x * scalar, m_y * scalar);
}
/**
* Divides the translation by a scalar and returns the new translation.
*
* <p>For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
*
* @param scalar The scalar to multiply by.
* @return The reference to the new mutated object.
*/
public Translation2d div(double scalar) {
return new Translation2d(m_x / scalar, m_y / scalar);
}
/**
* Divides the translation by a scalar and returns the new translation.
*
* <p>For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
*
* @param scalar The scalar to multiply by.
* @return The reference to the new mutated object.
*/
public Translation2d div(double scalar) {
return new Translation2d(m_x / scalar, m_y / scalar);
}
@Override
public String toString() {
return String.format("Translation2d(X: %.2f, Y: %.2f)", m_x, m_y);
}
@Override
public String toString() {
return String.format("Translation2d(X: %.2f, Y: %.2f)", m_x, m_y);
}
public static Translation2d fromRotation2d(Rotation2d rotation) {
return new Translation2d(rotation.getCos(), rotation.getSin());
}
public static Translation2d fromRotation2d(Rotation2d rotation) {
return new Translation2d(rotation.getCos(), rotation.getSin());
}
/**
* Checks equality between this Translation2d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Translation2d) {
return Math.abs(((Translation2d) obj).m_x - m_x) < 1E-9
&& Math.abs(((Translation2d) obj).m_y - m_y) < 1E-9;
}
return false;
}
/**
* Checks equality between this Translation2d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Translation2d) {
return Math.abs(((Translation2d) obj).m_x - m_x) < 1E-9
&& Math.abs(((Translation2d) obj).m_y - m_y) < 1E-9;
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_x, m_y);
}
@Override
public int hashCode() {
return Objects.hash(m_x, m_y);
}
static class TranslationSerializer extends StdSerializer<Translation2d> {
TranslationSerializer() {
super(Translation2d.class);
}
static class TranslationSerializer extends StdSerializer<Translation2d> {
TranslationSerializer() {
super(Translation2d.class);
}
@Override
public void serialize(
Translation2d value, JsonGenerator jgen, SerializerProvider provider)
throws IOException, JsonProcessingException {
@Override
public void serialize(Translation2d value, JsonGenerator jgen, SerializerProvider provider)
throws IOException, JsonProcessingException {
jgen.writeStartObject();
jgen.writeNumberField("x", value.m_x);
jgen.writeNumberField("y", value.m_y);
jgen.writeEndObject();
}
}
jgen.writeStartObject();
jgen.writeNumberField("x", value.m_x);
jgen.writeNumberField("y", value.m_y);
jgen.writeEndObject();
}
}
static class TranslationDeserializer extends StdDeserializer<Translation2d> {
TranslationDeserializer() {
super(Translation2d.class);
}
static class TranslationDeserializer extends StdDeserializer<Translation2d> {
TranslationDeserializer() {
super(Translation2d.class);
}
@Override
public Translation2d deserialize(JsonParser jp, DeserializationContext ctxt)
throws IOException, JsonProcessingException {
JsonNode node = jp.getCodec().readTree(jp);
double xval = node.get("x").numberValue().doubleValue();
double yval = node.get("y").numberValue().doubleValue();
@Override
public Translation2d deserialize(JsonParser jp, DeserializationContext ctxt)
throws IOException, JsonProcessingException {
JsonNode node = jp.getCodec().readTree(jp);
double xval = node.get("x").numberValue().doubleValue();
double yval = node.get("y").numberValue().doubleValue();
return new Translation2d(xval, yval);
}
}
return new Translation2d(xval, yval);
}
}
}

101
chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java
View File

@@ -10,67 +10,60 @@ package edu.wpi.first.wpilibj.geometry;
import java.util.Objects;
/**
* A change in distance along arc since the last pose update. We can use ideas
* from differential calculus to create new Pose2ds from a Twist2d and vise
* versa.
*
* <p>A Twist can be used to represent a difference between two poses.
*/
* A change in distance along arc since the last pose update. We can use ideas from differential
* calculus to create new Pose2ds from a Twist2d and vise versa.
*
* <p>A Twist can be used to represent a difference between two poses.
*/
@SuppressWarnings("MemberName")
public class Twist2d {
/**
* Linear "dx" component.
*/
public double dx;
/** Linear "dx" component. */
public double dx;
/**
* Linear "dy" component.
*/
public double dy;
/** Linear "dy" component. */
public double dy;
/**
* Angular "dtheta" component (radians).
*/
public double dtheta;
/** Angular "dtheta" component (radians). */
public double dtheta;
public Twist2d() {
}
public Twist2d() {}
/**
* Constructs a Twist2d with the given values.
* @param dx Change in x direction relative to robot.
* @param dy Change in y direction relative to robot.
* @param dtheta Change in angle relative to robot.
*/
public Twist2d(double dx, double dy, double dtheta) {
this.dx = dx;
this.dy = dy;
this.dtheta = dtheta;
}
/**
* Constructs a Twist2d with the given values.
*
* @param dx Change in x direction relative to robot.
* @param dy Change in y direction relative to robot.
* @param dtheta Change in angle relative to robot.
*/
public Twist2d(double dx, double dy, double dtheta) {
this.dx = dx;
this.dy = dy;
this.dtheta = dtheta;
}
@Override
public String toString() {
return String.format("Twist2d(dX: %.2f, dY: %.2f, dTheta: %.2f)", dx, dy, dtheta);
}
@Override
public String toString() {
return String.format("Twist2d(dX: %.2f, dY: %.2f, dTheta: %.2f)", dx, dy, dtheta);
}
/**
* Checks equality between this Twist2d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Twist2d) {
return Math.abs(((Twist2d) obj).dx - dx) < 1E-9
&& Math.abs(((Twist2d) obj).dy - dy) < 1E-9
&& Math.abs(((Twist2d) obj).dtheta - dtheta) < 1E-9;
}
return false;
}
/**
* Checks equality between this Twist2d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Twist2d) {
return Math.abs(((Twist2d) obj).dx - dx) < 1E-9
&& Math.abs(((Twist2d) obj).dy - dy) < 1E-9
&& Math.abs(((Twist2d) obj).dtheta - dtheta) < 1E-9;
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(dx, dy, dtheta);
}
@Override
public int hashCode() {
return Objects.hash(dx, dy, dtheta);
}
}

164
chameleon-server/src/main/java/edu/wpi/first/wpilibj/util/Units.java
View File

@@ -7,98 +7,94 @@
package edu.wpi.first.wpilibj.util;
/**
* Utility class that converts between commonly used units in FRC.
*/
/** Utility class that converts between commonly used units in FRC. */
public final class Units {
private static final double kInchesPerFoot = 12.0;
private static final double kMetersPerInch = 0.0254;
private static final double kSecondsPerMinute = 60;
private static final double kInchesPerFoot = 12.0;
private static final double kMetersPerInch = 0.0254;
private static final double kSecondsPerMinute = 60;
/**
* Utility class, so constructor is private.
*/
private Units() {
throw new UnsupportedOperationException("This is a utility class!");
}
/** Utility class, so constructor is private. */
private Units() {
throw new UnsupportedOperationException("This is a utility class!");
}
/**
* Converts given meters to feet.
*
* @param meters The meters to convert to feet.
* @return Feet converted from meters.
*/
public static double metersToFeet(double meters) {
return metersToInches(meters) / kInchesPerFoot;
}
/**
* Converts given meters to feet.
*
* @param meters The meters to convert to feet.
* @return Feet converted from meters.
*/
public static double metersToFeet(double meters) {
return metersToInches(meters) / kInchesPerFoot;
}
/**
* Converts given feet to meters.
*
* @param feet The feet to convert to meters.
* @return Meters converted from feet.
*/
public static double feetToMeters(double feet) {
return inchesToMeters(feet * kInchesPerFoot);
}
/**
* Converts given feet to meters.
*
* @param feet The feet to convert to meters.
* @return Meters converted from feet.
*/
public static double feetToMeters(double feet) {
return inchesToMeters(feet * kInchesPerFoot);
}
/**
* Converts given meters to inches.
*
* @param meters The meters to convert to inches.
* @return Inches converted from meters.
*/
public static double metersToInches(double meters) {
return meters / kMetersPerInch;
}
/**
* Converts given meters to inches.
*
* @param meters The meters to convert to inches.
* @return Inches converted from meters.
*/
public static double metersToInches(double meters) {
return meters / kMetersPerInch;
}
/**
* Converts given inches to meters.
*
* @param inches The inches to convert to meters.
* @return Meters converted from inches.
*/
public static double inchesToMeters(double inches) {
return inches * kMetersPerInch;
}
/**
* Converts given inches to meters.
*
* @param inches The inches to convert to meters.
* @return Meters converted from inches.
*/
public static double inchesToMeters(double inches) {
return inches * kMetersPerInch;
}
/**
* Converts given degrees to radians.
*
* @param degrees The degrees to convert to radians.
* @return Radians converted from degrees.
*/
public static double degreesToRadians(double degrees) {
return Math.toRadians(degrees);
}
/**
* Converts given degrees to radians.
*
* @param degrees The degrees to convert to radians.
* @return Radians converted from degrees.
*/
public static double degreesToRadians(double degrees) {
return Math.toRadians(degrees);
}
/**
* Converts given radians to degrees.
*
* @param radians The radians to convert to degrees.
* @return Degrees converted from radians.
*/
public static double radiansToDegrees(double radians) {
return Math.toDegrees(radians);
}
/**
* Converts given radians to degrees.
*
* @param radians The radians to convert to degrees.
* @return Degrees converted from radians.
*/
public static double radiansToDegrees(double radians) {
return Math.toDegrees(radians);
}
/**
* Converts rotations per minute to radians per second.
*
* @param rpm The rotations per minute to convert to radians per second.
* @return Radians per second converted from rotations per minute.
*/
public static double rotationsPerMinuteToRadiansPerSecond(double rpm) {
return rpm * Math.PI / (kSecondsPerMinute / 2);
}
/**
* Converts rotations per minute to radians per second.
*
* @param rpm The rotations per minute to convert to radians per second.
* @return Radians per second converted from rotations per minute.
*/
public static double rotationsPerMinuteToRadiansPerSecond(double rpm) {
return rpm * Math.PI / (kSecondsPerMinute / 2);
}
/**
* Converts radians per second to rotations per minute.
*
* @param radiansPerSecond The radians per second to convert to from rotations per minute.
* @return Rotations per minute converted from radians per second.
*/
public static double radiansPerSecondToRotationsPerMinute(double radiansPerSecond) {
return radiansPerSecond * (kSecondsPerMinute / 2) / Math.PI;
}
/**
* Converts radians per second to rotations per minute.
*
* @param radiansPerSecond The radians per second to convert to from rotations per minute.
* @return Rotations per minute converted from radians per second.
*/
public static double radiansPerSecondToRotationsPerMinute(double radiansPerSecond) {
return radiansPerSecond * (kSecondsPerMinute / 2) / Math.PI;
}
}