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

V2.1 (#37)

Browse Source 
* Toggle mode

* RangeSlider able to be disabled

* Added another window from the press of the button

* removed warning

* Major changes to colorpicker page

* Major changes to threshold tab

* fix for BGR not RGB

* Reverted changes of colorpicker.vue

* Updated Readme.md

Mainly Development Setup

* Update readme

small changes

* updated readme.md

Authors credit update

* updated readme.md 

added usb camera to hardware

* Updated UI readme

added nodejs installation and explanation

* Unit test vision (and implement networktable sending)

* finished color picker in the ui

* bugfix for correct tab

* cleanup and commenting

* Reveted changes to range slider

* bug fix for color picker

* Pipeline mat copy fix

* Added crosshair pipe

* compilation fix

* removed unsued comment

* moved calibration stuff to static class under 2d pipeline settings

* Added debug print commented

* Reverted subclass "Calibration"
Fixed bug where single point calibration causes crash when no target is found

* disable dual point crosshair for now

* Proper no target found fix, and snackbar error now showing

* Removed java fix for crash bug

* fixed problem with clear points in single point calibration

* change compatator to sqared values to make caluclation faster (no need for sqare root function)

* fixed roated camera too heigh for the UI

* Revert "fixed roated camera too heigh for the UI"

This reverts commit 50c8ecd345.

* fixed rotated camera too heigh for UI and corrected scalling

* updated pom to fix the camera buffer issue

* added pom variables

* Created new CaptureStaticProperties when rotating cam

* File stucture change, moved Pipeline implementation and their settings to /impl folder, refracted imports

* ignore auto created iml file

* ignore auto created iml file

* Fix lack of access problems after moving pipleline impl out of the same package

* Removed commented lines

* Fixed bug when selecting the same 90 deg rotation twice causes crosshair to move

* cleaned up single point calibration and bug fixed isBinary in driverMode

* Enum fix for the UI

* typo

* fixed Stream divisor problems see issue 19 in github issues

* fixed compilation error for test class

* Fixed problem when rotating camera with stream divisor

* Spelling + button style change

* Add speed limit to UI Updates

* Added runtime arg to fix settings permissions, and on-first-init

* Added FileHelper class to manage setting permissions for all files.

* Removed unnecessary argument

* NT can set pipeline to out on bound indexes

* bugfix in platform check

* added pipeline popup

* Round steam divisor resolution

* added regex test for camera name and pipe name

* Fix for Renaming pipeline leaves old config file #14

* Version change

* adde ( ) - and . to regex

* Replace NT timestamp with latency

* added back NT flush

* Add solvePNP, 3d tab on the UI, and some other misc bug fixes (#35)

* Rebase solvePNP on master

* added 3D tab minimap and csv reader

* More solvePNP

* Create draw pipe for pnp data

* SolvePNP piping work

* Move sorting into solvepnppipe

* Create calibration pipeline

* Update CalibrateSolvePNPPipeline.java

* add camera tilt angle

* Add calibration slider and snapshot button to 3D view

* Mirror updates in the socket handler

* add 3d calibration mode to the pipeline manager

* created calibration functions in ui and backend

* Start plumbing calibration

* Add snapshot and other handling to the RequestHandler

* added select resolution before starting calibration

* Rename solvePNPPipe to bounding box solve pnp pipe

* Update BoundingBoxSolvePNPPipe.java

* Add Mat serializer and CameraCalibrationConfig

* Begun calibration saving, fixed UI/Backend snapshot count mismatch

* Add (unplumbed) option to set checkerboard size

This will allow users to change the units their calibration is in

* Create chessboard.png

* Fix calibration NPE

* changed string serialization to a json send

* bug fixed cancellation button

* Fix spelling of snapshot in 3d.vue

* Plumb resolution change

* Set resolution during config, start on config serialization

* Update .gitignore

* Config fixes

* Start transition away from cvpipeline3d

* fix NPE on uncalibrated cameras

* clear list on fail

* Fix video mode index error

* ignore getters in camera calibration config

* Create json constructor for jsonmat

* get solvePNP mostly returning sane values

* Fix solvePNP bug and add unit test

* FIx calibration mat truncation

* added capture amount model upload and minimap data

* Standardize on meters in calibration and bounding box

* fix json out of bounds and handle null calibration more gracefully

* don't put text on calibrate image, go back to inches

* convert distance to meters

this means calibration will need to be in inches

* Actually save raw contor

* Update GroupContoursPipe.java

* Add all calibration return to camera capture

* hard code 2019 target

* bugfixed draw2d added fail calib popup, merge end and cancel

added the res index to the calib start

* Clarify error message and draw more fancy rectangles

* Cleanup memory in solvepnp

* re did minimap component

* fix npe if left/right is null

* remove references to 2d

* try-catch running the current pipeline

* Add method to find corners using the harris corner detector

* Possibly fix left/right missmatch

* Fix 3D Tab error

* FIx file permissions, mat serializer adjustments

* fixed mini map for field coordinates

* mini map changes fov

* Update SolvePNPPipe.java

* get rid of target corners

* some memory leak fixes

* fixed mini map location

* added position under minimap

* changed player fov look

* put all targets in the web send

* re did target send to ui added target tables, bugfix calibration

* fixed y position

* Add tilt angle to capture properties

* maybe fix y axis in minimap

* Add square size to onCalibrationEnding

* Possibly add square size to UI

* fix NPE with pitch

* Fix bug with sending multiple targets

* Only instantiate 3d stuff if we are in 3d mode

* Fix array list exceptions

* Fix bug in sort contors

list was truncated too early

* added download chess, tilt setting and ordinal tilt,

* added square size connection

* removed unused code

* Update pom version to 2.1-RELEASE

* Send camera calibrations to UI

* Stream pose list to a LIst

* Only stream necessary parts of the aux list entry

* Make broadcastMessage synchronized to prevent ConcurrentModificationExceptions

* added fps counter changed squaresize steps bug fixes in tables

* bugfix camera settings cam wont change

Authored-by: oriagranat9 <oriagranat9@gmail.com>

* disable pose print

* standardize on calibration in inches and add square size as a divisor

This should do the same thing as calibrating in the correct units but it doesnt like meeeeee

* updated pom for release, 3d calibration change, cosmetic changes

* WIP: 3D res filter

* bug fix in index parsing

* add good features to track point finder

* offset found corners by old tl position

* Add method to find most extreme per-quadrant points for solvePNP

* re did pipeline duplication

* Add release method to standard cv pipeline

* remove contor from tracked target

* Explicitly release all the intermediate results

* avoid creating new mats in group contors pipe

* removed mat release

* updated pom

* Actually cache the last target

We were trying to but i never hooked this part up

* Fix memory leak in sort contros pipe

* Fix memory leak in sort contors pipe

* Help more with memory usage in speckle reject and draw pipes

* minor bugfixes to the ui

* Start moving stream into vision process

Should probs move into a pipe to be idimatic

* justify bug fix

* Fix sort left/right bug

* Fix target grouping bug

* Avoid allocating a new mat in solvePNP, perf. increases in group contors pipe

* Event scripts (#36)

* Begin scripting work

* More scripting work

* Finalize scripting system

* Begin implementing script events

* Finalize script system

Co-authored-by: Banks T <btrout.dhrs@gmail.com>

Co-authored-by: OmerZ7 <zipory.omer@gmail.com>
Co-authored-by: Matt <matthew.morley.ca@gmail.com>
Co-authored-by: Banks T <btrout.dhrs@gmail.com>
This commit is contained in:
oriagranat9
2020-01-04 11:53:18 -08:00
committed by GitHub
parent 8a5052e0b3
commit 97a6e1419d
96 changed files with 4251 additions and 677 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
chameleon-server/src/main/java/com/chameleonvision/Exceptions/DuplicatedKeyException.java Normal file
View File

@@ -0,0 +1,7 @@
package com.chameleonvision.Exceptions;
public class DuplicatedKeyException extends Exception{
public DuplicatedKeyException(String message){
super(message);
}
}

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

@@ -2,7 +2,10 @@ package com.chameleonvision;
import com.chameleonvision.config.ConfigManager;
import com.chameleonvision.network.NetworkManager;
import com.chameleonvision.scripting.ScriptEventType;
import com.chameleonvision.scripting.ScriptManager;
import com.chameleonvision.util.Platform;
import com.chameleonvision.util.ShellExec;
import com.chameleonvision.util.Utilities;
import com.chameleonvision.vision.VisionManager;
import com.chameleonvision.web.Server;
@@ -41,6 +44,8 @@ public class Main {
if (!hasReportedConnectionFailure && logMessage.message.contains("timed out")) {
System.err.println("NT Connection has failed!");
hasReportedConnectionFailure = true;
} else if (logMessage.message.contains("connected")) {
ScriptManager.queueEvent(ScriptEventType.kNTConnected);
}
}
}
@@ -101,6 +106,9 @@ public class Main {
}
public static void main(String[] args) {
Runtime.getRuntime().addShutdownHook(new Thread(() -> ScriptManager.queueEvent(ScriptEventType.kProgramExit)));
if (CurrentPlatform.equals(Platform.UNSUPPORTED)) {
System.err.printf("Sorry, this platform is not supported. Give these details to the developers.\n%s\n", CurrentPlatform.toString());
return;
@@ -132,6 +140,14 @@ public class Main {
}
ConfigManager.initializeSettings();
if (!CurrentPlatform.isWindows()) {
ScriptManager.initialize();
} else {
System.out.println("Scripts not yet supported on Windows. ScriptEvents will be ignored.");
}
NetworkManager.initialize(manageNetwork);
if (ntServerMode) {
@@ -147,6 +163,8 @@ public class Main {
// NetworkTableInstance.getDefault().startClient("localhost");
}
ScriptManager.queueEvent(ScriptEventType.kProgramInit);
boolean visionSourcesOk = VisionManager.initializeSources();
if (!visionSourcesOk) {
System.out.println("No cameras connected!");
@@ -155,13 +173,13 @@ public class Main {
boolean visionProcessesOk = VisionManager.initializeProcesses();
if (!visionProcessesOk) {
System.err.println("shit");
System.err.println("Failed to start threads!");
return;
}
VisionManager.startProcesses();
System.out.printf("Starting Webserver at port %d\n", DEFAULT_PORT);
System.out.printf("Starting Web server at port %d\n", DEFAULT_PORT);
Server.main(DEFAULT_PORT);
}
}

64
chameleon-server/src/main/java/com/chameleonvision/config/CameraCalibrationConfig.java Normal file
View File

@@ -0,0 +1,64 @@
package com.chameleonvision.config;
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnore;
import com.fasterxml.jackson.annotation.JsonIgnoreType;
import com.fasterxml.jackson.annotation.JsonProperty;
import org.opencv.core.Mat;
import org.opencv.core.MatOfDouble;
import org.opencv.core.Size;
/**
* A class that holds a camera matrix and distortion coefficients for a given resolution
*/
public class CameraCalibrationConfig {
@JsonProperty("resolution") public final Size resolution;
@JsonProperty("cameraMatrix") public final JsonMat cameraMatrix;
@JsonProperty("distortionCoeffs") public final JsonMat distortionCoeffs;
@JsonProperty("squareSize") public final double squareSize;
@JsonCreator
public CameraCalibrationConfig(
@JsonProperty("resolution") Size resolution,
@JsonProperty("cameraMatrix") JsonMat cameraMatrix,
@JsonProperty("distortionCoeffs") JsonMat distortionCoeffs,
@JsonProperty("squareSize") double squareSize) {
this.resolution = resolution;
this.cameraMatrix = cameraMatrix;
this.distortionCoeffs = distortionCoeffs;
this.squareSize = squareSize;
}
public CameraCalibrationConfig(Size resolution, Mat cameraMatrix, Mat distortionCoeffs, double squareSize) {
this.resolution = resolution;
this.cameraMatrix = JsonMat.fromMat(cameraMatrix);
this.distortionCoeffs = JsonMat.fromMat(distortionCoeffs);
this.squareSize = squareSize;
}
@JsonIgnoreType
public static class UICameraCalibrationConfig {
public final int width;
public final int height;
public final double[] cameraMatrix;
public final double[] distortionCoeffs;
public UICameraCalibrationConfig(CameraCalibrationConfig config) {
width = (int) config.resolution.width;
height = (int) config.resolution.height;
cameraMatrix = config.cameraMatrix.data;
distortionCoeffs = config.distortionCoeffs.data;
}
}
@JsonIgnore
public Mat getCameraMatrixAsMat() {
return cameraMatrix.toMat();
}
@JsonIgnore
public MatOfDouble getDistortionCoeffsAsMat() {
return new MatOfDouble(distortionCoeffs.toMat());
}
}

120
chameleon-server/src/main/java/com/chameleonvision/config/CameraConfig.java
View File

@@ -1,5 +1,6 @@
package com.chameleonvision.config;
import com.chameleonvision.util.FileHelper;
import com.chameleonvision.util.JacksonHelper;
import com.chameleonvision.vision.pipeline.CVPipelineSettings;
@@ -8,58 +9,85 @@ import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.Path;
import java.nio.file.Paths;
import java.util.ArrayList;
import java.util.List;
import java.util.Objects;
public class CameraConfig {
private static final Path camerasConfigFolderPath = Paths.get(ConfigManager.SettingsPath.toString(), "cameras");
private static final Path camerasConfigFolderPath = Path.of(ConfigManager.SettingsPath.toString(), "cameras");
private final String cameraConfigName;
private final CameraJsonConfig preliminaryConfig;
private final Path configFolderPath;
private final Path configPath;
private final Path driverModePath;
private final Path calibrationPath;
final Path pipelineFolderPath;
public final PipelineConfig pipelineConfig;
CameraConfig(CameraJsonConfig config) {
preliminaryConfig = config;
cameraConfigName = preliminaryConfig.name.replace(' ', '_');
String cameraConfigName = preliminaryConfig.name.replace(' ', '_');
pipelineConfig = new PipelineConfig(this);
configFolderPath = Path.of(camerasConfigFolderPath.toString(), cameraConfigName);
configPath = Path.of(configFolderPath.toString(), "camera.json");
driverModePath = Path.of(configFolderPath.toString(), "drivermode.json");
calibrationPath = Path.of(configFolderPath.toString(), "calibration.json");
pipelineFolderPath = Paths.get(configFolderPath.toString(), "pipelines");
}
public FullCameraConfiguration load() {
checkFolder();
checkConfig();
checkDriverMode();
checkCalibration();
pipelineConfig.check();
return new FullCameraConfiguration(loadConfig(), pipelineConfig.load(), loadDriverMode(), this);
return new FullCameraConfiguration(loadConfig(), pipelineConfig.load(), loadDriverMode(), loadCalibration(), this);
}
private CameraJsonConfig loadConfig() {
CameraJsonConfig config = preliminaryConfig;
try {
config = JacksonHelper.deserializer(getConfigPath(), CameraJsonConfig.class);
config = JacksonHelper.deserializer(configPath, CameraJsonConfig.class);
} catch (IOException e) {
System.err.printf("Failed to load camera config: %s - using default.\n", getConfigPath().toString());
System.err.printf("Failed to load camera config: %s - using default.\n", configPath.toString());
}
return config;
}
private CVPipelineSettings loadDriverMode() {
CVPipelineSettings driverMode = new CVPipelineSettings();
driverMode.nickname = "DRIVERMODE";
try {
driverMode = JacksonHelper.deserializer(getDriverModePath(), CVPipelineSettings.class);
driverMode = JacksonHelper.deserializer(driverModePath, CVPipelineSettings.class);
} catch (IOException e) {
System.err.println("Failed to load camera drivermode: " + getDriverModePath().toString());
System.err.println("Failed to load camera drivermode: " + driverModePath.toString());
}
if (driverMode != null) {
driverMode.nickname = "DRIVERMODE";
driverMode.index = -1;
}
return driverMode;
}
private List<CameraCalibrationConfig> loadCalibration() {
List<CameraCalibrationConfig> calibrations = new ArrayList<>();
try {
calibrations = List.of(Objects.requireNonNull(JacksonHelper.deserializer(calibrationPath, CameraCalibrationConfig[].class)));
} catch (Exception e) {
System.err.println("Failed to load camera calibration: " + driverModePath.toString());
}
return calibrations;
}
void saveConfig(CameraJsonConfig config) {
try {
JacksonHelper.serializer(getConfigPath(), config);
JacksonHelper.serializer(configPath, config);
FileHelper.setFilePerms(configPath);
} catch (IOException e) {
System.err.println("Failed to save camera config file: " + getConfigPath().toString());
System.err.println("Failed to save camera config file: " + configPath.toString());
}
}
@@ -69,20 +97,33 @@ public class CameraConfig {
public void saveDriverMode(CVPipelineSettings driverMode) {
try {
JacksonHelper.serializer(getDriverModePath(), driverMode);
JacksonHelper.serializer(driverModePath, driverMode);
FileHelper.setFilePerms(driverModePath);
} catch (IOException e) {
System.err.println("Failed to save camera drivermode file: " + getDriverModePath().toString());
System.err.println("Failed to save camera drivermode file: " + driverModePath.toString());
}
}
public void saveCalibration(List<CameraCalibrationConfig> cal) {
CameraCalibrationConfig[] configs = cal.toArray(new CameraCalibrationConfig[0]);
try {
JacksonHelper.serializer(calibrationPath, configs);
FileHelper.setFilePerms(calibrationPath);
} catch (IOException e) {
System.err.println("Failed to save camera calibration file: " + calibrationPath.toString());
}
}
void checkFolder() {
if (!getConfigFolderExists()) {
if (!configFolderExists()) {
try {
if (!(new File(getConfigFolderPath().toUri()).mkdirs())) {
System.err.println("Failed to create camera config folder: " + getConfigFolderPath().toString());
if (!(new File(configFolderPath.toUri()).mkdirs())) {
System.err.println("Failed to create camera config folder: " + configFolderPath.toString());
}
FileHelper.setFilePerms(configFolderPath);
} catch(Exception e) {
System.err.println("Failed to create camera config folder: " + getConfigFolderPath().toString());
System.err.println("Failed to create camera config folder: " + configFolderPath.toString());
}
}
}
@@ -90,9 +131,10 @@ public class CameraConfig {
private void checkConfig() {
if (!configExists()) {
try {
JacksonHelper.serializer(getConfigPath(), preliminaryConfig);
JacksonHelper.serializer(configPath, preliminaryConfig);
FileHelper.setFilePerms(configPath);
} catch (IOException e) {
System.err.println("Failed to create camera config file: " + getConfigPath().toString());
System.err.println("Failed to create camera config file: " + configPath.toString());
}
}
}
@@ -102,38 +144,38 @@ public class CameraConfig {
try {
CVPipelineSettings newDriverModeSettings = new CVPipelineSettings();
newDriverModeSettings.nickname = "DRIVERMODE";
JacksonHelper.serializer(getDriverModePath(), newDriverModeSettings);
JacksonHelper.serializer(driverModePath, newDriverModeSettings);
FileHelper.setFilePerms(driverModePath);
} catch (IOException e) {
System.err.println("Failed to create camera drivermode file: " + getDriverModePath().toString());
System.err.println("Failed to create camera drivermode file: " + driverModePath.toString());
}
}
}
private Path getConfigFolderPath() {
return Paths.get(camerasConfigFolderPath.toString(), cameraConfigName);
private void checkCalibration() {
if (!calibrationExists()) {
try {
List<CameraCalibrationConfig> calibrations = new ArrayList<>();
JacksonHelper.serializer(calibrationPath, calibrations.toArray());
} catch (IOException e) {
System.err.println("Failed to create camera calibration file: " + calibrationPath.toString());
}
}
}
private Path getConfigPath() {
return Paths.get(getConfigFolderPath().toString(), "camera.json");
}
private Path getDriverModePath() {
return Paths.get(getConfigFolderPath().toString(), "drivermode.json");
}
private boolean getConfigFolderExists() {
return Files.exists(getConfigFolderPath());
}
Path getPipelineFolderPath() {
return Paths.get(getConfigFolderPath().toString(), "pipelines");
private boolean configFolderExists() {
return Files.exists(configFolderPath);
}
private boolean configExists() {
return getConfigFolderExists() && Files.exists(getConfigPath());
return configFolderExists() && Files.exists(configPath);
}
private boolean driverModeExists() {
return getConfigFolderExists() && Files.exists(getDriverModePath());
return configFolderExists() && Files.exists(driverModePath);
}
private boolean calibrationExists() {
return configFolderExists() && Files.exists(calibrationPath);
}
}

12
chameleon-server/src/main/java/com/chameleonvision/config/ConfigManager.java
View File

@@ -1,7 +1,7 @@
package com.chameleonvision.config;
import com.chameleonvision.util.ProgramDirectoryUtilities;
import com.chameleonvision.util.JacksonHelper;
import com.chameleonvision.Main;
import com.chameleonvision.util.*;
import com.chameleonvision.vision.pipeline.CVPipelineSettings;
import java.io.File;
@@ -16,7 +16,7 @@ import java.util.List;
public class ConfigManager {
private ConfigManager() {}
static final Path SettingsPath = Paths.get(ProgramDirectoryUtilities.getProgramDirectory(), "settings");
public static final Path SettingsPath = Paths.get(ProgramDirectoryUtilities.getProgramDirectory(), "settings");
private static final Path settingsFilePath = Paths.get(SettingsPath.toString(), "settings.json");
private static final LinkedHashMap<String, CameraConfig> cameraConfigs = new LinkedHashMap<>();
@@ -33,6 +33,9 @@ public class ConfigManager {
System.err.println("Failed to create settings folder: " + SettingsPath.toString());
}
Files.createDirectory(SettingsPath);
if (!Platform.CurrentPlatform.isWindows()) {
new ShellExec().executeBashCommand("sudo chmod -R 0777 " + SettingsPath.toString());
}
} catch (IOException e) {
if(!(e instanceof java.nio.file.FileAlreadyExistsException))
e.printStackTrace();
@@ -45,6 +48,7 @@ public class ConfigManager {
if (settingsFileEmpty || !settingsFileExists()) {
try {
JacksonHelper.serializer(settingsFilePath, settings);
FileHelper.setFilePerms(settingsFilePath);
} catch (IOException e) {
e.printStackTrace();
}
@@ -61,11 +65,13 @@ public class ConfigManager {
System.out.println("Settings folder: " + SettingsPath.toString());
checkSettingsFolder();
checkSettingsFile();
FileHelper.setAllPerms(SettingsPath);
}
private static void saveSettingsFile() {
try {
JacksonHelper.serializer(settingsFilePath, settings);
FileHelper.setFilePerms(settingsFilePath);
} catch (IOException e) {
System.err.println("Failed to save settings.json!");
}

8
chameleon-server/src/main/java/com/chameleonvision/config/FullCameraConfiguration.java
View File

@@ -7,13 +7,15 @@ import java.util.List;
public class FullCameraConfiguration {
public final CameraJsonConfig cameraConfig;
public final List<CVPipelineSettings> pipelines;
public final CVPipelineSettings drivermode;
public final CVPipelineSettings driverMode;
public final List<CameraCalibrationConfig> calibration;
public final CameraConfig fileConfig;
FullCameraConfiguration(CameraJsonConfig cameraConfig, List<CVPipelineSettings> pipelines, CVPipelineSettings drivermode, CameraConfig fileConfig) {
FullCameraConfiguration(CameraJsonConfig cameraConfig, List<CVPipelineSettings> pipelines, CVPipelineSettings driverMode, List<CameraCalibrationConfig> calibration, CameraConfig fileConfig) {
this.cameraConfig = cameraConfig;
this.pipelines = pipelines;
this.drivermode = drivermode;
this.driverMode = driverMode;
this.calibration = calibration;
this.fileConfig = fileConfig;
}
}

78
chameleon-server/src/main/java/com/chameleonvision/config/JsonMat.java Normal file
View File

@@ -0,0 +1,78 @@
package com.chameleonvision.config;
import com.fasterxml.jackson.annotation.JsonProperty;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import java.lang.reflect.Array;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.LinkedList;
import java.util.List;
public class JsonMat {
public final int rows;
public final int cols;
public final int type;
public final double[] data;
public JsonMat(int rows, int cols, double[] data) {
this(rows, cols, CvType.CV_64FC1, data);
}
public JsonMat(
@JsonProperty("rows") int rows,
@JsonProperty("cols") int cols,
@JsonProperty("type") int type,
@JsonProperty("data") double[] data) {
this.rows = rows;
this.cols = cols;
this.type = type;
this.data = data;
}
public Mat toMat() {
return toMat(this);
}
private static boolean isCameraMatrixMat(Mat mat) {
return mat.type() == CvType.CV_64FC1 && mat.cols() == 3 && mat.rows() == 3;
}
private static boolean isDistortionCoeffsMat(Mat mat) {
return mat.type() == CvType.CV_64FC1 && mat.cols() == 5 && mat.rows() == 1;
}
private static boolean isCalibrationMat(Mat mat) {
return isDistortionCoeffsMat(mat) || isCameraMatrixMat(mat);
}
public static double[] getDataFromMat(Mat mat) {
if (!isCalibrationMat(mat)) return null;
double[] data = new double[(int)(mat.total()*mat.elemSize())];
mat.get(0, 0, data);
int dataLen = -1;
if (isCameraMatrixMat(mat)) dataLen = 9;
if (isDistortionCoeffsMat(mat)) dataLen = 5;
// truncate Mat data to correct number data points.
return Arrays.copyOfRange(data, 0, dataLen);
}
public static JsonMat fromMat(Mat mat) {
if (!isCalibrationMat(mat)) return null;
return new JsonMat(mat.rows(), mat.cols(), getDataFromMat(mat));
}
public static Mat toMat(JsonMat jsonMat) {
if (jsonMat.type != CvType.CV_64FC1) return null;
Mat retMat = new Mat(jsonMat.rows, jsonMat.cols, jsonMat.type);
retMat.put(0, 0, jsonMat.data);
return retMat;
}
}

56
chameleon-server/src/main/java/com/chameleonvision/config/PipelineConfig.java
View File

@@ -1,7 +1,9 @@
package com.chameleonvision.config;
import com.chameleonvision.util.FileHelper;
import com.chameleonvision.util.JacksonHelper;
import com.chameleonvision.vision.pipeline.*;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipelineSettings;
import java.io.File;
import java.io.IOException;
@@ -13,9 +15,6 @@ import java.util.List;
public class PipelineConfig {
private static final String CVPipeline2DPrefix = "CV2D";
private static final String CVPipeline3DPrefix = "CV3D";
private final CameraConfig cameraConfig;
/**
@@ -27,15 +26,20 @@ public class PipelineConfig {
}
private void checkFolder() {
if ( !(new File(cameraConfig.getPipelineFolderPath().toUri()).mkdirs())) {
if (Files.notExists(cameraConfig.getPipelineFolderPath())) {
if ( !(new File(cameraConfig.pipelineFolderPath.toUri()).mkdirs())) {
if (Files.notExists(cameraConfig.pipelineFolderPath)) {
System.err.println("Failed to create pipelines folder.");
}
}
try {
FileHelper.setFilePerms(cameraConfig.pipelineFolderPath);
} catch (IOException e) {
// ignored
}
}
private File[] getPipelineFiles() {
return new File(cameraConfig.getPipelineFolderPath().toUri()).listFiles();
return new File(cameraConfig.pipelineFolderPath.toUri()).listFiles();
}
private boolean folderHasPipelines() {
@@ -49,15 +53,14 @@ public class PipelineConfig {
checkFolder();
// Check if there's at least one pipe
if (!folderHasPipelines()) {
save(new CVPipeline2dSettings());
save(new StandardCVPipelineSettings());
}
}
private Path getPipelinePath(CVPipelineSettings setting) {
String pipelineName = setting.nickname.replace(' ', '_');
String prefix = ((setting instanceof CVPipeline2dSettings) ? CVPipeline2DPrefix : CVPipeline3DPrefix) + "-";
String fullFileName = prefix + pipelineName + ".json";
return Path.of(cameraConfig.getPipelineFolderPath().toString(), fullFileName);
String fullFileName = pipelineName + ".json";
return Path.of(cameraConfig.pipelineFolderPath.toString(), fullFileName);
}
private boolean pipelineExists(CVPipelineSettings setting) {
@@ -68,20 +71,11 @@ public class PipelineConfig {
var path = getPipelinePath(settings);
if (settings instanceof CVPipeline3dSettings) {
try {
JacksonHelper.serializer(path, settings);
} catch (IOException e) {
e.printStackTrace();
}
} else if (settings instanceof CVPipeline2dSettings) {
try {
JacksonHelper.serializer(path, settings);
} catch (IOException e) {
e.printStackTrace();
}
} else {
throw new RuntimeException("saving non-2d and non-3d pipelines not implemented~");
try {
JacksonHelper.serializer(path, settings);
FileHelper.setFilePerms(path);
} catch (IOException e) {
e.printStackTrace();
}
}
@@ -124,24 +118,12 @@ public class PipelineConfig {
System.err.println("no pipes to load! loading default");
} else {
for(File pipelineFile : pipelineFiles) {
var name = pipelineFile.getName();
if(name.startsWith(CVPipeline3DPrefix)) {
// try to load 3d pipe
try {
var pipe = JacksonHelper.deserializer(Paths.get(pipelineFile.getPath()), CVPipeline3dSettings.class);
deserializedList.add(pipe);
} catch (IOException e) {
System.err.println("couldn't load cvpipeline3d");
}
} else if(name.startsWith(CVPipeline2DPrefix)) {
// try to load 2d pipe
try {
var pipe = JacksonHelper.deserializer(Paths.get(pipelineFile.getPath()), CVPipeline2dSettings.class);
var pipe = JacksonHelper.deserializer(Paths.get(pipelineFile.getPath()), StandardCVPipelineSettings.class);
deserializedList.add(pipe);
} catch (IOException e) {
System.err.println("couldn't load cvpipeline2d");
}
}
}
}

2
chameleon-server/src/main/java/com/chameleonvision/network/NetworkManager.java
View File

@@ -20,7 +20,7 @@ public class NetworkManager {
return;
}
Platform platform = Platform.getCurrentPlatform();
Platform platform = Platform.CurrentPlatform;
if (platform.isLinux()) {
networking = new LinuxNetworking();

14
chameleon-server/src/main/java/com/chameleonvision/scripting/ScriptCommandType.java Normal file
View File

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

23
chameleon-server/src/main/java/com/chameleonvision/scripting/ScriptConfig.java Normal file
View File

@@ -0,0 +1,23 @@
package com.chameleonvision.scripting;
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonProperty;
public class ScriptConfig {
public final ScriptEventType eventType;
public final String 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;
}
}

35
chameleon-server/src/main/java/com/chameleonvision/scripting/ScriptEvent.java Normal file
View File

@@ -0,0 +1,35 @@
package com.chameleonvision.scripting;
import com.chameleonvision.Debug;
import com.chameleonvision.util.ShellExec;
import java.io.IOException;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.LinkedList;
import java.util.List;
public class ScriptEvent {
private static final ShellExec executor = new ShellExec(true, true);
public final ScriptConfig config;
public ScriptEvent(ScriptConfig config) {
this.config = config;
}
public int run() throws IOException {
int retVal = executor.executeBashCommand(config.command);
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()) {
Debug.printInfo(String.format("Output from \"%s\" script: %s\n", config.eventType.name(), output));
}
Debug.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;
}
}

21
chameleon-server/src/main/java/com/chameleonvision/scripting/ScriptEventType.java Normal file
View File

@@ -0,0 +1,21 @@
package com.chameleonvision.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");
public final String value;
ScriptEventType(String value) {
this.value = value;
}
}

126
chameleon-server/src/main/java/com/chameleonvision/scripting/ScriptManager.java Normal file
View File

@@ -0,0 +1,126 @@
package com.chameleonvision.scripting;
import com.chameleonvision.Debug;
import com.chameleonvision.Main;
import com.chameleonvision.config.ConfigManager;
import com.chameleonvision.util.JacksonHelper;
import com.chameleonvision.util.LoopingRunnable;
import com.chameleonvision.util.Platform;
import com.chameleonvision.util.ProgramDirectoryUtilities;
import java.io.File;
import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.Path;
import java.nio.file.Paths;
import java.util.ArrayList;
import java.util.List;
import java.util.Objects;
import java.util.concurrent.BlockingQueue;
import java.util.concurrent.LinkedBlockingDeque;
import java.util.concurrent.LinkedBlockingQueue;
public class ScriptManager {
private ScriptManager() {}
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);
}
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 {
ScriptRunner(Long loopTimeMs) {
super(loopTimeMs);
}
@Override
protected void process() {
try {
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());
}
}
}
}
protected static class ScriptConfigManager {
protected static final Path scriptConfigPath = Paths.get(ConfigManager.SettingsPath.toString(), "scripts.json");
private ScriptConfigManager() {}
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));
}
try {
JacksonHelper.serializer(scriptConfigPath, eventsConfig.toArray(new ScriptConfig[0]));
} catch (IOException e) {
e.printStackTrace();
}
}
}
static List<ScriptConfig> loadConfig() {
try {
var raw = JacksonHelper.deserializer(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) {
//
}
}
}
public static void queueEvent(ScriptEventType eventType) {
if (!Platform.getCurrentPlatform().isWindows()) {
try {
queuedEvents.putLast(eventType);
Debug.printInfo("Queued event: " + eventType.name());
} catch (InterruptedException e) {
System.err.println("Failed to add event to queue: " + eventType.name());
}
}
}
}

44
chameleon-server/src/main/java/com/chameleonvision/util/FileHelper.java Normal file
View File

@@ -0,0 +1,44 @@
package com.chameleonvision.util;
import java.io.File;
import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.Path;
import java.nio.file.attribute.PosixFileAttributes;
import java.nio.file.attribute.PosixFilePermission;
import java.util.Arrays;
import java.util.HashSet;
import java.util.Set;
public class FileHelper {
private FileHelper() {}
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)) {
System.out.printf("setting perms on %s\n", path.toString());
Files.setPosixFilePermissions(path, perms);
if (thisFile.isDirectory()) {
for (File subfile : thisFile.listFiles()) {
setFilePerms(subfile.toPath());
}
}
}
}
}
public static void setAllPerms(Path path) {
String command = String.format("chmod 777 -R %s", path.toString());
try {
Process p = Runtime.getRuntime().exec(command);
p.waitFor();
} catch (Exception e) {
e.printStackTrace();
}
}
}

2
chameleon-server/src/main/java/com/chameleonvision/util/JacksonHelper.java
View File

@@ -4,9 +4,9 @@ import com.fasterxml.jackson.databind.ObjectMapper;
import com.fasterxml.jackson.databind.json.JsonMapper;
import com.fasterxml.jackson.databind.jsontype.BasicPolymorphicTypeValidator;
import com.fasterxml.jackson.databind.jsontype.PolymorphicTypeValidator;
import java.io.File;
import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.Path;
public class JacksonHelper {

5
chameleon-server/src/main/java/com/chameleonvision/util/MathHandler.java
View File

@@ -24,4 +24,9 @@ public class MathHandler {
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;
}
}

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

@@ -23,7 +23,7 @@ public enum Platform {
private static final String OS_ARCH = System.getProperty("os.arch");
public static final Platform CurrentPlatform = getCurrentPlatform();
public boolean isWindows() {
public boolean isWindows() {
return this == WINDOWS_64;
}
@@ -35,6 +35,10 @@ public enum Platform {
return this == MACOS_64;
}
public static boolean isRaspberryPi() {
return CurrentPlatform.equals(LINUX_RASPBIAN);
}
private static ShellExec shell = new ShellExec(true, false);
public boolean isRoot() {

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

@@ -24,19 +24,16 @@ public class ProgramDirectoryUtilities
{
if (runningFromJAR())
{
if (Platform.isRaspberryPi()) {
return "/boot/chameleon-vision";
}
return getCurrentJARDirectory();
} else
{
return System.getProperty("user.dir");
// return getCurrentProjectDirectory();
}
}
private static String getCurrentProjectDirectory()
{
return new File("").getAbsolutePath();
}
private static String getCurrentJARDirectory()
{
try

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

@@ -19,6 +19,43 @@ public class ShellExec {
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
* @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);
// 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.
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;
}
/**
* 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")

11
chameleon-server/src/main/java/com/chameleonvision/vision/VisionManager.java
View File

@@ -6,7 +6,6 @@ import com.chameleonvision.config.ConfigManager;
import com.chameleonvision.config.FullCameraConfiguration;
import com.chameleonvision.util.Helpers;
import com.chameleonvision.util.Platform;
import com.chameleonvision.vision.camera.CameraCapture;
import com.chameleonvision.vision.camera.USBCameraCapture;
import com.chameleonvision.vision.pipeline.CVPipelineSettings;
import edu.wpi.cscore.UsbCamera;
@@ -84,9 +83,9 @@ public class VisionManager {
CameraJsonConfig cameraJsonConfig = config.cameraConfig;
USBCameraCapture camera = new USBCameraCapture(cameraJsonConfig);
VisionProcess process = new VisionProcess(camera, cameraJsonConfig.name, config.pipelines);
process.pipelineManager.driverModePipeline.settings = config.drivermode;
USBCameraCapture camera = new USBCameraCapture(config);
VisionProcess process = new VisionProcess(camera, config);
process.pipelineManager.driverModePipeline.settings = config.driverMode;
visionProcesses.add(new VisionProcessManageable(i, cameraJsonConfig.name, process));
}
currentUIVisionProcess = getVisionProcessByIndex(0);
@@ -102,6 +101,10 @@ public class VisionManager {
return currentUIVisionProcess;
}
public static CameraConfig getCurrentCameraConfig() {
return getCameraConfig(currentUIVisionProcess);
}
public static CameraConfig getCameraConfig(VisionProcess process) {
String cameraName = process.getCamera().getProperties().name;
return Objects.requireNonNull(loadedCameraConfigs.stream().filter(x -> x.cameraConfig.name.equals(cameraName)).findFirst().orElse(null)).fileConfig;

203
chameleon-server/src/main/java/com/chameleonvision/vision/VisionProcess.java
View File

@@ -1,17 +1,26 @@
package com.chameleonvision.vision;
import com.chameleonvision.Debug;
import com.chameleonvision.config.CameraCalibrationConfig;
import com.chameleonvision.config.CameraConfig;
import com.chameleonvision.config.ConfigManager;
import com.chameleonvision.scripting.ScriptEventType;
import com.chameleonvision.scripting.ScriptManager;
import com.chameleonvision.config.FullCameraConfiguration;
import com.chameleonvision.util.LoopingRunnable;
import com.chameleonvision.vision.camera.CameraCapture;
import com.chameleonvision.util.MathHandler;
import com.chameleonvision.vision.camera.CameraStreamer;
import com.chameleonvision.vision.camera.USBCameraCapture;
import com.chameleonvision.vision.pipeline.*;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipeline;
import com.chameleonvision.vision.pipeline.impl.DriverVisionPipeline;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipelineSettings;
import com.chameleonvision.web.SocketHandler;
import com.fasterxml.jackson.core.JsonProcessingException;
import com.fasterxml.jackson.databind.ObjectMapper;
import edu.wpi.cscore.VideoMode;
import edu.wpi.first.networktables.*;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpiutil.CircularBuffer;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Mat;
@@ -21,6 +30,7 @@ import java.util.HashMap;
import java.util.List;
import java.util.concurrent.BlockingQueue;
import java.util.concurrent.LinkedBlockingDeque;
import java.util.stream.Collectors;
public class VisionProcess {
@@ -28,6 +38,7 @@ public class VisionProcess {
private final USBCameraCapture cameraCapture;
private final CameraStreamerRunnable streamRunnable;
private final VisionProcessRunnable visionRunnable;
private final CameraConfig fileConfig;
public final CameraStreamer cameraStreamer;
public PipelineManager pipelineManager;
@@ -37,6 +48,7 @@ public class VisionProcess {
// network table stuff
private final NetworkTable defaultTable;
private NetworkTableInstance tableInstance;
private NetworkTableEntry ntPipelineEntry;
public NetworkTableEntry ntDriverModeEntry;
private int ntDriveModeListenerID;
@@ -45,17 +57,22 @@ public class VisionProcess {
private NetworkTableEntry ntPitchEntry;
private NetworkTableEntry ntAuxListEntry;
private NetworkTableEntry ntAreaEntry;
private NetworkTableEntry ntTimeStampEntry;
private NetworkTableEntry ntLatencyEntry;
private NetworkTableEntry ntValidEntry;
private NetworkTableEntry ntPoseEntry;
private ObjectMapper objectMapper = new ObjectMapper();
VisionProcess(USBCameraCapture cameraCapture, String name, List<CVPipelineSettings> loadedPipelineSettings) {
private long lastUIUpdateMs = 0;
VisionProcess(USBCameraCapture cameraCapture, FullCameraConfiguration config) {
this.cameraCapture = cameraCapture;
pipelineManager = new PipelineManager(this, loadedPipelineSettings);
fileConfig = config.fileConfig;
pipelineManager = new PipelineManager(this, config.pipelines);
// Thread to put frames on the dashboard
this.cameraStreamer = new CameraStreamer(cameraCapture, name);
this.cameraStreamer = new CameraStreamer(cameraCapture, config.cameraConfig.name, pipelineManager.getCurrentPipeline().settings.streamDivisor);
this.streamRunnable = new CameraStreamerRunnable(30, cameraStreamer);
// Thread to process vision data
@@ -74,10 +91,10 @@ public class VisionProcess {
visionThread.setName(getCamera().getProperties().name + " - Vision Thread");
visionThread.start();
System.out.println("Starting stream thread.");
var streamThread = new Thread(streamRunnable);
streamThread.setName(getCamera().getProperties().name + " - Stream Thread");
streamThread.start();
// System.out.println("Starting stream thread.");
// var streamThread = new Thread(streamRunnable);
// streamThread.setName(getCamera().getProperties().name + " - Stream Thread");
// streamThread.start();
}
/**
@@ -99,14 +116,16 @@ public class VisionProcess {
}
private void initNT(NetworkTable newTable) {
tableInstance = newTable.getInstance();
ntPipelineEntry = newTable.getEntry("pipeline");
ntDriverModeEntry = newTable.getEntry("driver_mode");
ntPitchEntry = newTable.getEntry("pitch");
ntYawEntry = newTable.getEntry("yaw");
ntAreaEntry = newTable.getEntry("area");
ntTimeStampEntry = newTable.getEntry("timestamp");
ntLatencyEntry = newTable.getEntry("latency");
ntValidEntry = newTable.getEntry("is_valid");
ntAuxListEntry = newTable.getEntry("aux_targets");
ntPoseEntry = newTable.getEntry("poseList");
ntDriveModeListenerID = ntDriverModeEntry.addListener(this::setDriverMode, EntryListenerFlags.kUpdate);
ntPipelineListenerID = ntPipelineEntry.addListener(this::setPipeline, EntryListenerFlags.kUpdate);
ntDriverModeEntry.setBoolean(false);
@@ -120,11 +139,13 @@ public class VisionProcess {
public void setDriverMode(boolean driverMode) {
pipelineManager.setDriverMode(driverMode);
ScriptManager.queueEvent(driverMode ? ScriptEventType.kEnterDriverMode : ScriptEventType.kExitDriverMode);
SocketHandler.sendFullSettings();
}
/**
* Method called by the nt entry listener to update the next pipeline.
*
* @param notification the notification
*/
private void setPipeline(EntryNotification notification) {
@@ -136,76 +157,101 @@ public class VisionProcess {
// if it's null, we haven't even started the program yet, so just return
// otherwise, set it.
if(ntDriverModeEntry != null) {
if (ntDriverModeEntry != null) {
ntDriverModeEntry.setBoolean(isDriverMode);
}
}
private void updateUI(CVPipelineResult data) {
if(cameraCapture.getProperties().name.equals(ConfigManager.settings.currentCamera)) {
HashMap<String, Object> WebSend = new HashMap<>();
HashMap<String, Object> point = new HashMap<>();
HashMap<String, Object> calculated = new HashMap<>();
List<Double> center = new ArrayList<>();
if (data.hasTarget) {
if(data instanceof CVPipeline2d.CVPipeline2dResult) {
CVPipeline2d.CVPipeline2dResult result = (CVPipeline2d.CVPipeline2dResult) data;
CVPipeline2d.Target2d bestTarget = result.targets.get(0);
center.add(bestTarget.rawPoint.center.x);
center.add(bestTarget.rawPoint.center.y);
calculated.put("pitch", bestTarget.pitch);
calculated.put("yaw", bestTarget.yaw);
calculated.put("area", bestTarget.area);
} else if (data instanceof CVPipeline3d.CVPipeline3dResult) {
// TODO: (2.1) 3d stuff in UI
// 30 "FPS" update rate
long currentMillis = System.currentTimeMillis();
if (currentMillis - lastUIUpdateMs > 1000 / 30) {
lastUIUpdateMs = currentMillis;
if (cameraCapture.getProperties().name.equals(ConfigManager.settings.currentCamera)) {
HashMap<String, Object> WebSend = new HashMap<>();
HashMap<String, Object> point = new HashMap<>();
HashMap<String, Object> pointMap = new HashMap<>();
ArrayList<Object> webTargets = new ArrayList<Object>();
List<Double> center = new ArrayList<>();
if (data.hasTarget) {
if (data instanceof StandardCVPipeline.StandardCVPipelineResult) {
StandardCVPipeline.StandardCVPipelineResult result = (StandardCVPipeline.StandardCVPipelineResult) data;
StandardCVPipeline.TrackedTarget bestTarget = result.targets.get(0);
if (((StandardCVPipelineSettings) pipelineManager.getCurrentPipeline().settings).multiple) {
for (var target : result.targets) {
pointMap = new HashMap<>();
pointMap.put("pitch", target.pitch);
pointMap.put("yaw", target.yaw);
pointMap.put("area", target.area);
pointMap.put("pose", target.cameraRelativePose);
webTargets.add(pointMap);
}
} else {
pointMap.put("pitch", bestTarget.pitch);
pointMap.put("yaw", bestTarget.yaw);
pointMap.put("area", bestTarget.area);
pointMap.put("pose", bestTarget.cameraRelativePose);
webTargets.add(pointMap);
}
center.add(bestTarget.minAreaRect.center.x);
center.add(bestTarget.minAreaRect.center.y);
}
} else {
pointMap.put("pitch", null);
pointMap.put("yaw", null);
pointMap.put("area", null);
pointMap.put("pose", new Pose2d());
webTargets.add(pointMap);
center.add(null);
center.add(null);
calculated.put("pitch", null);
calculated.put("yaw", null);
calculated.put("area", null);
}
} else {
center.add(null);
center.add(null);
calculated.put("pitch", null);
calculated.put("yaw", null);
calculated.put("area", null);
point.put("fps", visionRunnable.fps);
point.put("targets", webTargets);
point.put("rawPoint", center);
WebSend.put("point", point);
SocketHandler.broadcastMessage(WebSend);
}
point.put("fps", visionRunnable.fps);
point.put("calculated", calculated);
point.put("rawPoint", center);
WebSend.put("point", point);
SocketHandler.broadcastMessage(WebSend);
}
}
private void updateNetworkTableData(CVPipelineResult data) {
ntValidEntry.setBoolean(data.hasTarget);
if(data.hasTarget && !(data instanceof DriverVisionPipeline.DriverPipelineResult)) {
if(data instanceof CVPipeline2d.CVPipeline2dResult) {
if (data.hasTarget && !(data instanceof DriverVisionPipeline.DriverPipelineResult)) {
if (data instanceof StandardCVPipeline.StandardCVPipelineResult) {
//noinspection unchecked
List<CVPipeline2d.Target2d> targets = (List<CVPipeline2d.Target2d>) data.targets;
ntTimeStampEntry.setDouble(data.imageTimestamp);
List<StandardCVPipeline.TrackedTarget> targets = (List<StandardCVPipeline.TrackedTarget>) data.targets;
ntLatencyEntry.setDouble(MathHandler.roundTo(data.processTime * 1e-6, 3));
ntPitchEntry.setDouble(targets.get(0).pitch);
ntYawEntry.setDouble(targets.get(0).yaw);
ntAreaEntry.setDouble(targets.get(0).area);
try {
ntAuxListEntry.setString(objectMapper.writeValueAsString(targets));
ntAuxListEntry.setString(objectMapper.writeValueAsString(targets.stream()
.map(it -> List.of(it.pitch, it.yaw, it.area, it.cameraRelativePose))
.collect(Collectors.toList())));
// TODO: (2.1) 3d stuff...
ntPoseEntry.setString(objectMapper.writeValueAsString(targets.stream().map(target -> target.cameraRelativePose).collect(Collectors.toList())));
} catch (JsonProcessingException e) {
e.printStackTrace();
}
} else if (data instanceof CVPipeline3d.CVPipeline3dResult) {
// TODO: (2.1) 3d stuff...
} else {
ntPitchEntry.setDouble(0.0);
ntYawEntry.setDouble(0.0);
ntAreaEntry.setDouble(0.0);
ntLatencyEntry.setDouble(0.0);
ntAuxListEntry.setString("");
}
} else {
ntPitchEntry.setDouble(0.0);
ntYawEntry.setDouble(0.0);
ntAreaEntry.setDouble(0.0);
ntTimeStampEntry.setDouble(0.0);
ntAuxListEntry.setString("");
}
tableInstance.flush();
}
public void setVideoMode(VideoMode newMode) {
@@ -229,6 +275,27 @@ public class VisionProcess {
return pipelineManager.driverModePipeline.settings;
}
public void addCalibration(CameraCalibrationConfig cal) {
cameraCapture.addCalibrationData(cal);
System.out.println("saving to file");
fileConfig.saveCalibration(cameraCapture.getConfig());
}
public void setIs3d(Boolean value) {
var settings = pipelineManager.getCurrentPipeline().settings;
if (settings instanceof StandardCVPipelineSettings) {
((StandardCVPipelineSettings) settings).is3D = value;
}
}
public boolean getIs3d() {
var settings = pipelineManager.getCurrentPipeline().settings;
if (settings instanceof StandardCVPipelineSettings) {
return ((StandardCVPipelineSettings) settings).is3D;
}
return false;
}
/**
* VisionProcessRunnable will process images as quickly as possible
*/
@@ -240,14 +307,22 @@ public class VisionProcess {
@Override
public void run() {
var lastUpdateTimeNanos = System.nanoTime();
while(!Thread.interrupted()) {
var lastStreamTimeMs = System.currentTimeMillis();
while (!Thread.interrupted()) {
// blocking call, will block until camera has a new frame.
Pair<Mat, Long> camData = cameraCapture.getFrame();
Mat camFrame = camData.getLeft();
if (camFrame.cols() > 0 && camFrame.rows() > 0) {
CVPipelineResult result = pipelineManager.getCurrentPipeline().runPipeline(camFrame);
CVPipelineResult result = null;
try {
result = pipelineManager.getCurrentPipeline().runPipeline(camFrame);
} catch (Exception e) {
System.err.println("Exception in vision process " + getCamera().getProperties().getNickname() + "!");
e.printStackTrace();
}
camFrame.release();
if (result != null) {
@@ -259,8 +334,16 @@ public class VisionProcess {
}
try {
streamFrameQueue.clear();
streamFrameQueue.add(lastPipelineResult.outputMat);
// streamFrameQueue.clear();
// streamFrameQueue.add(lastPipelineResult.outputMat);
var currentTime = System.currentTimeMillis();
if((currentTime - lastStreamTimeMs)/1000d > 1.0 / 30.0) {
cameraStreamer.runStream(lastPipelineResult.outputMat);
// System.out.println("Ran stream in " + (System.currentTimeMillis() - currentTime) + "ms!");
lastStreamTimeMs = currentTime;
lastPipelineResult.outputMat.release();
}
} catch (Exception e) {
Debug.printInfo("Vision running faster than stream.");
}
@@ -274,7 +357,7 @@ public class VisionProcess {
double getAverageFPS() {
var temp = 0.0;
for(int i = 0; i < 7; i++) {
for (int i = 0; i < 7; i++) {
temp += fpsAveragingBuffer.get(i);
}
temp /= 7.0;
@@ -290,7 +373,7 @@ public class VisionProcess {
private CameraStreamerRunnable(int cameraFPS, CameraStreamer streamer) {
// add 2 FPS to allow for a bit of overhead
super(1000L/(cameraFPS + 2));
super(1000L / (cameraFPS + 2));
this.streamer = streamer;
}

6
chameleon-server/src/main/java/com/chameleonvision/vision/camera/CameraCapture.java
View File

@@ -1,9 +1,12 @@
package com.chameleonvision.vision.camera;
import com.chameleonvision.config.CameraCalibrationConfig;
import com.chameleonvision.vision.image.CaptureProperties;
import com.chameleonvision.vision.image.ImageCapture;
import edu.wpi.cscore.VideoMode;
import java.util.List;
public interface CameraCapture extends ImageCapture {
CaptureProperties getProperties();
@@ -39,4 +42,7 @@ public interface CameraCapture extends ImageCapture {
* @param gain the new gain to set the camera to
*/
void setGain(int gain);
CameraCalibrationConfig getCurrentCalibrationData();
List<CameraCalibrationConfig> getAllCalibrationData();
}

40
chameleon-server/src/main/java/com/chameleonvision/vision/camera/CameraStreamer.java
View File

@@ -14,13 +14,14 @@ import org.opencv.imgproc.Imgproc;
public class CameraStreamer {
private final CameraCapture cameraCapture;
private final String name;
private StreamDivisor divisor = StreamDivisor.NONE;
private StreamDivisor divisor;
private CvSource cvSource;
private final Object streamBufferLock = new Object();
private Mat streamBuffer = new Mat();
private Size size;
public CameraStreamer(CameraCapture cameraCapture, String name) {
public CameraStreamer(CameraCapture cameraCapture, String name,StreamDivisor div) {
this.divisor = div;
this.cameraCapture = cameraCapture;
this.name = name;
this.cvSource = CameraServer.getInstance().putVideo(name,
@@ -35,28 +36,33 @@ public class CameraStreamer {
}
public void setDivisor(StreamDivisor newDivisor, boolean updateUI) {
if (divisor != newDivisor) {
this.divisor = newDivisor;
var camValues = cameraCapture.getProperties();
var newWidth = camValues.getStaticProperties().imageWidth / newDivisor.value;
var newHeight = camValues.getStaticProperties().imageHeight / newDivisor.value;
this.size = new Size(newWidth, newHeight);
synchronized (streamBufferLock) {
this.streamBuffer = new Mat(newWidth, newHeight, CvType.CV_8UC3);
this.cvSource = CameraServer.getInstance().putVideo(this.name,
cameraCapture.getProperties().getStaticProperties().imageWidth / divisor.value,
cameraCapture.getProperties().getStaticProperties().imageHeight / divisor.value);
}
if (updateUI) {
SocketHandler.sendFullSettings();
}
this.divisor = newDivisor;
var camValues = cameraCapture.getProperties();
var newWidth = camValues.getStaticProperties().imageWidth / newDivisor.value;
var newHeight = camValues.getStaticProperties().imageHeight / newDivisor.value;
this.size = new Size(newWidth, newHeight);
synchronized (streamBufferLock) {
this.streamBuffer = new Mat(newWidth, newHeight, CvType.CV_8UC3);
VideoMode oldVideoMode = cvSource.getVideoMode();
cvSource.setVideoMode(new VideoMode(oldVideoMode.pixelFormat,
cameraCapture.getProperties().getStaticProperties().imageWidth / divisor.value,
cameraCapture.getProperties().getStaticProperties().imageHeight / divisor.value,
oldVideoMode.fps));
}
if (updateUI) {
SocketHandler.sendFullSettings();
}
}
public StreamDivisor getDivisor() {
return divisor;
}
public void recalculateDivision() {
setDivisor(this.divisor, false);
}
public void setNewVideoMode(VideoMode newVideoMode) {
// Trick to update cvSource and streamBuffer to the new resolution
// Must change the cameraProcess resolution first

43
chameleon-server/src/main/java/com/chameleonvision/vision/camera/USBCameraCapture.java
View File

@@ -1,7 +1,7 @@
package com.chameleonvision.vision.camera;
import com.chameleonvision.config.CameraJsonConfig;
import com.chameleonvision.vision.image.CaptureProperties;
import com.chameleonvision.config.CameraCalibrationConfig;
import com.chameleonvision.config.FullCameraConfiguration;
import edu.wpi.cscore.CvSink;
import edu.wpi.cscore.UsbCamera;
import edu.wpi.cscore.VideoException;
@@ -9,14 +9,23 @@ import edu.wpi.cscore.VideoMode;
import edu.wpi.first.cameraserver.CameraServer;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Mat;
import org.opencv.core.Size;
import org.opencv.imgcodecs.Imgcodecs;
import java.util.ArrayList;
import java.util.List;
public class USBCameraCapture implements CameraCapture {
private final UsbCamera baseCamera;
private final CvSink cvSink;
private List<CameraCalibrationConfig> calibrationList;
private Mat imageBuffer = new Mat();
private USBCaptureProperties properties;
public USBCameraCapture(CameraJsonConfig config) {
public USBCameraCapture(FullCameraConfiguration fullCameraConfiguration) {
var config = fullCameraConfiguration.cameraConfig;
this.calibrationList = new ArrayList<>(); //fullCameraConfiguration.calibration;
calibrationList.addAll(fullCameraConfiguration.calibration);
baseCamera = new UsbCamera(config.name, config.path);
cvSink = CameraServer.getInstance().getVideo(baseCamera);
properties = new USBCaptureProperties(baseCamera, config);
@@ -25,6 +34,24 @@ public class USBCameraCapture implements CameraCapture {
setVideoMode(videoMode);
}
public CameraCalibrationConfig getCalibration(Size size) {
for(var calibration: calibrationList) {
if(calibration.resolution.equals(size)) return calibration;
}
return null;
}
public CameraCalibrationConfig getCalibration(VideoMode mode) {
return getCalibration(new Size(mode.width, mode.height));
}
public void addCalibrationData(CameraCalibrationConfig newConfig) {
calibrationList.add(newConfig);
}
public List<CameraCalibrationConfig> getConfig() {
return calibrationList;
}
@Override
public USBCaptureProperties getProperties() {
@@ -91,4 +118,14 @@ public class USBCameraCapture implements CameraCapture {
}
}
}
@Override
public CameraCalibrationConfig getCurrentCalibrationData() {
return getCalibration(getCurrentVideoMode());
}
@Override
public List<CameraCalibrationConfig> getAllCalibrationData() {
return calibrationList;
}
}

9
chameleon-server/src/main/java/com/chameleonvision/vision/camera/USBCaptureProperties.java
View File

@@ -5,6 +5,7 @@ import com.chameleonvision.util.Platform;
import com.chameleonvision.vision.image.CaptureProperties;
import edu.wpi.cscore.UsbCamera;
import edu.wpi.cscore.VideoMode;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import java.util.Arrays;
import java.util.List;
@@ -12,7 +13,7 @@ import java.util.function.Predicate;
import java.util.stream.Collectors;
import java.util.stream.Stream;
public class USBCaptureProperties extends com.chameleonvision.vision.image.CaptureProperties {
public class USBCaptureProperties extends CaptureProperties {
public static final double DEFAULT_FOV = 70;
private static final int DEFAULT_EXPOSURE = 50;
private static final int DEFAULT_BRIGHTNESS = 50;
@@ -100,10 +101,16 @@ public class USBCaptureProperties extends com.chameleonvision.vision.image.Captu
return videoModes;
}
public VideoMode getVideoMode(int index){
return videoModes.get(index);
}
public VideoMode getCurrentVideoMode() { return staticProperties.mode; }
public int getCurrentVideoModeIndex(){
return getVideoModes().indexOf(getCurrentVideoMode());
}
}

2
chameleon-server/src/main/java/com/chameleonvision/vision/enums/ImageRotationMode.java
View File

@@ -13,4 +13,6 @@ public enum ImageRotationMode {
ImageRotationMode(int value) {
this.value = value;
}
public boolean isRotated(){return this.value==DEG_90.value||this.value==DEG_270.value;}
}

12
chameleon-server/src/main/java/com/chameleonvision/vision/image/CaptureProperties.java
View File

@@ -2,11 +2,13 @@ package com.chameleonvision.vision.image;
import com.chameleonvision.vision.camera.CaptureStaticProperties;
import edu.wpi.cscore.VideoMode;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import org.opencv.core.Mat;
public class CaptureProperties {
protected CaptureStaticProperties staticProperties;
private Rotation2d tilt = new Rotation2d();
protected CaptureProperties() {
}
@@ -14,8 +16,16 @@ public class CaptureProperties {
public CaptureProperties(VideoMode videoMode, double fov) {
staticProperties = new CaptureStaticProperties(videoMode, fov);
}
public void setStaticProperties(CaptureStaticProperties staticProperties) {this.staticProperties = staticProperties;}
public CaptureStaticProperties getStaticProperties() {
return staticProperties;
}
public Rotation2d getTilt() {
return tilt;
}
public void setTilt(Rotation2d tilt) {
this.tilt = tilt;
}
}

12
chameleon-server/src/main/java/com/chameleonvision/vision/image/StaticImageCapture.java
View File

@@ -1,5 +1,6 @@
package com.chameleonvision.vision.image;
import com.chameleonvision.config.CameraCalibrationConfig;
import com.chameleonvision.vision.camera.CameraCapture;
import com.chameleonvision.vision.camera.USBCaptureProperties;
import edu.wpi.cscore.VideoMode;
@@ -9,6 +10,7 @@ import org.opencv.imgcodecs.Imgcodecs;
import java.nio.file.Files;
import java.nio.file.Path;
import java.util.List;
public class StaticImageCapture implements CameraCapture {
@@ -73,4 +75,14 @@ public class StaticImageCapture implements CameraCapture {
public void setGain(int gain) {
// do nothing
}
@Override
public CameraCalibrationConfig getCurrentCalibrationData() {
return null;
}
@Override
public List<CameraCalibrationConfig> getAllCalibrationData() {
return null;
}
}

6
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/CVPipeline.java
View File

@@ -9,7 +9,7 @@ import org.opencv.core.Mat;
*/
public abstract class CVPipeline<R extends CVPipelineResult, S extends CVPipelineSettings> {
protected Mat outputMat = new Mat();
CameraCapture cameraCapture;
protected CameraCapture cameraCapture;
public S settings;
protected CVPipeline(S settings) {
@@ -29,8 +29,4 @@ public abstract class CVPipeline<R extends CVPipelineResult, S extends CVPipelin
cameraCapture.setGain((int) settings.gain);
}
abstract public R runPipeline(Mat inputMat);
public boolean is3D() {
return (this instanceof CVPipeline3d);
}
}

35
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/CVPipeline3d.java
View File

@@ -1,35 +0,0 @@
package com.chameleonvision.vision.pipeline;
import org.opencv.core.Mat;
import java.util.List;
import static com.chameleonvision.vision.pipeline.CVPipeline3d.*;
public class CVPipeline3d extends CVPipeline<CVPipeline3dResult, CVPipeline3dSettings> {
protected CVPipeline3d(CVPipeline3dSettings settings) {
super(settings);
}
CVPipeline3d() {
super(new CVPipeline3dSettings());
}
@Override
public CVPipeline3dResult runPipeline(Mat inputMat) {
return null;
}
public static class CVPipeline3dResult extends CVPipelineResult<Target3d> {
public CVPipeline3dResult(List<Target3d> targets, Mat outputMat, long processTime) {
super(targets, outputMat, processTime);
}
}
public static class Target3d extends CVPipeline2d.Target2d {
// TODO: (2.1) Define 3d-specific target data
}
}

7
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/CVPipeline3dSettings.java
View File

@@ -1,7 +0,0 @@
package com.chameleonvision.vision.pipeline;
public class CVPipeline3dSettings extends CVPipeline2dSettings {
// TODO: (2.1) define 3d-specific pipeline settings
// add 3d-specific property to ensure serializing/deserializing works
public boolean placeholder = false;
}

2
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/Pipe.java → chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/Pipe.java
View File

@@ -1,4 +1,4 @@
package com.chameleonvision.vision.pipeline.pipes;
package com.chameleonvision.vision.pipeline;
import org.apache.commons.lang3.tuple.Pair;

84
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/PipelineManager.java
View File

@@ -1,10 +1,13 @@
package com.chameleonvision.vision.pipeline;
import com.chameleonvision.Exceptions.DuplicatedKeyException;
import com.chameleonvision.config.CameraConfig;
import com.chameleonvision.config.ConfigManager;
import com.chameleonvision.vision.VisionManager;
import com.chameleonvision.vision.VisionProcess;
import com.chameleonvision.vision.pipeline.impl.*;
import com.chameleonvision.web.SocketHandler;
import edu.wpi.cscore.VideoMode;
import edu.wpi.first.networktables.NetworkTableEntry;
import java.util.Comparator;
@@ -16,10 +19,12 @@ import java.util.List;
public class PipelineManager {
private static final int DRIVERMODE_INDEX = -1;
private static final int CAL_3D_INDEX = -2;
public final LinkedList<CVPipeline> pipelines = new LinkedList<>();
public final CVPipeline driverModePipeline = new DriverVisionPipeline(new CVPipelineSettings());
public final Calibrate3dPipeline calib3dPipe = new Calibrate3dPipeline(new StandardCVPipelineSettings());
private final VisionProcess parentProcess;
private int lastPipelineIndex;
@@ -29,7 +34,7 @@ public class PipelineManager {
public PipelineManager(VisionProcess visionProcess, List<CVPipelineSettings> loadedPipelineSettings) {
parentProcess = visionProcess;
if (loadedPipelineSettings == null || loadedPipelineSettings.size() == 0) {
pipelines.add(new CVPipeline2d("New Pipeline"));
pipelines.add(new StandardCVPipeline("New Pipeline"));
} else {
for (CVPipelineSettings setting : loadedPipelineSettings) {
addInternalPipeline(setting);
@@ -71,10 +76,8 @@ public class PipelineManager {
}
private void addInternalPipeline(CVPipelineSettings setting) {
if (setting instanceof CVPipeline3dSettings) {
pipelines.add(new CVPipeline3d((CVPipeline3dSettings) setting));
} else if (setting instanceof CVPipeline2dSettings) {
pipelines.add(new CVPipeline2d((CVPipeline2dSettings) setting));
if (setting instanceof StandardCVPipelineSettings) {
pipelines.add(new StandardCVPipeline((StandardCVPipelineSettings) setting));
} else {
System.out.println("Non 2D/3D pipelines not supported!");
}
@@ -82,11 +85,18 @@ public class PipelineManager {
}
public void setDriverMode(boolean driverMode) {
if (driverMode) {
setCurrentPipeline(DRIVERMODE_INDEX);
} else {
setCurrentPipeline(lastPipelineIndex);
}
if (driverMode) setCurrentPipeline(DRIVERMODE_INDEX);
else setCurrentPipeline(lastPipelineIndex);
}
public void setCalibrationMode(boolean calibrationMode) {
setCurrentPipeline((calibrationMode ? CAL_3D_INDEX : lastPipelineIndex));
}
public void enableCalibrationMode(VideoMode mode) {
parentProcess.setVideoMode(mode);
calib3dPipe.setVideoMode(mode);
setCalibrationMode(true);
}
public boolean getDriverMode() {
@@ -98,32 +108,45 @@ public class PipelineManager {
}
public CVPipeline getCurrentPipeline() {
if (currentPipelineIndex <= DRIVERMODE_INDEX) {
if (currentPipelineIndex == DRIVERMODE_INDEX) {
return driverModePipeline;
} else if (currentPipelineIndex <= CAL_3D_INDEX) {
return calib3dPipe;
} else {
return pipelines.get(currentPipelineIndex);
}
}
public void setCurrentPipeline(int index) {
CVPipeline newPipeline;
CVPipeline newPipeline=null;
if (index == DRIVERMODE_INDEX) {
newPipeline = driverModePipeline;
// if we're changing into driver mode, try to set the nt entry to frue
// if we're changing into driver mode, try to set the nt entry to true
parentProcess.setDriverModeEntry(true);
} else if (index == CAL_3D_INDEX) {
parentProcess.setDriverModeEntry(true);
} else {
newPipeline = pipelines.get(index);
// if we're switching out of driver mode, try to set the nt entry to false
parentProcess.setDriverModeEntry(false);
newPipeline = calib3dPipe;
} else {
if (index < pipelines.size()&&index>=0) {
newPipeline = pipelines.get(index);
// if we're switching out of driver mode, try to set the nt entry to false
parentProcess.setDriverModeEntry(false);
}
else
{
//TODO alert/warn user that pipeline doesnt exsits
System.err.println("Index is out of bounds");
}
}
if (newPipeline != null) {
lastPipelineIndex = currentPipelineIndex;
currentPipelineIndex = index;
getCurrentPipeline().initPipeline(parentProcess.getCamera());
if(ConfigManager.settings.currentCamera.equals(parentProcess.getCamera().getProperties().name)) {
if (ConfigManager.settings.currentCamera.equals(parentProcess.getCamera().getProperties().name)) {
ConfigManager.settings.currentPipeline = currentPipelineIndex;
HashMap<String, Object> pipeChange = new HashMap<>();
@@ -136,7 +159,9 @@ public class PipelineManager {
}
}
newPipeline.initPipeline(parentProcess.getCamera());
if(ntIndexEntry != null) {
if (parentProcess.cameraStreamer != null)
parentProcess.cameraStreamer.setDivisor(newPipeline.settings.streamDivisor, true);
if (ntIndexEntry != null) {
ntIndexEntry.setDouble(index);
}
}
@@ -153,13 +178,9 @@ public class PipelineManager {
savePipelineConfig(pipeline.settings);
}
public void addNewPipeline(boolean is3D) {
CVPipeline newPipeline;
if (!is3D) {
newPipeline = new CVPipeline2d();
} else {
newPipeline = new CVPipeline3d();
}
public void addNewPipeline(String piplineName) {
StandardCVPipeline newPipeline = new StandardCVPipeline();
newPipeline.settings.nickname = piplineName;
newPipeline.settings.index = pipelines.size();
addPipeline(newPipeline);
}
@@ -168,19 +189,22 @@ public class PipelineManager {
return pipelines.get(index);
}
public void duplicatePipeline(CVPipelineSettings pipeline) {
public void duplicatePipeline(CVPipelineSettings pipeline) throws DuplicatedKeyException {
duplicatePipeline(pipeline, parentProcess);
}
public void duplicatePipeline(CVPipelineSettings pipeline, VisionProcess destinationProcess) {
public void duplicatePipeline(CVPipelineSettings pipeline, VisionProcess destinationProcess) throws DuplicatedKeyException {
pipeline.index = destinationProcess.pipelineManager.pipelines.size();
pipeline.nickname += "(Copy)";
destinationProcess.pipelineManager.addPipeline(pipeline);
if (destinationProcess.pipelineManager.pipelines.stream().anyMatch(c -> c.settings.nickname.equals(pipeline.nickname))){
throw new DuplicatedKeyException("key Already exists");
} else{
destinationProcess.pipelineManager.addPipeline(pipeline);
}
}
public void renameCurrentPipeline(String newName) {
CVPipelineSettings settings = getCurrentPipeline().settings;
settings.nickname = newName;
renamePipelineConfig(settings, newName);
}

167
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/Calibrate3dPipeline.java Normal file
View File

@@ -0,0 +1,167 @@
package com.chameleonvision.vision.pipeline.impl;
import com.chameleonvision.config.CameraCalibrationConfig;
import com.chameleonvision.config.ConfigManager;
import com.chameleonvision.vision.VisionManager;
import com.chameleonvision.vision.camera.CameraCapture;
import com.chameleonvision.vision.pipeline.CVPipeline;
import com.fasterxml.jackson.core.JsonProcessingException;
import com.fasterxml.jackson.databind.ObjectMapper;
import edu.wpi.cscore.VideoMode;
import edu.wpi.first.wpilibj.util.Units;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.*;
import org.opencv.imgproc.Imgproc;
import java.util.ArrayList;
import java.util.List;
public class Calibrate3dPipeline extends CVPipeline<DriverVisionPipeline.DriverPipelineResult, StandardCVPipelineSettings> {
private int checkerboardSquaresHigh = 7;
private int checkerboardSquaresWide = 7;
private MatOfPoint3f objP_ORIG;
private MatOfPoint3f objP;// new MatOfPoint3f(checkerboardSquaresHigh + checkerboardSquaresWide, 3);//(checkerboardSquaresWide * checkerboardSquaresHigh, 3);
private Size patternSize = new Size(checkerboardSquaresHigh, checkerboardSquaresWide);
private Size imageSize;
double checkerboardSquareSize = 1; // inches!
private MatOfPoint2f calibrationOutput = new MatOfPoint2f();
private List<Mat> objpoints = new ArrayList<>();
private List<Mat> imgpoints = new ArrayList<>();
public static double checkerboardSquareSizeUnits = Units.inchesToMeters(1.0);
public static final int MIN_COUNT = 15;
private VideoMode calibrationMode;
private final Size windowSize = new Size(11, 11);
private final Size zeroZone = new Size(-1, -1);
private TermCriteria criteria = new TermCriteria(3, 30, 0.001); //(Imgproc.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
private int captureCount = 0;
private boolean wantsSnapshot = false;
private double squareSizeInches;
public Calibrate3dPipeline(StandardCVPipelineSettings settings) {
super(settings);
objP_ORIG = new MatOfPoint3f();
objP = new MatOfPoint3f();
for(int i = 0; i < checkerboardSquaresHigh * checkerboardSquaresWide; i++) {
objP_ORIG.push_back(new MatOfPoint3f(new Point3(i / checkerboardSquaresWide, i % checkerboardSquaresHigh, 0.0f)));
}
setSquareSize(checkerboardSquareSizeUnits);
objpoints.forEach(Mat::release);
imgpoints.forEach(Mat::release);
objpoints.clear();
imgpoints.clear();
}
public void setSquareSize(double size) {
this.squareSizeInches = size;
}
public void takeSnapshot() {
wantsSnapshot = true;
}
public boolean hasEnoughSnapshots() {
return captureCount >= MIN_COUNT - 1;
}
@Override
public DriverVisionPipeline.DriverPipelineResult runPipeline(Mat inputMat) {
// look for checkerboard
Imgproc.cvtColor(inputMat, inputMat, Imgproc.COLOR_BGR2GRAY);
var checkerboardFound = Calib3d.findChessboardCorners(inputMat, patternSize, calibrationOutput);
if(!checkerboardFound) {
Imgproc.cvtColor(inputMat, inputMat, Imgproc.COLOR_GRAY2BGR);
return new DriverVisionPipeline.DriverPipelineResult(null, inputMat, 0);
}
// System.out.println("[SolvePNP] checkerboard found!!");
// cool we found a checkerboard
// do corner subpixel
Imgproc.cornerSubPix(inputMat, calibrationOutput, windowSize, zeroZone, criteria);
// convert back to BGR
Imgproc.cvtColor(inputMat, inputMat, Imgproc.COLOR_GRAY2BGR);
// draw the chessboard
Calib3d.drawChessboardCorners(inputMat, patternSize, calibrationOutput, true);
if(wantsSnapshot) {
this.imageSize = new Size(inputMat.width(), inputMat.height());
var mat = new MatOfPoint3f();
calibrationOutput.copyTo(mat);
this.objpoints.add(objP);
imgpoints.add(mat);
captureCount++;
wantsSnapshot = false;
}
imageSize = new Size(inputMat.width(), inputMat.height());
return new DriverVisionPipeline.DriverPipelineResult(null, inputMat, 0);
}
@Override
public void initPipeline(CameraCapture camera) {
super.initPipeline(camera);
objpoints.clear();
imgpoints.clear();
captureCount = 0;
}
public boolean tryCalibration() {
if (!hasEnoughSnapshots()) return false;
Mat cameraMatrix = new Mat();
Mat distortionCoeffs = new Mat();
List<Mat> rvecs = new ArrayList<>();
List<Mat> tvecs = new ArrayList<>();
try {
Calib3d.calibrateCamera(objpoints, imgpoints, imageSize, cameraMatrix, distortionCoeffs, rvecs, tvecs);
} catch(Exception e) {
System.err.println("Camera calibration failed!");
initPipeline(cameraCapture);
return false;
}
VideoMode currentVidMode = cameraCapture.getCurrentVideoMode();
Size resolution = new Size(currentVidMode.width, currentVidMode.height);
CameraCalibrationConfig cal = new CameraCalibrationConfig(resolution, cameraMatrix, distortionCoeffs, squareSizeInches);
VisionManager.getCurrentUIVisionProcess().addCalibration(cal);
try {
System.out.printf("CALIBRATION SUCCESS! camMatrix: \n%s\ndistortionCoeffs:\n%s\n",
new ObjectMapper().writeValueAsString(cal.cameraMatrix), new ObjectMapper().writeValueAsString(cal.distortionCoeffs));
} catch (JsonProcessingException e) {
e.printStackTrace();
}
ConfigManager.saveGeneralSettings();
return true;
}
public void setVideoMode(VideoMode mode){
this.calibrationMode = mode;
}
public int getSnapshotCount() {
return captureCount + 1;
}
}

25
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/DriverVisionPipeline.java → chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/DriverVisionPipeline.java
View File

@@ -1,8 +1,12 @@
package com.chameleonvision.vision.pipeline;
package com.chameleonvision.vision.pipeline.impl;
import com.chameleonvision.util.MemoryManager;
import com.chameleonvision.vision.camera.CameraCapture;
import com.chameleonvision.vision.pipeline.pipes.Draw2dContoursPipe;
import com.chameleonvision.vision.enums.CalibrationMode;
import com.chameleonvision.vision.pipeline.CVPipeline;
import com.chameleonvision.vision.pipeline.CVPipelineResult;
import com.chameleonvision.vision.pipeline.CVPipelineSettings;
import com.chameleonvision.vision.pipeline.pipes.Draw2dCrosshairPipe;
import com.chameleonvision.vision.pipeline.pipes.RotateFlipPipe;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Mat;
@@ -10,14 +14,13 @@ import org.opencv.core.RotatedRect;
import java.util.List;
import static com.chameleonvision.vision.pipeline.DriverVisionPipeline.DriverPipelineResult;
import static com.chameleonvision.vision.pipeline.impl.DriverVisionPipeline.DriverPipelineResult;
public class DriverVisionPipeline extends CVPipeline<DriverPipelineResult, CVPipelineSettings> {
private RotateFlipPipe rotateFlipPipe;
private Draw2dContoursPipe draw2dContoursPipe;
private Draw2dContoursPipe.Draw2dContoursSettings draw2dContoursSettings = new Draw2dContoursPipe.Draw2dContoursSettings();
private final List<RotatedRect> blankList = List.of();
private Draw2dCrosshairPipe drawCrosshairPipe;
private Draw2dCrosshairPipe.Draw2dCrosshairPipeSettings crosshairPipeSettings = new Draw2dCrosshairPipe.Draw2dCrosshairPipeSettings();
private final MemoryManager memoryManager = new MemoryManager(200, 20000);
@@ -30,22 +33,20 @@ public class DriverVisionPipeline extends CVPipeline<DriverPipelineResult, CVPip
public void initPipeline(CameraCapture capture) {
super.initPipeline(capture);
rotateFlipPipe = new RotateFlipPipe(settings.rotationMode, settings.flipMode);
draw2dContoursSettings.showCrosshair = true;
draw2dContoursPipe = new Draw2dContoursPipe(draw2dContoursSettings, cameraCapture.getProperties().getStaticProperties());
crosshairPipeSettings.showCrosshair=true;
drawCrosshairPipe = new Draw2dCrosshairPipe(crosshairPipeSettings, CalibrationMode.None,null,0,0);
}
@Override
public DriverPipelineResult runPipeline(Mat inputMat) {
rotateFlipPipe.setConfig(settings.rotationMode, settings.flipMode);
draw2dContoursPipe.setConfig(false, cameraCapture.getProperties().getStaticProperties());
Pair<Mat, Long> rotateFlipResult = rotateFlipPipe.run(inputMat);
Pair<Mat, Long> draw2dContoursResult = draw2dContoursPipe.run(Pair.of(rotateFlipResult.getLeft(), blankList));
Pair<Mat, Long> draw2dCrosshairResult = drawCrosshairPipe.run(Pair.of(rotateFlipResult.getLeft(),null));
memoryManager.run();
return new DriverPipelineResult(null, draw2dContoursResult.getLeft(), 0);
return new DriverPipelineResult(null, draw2dCrosshairResult.getLeft(), 0);
}
public static class DriverPipelineResult extends CVPipelineResult<Void> {

127
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/CVPipeline2d.java → chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/StandardCVPipeline.java
View File

@@ -1,19 +1,22 @@
package com.chameleonvision.vision.pipeline;
package com.chameleonvision.vision.pipeline.impl;
import com.chameleonvision.Main;
import com.chameleonvision.util.MemoryManager;
import com.chameleonvision.vision.camera.CameraCapture;
import com.chameleonvision.vision.camera.CaptureStaticProperties;
import com.chameleonvision.vision.pipeline.CVPipeline;
import com.chameleonvision.vision.pipeline.CVPipelineResult;
import com.chameleonvision.vision.pipeline.pipes.*;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.*;
import java.util.List;
import static com.chameleonvision.vision.pipeline.CVPipeline2d.*;
import static com.chameleonvision.vision.pipeline.impl.StandardCVPipeline.*;
@SuppressWarnings("WeakerAccess")
public class CVPipeline2d extends CVPipeline<CVPipeline2dResult, CVPipeline2dSettings> {
public class StandardCVPipeline extends CVPipeline<StandardCVPipelineResult, StandardCVPipelineSettings> {
private Mat rawCameraMat = new Mat();
@@ -29,21 +32,25 @@ public class CVPipeline2d extends CVPipeline<CVPipeline2dResult, CVPipeline2dSet
private Collect2dTargetsPipe collect2dTargetsPipe;
private Draw2dContoursPipe.Draw2dContoursSettings draw2dContoursSettings;
private Draw2dContoursPipe draw2dContoursPipe;
private Draw2dCrosshairPipe draw2dCrosshairPipe;
private DrawSolvePNPPipe drawSolvePNPPipe;
private SolvePNPPipe solvePNPPipe;
private Draw2dCrosshairPipe.Draw2dCrosshairPipeSettings draw2dCrosshairPipeSettings;
private OutputMatPipe outputMatPipe;
private String pipelineTimeString = "";
private CaptureStaticProperties camProps;
private Scalar hsvLower, hsvUpper;
public CVPipeline2d() {
super(new CVPipeline2dSettings());
public StandardCVPipeline() {
super(new StandardCVPipelineSettings());
}
public CVPipeline2d(String name) {
super(name, new CVPipeline2dSettings());
public StandardCVPipeline(String name) {
super(name, new StandardCVPipelineSettings());
}
public CVPipeline2d(CVPipeline2dSettings settings) {
public StandardCVPipeline(StandardCVPipelineSettings settings) {
super(settings);
}
@@ -64,28 +71,33 @@ public class CVPipeline2d extends CVPipeline<CVPipeline2dResult, CVPipeline2dSet
speckleRejectPipe = new SpeckleRejectPipe(settings.speckle.doubleValue());
groupContoursPipe = new GroupContoursPipe(settings.targetGroup, settings.targetIntersection);
sortContoursPipe = new SortContoursPipe(settings.sortMode, camProps, 5);
collect2dTargetsPipe = new Collect2dTargetsPipe(settings.calibrationMode, settings.point,
settings.dualTargetCalibrationM, settings.dualTargetCalibrationB, camProps);
collect2dTargetsPipe = new Collect2dTargetsPipe(settings.calibrationMode,settings.point,settings.dualTargetCalibrationM,settings.dualTargetCalibrationB, camProps);
draw2dContoursSettings = new Draw2dContoursPipe.Draw2dContoursSettings();
draw2dCrosshairPipeSettings = new Draw2dCrosshairPipe.Draw2dCrosshairPipeSettings();
// TODO: make settable from UI? config?
draw2dContoursSettings.showCentroid = false;
draw2dContoursSettings.showCrosshair = true;
draw2dContoursSettings.boxOutlineSize = 2;
draw2dContoursSettings.showRotatedBox = true;
draw2dContoursSettings.showMaximumBox = true;
draw2dContoursSettings.showMultiple = settings.multiple;
draw2dCrosshairPipeSettings.showCrosshair=true;
draw2dContoursPipe = new Draw2dContoursPipe(draw2dContoursSettings, camProps);
draw2dCrosshairPipe=new Draw2dCrosshairPipe(draw2dCrosshairPipeSettings,settings.calibrationMode,settings.point,settings.dualTargetCalibrationM,settings.dualTargetCalibrationB);
outputMatPipe = new OutputMatPipe(settings.isBinary);
}
private final MemoryManager memManager = new MemoryManager(120, 20000);
private StandardCVPipelineResult resultCache = new StandardCVPipelineResult(List.of(), new Mat(), 0L);
@Override
public CVPipeline2dResult runPipeline(Mat inputMat) {
public StandardCVPipelineResult runPipeline(Mat inputMat) {
long totalPipelineTimeNanos = 0;
long pipelineStartTimeNanos = System.nanoTime();
resultCache.release();
if (cameraCapture == null) {
throw new RuntimeException("Pipeline was not initialized before being run!");
}
@@ -100,8 +112,6 @@ public class CVPipeline2d extends CVPipeline<CVPipeline2dResult, CVPipeline2dSet
pipelineTimeString = "";
inputMat.copyTo(rawCameraMat);
// prepare pipes
camProps = cameraCapture.getProperties().getStaticProperties();
hsvLower = new Scalar(settings.hue.get(0).intValue(), settings.saturation.get(0).intValue(), settings.value.get(0).intValue());
@@ -114,17 +124,28 @@ public class CVPipeline2d extends CVPipeline<CVPipeline2dResult, CVPipeline2dSet
speckleRejectPipe.setConfig(settings.speckle.doubleValue());
groupContoursPipe.setConfig(settings.targetGroup, settings.targetIntersection);
sortContoursPipe.setConfig(settings.sortMode, camProps, 5);
collect2dTargetsPipe.setConfig(settings.calibrationMode, settings.point,
settings.dualTargetCalibrationM, settings.dualTargetCalibrationB, camProps);
collect2dTargetsPipe = new Collect2dTargetsPipe(settings.calibrationMode,settings.point,settings.dualTargetCalibrationM,settings.dualTargetCalibrationB, camProps);
draw2dContoursPipe.setConfig(settings.multiple, camProps);
draw2dCrosshairPipe.setConfig(draw2dCrosshairPipeSettings,settings.calibrationMode,settings.point,settings.dualTargetCalibrationM,settings.dualTargetCalibrationB);
outputMatPipe.setConfig(settings.isBinary);
if(settings.is3D) {
if(solvePNPPipe == null) solvePNPPipe = new SolvePNPPipe(settings, cameraCapture.getCurrentCalibrationData(), cameraCapture.getProperties().getTilt());
if(drawSolvePNPPipe == null) drawSolvePNPPipe = new DrawSolvePNPPipe(cameraCapture.getCurrentCalibrationData());
solvePNPPipe.setConfig(settings, cameraCapture.getCurrentCalibrationData(), cameraCapture.getProperties().getTilt());
drawSolvePNPPipe.setConfig(cameraCapture.getCurrentCalibrationData());
}
long pipeInitTimeNanos = System.nanoTime() - pipelineStartTimeNanos;
// run pipes
Pair<Mat, Long> rotateFlipResult = rotateFlipPipe.run(inputMat);
totalPipelineTimeNanos += rotateFlipResult.getRight();
inputMat.copyTo(rawCameraMat);
// Pair<Mat, Long> blurResult = blurPipe.run(rotateFlipResult.getLeft());
// totalPipelineTimeNanos += blurResult.getRight();
@@ -143,22 +164,47 @@ public class CVPipeline2d extends CVPipeline<CVPipeline2dResult, CVPipeline2dSet
Pair<List<MatOfPoint>, Long> speckleRejectResult = speckleRejectPipe.run(filterContoursResult.getLeft());
totalPipelineTimeNanos += speckleRejectResult.getRight();
Pair<List<RotatedRect>, Long> groupContoursResult = groupContoursPipe.run(speckleRejectResult.getLeft());
Pair<List<TrackedTarget>, Long> groupContoursResult = groupContoursPipe.run(speckleRejectResult.getLeft());
totalPipelineTimeNanos += groupContoursResult.getRight();
Pair<List<RotatedRect>, Long> sortContoursResult = sortContoursPipe.run(groupContoursResult.getLeft());
Pair<List<TrackedTarget>, Long> sortContoursResult = sortContoursPipe.run(groupContoursResult.getLeft());
totalPipelineTimeNanos += sortContoursResult.getRight();
Pair<List<Target2d>, Long> collect2dTargetsResult = collect2dTargetsPipe.run(Pair.of(sortContoursResult.getLeft(), camProps));
Pair<List<TrackedTarget>, Long> collect2dTargetsResult = collect2dTargetsPipe.run(Pair.of(sortContoursResult.getLeft(), camProps));
totalPipelineTimeNanos += collect2dTargetsResult.getRight();
// takes pair of (Mat of original camera image (8UC3), Mat of HSV thresholded image(8UC1))
Pair<Mat, Long> outputMatResult = outputMatPipe.run(Pair.of(rotateFlipResult.getLeft(), hsvResult.getLeft()));
Pair<Mat, Long> outputMatResult = outputMatPipe.run(Pair.of(rawCameraMat, hsvResult.getLeft()));
totalPipelineTimeNanos += outputMatResult.getRight();
Pair<Mat, Long> result;
if(!settings.is3D) {
// takes pair of (Mat to draw on, List<RotatedRect> of sorted contours)
result = draw2dContoursPipe.run(Pair.of(outputMatResult.getLeft(), sortContoursResult.getLeft()));
totalPipelineTimeNanos += result.getRight();
} else {
result = outputMatResult;
}
// takes pair of (Mat to draw on, List<RotatedRect> of sorted contours)
Pair<Mat, Long> draw2dContoursResult = draw2dContoursPipe.run(Pair.of(outputMatResult.getLeft(), sortContoursResult.getLeft()));
totalPipelineTimeNanos += draw2dContoursResult.getRight();
Pair<Mat, Long> draw2dCrosshairResult = draw2dCrosshairPipe.run(Pair.of(result.getLeft(),collect2dTargetsResult.getLeft()));
totalPipelineTimeNanos += draw2dCrosshairResult.getRight();
Mat outputMat;
if(settings.is3D) {
// once we've sorted our targets, perform solvePNP. The number of "best targets" is limited by the above pipe
Pair<List<TrackedTarget>, Long> solvePNPResult = solvePNPPipe.run(collect2dTargetsResult.getLeft());
totalPipelineTimeNanos += solvePNPResult.getRight();
Pair<Mat, Long> draw3dContoursResult = drawSolvePNPPipe.run(Pair.of(outputMatResult.getLeft(), solvePNPResult.getLeft()));
totalPipelineTimeNanos += draw3dContoursResult.getRight();
outputMat = draw3dContoursResult.getLeft();
} else {
outputMat = draw2dCrosshairResult.getLeft();
}
if (Main.testMode) {
pipelineTimeString += String.format("PipeInit: %.2fms, ", pipeInitTimeNanos / 1000000.0);
@@ -173,7 +219,8 @@ public class CVPipeline2d extends CVPipeline<CVPipeline2dResult, CVPipeline2dSet
pipelineTimeString += String.format("SortContours: %.2fms, ", sortContoursResult.getRight() / 1000000.0);
pipelineTimeString += String.format("Collect2dTargets: %.2fms, ", collect2dTargetsResult.getRight() / 1000000.0);
pipelineTimeString += String.format("OutputMat: %.2fms, ", outputMatResult.getRight() / 1000000.0);
pipelineTimeString += String.format("Draw2dContours: %.2fms, ", draw2dContoursResult.getRight() / 1000000.0);
pipelineTimeString += String.format("Draw2dContours: %.2fms, ", result.getRight() / 1000000.0);
pipelineTimeString += String.format("Draw2dCrosshair: %.2fms, ", draw2dCrosshairResult.getRight() / 1000000.0);
System.out.println(pipelineTimeString);
double totalPipelineTimeMillis = totalPipelineTimeNanos / 1000000.0;
@@ -186,21 +233,43 @@ public class CVPipeline2d extends CVPipeline<CVPipeline2dResult, CVPipeline2dSet
memManager.run();
return new CVPipeline2dResult(collect2dTargetsResult.getLeft(), draw2dContoursResult.getLeft(), totalPipelineTimeNanos);
resultCache = new StandardCVPipelineResult(collect2dTargetsResult.getLeft(), outputMat, totalPipelineTimeNanos);
return resultCache;
}
public static class CVPipeline2dResult extends CVPipelineResult<Target2d> {
public CVPipeline2dResult(List<Target2d> targets, Mat outputMat, long processTimeNanos) {
public static class StandardCVPipelineResult extends CVPipelineResult<TrackedTarget> {
public StandardCVPipelineResult(List<TrackedTarget> targets, Mat outputMat, long processTimeNanos) {
super(targets, outputMat, processTimeNanos);
}
public void release() {
targets.forEach(TrackedTarget::release);
outputMat.release();
}
}
public static class Target2d {
public static class TrackedTarget {
public double calibratedX = 0.0;
public double calibratedY = 0.0;
public double pitch = 0.0;
public double yaw = 0.0;
public double area = 0.0;
public RotatedRect rawPoint;
public RotatedRect minAreaRect;
// 3d stuff
public Pose2d cameraRelativePose = new Pose2d();
public Mat rVector = new Mat();
public Mat tVector = new Mat();
public MatOfPoint2f imageCornerPoints = new MatOfPoint2f();
public Pair<Rect, Rect> leftRightDualTargetPair = null;
public Pair<RotatedRect, RotatedRect> leftRightRotatedRect = null;
public void release() {
rVector.release();
tVector.release();
imageCornerPoints.release();
}
}
}

16
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/CVPipeline2dSettings.java → chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/StandardCVPipelineSettings.java
View File

@@ -1,14 +1,21 @@
package com.chameleonvision.vision.pipeline;
package com.chameleonvision.vision.pipeline.impl;
import com.chameleonvision.vision.enums.CalibrationMode;
import com.chameleonvision.vision.enums.SortMode;
import com.chameleonvision.vision.enums.TargetGroup;
import com.chameleonvision.vision.enums.TargetIntersection;
import com.chameleonvision.vision.pipeline.CVPipelineSettings;
import com.fasterxml.jackson.annotation.JsonIgnore;
import edu.wpi.first.wpilibj.util.Units;
import org.opencv.core.Mat;
import org.opencv.core.MatOfDouble;
import org.opencv.core.Point;
import org.opencv.core.Point3;
import java.util.Arrays;
import java.util.List;
public class CVPipeline2dSettings extends CVPipelineSettings {
public class StandardCVPipelineSettings extends CVPipelineSettings {
public List<Number> hue = Arrays.asList(50, 180);
public List<Number> saturation = Arrays.asList(50, 255);
public List<Number> value = Arrays.asList(50, 255);
@@ -27,4 +34,9 @@ public class CVPipeline2dSettings extends CVPipelineSettings {
public CalibrationMode calibrationMode = CalibrationMode.None;
public double dualTargetCalibrationM = 1;
public double dualTargetCalibrationB = 0;
// 3d stuff
public double targetWidth = 15.5, targetHeight = 6.0;
public boolean is3D = false;
}

4
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/BlurPipe.java
View File

@@ -1,10 +1,8 @@
package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.vision.pipeline.Pipe;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.CvException;
import org.opencv.core.Mat;
import org.opencv.core.Size;
import org.opencv.imgproc.Imgproc;
public class BlurPipe implements Pipe<Mat, Mat> {

58
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/Collect2dTargetsPipe.java
View File

@@ -1,45 +1,38 @@
package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.vision.camera.CaptureStaticProperties;
import com.chameleonvision.vision.pipeline.CVPipeline2d;
import com.chameleonvision.vision.pipeline.Pipe;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipeline;
import com.chameleonvision.vision.enums.CalibrationMode;
import org.apache.commons.lang3.tuple.Pair;
import org.apache.commons.math3.util.FastMath;
import org.opencv.core.Mat;
import org.opencv.core.RotatedRect;
import java.util.ArrayList;
import java.util.List;
public class Collect2dTargetsPipe implements Pipe<Pair<List<RotatedRect>, CaptureStaticProperties>, List<CVPipeline2d.Target2d>> {
public class Collect2dTargetsPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTarget>, CaptureStaticProperties>, List<StandardCVPipeline.TrackedTarget>> {
private CalibrationMode calibrationMode;
private CaptureStaticProperties camProps;
private CalibrationMode calibrationMode;
private List<Number> calibrationPoint;
private double calibrationM, calibrationB;
private List<StandardCVPipeline.TrackedTarget> targets = new ArrayList<>();
private List<CVPipeline2d.Target2d> targets = new ArrayList<>();
public Collect2dTargetsPipe(CalibrationMode calibrationMode, List<Number> calibrationPoint,
double calibrationM, double calibrationB, CaptureStaticProperties camProps) {
this.calibrationMode = calibrationMode;
this.camProps = camProps;
this.calibrationPoint = calibrationPoint;
this.calibrationM = calibrationM;
this.calibrationB = calibrationB;
public Collect2dTargetsPipe(CalibrationMode calibrationMode, List<Number> calibrationPoint, double calibrationM, double calibrationB, CaptureStaticProperties camProps) {
setConfig(calibrationMode, calibrationPoint, calibrationM, calibrationB, camProps);
}
public void setConfig(CalibrationMode calibrationMode, List<Number> calibrationPoint,
double calibrationM, double calibrationB, CaptureStaticProperties camProps) {
public void setConfig(CalibrationMode calibrationMode, List<Number> calibrationPoint, double calibrationM, double calibrationB, CaptureStaticProperties camProps) {
this.calibrationMode = calibrationMode;
this.camProps = camProps;
this.calibrationPoint = calibrationPoint;
this.calibrationM = calibrationM;
this.calibrationB = calibrationB;
this.camProps = camProps;
}
@Override
public Pair<List<CVPipeline2d.Target2d>, Long> run(Pair<List<RotatedRect>, CaptureStaticProperties> inputPair) {
public Pair<List<StandardCVPipeline.TrackedTarget>, Long> run(Pair<List<StandardCVPipeline.TrackedTarget>, CaptureStaticProperties> inputPair) {
long processStartNanos = System.nanoTime();
targets.clear();
@@ -47,27 +40,30 @@ public class Collect2dTargetsPipe implements Pipe<Pair<List<RotatedRect>, Captur
var imageArea = inputPair.getRight().imageArea;
if (input.size() > 0) {
for (RotatedRect r : input) {
CVPipeline2d.Target2d t = new CVPipeline2d.Target2d();
t.rawPoint = r;
switch (calibrationMode) {
for (var t : input) {
switch (this.calibrationMode) {
case Single:
if(this.calibrationPoint.isEmpty())
{
this.calibrationPoint.add(camProps.centerX);
this.calibrationPoint.add(camProps.centerY);
}
t.calibratedX = this.calibrationPoint.get(0).doubleValue();
t.calibratedY = this.calibrationPoint.get(1).doubleValue();
break;
case None:
t.calibratedX = camProps.centerX;
t.calibratedY = camProps.centerY;
break;
case Single:
t.calibratedX = calibrationPoint.get(0).doubleValue();
t.calibratedY = calibrationPoint.get(1).doubleValue();
break;
case Dual:
t.calibratedX = (r.center.y - calibrationB) / calibrationM;
t.calibratedY = (r.center.x * calibrationM) + calibrationB;
t.calibratedX = (t.minAreaRect.center.y - this.calibrationB) / this.calibrationM;
t.calibratedY = (t.minAreaRect.center.x * this.calibrationM) + this.calibrationB;
break;
}
t.pitch = calculatePitch(r.center.y, t.calibratedY);
t.yaw = calculateYaw(r.center.x, t.calibratedX);
t.area = r.size.area() / imageArea;
t.pitch = calculatePitch(t.minAreaRect.center.y, t.calibratedY);
t.yaw = calculateYaw(t.minAreaRect.center.x, t.calibratedX);
t.area = t.minAreaRect.size.area() / imageArea;
targets.add(t);
}

24
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/Draw2dContoursPipe.java
View File

@@ -2,6 +2,8 @@ package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.vision.camera.CaptureStaticProperties;
import com.chameleonvision.util.Helpers;
import com.chameleonvision.vision.pipeline.Pipe;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipeline;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Point;
import org.opencv.core.*;
@@ -11,7 +13,7 @@ import java.awt.*;
import java.util.ArrayList;
import java.util.List;
public class Draw2dContoursPipe implements Pipe<Pair<Mat, List<RotatedRect>>, Mat> {
public class Draw2dContoursPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.TrackedTarget>>, Mat> {
private final Draw2dContoursSettings settings;
private CaptureStaticProperties camProps;
@@ -37,10 +39,10 @@ public class Draw2dContoursPipe implements Pipe<Pair<Mat, List<RotatedRect>>, Ma
}
@Override
public Pair<Mat, Long> run(Pair<Mat, List<RotatedRect>> input) {
public Pair<Mat, Long> run(Pair<Mat, List<StandardCVPipeline.TrackedTarget>> input) {
long processStartNanos = System.nanoTime();
if (settings.showCrosshair || settings.showCentroid || settings.showMaximumBox || settings.showRotatedBox) {
if (settings.showCentroid || settings.showMaximumBox || settings.showRotatedBox) {
// input.getLeft().copyTo(processBuffer);
// processBuffer = input.getLeft();
@@ -49,11 +51,13 @@ public class Draw2dContoursPipe implements Pipe<Pair<Mat, List<RotatedRect>>, Ma
if (i != 0 && !settings.showMultiple){
break;
}
RotatedRect r = input.getRight().get(i);
StandardCVPipeline.TrackedTarget target = input.getRight().get(i);
RotatedRect r = input.getRight().get(i).minAreaRect;
if (r == null) continue;
drawnContours.forEach(Mat::release);
drawnContours.clear();
drawnContours = new ArrayList<>();
r.points(vertices);
contour.fromArray(vertices);
@@ -77,34 +81,22 @@ public class Draw2dContoursPipe implements Pipe<Pair<Mat, List<RotatedRect>>, Ma
}
}
if (settings.showCrosshair) {
xMax.set(new double[] {camProps.centerX + 10, camProps.centerY});
xMin.set(new double[] {camProps.centerX - 10, camProps.centerY});
yMax.set(new double[] {camProps.centerX, camProps.centerY + 10});
yMin.set(new double[] {camProps.centerX, camProps.centerY - 10});
Imgproc.line(input.getLeft(), xMax, xMin, Helpers.colorToScalar(settings.crosshairColor), 2);
Imgproc.line(input.getLeft(), yMax, yMin, Helpers.colorToScalar(settings.crosshairColor), 2);
}
// processBuffer.copyTo(outputMat);
// processBuffer.release();
} else {
// input.getLeft().copyTo(outputMat);
}
long processTime = System.nanoTime() - processStartNanos;
return Pair.of(input.getLeft(), processTime);
}
public static class Draw2dContoursSettings {
public boolean showCentroid = false;
public boolean showCrosshair = false;
public boolean showMultiple = false;
public int boxOutlineSize = 0;
public boolean showRotatedBox = false;
public boolean showMaximumBox = false;
public Color centroidColor = Color.GREEN;
public Color crosshairColor = Color.GREEN;
public Color rotatedBoxColor = Color.BLUE;
public Color maximumBoxColor = Color.RED;
}

84
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/Draw2dCrosshairPipe.java Normal file
View File

@@ -0,0 +1,84 @@
package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.util.Helpers;
import com.chameleonvision.vision.enums.CalibrationMode;
import com.chameleonvision.vision.pipeline.Pipe;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipeline;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Mat;
import org.opencv.core.Point;
import org.opencv.imgproc.Imgproc;
import java.awt.*;
import java.util.List;
public class Draw2dCrosshairPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.TrackedTarget>>, Mat> {
//Settings
private Draw2dCrosshairPipeSettings crosshairSettings;
private CalibrationMode calibrationMode;
private List<Number> calibrationPoint;
private double calibrationM, calibrationB;
private Point xMax = new Point(), xMin = new Point(), yMax = new Point(), yMin = new Point();
public Draw2dCrosshairPipe(Draw2dCrosshairPipeSettings crosshairSettings, CalibrationMode calibrationMode, List<Number> calibrationPoint, double calibrationM, double calibrationB) {
setConfig(crosshairSettings, calibrationMode, calibrationPoint, calibrationM, calibrationB);
}
public void setConfig(Draw2dCrosshairPipeSettings crosshairSettings, CalibrationMode calibrationMode, List<Number> calibrationPoint, double calibrationM, double calibrationB) {
this.crosshairSettings = crosshairSettings;
this.calibrationMode = calibrationMode;
this.calibrationPoint = calibrationPoint;
this.calibrationM = calibrationM;
this.calibrationB = calibrationB;
}
@Override
public Pair<Mat, Long> run(Pair<Mat, List<StandardCVPipeline.TrackedTarget>> inputPair) {
long processStartNanos = System.nanoTime();
Mat image = inputPair.getLeft();
List<StandardCVPipeline.TrackedTarget> targets = inputPair.getRight();
double x = 0, y = 0, scale = image.cols() / 32.0;
drawCrosshair:
if (this.crosshairSettings.showCrosshair) {
x = image.cols() / 2;
y = image.rows() / 2;
switch (this.calibrationMode) {
case Single:
if(this.calibrationPoint.isEmpty())
{
this.calibrationPoint.add(x);
this.calibrationPoint.add(y);
}
x = this.calibrationPoint.get(0).intValue();
y = this.calibrationPoint.get(1).intValue();
break;
case Dual:
// if (targets != null && !targets.isEmpty()) {
// x = targets.get(0).calibratedX;
// y = targets.get(0).calibratedY;
// //TODO dual point calibration crosshair checks
// } else
// break drawCrosshair;
break;
}
xMax.set(new double[]{x + scale, y});
xMin.set(new double[]{x - scale, y});
yMax.set(new double[]{x, y + scale});
yMin.set(new double[]{x, y - scale});
Imgproc.line(inputPair.getLeft(), xMax, xMin, Helpers.colorToScalar(this.crosshairSettings.crosshairColor), 2);
Imgproc.line(inputPair.getLeft(), yMax, yMin, Helpers.colorToScalar(this.crosshairSettings.crosshairColor), 2);
}
long processTime = System.nanoTime() - processStartNanos;
return Pair.of(inputPair.getLeft(), processTime);
}
public static class Draw2dCrosshairPipeSettings {
public boolean showCrosshair = true;
public Color crosshairColor = Color.GREEN;
}
}

109
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/DrawSolvePNPPipe.java Normal file
View File

@@ -0,0 +1,109 @@
package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.config.CameraCalibrationConfig;
import com.chameleonvision.util.Helpers;
import com.chameleonvision.vision.pipeline.Pipe;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipeline;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.*;
import org.opencv.core.Point;
import org.opencv.imgproc.Imgproc;
import java.awt.*;
import java.util.List;
public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.TrackedTarget>>, Mat> {
private MatOfPoint3f boxCornerMat = new MatOfPoint3f();
public Scalar color = Helpers.colorToScalar(Color.GREEN);
public DrawSolvePNPPipe(CameraCalibrationConfig settings) {
setConfig(settings);
setObjectBox(14.5, 6, 2);
}
public void setObjectBox(double targetWidth, double targetHeight, double targetDepth) {
// implementation from 5190 Green Hope Falcons
boxCornerMat.release();
boxCornerMat = new MatOfPoint3f(
new Point3(-targetWidth/2d, -targetHeight/2d, 0),
new Point3(-targetWidth/2d, targetHeight/2d, 0),
new Point3(targetWidth/2d, targetHeight/2d, 0),
new Point3(targetWidth/2d, -targetHeight/2d, 0),
new Point3(-targetWidth/2d, -targetHeight/2d, -targetDepth),
new Point3(-targetWidth/2d, targetHeight/2d, -targetDepth),
new Point3(targetWidth/2d, targetHeight/2d, -targetDepth),
new Point3(targetWidth/2d, -targetHeight/2d, -targetDepth)
);
}
private Mat cameraMatrix = new Mat();
private MatOfDouble distortionCoefficients = new MatOfDouble();
public void setConfig(CameraCalibrationConfig config) {
if(config == null) {
System.err.println("got passed a null config! Returning...");
return;
}
setConfig(config.getCameraMatrixAsMat(), config.getDistortionCoeffsAsMat());
}
public void setConfig(Mat cameraMatrix_, MatOfDouble distortionMatrix_) {
this.cameraMatrix = cameraMatrix_;
this.distortionCoefficients = distortionMatrix_;
}
@Override
public Pair<Mat, Long> run(Pair<Mat, List<StandardCVPipeline.TrackedTarget>> targets) {
long processStartNanos = System.nanoTime();
var image = targets.getLeft();
for(var it : targets.getRight()) {
MatOfPoint2f imagePoints = new MatOfPoint2f();
try {
Calib3d.projectPoints(boxCornerMat, it.rVector, it.tVector, this.cameraMatrix, this.distortionCoefficients, imagePoints, new Mat() , 0);
} catch (Exception e) {
e.printStackTrace();
}
var pts = imagePoints.toList();
// draw left and right targets if possible
if(it.leftRightDualTargetPair != null) {
var left = it.leftRightDualTargetPair.getLeft();
var right = it.leftRightDualTargetPair.getRight();
Imgproc.rectangle(image, left.tl(), left.br(), new Scalar(200, 200, 0), 4);
Imgproc.rectangle(image, right.tl(), right.br(), new Scalar(200, 200, 0), 2);
}
// draw corners
for(int i = 0; i < it.imageCornerPoints.rows(); i++) {
var point = new Point(it.imageCornerPoints.get(i, 0));
Imgproc.circle(image, point, 4, new Scalar(0, 255, 0), 5);
}
// sketch out floor
Imgproc.line(image, pts.get(0), pts.get(1), new Scalar(0, 255, 0), 3);
Imgproc.line(image, pts.get(1), pts.get(2), new Scalar(0, 255, 0), 3);
Imgproc.line(image, pts.get(2), pts.get(3), new Scalar(0, 255, 0), 3);
Imgproc.line(image, pts.get(3), pts.get(0), new Scalar(0, 255, 0), 3);
// draw pillars
Imgproc.line(image, pts.get(0), pts.get(4), new Scalar(255, 0, 0), 3);
Imgproc.line(image, pts.get(1), pts.get(5), new Scalar(255, 0, 0), 3);
Imgproc.line(image, pts.get(2), pts.get(6), new Scalar(255, 0, 0), 3);
Imgproc.line(image, pts.get(3), pts.get(7), new Scalar(255, 0, 0), 3);
// draw top
Imgproc.line(image, pts.get(4), pts.get(5), new Scalar(0, 0, 255), 3);
Imgproc.line(image, pts.get(5), pts.get(6), new Scalar(0, 0, 255), 3);
Imgproc.line(image, pts.get(6), pts.get(7), new Scalar(0, 0, 255), 3);
Imgproc.line(image, pts.get(7), pts.get(4), new Scalar(0, 0, 255), 3);
}
long processTime = System.nanoTime() - processStartNanos;
return Pair.of(image, processTime);
}
}

4
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/ErodeDilatePipe.java
View File

@@ -1,5 +1,6 @@
package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.vision.pipeline.Pipe;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Mat;
import org.opencv.core.Size;
@@ -11,9 +12,6 @@ public class ErodeDilatePipe implements Pipe<Mat, Mat> {
private boolean dilate;
private Mat kernel;
private Mat processBuffer = new Mat();
private Mat outputMat = new Mat();
public ErodeDilatePipe(boolean erode, boolean dilate, int kernelSize) {
this.erode = erode;
this.dilate = dilate;

1
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/FilterContoursPipe.java
View File

@@ -2,6 +2,7 @@ package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.vision.camera.CaptureStaticProperties;
import com.chameleonvision.util.MathHandler;
import com.chameleonvision.vision.pipeline.Pipe;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.MatOfPoint;
import org.opencv.core.MatOfPoint2f;

1
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/FindContoursPipe.java
View File

@@ -1,5 +1,6 @@
package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.vision.pipeline.Pipe;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint;

40
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/GroupContoursPipe.java
View File

@@ -3,6 +3,8 @@ package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.util.MathHandler;
import com.chameleonvision.vision.enums.TargetGroup;
import com.chameleonvision.vision.enums.TargetIntersection;
import com.chameleonvision.vision.pipeline.Pipe;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipeline;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.*;
import org.opencv.imgproc.Imgproc;
@@ -13,7 +15,7 @@ import java.util.Collections;
import java.util.Comparator;
import java.util.List;
public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<RotatedRect>> {
public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<StandardCVPipeline.TrackedTarget>> {
private static final Comparator<MatOfPoint> sortByMomentsX =
Comparator.comparingDouble(GroupContoursPipe::calcMomentsX);
@@ -21,7 +23,7 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<RotatedRec
private TargetGroup group;
private TargetIntersection intersection;
private List<RotatedRect> groupedContours = new ArrayList<>();
private List<StandardCVPipeline.TrackedTarget> groupedContours = new ArrayList<>();
private MatOfPoint2f intersectMatA = new MatOfPoint2f();
private MatOfPoint2f intersectMatB = new MatOfPoint2f();
@@ -36,9 +38,10 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<RotatedRec
}
@Override
public Pair<List<RotatedRect>, Long> run(List<MatOfPoint> input) {
public Pair<List<StandardCVPipeline.TrackedTarget>, Long> run(List<MatOfPoint> input) {
long processStartNanos = System.nanoTime();
groupedContours.forEach(StandardCVPipeline.TrackedTarget::release);
groupedContours.clear();
if (input.size() > (group.equals(TargetGroup.Single) ? 0 : 1)) {
@@ -55,7 +58,9 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<RotatedRec
contour.fromArray(c.toArray());
if (contour.cols() != 0 && contour.rows() != 0) {
RotatedRect rect = Imgproc.minAreaRect(contour);
groupedContours.add(rect);
var target = new StandardCVPipeline.TrackedTarget();
target.minAreaRect = rect;
groupedContours.add(target);
}
});
break;
@@ -77,15 +82,34 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<RotatedRec
intersectMatA.release();
intersectMatB.release();
firstContour.release();
secondContour.release();
MatOfPoint2f contour = new MatOfPoint2f();
contour.fromList(finalContourList);
if (contour.cols() != 0 && contour.rows() != 0) {
RotatedRect rect = Imgproc.minAreaRect(contour);
groupedContours.add(rect);
var target = new StandardCVPipeline.TrackedTarget();
target.minAreaRect = rect;
target.leftRightDualTargetPair =
Pair.of(Imgproc.boundingRect(firstContour),
Imgproc.boundingRect(secondContour));
tempRectMat.fromArray(firstContour.toArray());
var minAreaRect1 = Imgproc.minAreaRect(tempRectMat);
tempRectMat.fromArray(secondContour.toArray());
var minAreaRect2 = Imgproc.minAreaRect(tempRectMat);
target.leftRightRotatedRect =
Pair.of(minAreaRect1, minAreaRect2);
groupedContours.add(target);
firstContour.release();
secondContour.release();
// skip the next contour because it's been grouped already
i += 1;
}
} catch (IndexOutOfBoundsException e) {
finalContourList.clear();
@@ -100,6 +124,8 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<RotatedRec
return Pair.of(groupedContours, processTime);
}
MatOfPoint2f tempRectMat = new MatOfPoint2f();
private static double calcMomentsX(MatOfPoint c) {
Moments m = Imgproc.moments(c);
return (m.get_m10() / m.get_m00());

1
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/HsvPipe.java
View File

@@ -1,5 +1,6 @@
package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.vision.pipeline.Pipe;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Core;
import org.opencv.core.CvException;

1
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/OutputMatPipe.java
View File

@@ -1,5 +1,6 @@
package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.vision.pipeline.Pipe;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.CvException;
import org.opencv.core.Mat;

1
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/RotateFlipPipe.java
View File

@@ -2,6 +2,7 @@ package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.vision.enums.ImageFlipMode;
import com.chameleonvision.vision.enums.ImageRotationMode;
import com.chameleonvision.vision.pipeline.Pipe;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Core;
import org.opencv.core.Mat;

365
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/SolvePNPPipe.java Normal file
View File

@@ -0,0 +1,365 @@
package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.config.CameraCalibrationConfig;
import com.chameleonvision.vision.pipeline.Pipe;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipeline;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipelineSettings;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import org.apache.commons.lang3.tuple.Pair;
import org.apache.commons.math3.util.FastMath;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.*;
import org.opencv.imgproc.Imgproc;
import java.lang.reflect.Array;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Comparator;
import java.util.List;
import java.util.stream.Collectors;
public class SolvePNPPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>, List<StandardCVPipeline.TrackedTarget>> {
private Double tilt_angle;
private MatOfPoint3f objPointsMat = new MatOfPoint3f();
private Mat rVec = new Mat();
private Mat tVec = new Mat();
private Mat rodriguez = new Mat();
private Mat pzero_world = new Mat();
private Mat cameraMatrix = new Mat();
Mat rot_inv = new Mat();
Mat kMat = new Mat();
private MatOfDouble distortionCoefficients = new MatOfDouble();
private List<StandardCVPipeline.TrackedTarget> poseList = new ArrayList<>();
Comparator<Point> leftRightComparator = Comparator.comparingDouble(point -> point.x);
Comparator<Point> verticalComparator = Comparator.comparingDouble(point -> point.y);
private double distanceDivisor = 1.0;
Mat scaledTvec = new Mat();
public SolvePNPPipe(StandardCVPipelineSettings settings, CameraCalibrationConfig calibration, Rotation2d tilt) {
super();
setCameraCoeffs(calibration);
setTarget(settings.targetWidth, settings.targetHeight);
this.tilt_angle = tilt.getRadians();
}
public void setTarget(double targetWidth, double targetHeight) {
// order is left top, left bottom, right bottom, right top
List<Point3> corners = List.of(
new Point3(-targetWidth / 2.0, targetHeight / 2.0, 0.0),
new Point3(-targetWidth / 2.0, -targetHeight / 2.0, 0.0),
new Point3(targetWidth / 2.0, -targetHeight / 2.0, 0.0),
new Point3(targetWidth / 2.0, targetHeight / 2.0, 0.0)
);
setObjectCorners(corners);
}
public void setObjectCorners(List<Point3> objectCorners) {
objPointsMat.release();
objPointsMat = new MatOfPoint3f();
objPointsMat.fromList(objectCorners);
}
public void setConfig(StandardCVPipelineSettings settings, CameraCalibrationConfig camConfig, Rotation2d tilt) {
setCameraCoeffs(camConfig);
setTarget(settings.targetWidth, settings.targetHeight);
tilt_angle = tilt.getRadians();
}
private void setCameraCoeffs(CameraCalibrationConfig settings) {
if(settings == null) {
System.err.println("SolvePNP can only run on a calibrated resolution, and this one is not! Please calibrate to use solvePNP.");
return;
}
if(cameraMatrix != settings.getCameraMatrixAsMat()) {
cameraMatrix.release();
settings.getCameraMatrixAsMat().copyTo(cameraMatrix);
}
if(distortionCoefficients != settings.getDistortionCoeffsAsMat()) {
distortionCoefficients.release();
settings.getDistortionCoeffsAsMat().copyTo(distortionCoefficients);
}
this.distanceDivisor = settings.squareSize;
}
@Override
public Pair<List<StandardCVPipeline.TrackedTarget>, Long> run(List<StandardCVPipeline.TrackedTarget> targets) {
long processStartNanos = System.nanoTime();
poseList.clear();
for(var target: targets) {
var corners = (target.leftRightDualTargetPair != null) ? findCorner2019(target) : findBoundingBoxCorners(target);
var pose = calculatePose(corners, target);
if(pose != null) poseList.add(pose);
}
long processTime = System.nanoTime() - processStartNanos;
return Pair.of(poseList, processTime);
}
private MatOfPoint2f findCorner2019(StandardCVPipeline.TrackedTarget target) {
if(target.leftRightDualTargetPair == null) return null;
var left = target.leftRightDualTargetPair.getLeft();
var right = target.leftRightDualTargetPair.getRight();
// flip if the "left" target is to the right
if(left.x > right.x) {
var temp = left;
left = right;
right = temp;
}
var points = new MatOfPoint2f();
points.fromArray(
new Point(left.x, left.y + left.height),
new Point(left.x, left.y),
new Point(right.x + right.width, right.y),
new Point(right.x + right.width, right.y + right.height)
);
return points;
}
private MatOfPoint2f findCornerMinAreaRect(StandardCVPipeline.TrackedTarget target) {
if(target.leftRightRotatedRect == null) return null;
var centroid = target.minAreaRect.center;
var left = target.leftRightRotatedRect.getLeft();
var right = target.leftRightRotatedRect.getRight();
// flip if the "left" target is to the right
if(left.center.x > right.center.x) {
var temp = left;
left = right;
right = temp;
}
var leftPoints = new Point[4];
left.points(leftPoints);
var rightPoints = new Point[4];
right.points(rightPoints);
ArrayList<Point> combinedList = new ArrayList<>(List.of(leftPoints));
combinedList.addAll(List.of(rightPoints));
// start looking in the top left quadrant
Comparator<Point> distanceProvider = Comparator.comparingDouble((Point point) -> FastMath.sqrt(FastMath.pow(centroid.x - point.x, 2) + FastMath.pow(centroid.y - point.y, 2)));
var tl = combinedList.stream().filter(point -> point.x < centroid.x && point.y < centroid.y).max(distanceProvider).get();
var tr = combinedList.stream().filter(point -> point.x > centroid.x && point.y < centroid.y).max(distanceProvider).get();
var bl = combinedList.stream().filter(point -> point.x < centroid.x && point.y > centroid.y).max(distanceProvider).get();
var br = combinedList.stream().filter(point -> point.x > centroid.x && point.y > centroid.y).max(distanceProvider).get();
boundingBoxResultMat.release();
boundingBoxResultMat.fromList(List.of(tl, bl, br, tr));
return boundingBoxResultMat;
}
private MatOfPoint2f findBoundingBoxCorners(StandardCVPipeline.TrackedTarget target) {
// List<Pair<MatOfPoint2f, CVPipeline2d.Target2d>> list = new ArrayList<>();
// // find the corners based on the bounding box
// // order is left top, left bottom, right bottom, right top
// extract the corners
var points = new Point[4];
target.minAreaRect.points(points);
// find the tl/tr/bl/br corners
// first, min by left/right
var list_ = Arrays.asList(points);
list_.sort(leftRightComparator);
// of this, we now have left and right
// sort to get top and bottom
var left = new ArrayList<>(List.of(list_.get(0), list_.get(1)));
left.sort(verticalComparator);
var right = new ArrayList<>(List.of(list_.get(2), list_.get(3)));
right.sort(verticalComparator);
// tl tr bl br
var tl = left.get(0);
var bl = left.get(1);
var tr = right.get(0);
var br = right.get(1);
boundingBoxResultMat.release();
boundingBoxResultMat.fromList(List.of(tl, bl, br, tr));
return boundingBoxResultMat;
}
MatOfPoint2f boundingBoxResultMat = new MatOfPoint2f();
MatOfPoint2f goodFeaturesResultMat = new MatOfPoint2f();
private Mat dstNorm = new Mat();
private Mat dstNormScaled = new Mat();
List<Point> tempCornerList = new ArrayList<>();
/**
* Find the corners in an image.
* @param targetImage the image to find corners in.
* @return the corners found in the image.
*/
@Deprecated
private List<Point> findCornerHarris(Mat targetImage) {
// convert the image to greyscale
var gray = new Mat();
Imgproc.cvtColor(targetImage, gray, Imgproc.COLOR_BGR2GRAY);
Mat dst = Mat.zeros(targetImage.size(), CvType.CV_8U);
// constants
final int blockSize = 2;
final int apertureSize = 3;
final double k = 0.04;
final int threshold = 200;
/// Detecting corners
Imgproc.cornerHarris(gray, dst, blockSize, apertureSize, k);
/// Normalizing
Core.normalize(dst, dstNorm, 0, 255, Core.NORM_MINMAX);
Core.convertScaleAbs(dstNorm, dstNormScaled);
/// Drawing a circle around corners
float[] dstNormData = new float[(int) (dstNorm.total() * dstNorm.channels())];
dstNorm.get(0, 0, dstNormData);
tempCornerList.clear();
for (int i = 0; i < dstNorm.rows(); i++) {
for (int j = 0; j < dstNorm.cols(); j++) {
if ((int) dstNormData[i * dstNorm.cols() + j] > threshold) {
tempCornerList.add(new Point(j, i));
}
}
}
return tempCornerList;
}
@Deprecated
private MatOfPoint2f findGoodFeaturesToTrack2019(StandardCVPipeline.TrackedTarget target, Mat srcImage) {
// Imgproc.approxPolyDP(new MatOfPoint2f(max_contour.toArray()),approx,epsilon,true);
// start by looking at the targets
var leftRight = target.leftRightDualTargetPair;
var reverse = (leftRight.getLeft().x < leftRight.getRight().x);
var left = reverse ? leftRight.getLeft() : leftRight.getRight();
var right = !reverse ? leftRight.getLeft() : leftRight.getRight();
var boundingTl = left.tl();
var boundingBr = right.br();
var slightlyBiggerTl = new Point(
Math.max(0, boundingTl.x - 5),
Math.max(0, boundingTl.y - 5)
);
var slightlyBiggerBr = new Point(
Math.min(srcImage.rows(), boundingBr.x + 5),
Math.min(srcImage.cols(), boundingBr.y + 5)
);
var rect = new Rect(slightlyBiggerTl, slightlyBiggerBr);
var croppedImage = srcImage.submat(rect);
var corners = new MatOfPoint();
Imgproc.goodFeaturesToTrack(croppedImage, corners, 8,0.5,5);
List<Point> cornerList = new ArrayList<>(corners.toList());
if(cornerList.size() != 8 && cornerList.size() != 4) return null;
cornerList.sort(leftRightComparator);
cornerList = cornerList.stream().map(point ->
new Point(point.x + slightlyBiggerTl.x, point.y + slightlyBiggerTl.y))
.collect(Collectors.toList());
// of these, we want the two leftmost and two rightmost points
var left1 = cornerList.get(0);
var left2 = cornerList.get(1);
var right1 = cornerList.get(0);
var right2 = cornerList.get(1);
// TODO maximize distance from the center rather than naively assume the leftmost and rightmost
// will have to do per quadrant
var leftOrder = left1.y < left2.y;
var rightOrder = right1.y < right2.y;
var tl = leftOrder ? left1 : left2;
var bl = !leftOrder ? left1 : left2;
var tr = rightOrder ? right1 : right2;
var br = !rightOrder ? right1 : right2;
goodFeaturesResultMat.release();
goodFeaturesResultMat.fromList(List.of(tl, bl, br, tr));
return goodFeaturesResultMat;
}
private StandardCVPipeline.TrackedTarget calculatePose(MatOfPoint2f imageCornerPoints, StandardCVPipeline.TrackedTarget target) {
if(objPointsMat.rows() != imageCornerPoints.rows() || cameraMatrix.rows() < 2 || distortionCoefficients.cols() < 4) {
System.err.println("can't do solvePNP with invalid params!");
return null;
}
imageCornerPoints.copyTo(target.imageCornerPoints);
try {
Calib3d.solvePnP(objPointsMat, imageCornerPoints, cameraMatrix, distortionCoefficients, rVec, tVec);
} catch (Exception e) {
e.printStackTrace();
return null;
}
// Algorithm from team 5190 Green Hope Falcons
// var tilt_angle = 0.0; // TODO add to settings
var x = tVec.get(0, 0)[0];
var z = FastMath.sin(tilt_angle) * tVec.get(1, 0)[0] + tVec.get(2, 0)[0] * FastMath.cos(tilt_angle);
// distance in the horizontal plane between camera and target
var distance = FastMath.sqrt(x * x + z * z);
// horizontal angle between center camera and target
@SuppressWarnings("SuspiciousNameCombination")
var angle1 = FastMath.atan2(x, z);
Calib3d.Rodrigues(rVec, rodriguez);
Core.transpose(rodriguez, rot_inv);
// This should be pzero_world = numpy.matmul(rot_inv, -tvec)
// pzero_world = rot_inv.mul(matScale(tVec, -1));
scaledTvec = matScale(tVec, -1);
Core.gemm(rot_inv, scaledTvec, 1, kMat, 0, pzero_world);
var angle2 = FastMath.atan2(pzero_world.get(0, 0)[0], pzero_world.get(2, 0)[0]);
var targetAngle = -angle1; // radians
var targetRotation = -angle2; // radians
var targetDistance = distance * 25.4 / 1000d / distanceDivisor; // This should be meters
var targetLocation = new Translation2d(targetDistance * FastMath.cos(targetAngle), targetDistance * FastMath.sin(targetAngle));
target.cameraRelativePose = new Pose2d(targetLocation, new Rotation2d(targetRotation));
target.rVector = rVec;
target.tVector = tVec;
return target;
}
/**
* Element-wise scale a matrix by a given factor
* @param src the source matrix
* @param factor by how much to scale each element
* @return the scaled matrix
*/
public Mat matScale(Mat src, double factor) {
Mat dst = new Mat(src.rows(),src.cols(),src.type());
Scalar s = new Scalar(factor); // TODO check if we need to add more elements to this
Core.multiply(src, s, dst);
return dst;
}
}

37
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/SortContoursPipe.java
View File

@@ -2,32 +2,34 @@ package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.vision.camera.CaptureStaticProperties;
import com.chameleonvision.vision.enums.SortMode;
import com.chameleonvision.vision.pipeline.Pipe;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipeline;
import org.apache.commons.lang3.tuple.Pair;
import org.apache.commons.math3.util.FastMath;
import org.opencv.core.RotatedRect;
import org.opencv.core.*;
import java.util.ArrayList;
import java.util.Comparator;
import java.util.List;
public class SortContoursPipe implements Pipe<List<RotatedRect>, List<RotatedRect>> {
public class SortContoursPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>, List<StandardCVPipeline.TrackedTarget>> {
private final Comparator<RotatedRect> SortByCentermostComparator = Comparator.comparingDouble(this::calcCenterDistance);
private final Comparator<StandardCVPipeline.TrackedTarget> SortByCentermostComparator = Comparator.comparingDouble(this::calcSquareCenterDistance);
private static final Comparator<RotatedRect> SortByLargestComparator = (rect1, rect2) -> Double.compare(rect2.size.area(), rect1.size.area());
private static final Comparator<RotatedRect> SortBySmallestComparator = SortByLargestComparator.reversed();
private static final Comparator<StandardCVPipeline.TrackedTarget> SortByLargestComparator = (rect1, rect2) -> Double.compare(rect2.minAreaRect.size.area(), rect1.minAreaRect.size.area());
private static final Comparator<StandardCVPipeline.TrackedTarget> SortBySmallestComparator = SortByLargestComparator.reversed();
private static final Comparator<RotatedRect> SortByHighestComparator = (rect1, rect2) -> Double.compare(rect2.center.y, rect1.center.y);
private static final Comparator<RotatedRect> SortByLowestComparator = SortByHighestComparator.reversed();
private static final Comparator<StandardCVPipeline.TrackedTarget> SortByHighestComparator = (rect1, rect2) -> Double.compare(rect1.minAreaRect.center.y, rect2.minAreaRect.center.y);
private static final Comparator<StandardCVPipeline.TrackedTarget> SortByLowestComparator = SortByHighestComparator.reversed();
private static final Comparator<RotatedRect> SortByLeftmostComparator = Comparator.comparingDouble(rect -> rect.center.x);
private static final Comparator<RotatedRect> SortByRightmostComparator = SortByLeftmostComparator.reversed();
public static final Comparator<StandardCVPipeline.TrackedTarget> SortByLeftmostComparator = Comparator.comparingDouble(target -> target.minAreaRect.center.x);
private static final Comparator<StandardCVPipeline.TrackedTarget> SortByRightmostComparator = SortByLeftmostComparator.reversed();
private SortMode sort;
private CaptureStaticProperties camProps;
private int maxTargets;
private List<RotatedRect> sortedContours = new ArrayList<>();
private List<StandardCVPipeline.TrackedTarget> sortedContours = new ArrayList<>();
public SortContoursPipe(SortMode sort, CaptureStaticProperties camProps, int maxTargets) {
this.sort = sort;
@@ -42,13 +44,13 @@ public class SortContoursPipe implements Pipe<List<RotatedRect>, List<RotatedRec
}
@Override
public Pair<List<RotatedRect>, Long> run(List<RotatedRect> input) {
public Pair<List<StandardCVPipeline.TrackedTarget>, Long> run(List<StandardCVPipeline.TrackedTarget> input) {
long processStartNanos = System.nanoTime();
sortedContours.clear();
if (input.size() > 0) {
sortedContours.addAll(input.subList(0, Math.min(input.size(), maxTargets - 1)));
sortedContours.addAll(input);
switch (sort) {
case Largest:
@@ -77,11 +79,16 @@ public class SortContoursPipe implements Pipe<List<RotatedRect>, List<RotatedRec
}
}
var sublistedContors = new ArrayList<>(sortedContours.subList(0, Math.min(input.size(), maxTargets - 1)));
sortedContours.subList(Math.min(input.size(), maxTargets - 1), sortedContours.size()).forEach(StandardCVPipeline.TrackedTarget::release);
sortedContours.clear();
sortedContours = new ArrayList<>();
long processTime = System.nanoTime() - processStartNanos;
return Pair.of(sortedContours, processTime);
return Pair.of(sublistedContors, processTime);
}
private double calcCenterDistance(RotatedRect rect) {
return FastMath.sqrt(FastMath.pow(camProps.centerX - rect.center.x, 2) + FastMath.pow(camProps.centerY - rect.center.y, 2));
private double calcSquareCenterDistance(StandardCVPipeline.TrackedTarget rect) {
return FastMath.sqrt(FastMath.pow(camProps.centerX - rect.minAreaRect.center.x, 2) + FastMath.pow(camProps.centerY - rect.minAreaRect.center.y, 2));
}
}

5
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/SpeckleRejectPipe.java
View File

@@ -1,7 +1,10 @@
package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.vision.pipeline.Pipe;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint;
import org.opencv.core.MatOfPoint2f;
import org.opencv.imgproc.Imgproc;
import java.util.ArrayList;
@@ -25,7 +28,9 @@ public class SpeckleRejectPipe implements Pipe<List<MatOfPoint>, List<MatOfPoint
public Pair<List<MatOfPoint>, Long> run(List<MatOfPoint> input) {
long processStartNanos = System.nanoTime();
despeckledContours.forEach(MatOfPoint::release);
despeckledContours.clear();
despeckledContours = new ArrayList<>();
if (input.size() > 0) {
double averageArea = 0.0;

131
chameleon-server/src/main/java/com/chameleonvision/web/RequestHandler.java
View File

@@ -1,21 +1,35 @@
package com.chameleonvision.web;
import com.chameleonvision.Exceptions.DuplicatedKeyException;
import com.chameleonvision.config.ConfigManager;
import com.chameleonvision.network.NetworkIPMode;
import com.chameleonvision.vision.VisionManager;
import com.chameleonvision.vision.VisionProcess;
import com.chameleonvision.vision.camera.CameraCapture;
import com.chameleonvision.vision.camera.USBCameraCapture;
import com.chameleonvision.vision.pipeline.CVPipelineSettings;
import com.chameleonvision.vision.pipeline.PipelineManager;
import com.chameleonvision.vision.pipeline.impl.Calibrate3dPipeline;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipeline;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipelineSettings;
import com.fasterxml.jackson.core.JsonProcessingException;
import com.fasterxml.jackson.databind.JsonMappingException;
import com.fasterxml.jackson.databind.ObjectMapper;
import edu.wpi.cscore.VideoMode;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import io.javalin.http.Context;
import io.javalin.http.Handler;
import java.io.IOException;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
public class RequestHandler {
private static final ObjectMapper kObjectMapper = new ObjectMapper();
public static void onGeneralSettings(Context ctx) {
ObjectMapper objectMapper = new ObjectMapper();
ObjectMapper objectMapper = kObjectMapper;
try {
Map map = objectMapper.readValue(ctx.body(), Map.class);
@@ -35,21 +49,67 @@ public class RequestHandler {
}
}
public static void onDuplicatePipeline(Context ctx) {
ObjectMapper objectMapper = kObjectMapper;
try {
Map data = objectMapper.readValue(ctx.body(), Map.class);
int cameraIndex = (Integer) data.getOrDefault("camera", -1);
var pipelineIndex = (Integer) data.get("pipeline");
StandardCVPipelineSettings origPipeline = (StandardCVPipelineSettings) VisionManager.getCurrentUIVisionProcess().pipelineManager.getPipeline(pipelineIndex).settings;
String tmp = objectMapper.writeValueAsString(origPipeline);
StandardCVPipelineSettings newPipeline = objectMapper.readValue(tmp, StandardCVPipelineSettings.class);
if (cameraIndex == -1) { // same camera
VisionManager.getCurrentUIVisionProcess().pipelineManager.duplicatePipeline(newPipeline);
} else { // another camera
var cam = VisionManager.getVisionProcessByIndex(cameraIndex);
if (cam != null) {
if (cam.getCamera().getProperties().videoModes.size() < newPipeline.videoModeIndex) {
newPipeline.videoModeIndex = cam.getCamera().getProperties().videoModes.size() - 1;
}
if (newPipeline.is3D){
var calibration = cam.getCamera().getCalibration(cam.getCamera().getProperties().getVideoMode(newPipeline.videoModeIndex));
if (calibration == null){
newPipeline.is3D = false;
}
}
VisionManager.getCurrentUIVisionProcess().pipelineManager.duplicatePipeline(newPipeline, cam);
ctx.status(200);
} else {
ctx.status(500);
}
}
} catch (JsonProcessingException | DuplicatedKeyException ex) {
ctx.status(500);
}
}
public static void onCameraSettings(Context ctx) {
ObjectMapper objectMapper = new ObjectMapper();
ObjectMapper objectMapper = kObjectMapper;
try {
Map camSettings = objectMapper.readValue(ctx.body(), Map.class);
VisionProcess currentVisionProcess = VisionManager.getCurrentUIVisionProcess();
USBCameraCapture currentCamera = currentVisionProcess.getCamera();
double newFOV;
double newFOV, tilt;
try {
newFOV = (Double) camSettings.get("fov");
} catch (Exception ignored) {
newFOV = (Integer) camSettings.get("fov");
}
try {
tilt = (Double) camSettings.get("tilt");
} catch (Exception ignored) {
tilt = (Integer) camSettings.get("tilt");
}
currentCamera.getProperties().setFOV(newFOV);
currentCamera.getProperties().setTilt(Rotation2d.fromDegrees(tilt));
VisionManager.saveCurrentCameraSettings();
SocketHandler.sendFullSettings();
ctx.status(200);
@@ -58,4 +118,67 @@ public class RequestHandler {
ctx.status(500);
}
}
public static void onCalibrationStart(Context ctx) throws JsonProcessingException {
PipelineManager pipeManager = VisionManager.getCurrentUIVisionProcess().pipelineManager;
ObjectMapper objectMapper = kObjectMapper;
var data = objectMapper.readValue(ctx.body(), Map.class);
int resolutionIndex = (Integer) data.get("resolution");
double squareSize;
try {
squareSize = (Double) data.get("squareSize");
} catch (Exception e) {
squareSize = (Integer) data.get("squareSize");
}
// convert from mm to meters
pipeManager.calib3dPipe.setSquareSize(squareSize);
VisionManager.getCurrentUIVisionProcess().pipelineManager.calib3dPipe.settings.videoModeIndex = resolutionIndex;
VisionManager.getCurrentUIVisionProcess().pipelineManager.setCalibrationMode(true);
VisionManager.getCurrentUIVisionProcess().getCamera().setVideoMode(resolutionIndex);
}
public static void onSnapshot(Context ctx) {
Calibrate3dPipeline calPipe = VisionManager.getCurrentUIVisionProcess().pipelineManager.calib3dPipe;
calPipe.takeSnapshot();
HashMap<String, Object> toSend = new HashMap<>();
toSend.put("snapshotCount", calPipe.getSnapshotCount());
toSend.put("hasEnough", calPipe.hasEnoughSnapshots());
ctx.json(toSend);
ctx.status(200);
}
public static void onCalibrationEnding(Context ctx) throws JsonProcessingException {
PipelineManager pipeManager = VisionManager.getCurrentUIVisionProcess().pipelineManager;
var data = kObjectMapper.readValue(ctx.body(), Map.class);
double squareSize;
try {
squareSize = (Double) data.get("squareSize");
} catch (Exception e) {
squareSize = (Integer) data.get("squareSize");
}
pipeManager.calib3dPipe.setSquareSize(squareSize);
System.out.println("Finishing Cal");
if (pipeManager.calib3dPipe.hasEnoughSnapshots()) {
if (pipeManager.calib3dPipe.tryCalibration()) {
ctx.status(200);
} else {
System.err.println("CALFAIL");
ctx.status(500);
}
}
pipeManager.setCalibrationMode(false);
ctx.status(200);
}
public static void onPnpModel(Context ctx) throws JsonProcessingException {
System.out.println(ctx.body());
ObjectMapper objectMapper = kObjectMapper;
List points = objectMapper.readValue(ctx.body(), List.class);
System.out.println(points);
}
}

5
chameleon-server/src/main/java/com/chameleonvision/web/Server.java
View File

@@ -30,6 +30,11 @@ public class Server {
});
app.post("/api/settings/general", RequestHandler::onGeneralSettings);
app.post("/api/settings/camera", RequestHandler::onCameraSettings);
app.post("/api/vision/duplicate", RequestHandler::onDuplicatePipeline);
app.post("/api/settings/startCalibration", RequestHandler::onCalibrationStart);
app.post("/api/settings/snapshot", RequestHandler::onSnapshot);
app.post("/api/settings/endCalibration", RequestHandler::onCalibrationEnding);
app.post("/api/vision/pnpModel", RequestHandler::onPnpModel);
app.start(port);
}
}

124
chameleon-server/src/main/java/com/chameleonvision/web/SocketHandler.java
View File

@@ -1,16 +1,21 @@
package com.chameleonvision.web;
import com.chameleonvision.config.CameraCalibrationConfig;
import com.chameleonvision.config.ConfigManager;
import com.chameleonvision.vision.VisionManager;
import com.chameleonvision.vision.VisionProcess;
import com.chameleonvision.vision.camera.CameraCapture;
import com.chameleonvision.vision.camera.CaptureStaticProperties;
import com.chameleonvision.vision.camera.USBCameraCapture;
import com.chameleonvision.vision.enums.ImageRotationMode;
import com.chameleonvision.vision.enums.StreamDivisor;
import com.chameleonvision.vision.pipeline.CVPipeline;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipeline;
import com.chameleonvision.vision.pipeline.CVPipelineSettings;
import com.fasterxml.jackson.core.JsonProcessingException;
import com.fasterxml.jackson.core.type.TypeReference;
import com.fasterxml.jackson.databind.ObjectMapper;
import edu.wpi.cscore.VideoMode;
import io.javalin.websocket.WsBinaryMessageContext;
import io.javalin.websocket.WsCloseContext;
import io.javalin.websocket.WsConnectContext;
@@ -20,10 +25,9 @@ import org.msgpack.jackson.dataformat.MessagePackFactory;
import java.lang.reflect.Field;
import java.nio.ByteBuffer;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.*;
import java.util.List;
import java.util.Map;
import java.util.stream.Collectors;
public class SocketHandler {
@@ -31,6 +35,8 @@ public class SocketHandler {
private static List<WsContext> users;
private static ObjectMapper objectMapper;
private static final Object broadcastLock = new Object();
SocketHandler() {
users = new ArrayList<>();
objectMapper = new ObjectMapper(new MessagePackFactory());
@@ -54,7 +60,7 @@ public class SocketHandler {
VisionProcess currentProcess = VisionManager.getCurrentUIVisionProcess();
CameraCapture currentCamera = currentProcess.getCamera();
CVPipeline currentPipeline = currentProcess.pipelineManager.getCurrentPipeline();
// System.out.println("entry.getKey()+entry.getValue()= " + entry.getKey() + entry.getValue());
switch (entry.getKey()) {
case "driverMode": {
HashMap<String, Object> data = (HashMap<String, Object>) entry.getValue();
@@ -77,39 +83,17 @@ public class SocketHandler {
VisionManager.saveCurrentCameraPipelines();
break;
}
case "duplicatePipeline": {
HashMap pipelineVals = (HashMap) entry.getValue();
int pipelineIndex = (int) pipelineVals.get("pipeline");
int cameraIndex = (int) pipelineVals.get("camera");
ObjectMapper mapper = new ObjectMapper();
CVPipelineSettings origPipeline = currentProcess.pipelineManager.getPipeline(pipelineIndex).settings;
String val = mapper.writeValueAsString(origPipeline);
CVPipelineSettings newPipeline = mapper.readValue(val, origPipeline.getClass());
if (cameraIndex != -1) {
VisionProcess newProcess = VisionManager.getVisionProcessByIndex(cameraIndex);
if (newProcess != null) {
currentProcess.pipelineManager.duplicatePipeline(newPipeline, newProcess);
} else {
System.err.println("Failed to get destination camera for pipeline duplication!");
}
} else {
currentProcess.pipelineManager.duplicatePipeline(newPipeline);
}
VisionManager.saveCurrentCameraPipelines();
case "addNewPipeline": {
// HashMap<String, Object> data = (HashMap<String, Object>) entry.getValue();
String pipeName = (String) entry.getValue();
// TODO: add to UI selection for new 2d/3d
currentProcess.pipelineManager.addNewPipeline(pipeName);
sendFullSettings();
VisionManager.saveCurrentCameraPipelines();
break;
}
case "command": {
switch ((String) entry.getValue()) {
case "addNewPipeline":
// TODO: add to UI selection for new 2d/3d
boolean is3d = false;
currentProcess.pipelineManager.addNewPipeline(is3d);
sendFullSettings();
VisionManager.saveCurrentCameraPipelines();
break;
case "deleteCurrentPipeline":
currentProcess.pipelineManager.deleteCurrentPipeline();
sendFullSettings();
@@ -129,20 +113,54 @@ public class SocketHandler {
sendFullSettings();
break;
}
case "is3D": {
VisionManager.getCurrentUIVisionProcess().setIs3d((Boolean) entry.getValue());
break;
}
case "currentPipeline": {
currentProcess.pipelineManager.setCurrentPipeline((Integer) entry.getValue());
sendFullSettings();
break;
}
case "isPNPCalibration": {
currentProcess.pipelineManager.setCalibrationMode((Boolean) entry.getValue());
break;
}
case "takeCalibrationSnapshot": {
currentProcess.pipelineManager.calib3dPipe.takeSnapshot();
}
default: {
// only change settings when we aren't in driver mode
if(currentProcess.pipelineManager.getDriverMode()) {
switch (entry.getKey()) {//Pre field value set
case "rotationMode": {//Create new CaptureStaticProperties with new width and height, reset crosshair calib
ImageRotationMode oldRot = currentPipeline.settings.rotationMode;
ImageRotationMode newRot = ImageRotationMode.class.getEnumConstants()[(Integer) entry.getValue()];
CaptureStaticProperties prop = currentCamera.getProperties().getStaticProperties();
int width, height;
if (oldRot.isRotated() != newRot.isRotated()) {
width = prop.mode.height;
height = prop.mode.width;
//Creates new video mode with new width and height to create new CaptureStaticProperties and applies it
currentCamera.getProperties().setStaticProperties(new CaptureStaticProperties(
new VideoMode(prop.mode.pixelFormat, width, height, prop.mode.fps), prop.fov));
}
prop = currentCamera.getProperties().getStaticProperties();
currentProcess.cameraStreamer.recalculateDivision();
if (currentPipeline instanceof StandardCVPipeline)
((StandardCVPipeline) currentPipeline).settings.point = Arrays.asList(prop.mode.width / 2, prop.mode.height / 2);//Reset Crosshair in single point calib
break;
}
}
if (currentProcess.pipelineManager.getDriverMode()) {
setField(currentProcess.pipelineManager.driverModePipeline.settings, entry.getKey(), entry.getValue());
} else {
setField(currentPipeline.settings, entry.getKey(), entry.getValue());
}
//Post field value set
switch (entry.getKey()) {
case "exposure": {
currentCamera.setExposure((Integer) entry.getValue());
@@ -152,11 +170,14 @@ public class SocketHandler {
currentCamera.setBrightness((Integer) entry.getValue());
break;
}
case "videoModeIndex":{
case "videoModeIndex": {
if (currentPipeline instanceof StandardCVPipeline)
((StandardCVPipeline) currentPipeline).settings.point = new ArrayList<>();//This will reset the calibration
currentCamera.setVideoMode((Integer) entry.getValue());
currentProcess.cameraStreamer.recalculateDivision();
break;
}
case "streamDivisor":{
case "streamDivisor": {
currentProcess.cameraStreamer.setDivisor(StreamDivisor.values()[(Integer) entry.getValue()], true);
break;
}
@@ -186,18 +207,22 @@ public class SocketHandler {
}
private static void broadcastMessage(Object obj, WsContext userToSkip) {
if (users != null)
for (WsContext user : users) {
if (userToSkip != null && user.getSessionId().equals(userToSkip.getSessionId())) {
continue;
}
try {
ByteBuffer b = ByteBuffer.wrap(objectMapper.writeValueAsBytes(obj));
user.send(b);
} catch (JsonProcessingException e) {
e.printStackTrace();
synchronized (broadcastLock) {
if (users != null) {
var userList = users;
for (WsContext user : userList) {
if (userToSkip != null && user.getSessionId().equals(userToSkip.getSessionId())) {
continue;
}
try {
ByteBuffer b = ByteBuffer.wrap(objectMapper.writeValueAsBytes(obj));
user.send(b);
} catch (JsonProcessingException e) {
e.printStackTrace();
}
}
}
}
}
public static void broadcastMessage(Object obj) {
@@ -236,9 +261,16 @@ public class SocketHandler {
HashMap<String, Object> tmp = new HashMap<>();
VisionProcess currentVisionProcess = VisionManager.getCurrentUIVisionProcess();
USBCameraCapture currentCamera = VisionManager.getCurrentUIVisionProcess().getCamera();
tmp.put("fov", currentCamera.getProperties().getFOV());
tmp.put("streamDivisor", currentVisionProcess.cameraStreamer.getDivisor().ordinal());
tmp.put("resolution", currentVisionProcess.getCamera().getProperties().getCurrentVideoModeIndex());
tmp.put("tilt", currentVisionProcess.getCamera().getProperties().getTilt().getDegrees());
List<CameraCalibrationConfig.UICameraCalibrationConfig> calibrations = currentCamera.getAllCalibrationData().stream()
.map(CameraCalibrationConfig.UICameraCalibrationConfig::new).collect(Collectors.toList());
tmp.put("calibration", calibrations);
return tmp;
}

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

@@ -0,0 +1,271 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.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;
import com.fasterxml.jackson.databind.DeserializationContext;
import com.fasterxml.jackson.databind.JsonNode;
import com.fasterxml.jackson.databind.SerializerProvider;
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;
/**
* 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;
/**
* 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;
}
/**
* 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);
}
/**
* 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 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()));
}
/**
* 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;
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));
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;
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;
}
Translation2d translationPart = transform.getTranslation().rotateBy(
new Rotation2d(halfThetaByTanOfHalfDtheta, -halfDtheta)
).times(Math.hypot(halfThetaByTanOfHalfDtheta, halfDtheta));
return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta);
}
@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;
}
@Override
public int hashCode() {
return Objects.hash(m_translation, m_rotation);
}
static class PoseSerializer extends StdSerializer<Pose2d> {
PoseSerializer() {
super(Pose2d.class);
}
@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();
}
}
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);
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);
}
}
}

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

@@ -0,0 +1,251 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.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;
import com.fasterxml.jackson.databind.DeserializationContext;
import com.fasterxml.jackson.databind.JsonNode;
import com.fasterxml.jackson.databind.SerializerProvider;
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;
/**
* 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;
/**
* 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 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));
}
/**
* 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());
}
/**
* 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);
}
/**
* 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 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 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;
}
@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;
}
@Override
public int hashCode() {
return Objects.hash(m_value);
}
static class RotationSerializer extends StdSerializer<Rotation2d> {
RotationSerializer() {
super(Rotation2d.class);
}
@Override
public void serialize(
Rotation2d value, JsonGenerator jgen, SerializerProvider provider)
throws IOException, JsonProcessingException {
jgen.writeStartObject();
jgen.writeNumberField("radians", value.m_value);
jgen.writeEndObject();
}
}
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();
return new Rotation2d(radians);
}
}
}

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

@@ -0,0 +1,106 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.geometry;
import java.util.Objects;
/**
* Represents a transformation for a Pose2d.
*/
public class Transform2d {
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());
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 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));
}
/**
* 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;
}
@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;
}
@Override
public int hashCode() {
return Objects.hash(m_translation, m_rotation);
}
}

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

@@ -0,0 +1,243 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.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;
import com.fasterxml.jackson.databind.DeserializationContext;
import com.fasterxml.jackson.databind.JsonNode;
import com.fasterxml.jackson.databind.SerializerProvider;
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;
/**
* 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;
/**
* 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;
}
/**
* 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 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);
}
/**
* 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);
}
/**
* 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);
}
/**
* 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);
}
@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());
}
/**
* 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);
}
static class TranslationSerializer extends StdSerializer<Translation2d> {
TranslationSerializer() {
super(Translation2d.class);
}
@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();
}
}
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();
return new Translation2d(xval, yval);
}
}
}

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

@@ -0,0 +1,76 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.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.
*/
@SuppressWarnings("MemberName")
public class Twist2d {
/**
* Linear "dx" component.
*/
public double dx;
/**
* Linear "dy" component.
*/
public double dy;
/**
* Angular "dtheta" component (radians).
*/
public double dtheta;
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;
}
@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;
}
@Override
public int hashCode() {
return Objects.hash(dx, dy, dtheta);
}
}

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

@@ -0,0 +1,104 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.util;
/**
* 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;
/**
* 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 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 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 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 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;
}
}