diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/configuration/ConfigManager.java b/chameleon-server/src/main/java/com/chameleonvision/common/configuration/ConfigManager.java index b89b8ab9e..03777de33 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/configuration/ConfigManager.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/configuration/ConfigManager.java @@ -5,7 +5,6 @@ public class ConfigManager { private final ConfigFolder rootFolder; protected ConfigManager() { - rootFolder = new ConfigFolder(""); } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/DataConsumer.java b/chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/DataConsumer.java index 13a8c08e9..c64c30457 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/DataConsumer.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/DataConsumer.java @@ -1,3 +1,6 @@ package com.chameleonvision.common.datatransfer; -public interface DataConsumer {} +import com.chameleonvision.common.vision.processes.Data; +import java.util.function.Consumer; + +public interface DataConsumer extends Consumer {} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/networktables/NetworkTablesManager.java b/chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/networktables/NetworkTablesManager.java index e7e79c65b..b58c3e95d 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/networktables/NetworkTablesManager.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/networktables/NetworkTablesManager.java @@ -1,19 +1,19 @@ package com.chameleonvision.common.datatransfer.networktables; +import com.chameleonvision.common.logging.LogGroup; +import com.chameleonvision.common.logging.Logger; import com.chameleonvision.common.scripting.ScriptEventType; import com.chameleonvision.common.scripting.ScriptManager; import edu.wpi.first.networktables.LogMessage; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import java.util.function.Consumer; -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; public class NetworkTablesManager { private NetworkTablesManager() {} - private static final Logger logger = LoggerFactory.getLogger(NetworkTablesManager.class); + private static final Logger logger = new Logger(NetworkTablesManager.class, LogGroup.General); private static final NetworkTableInstance ntInstance = NetworkTableInstance.getDefault(); diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/logging/Level.java b/chameleon-server/src/main/java/com/chameleonvision/common/logging/Level.java new file mode 100644 index 000000000..8883dcd54 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/logging/Level.java @@ -0,0 +1,19 @@ +package com.chameleonvision.common.logging; + +public enum Level { + OFF(0, Logger.ANSI_BLACK), + ERROR(1, Logger.ANSI_RED), + WARN(2, Logger.ANSI_YELLOW), + INFO(3, Logger.ANSI_GREEN), + DEBUG(4, Logger.ANSI_WHITE), + TRACE(5, Logger.ANSI_CYAN), + DE_PEST(6, Logger.ANSI_WHITE); + + public final String colorCode; + public final int code; + + Level(int code, String colorCode) { + this.code = code; + this.colorCode = colorCode; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/logging/LogGroup.java b/chameleon-server/src/main/java/com/chameleonvision/common/logging/LogGroup.java new file mode 100644 index 000000000..2b9df0acc --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/logging/LogGroup.java @@ -0,0 +1,8 @@ +package com.chameleonvision.common.logging; + +public enum LogGroup { + Camera, + Server, + VisionProcess, + General +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/logging/Logger.java b/chameleon-server/src/main/java/com/chameleonvision/common/logging/Logger.java new file mode 100644 index 000000000..0a18ee1b9 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/logging/Logger.java @@ -0,0 +1,157 @@ +package com.chameleonvision.common.logging; + +import java.io.IOException; +import java.nio.ByteBuffer; +import java.nio.channels.AsynchronousFileChannel; +import java.nio.file.Path; +import java.nio.file.StandardOpenOption; +import java.text.SimpleDateFormat; +import java.util.ArrayList; +import java.util.Date; +import java.util.HashMap; +import java.util.List; + +public class Logger { + + private final String className; + + public static final String ANSI_RESET = "\u001B[0m"; + public static final String ANSI_BLACK = "\u001B[30m"; + public static final String ANSI_RED = "\u001B[31m"; + public static final String ANSI_GREEN = "\u001B[32m"; + public static final String ANSI_YELLOW = "\u001B[33m"; + public static final String ANSI_BLUE = "\u001B[34m"; + public static final String ANSI_PURPLE = "\u001B[35m"; + public static final String ANSI_CYAN = "\u001B[36m"; + public static final String ANSI_WHITE = "\u001B[37m"; + + private static SimpleDateFormat simpleDateFormat = new SimpleDateFormat("yyyy-MM-dd HH:mm:ss"); + private final LogGroup group; + + public Logger(Class clazz, LogGroup group) { + this.className = clazz.getSimpleName(); + this.group = group; + } + + public static String getDate() { + return simpleDateFormat.format(new Date()); + } + + public static String format( + String logMessage, Level level, LogGroup group, String clazz, boolean color) { + var date = getDate(); + var builder = new StringBuilder(); + if (color) builder.append(level.colorCode); + builder + .append("[") + .append(date) + .append("] [") + .append(group) + .append(" - ") + .append(clazz) + .append("] [") + .append(level.name()) + .append("] ") + .append(logMessage); + if (color) builder.append(ANSI_RESET); + return builder.toString(); + } + + private static HashMap levelMap = new HashMap<>(); + private static List currentAppenders = new ArrayList<>(); + + static { + levelMap.put(LogGroup.Camera, Level.INFO); + levelMap.put(LogGroup.General, Level.INFO); + levelMap.put(LogGroup.Server, Level.INFO); + levelMap.put(LogGroup.VisionProcess, Level.INFO); + } + + static { + currentAppenders.add(new ConsoleAppender()); + } + + public static void addFileAppender(Path logFilePath) { + var file = logFilePath.toFile(); + if (!file.exists()) { + try { + file.getParentFile().mkdirs(); + file.createNewFile(); + } catch (IOException e) { + e.printStackTrace(); + } + } + currentAppenders.add(new AsyncFileAppender(logFilePath)); + } + + public static void setLevel(LogGroup group, Level newLevel) { + levelMap.put(group, newLevel); + } + + private static void log(String message, Level level, LogGroup group, String clazz) { + for (var a : currentAppenders) { + var shouldColor = a instanceof ConsoleAppender; + var formattedMessage = format(message, level, group, clazz, shouldColor); + a.log(formattedMessage); + } + } + + private static boolean shouldLog(Level logLevel, LogGroup group) { + return logLevel.code <= levelMap.get(group).code; + } + + public void error(String message) { + if (shouldLog(Level.ERROR, group)) log(message, Level.ERROR, group, className); + } + + public void warn(String message) { + if (shouldLog(Level.WARN, group)) log(message, Level.WARN, group, className); + } + + public void info(String message) { + if (shouldLog(Level.INFO, group)) log(message, Level.INFO, group, className); + } + + public void debug(String message) { + if (shouldLog(Level.DEBUG, group)) log(message, Level.DEBUG, group, className); + } + + public void trace(String message) { + if (shouldLog(Level.TRACE, group)) log(message, Level.TRACE, group, className); + } + + public void de_pest(String message) { + if (shouldLog(Level.DE_PEST, group)) log(message, Level.DE_PEST, group, className); + } + + private abstract static class Appender { + abstract void log(String message); + } + + private static class ConsoleAppender extends Appender { + @Override + void log(String message) { + System.out.println(message); + } + } + + private static class AsyncFileAppender extends Appender { + private Path filePath; + + public AsyncFileAppender(Path logFilePath) { + this.filePath = logFilePath; + } + + @Override + void log(String message) { + try (AsynchronousFileChannel asyncFile = + AsynchronousFileChannel.open( + filePath, StandardOpenOption.WRITE, StandardOpenOption.CREATE)) { + + asyncFile.write(ByteBuffer.wrap(message.getBytes()), 0); + } catch (IOException e) { + e.printStackTrace(); + } + } + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/networking/LinuxNetworking.java b/chameleon-server/src/main/java/com/chameleonvision/common/networking/LinuxNetworking.java index b02e9df16..e8721f060 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/networking/LinuxNetworking.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/networking/LinuxNetworking.java @@ -1,5 +1,7 @@ package com.chameleonvision.common.networking; +import com.chameleonvision.common.logging.LogGroup; +import com.chameleonvision.common.logging.Logger; import java.io.File; import java.io.IOException; import java.net.InetAddress; @@ -9,18 +11,16 @@ import java.util.ArrayList; import java.util.Collections; import java.util.List; import org.apache.commons.io.FileUtils; -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; public class LinuxNetworking extends SysNetworking { private static final String PATH = "/etc/dhcpcd.conf"; - private Logger logger = LoggerFactory.getLogger(LinuxNetworking.class); + private Logger logger = new Logger(LinuxNetworking.class, LogGroup.General); @Override public boolean setDHCP() { File dhcpConf = new File(PATH); - logger.debug("Removing static IP from {}", PATH); + logger.debug("Removing static IP from " + PATH); if (dhcpConf.exists()) { try { List lines = FileUtils.readLines(dhcpConf, StandardCharsets.UTF_8); diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptEvent.java b/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptEvent.java index 4dd6e67b7..bed6ce617 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptEvent.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptEvent.java @@ -1,15 +1,15 @@ package com.chameleonvision.common.scripting; +import com.chameleonvision.common.logging.LogGroup; +import com.chameleonvision.common.logging.Logger; import com.chameleonvision.common.util.ShellExec; import java.io.IOException; -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; public class ScriptEvent { private static final ShellExec executor = new ShellExec(true, true); public final ScriptConfig config; - private final Logger logger = LoggerFactory.getLogger(ScriptEvent.class); + private final Logger logger = new Logger(ScriptEvent.class, LogGroup.General); public ScriptEvent(ScriptConfig config) { this.config = config; diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptManager.java b/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptManager.java index e3839b1a9..5f83f6ec1 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptManager.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptManager.java @@ -1,5 +1,7 @@ package com.chameleonvision.common.scripting; +import com.chameleonvision.common.logging.LogGroup; +import com.chameleonvision.common.logging.Logger; import com.chameleonvision.common.util.LoopingRunnable; import com.chameleonvision.common.util.Platform; import com.chameleonvision.common.util.file.JacksonUtils; @@ -10,12 +12,10 @@ import java.nio.file.Paths; import java.util.ArrayList; import java.util.List; import java.util.concurrent.LinkedBlockingDeque; -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; public class ScriptManager { - private static final Logger logger = LoggerFactory.getLogger(ScriptManager.class); + private static final Logger logger = new Logger(ScriptManager.class, LogGroup.General); private ScriptManager() {} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/util/file/FileUtils.java b/chameleon-server/src/main/java/com/chameleonvision/common/util/file/FileUtils.java index 9c838133b..25629947b 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/util/file/FileUtils.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/util/file/FileUtils.java @@ -1,5 +1,7 @@ package com.chameleonvision.common.util.file; +import com.chameleonvision.common.logging.LogGroup; +import com.chameleonvision.common.logging.Logger; import com.chameleonvision.common.util.Platform; import java.io.File; import java.io.IOException; @@ -10,14 +12,12 @@ import java.nio.file.attribute.PosixFilePermission; import java.util.Arrays; import java.util.HashSet; import java.util.Set; -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; public class FileUtils { private FileUtils() {} - private static Logger logger = LoggerFactory.getLogger(FileUtils.class); + private static Logger logger = new Logger(FileUtils.class, LogGroup.General); private static final Set allReadWriteExecutePerms = new HashSet<>(Arrays.asList(PosixFilePermission.values())); diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/FrameConsumer.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/FrameConsumer.java index aa2ae4e8b..93d107999 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/FrameConsumer.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/FrameConsumer.java @@ -1,5 +1,5 @@ package com.chameleonvision.common.vision.frame; -public interface FrameConsumer { - void consume(Frame frame); -} +import java.util.function.Consumer; + +public interface FrameConsumer extends Consumer {} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/FrameProvider.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/FrameProvider.java index 37cd24078..42dad5f93 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/FrameProvider.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/FrameProvider.java @@ -1,5 +1,7 @@ package com.chameleonvision.common.vision.frame; -public interface FrameProvider { - Frame getFrame(); +import java.util.function.Supplier; + +public interface FrameProvider extends Supplier { + String getName(); } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/consumer/DummyFrameConsumer.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/consumer/DummyFrameConsumer.java index 3ea069317..8d1ea5ea4 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/consumer/DummyFrameConsumer.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/consumer/DummyFrameConsumer.java @@ -5,7 +5,7 @@ import com.chameleonvision.common.vision.frame.FrameConsumer; public class DummyFrameConsumer implements FrameConsumer { @Override - public void consume(Frame frame) { + public void accept(Frame frame) { frame.release(); // lol ez } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/consumer/MJPGFrameConsumer.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/consumer/MJPGFrameConsumer.java index 8bb4c1e5e..72693bc5d 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/consumer/MJPGFrameConsumer.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/consumer/MJPGFrameConsumer.java @@ -6,7 +6,7 @@ import org.apache.commons.lang3.NotImplementedException; public class MJPGFrameConsumer implements FrameConsumer { @Override - public void consume(Frame frame) { + public void accept(Frame frame) { throw new NotImplementedException(""); } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/provider/FileFrameProvider.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/provider/FileFrameProvider.java index f923e1e01..d3be0a7ae 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/provider/FileFrameProvider.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/provider/FileFrameProvider.java @@ -15,6 +15,8 @@ import org.opencv.imgcodecs.Imgcodecs; * path}. */ public class FileFrameProvider implements FrameProvider { + private static int count = 0; + private Frame m_frame; private Path m_path; @@ -60,7 +62,7 @@ public class FileFrameProvider implements FrameProvider { /** * Set image reloading. If true this will reload the image from the path set in the constructor - * every time {@link FileFrameProvider#getFrame()} is called. + * every time {@link FileFrameProvider#get()} is called. * * @param reloadImage True to enable image reloading. */ @@ -78,11 +80,16 @@ public class FileFrameProvider implements FrameProvider { } @Override - public Frame getFrame() { + public Frame get() { if (m_reloadImage) { loadImage(); } return m_frame; } + + @Override + public String getName() { + return "FileFrameProvider" + count++ + " - " + m_path.getFileName(); + } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/provider/NetworkFrameProvider.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/provider/NetworkFrameProvider.java index ad63a283c..af22f2468 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/provider/NetworkFrameProvider.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/provider/NetworkFrameProvider.java @@ -5,8 +5,15 @@ import com.chameleonvision.common.vision.frame.FrameProvider; import org.apache.commons.lang3.NotImplementedException; public class NetworkFrameProvider implements FrameProvider { + private int count = 0; + @Override - public Frame getFrame() { + public Frame get() { throw new NotImplementedException(""); } + + @Override + public String getName() { + return "NetworkFrameProvider" + count++; + } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/provider/USBFrameProvider.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/provider/USBFrameProvider.java index e942429d4..10cd35597 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/provider/USBFrameProvider.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/provider/USBFrameProvider.java @@ -5,8 +5,15 @@ import com.chameleonvision.common.vision.frame.FrameProvider; import org.apache.commons.lang3.NotImplementedException; public class USBFrameProvider implements FrameProvider { + private static int count = 0; + @Override - public Frame getFrame() { + public Frame get() { throw new NotImplementedException(""); } + + @Override + public String getName() { + return "USBFrameProvider" + count++; + } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/CVMat.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/CVMat.java index 116e3b97c..9c713a55b 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/CVMat.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/CVMat.java @@ -24,7 +24,11 @@ public class CVMat implements Releasable { public CVMat(Mat mat) { this.mat = mat; if (allMats.add(mat)) { - System.out.println("(CVMat) Added new Mat from: \n" + ReflectionUtils.getNthCaller(3)); + System.out.println( + "(CVMat) Added new Mat (count: " + + allMats.size() + + ") from: " + + ReflectionUtils.getNthCaller(3)); } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/Contour.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/Contour.java index 740189530..f4cafa949 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/Contour.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/Contour.java @@ -177,9 +177,9 @@ public class Contour implements Releasable { @Override public void release() { - mat.release(); - mat2f.release(); - convexHull.release(); + if (mat != null) mat.release(); + if (mat2f != null) mat2f.release(); + if (convexHull != null) convexHull.release(); } public static MatOfPoint2f convertIndexesToPoints(MatOfPoint contour, MatOfInt indexes) { diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/Draw3dTargetsPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/Draw3dTargetsPipe.java index 1b5014121..194d66dff 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/Draw3dTargetsPipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/Draw3dTargetsPipe.java @@ -112,11 +112,13 @@ public class Draw3dTargetsPipe public static class Draw3dContoursParams { private final int radius = 2; private final Color color = Color.RED; - private final TargetModel targetModel = TargetModel.get2020Target(); + private final TargetModel targetModel; private final CameraCalibrationCoefficients cameraCalibrationCoefficients; - public Draw3dContoursParams(CameraCalibrationCoefficients cameraCalibrationCoefficients) { + public Draw3dContoursParams( + CameraCalibrationCoefficients cameraCalibrationCoefficients, TargetModel targetModel) { this.cameraCalibrationCoefficients = cameraCalibrationCoefficients; + this.targetModel = targetModel; } } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/AdvancedPipelineSettings.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/AdvancedPipelineSettings.java new file mode 100644 index 000000000..36db264c8 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/AdvancedPipelineSettings.java @@ -0,0 +1,47 @@ +package com.chameleonvision.common.vision.pipeline; + +import com.chameleonvision.common.util.numbers.DoubleCouple; +import com.chameleonvision.common.util.numbers.IntegerCouple; +import com.chameleonvision.common.vision.opencv.ContourSortMode; +import com.chameleonvision.common.vision.target.RobotOffsetPointMode; +import com.chameleonvision.common.vision.target.TargetOffsetPointEdge; +import com.chameleonvision.common.vision.target.TargetOrientation; + +public class AdvancedPipelineSettings extends CVPipelineSettings { + public IntegerCouple hsvHue = new IntegerCouple(50, 180); + public IntegerCouple hsvSaturation = new IntegerCouple(50, 255); + public IntegerCouple hsvValue = new IntegerCouple(50, 255); + + public boolean outputShowThresholded = false; + public boolean outputShowMultipleTargets = false; + + public boolean erode = false; + public boolean dilate = false; + + public DoubleCouple contourArea = new DoubleCouple(0.0, 100.0); + public DoubleCouple contourRatio = new DoubleCouple(0.0, 20.0); + public DoubleCouple contourExtent = new DoubleCouple(0.0, 100.0); + public int contourSpecklePercentage = 5; + + // the order in which to sort contours to find the most desirable + public ContourSortMode contourSortMode = ContourSortMode.Largest; + + // the edge (or not) of the target to consider the center point (Top, Bottom, Left, Right, + // Center) + public TargetOffsetPointEdge contourTargetOffsetPointEdge = TargetOffsetPointEdge.Center; + + // orientation of the target in terms of aspect ratio + public TargetOrientation contourTargetOrientation = TargetOrientation.Landscape; + + // the mode in which to offset target center point based on the camera being offset on the + // robot + // (None, Single Point, Dual Point) + public RobotOffsetPointMode offsetRobotOffsetMode = RobotOffsetPointMode.None; + + // the point set by the user in Single Point Offset mode (maybe double too? idr) + public DoubleCouple offsetCalibrationPoint = new DoubleCouple(); + + // the two values that define the line of the Dual Point Offset calibration (think y=mx+b) + public double offsetDualLineM = 1; + public double offsetDualLineB = 0; +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/CVPipeline.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/CVPipeline.java index cd846a802..41098e617 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/CVPipeline.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/CVPipeline.java @@ -5,15 +5,23 @@ import com.chameleonvision.common.vision.frame.Frame; import com.chameleonvision.common.vision.frame.FrameStaticProperties; public abstract class CVPipeline { + protected S settings; - protected abstract void setPipeParams(S settings, FrameStaticProperties frameStaticProperties); + protected abstract void setPipeParams(FrameStaticProperties frameStaticProperties, S settings); protected abstract R process(Frame frame, S settings); - public R run(Frame frame, S settings) { + public S getSettings() { + return settings; + } + + public R run(Frame frame) { long pipelineStartNanos = System.nanoTime(); - setPipeParams(settings, frame.frameStaticProperties); + if (settings == null) { + throw new RuntimeException("No settings provided for pipeline!"); + } + setPipeParams(frame.frameStaticProperties, settings); R result = process(frame, settings); diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/Calibration3dPipeline.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/Calibration3dPipeline.java index c4fcfef8f..fbfcabbb4 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/Calibration3dPipeline.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/Calibration3dPipeline.java @@ -1,3 +1,22 @@ package com.chameleonvision.common.vision.pipeline; -public class Calibration3dPipeline {} +import com.chameleonvision.common.vision.frame.Frame; +import com.chameleonvision.common.vision.frame.FrameStaticProperties; + +public class Calibration3dPipeline extends CVPipeline { + + // TODO: Everything here + + public Calibration3dPipeline() { + settings = new CVPipelineSettings(); + } + + @Override + protected void setPipeParams( + FrameStaticProperties frameStaticProperties, CVPipelineSettings settings) {} + + @Override + protected CVPipelineResult process(Frame frame, CVPipelineSettings settings) { + return null; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/ColoredShapePipeline.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/ColoredShapePipeline.java index 22ecc0055..aa8af682e 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/ColoredShapePipeline.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/ColoredShapePipeline.java @@ -7,7 +7,7 @@ public class ColoredShapePipeline extends CVPipeline { @Override protected void setPipeParams( - ColoredShapePipelineSettings settings, FrameStaticProperties frameStaticProperties) {} + FrameStaticProperties frameStaticProperties, ColoredShapePipelineSettings settings) {} @Override protected CVPipelineResult process(Frame frame, ColoredShapePipelineSettings settings) { diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/ColoredShapePipelineSettings.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/ColoredShapePipelineSettings.java index d61fbae92..feed77b98 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/ColoredShapePipelineSettings.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/ColoredShapePipelineSettings.java @@ -2,6 +2,6 @@ package com.chameleonvision.common.vision.pipeline; import com.chameleonvision.common.vision.opencv.ContourShape; -public class ColoredShapePipelineSettings extends CVPipelineSettings { +public class ColoredShapePipelineSettings extends AdvancedPipelineSettings { ContourShape desiredShape; } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/DriverModePipeline.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/DriverModePipeline.java index 10930bd1e..ad1971c81 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/DriverModePipeline.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/DriverModePipeline.java @@ -19,9 +19,13 @@ public class DriverModePipeline private final Draw2dCrosshairPipe draw2dCrosshairPipe = new Draw2dCrosshairPipe(); + public DriverModePipeline() { + settings = new DriverModePipelineSettings(); + } + @Override protected void setPipeParams( - DriverModePipelineSettings settings, FrameStaticProperties frameStaticProperties) { + FrameStaticProperties frameStaticProperties, DriverModePipelineSettings settings) { RotateImagePipe.RotateImageParams rotateImageParams = new RotateImagePipe.RotateImageParams(settings.inputImageRotationMode); rotateImagePipe.setParams(rotateImageParams); diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/ReflectivePipeline.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/ReflectivePipeline.java index 6864ead35..e3e5c2e3c 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/ReflectivePipeline.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/ReflectivePipeline.java @@ -50,9 +50,13 @@ public class ReflectivePipeline extends CVPipeline userPipelines; + private final Calibration3dPipeline calibration3dPipeline = new Calibration3dPipeline(); + private final DriverModePipeline driverModePipeline = new DriverModePipeline(); + + /** Index of the currently active pipeline. */ + private int currentPipelineIndex = DRIVERMODE_INDEX; + + /** + * Index of the last active user-created pipeline.
+ *
+ * Used only when switching from any of the built-in pipelines back to a user-created pipeline. + */ + private int lastPipelineIndex; + + /** + * Creates a PipelineManager with a DriverModePipeline, a Calibration3dPipeline, and all provided + * pipelines. + * + * @param userPipelines Pipelines to add to the manager. + */ + public PipelineManager(List userPipelines) { + this.userPipelines = userPipelines; + } + + /** Creates a PipelineManager with a DriverModePipeline, and a Calibration3dPipeline. */ + public PipelineManager() { + this(List.of()); + } + + /** + * Get a pipeline by index. + * + * @param index Index of desired pipeline. + * @return The desired pipeline. + */ + public CVPipeline getPipeline(int index) { + if (index < 0) { + switch (index) { + case DRIVERMODE_INDEX: + return driverModePipeline; + case CAL_3D_INDEX: + return calibration3dPipeline; + } + } + + return userPipelines.get(index); + } + + /** + * Get the settings for a pipeline by index. + * + * @param index Index of pipeline whose settings need getting. + * @return The gotten settings of the pipeline whose index was provided. + */ + public CVPipelineSettings getPipelineSettings(int index) { + return getPipeline(index).getSettings(); + } + + /** + * Get the currently active pipeline. + * + * @return The currently active pipeline. + */ + public CVPipeline getCurrentPipeline() { + return getPipeline(currentPipelineIndex); + } + + /** + * Get the currently active pipelines settings + * + * @return The currently active pipelines settings + */ + public CVPipelineSettings getCurrentPipelineSettings() { + return getPipelineSettings(currentPipelineIndex); + } + + /** + * Internal method for setting the active pipeline.
+ *
+ * All externally accessible methods that intend to change the active pipeline MUST go through + * here to ensure all proper steps are taken. + * + * @param index Index of pipeline to be active + */ + private void setPipelineInternal(int index) { + if (index < 0) { + lastPipelineIndex = currentPipelineIndex; + } + + currentPipelineIndex = index; + } + + /** + * Leaves the current built-in pipeline, if applicable, and sets the active pipeline to the most + * recently active user-created pipeline. + */ + public void exitAuxiliaryPipeline() { + if (currentPipelineIndex < 0) { + setPipelineInternal(lastPipelineIndex); + } + } + + private static final Comparator IndexComparator = + (o1, o2) -> { + int o1Index = o1.getSettings().pipelineIndex; + int o2Index = o2.getSettings().pipelineIndex; + + if (o1Index == o2Index) { + return 0; + } else if (o1Index < o2Index) { + return -1; + } + return 1; + }; + + /** + * Sorts the pipeline list by index, and reassigns their indexes to match the new order.
+ *
+ * I don't like this but I have no other ideas, and it works so ¯\_(ツ)_/¯ + */ + private void reassignIndexes() { + userPipelines.sort(IndexComparator); + for (int i = 0; i < userPipelines.size(); i++) { + getPipelineSettings(i).pipelineIndex = i; + } + } + + // TODO: adding/removing pipelines +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/processes/VisionModule.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/processes/VisionModule.java new file mode 100644 index 000000000..6b1ebb68e --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/processes/VisionModule.java @@ -0,0 +1,66 @@ +package com.chameleonvision.common.vision.processes; + +import com.chameleonvision.common.datatransfer.DataConsumer; +import com.chameleonvision.common.vision.frame.Frame; +import com.chameleonvision.common.vision.frame.FrameConsumer; +import com.chameleonvision.common.vision.pipeline.CVPipelineResult; +import java.util.LinkedList; + +/** +* This is the God Class +* +*

VisionModule has a pipeline manager, vision runner, and data providers. The data providers +* provide info on settings changes. VisionModuleManager holds a list of all current vision modules. +*/ +public class VisionModule { + + private final PipelineManager pipelineManager; + private final VisionSource visionSource; + private final VisionRunner visionRunner; + private final LinkedList dataConsumers = new LinkedList<>(); + private final LinkedList frameConsumers = new LinkedList<>(); + + public VisionModule(PipelineManager pipelineManager, VisionSource visionSource) { + this.pipelineManager = pipelineManager; + this.visionSource = visionSource; + this.visionRunner = + new VisionRunner( + this.visionSource.getFrameProvider(), + this.pipelineManager::getCurrentPipeline, + this::consumeResult); + } + + public void start() { + visionRunner.startProcess(); + } + + void consumeResult(CVPipelineResult result) { + // TODO: put result in to Data + var data = new Data(); + data.result = result; + consumeData(data); + + var frame = result.outputFrame; + consumeFrame(frame); + } + + void consumeData(Data data) { + for (var dataConsumer : dataConsumers) { + dataConsumer.accept(data); + } + } + + public void addDataConsumer(DataConsumer dataConsumer) { + dataConsumers.add(dataConsumer); + } + + public void addFrameConsumer(FrameConsumer frameConsumer) { + frameConsumers.add(frameConsumer); + } + + void consumeFrame(Frame frame) { + for (var frameConsumer : frameConsumers) { + frameConsumer.accept(frame); + } + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/processes/VisionModuleManager.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/processes/VisionModuleManager.java new file mode 100644 index 000000000..c72d8caeb --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/processes/VisionModuleManager.java @@ -0,0 +1,25 @@ +package com.chameleonvision.common.vision.processes; + +import java.util.ArrayList; +import java.util.List; + +/** VisionModuleManager has many VisionModules, and provides camera configuration data to them. */ +public class VisionModuleManager { + protected final List visionModules = new ArrayList<>(); + + public VisionModuleManager(List visionSources) { + for (var visionSource : visionSources) { + + // TODO: loading existing pipelines from config + var pipelineManager = new PipelineManager(); + + visionModules.add(new VisionModule(pipelineManager, visionSource)); + } + } + + public void startModules() { + for (var visionModule : visionModules) { + visionModule.start(); + } + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/processes/VisionRunner.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/processes/VisionRunner.java new file mode 100644 index 000000000..d311cf7d1 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/processes/VisionRunner.java @@ -0,0 +1,66 @@ +package com.chameleonvision.common.vision.processes; + +import com.chameleonvision.common.vision.frame.Frame; +import com.chameleonvision.common.vision.frame.FrameProvider; +import com.chameleonvision.common.vision.pipeline.CVPipeline; +import com.chameleonvision.common.vision.pipeline.CVPipelineResult; +import java.util.function.Consumer; +import java.util.function.Supplier; + +/** VisionRunner has a frame supplier, a pipeline supplier, and a result consumer */ +@SuppressWarnings("rawtypes") +public class VisionRunner { + + private final Thread visionProcessThread; + private final Supplier frameSupplier; + private final Supplier pipelineSupplier; + private final Consumer pipelineResultConsumer; + + private long loopCount; + + /** + * VisionRunner contains a Thread to run a pipeline, given a frame, and + * will give the result to the consumer. + * + * @param frameSupplier The supplier of the latest frame. + * @param pipelineSupplier The supplier of the current pipeline. + * @param pipelineResultConsumer The consumer of the latest result. + */ + public VisionRunner( + FrameProvider frameSupplier, + Supplier pipelineSupplier, + Consumer pipelineResultConsumer) { + this.frameSupplier = frameSupplier; + this.pipelineSupplier = pipelineSupplier; + this.pipelineResultConsumer = pipelineResultConsumer; + + this.visionProcessThread = new Thread(this::update); + this.visionProcessThread.setName("VisionRunner - " + frameSupplier.getName()); + } + + public void startProcess() { + visionProcessThread.start(); + } + + private boolean hasThrown; + + private void update() { + while (!Thread.interrupted()) { + loopCount++; + var pipeline = pipelineSupplier.get(); + var frame = frameSupplier.get(); + + try { + var pipelineResult = pipeline.run(frame); + pipelineResultConsumer.accept(pipelineResult); + } catch (Exception ex) { + if (hasThrown) { + System.err.println( + "Exception in thread \"" + visionProcessThread.getName() + "\", loop " + loopCount); + ex.printStackTrace(); + hasThrown = true; + } + } + } + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/processes/VisionSource.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/processes/VisionSource.java new file mode 100644 index 000000000..a0a32f5b2 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/processes/VisionSource.java @@ -0,0 +1,9 @@ +package com.chameleonvision.common.vision.processes; + +import com.chameleonvision.common.vision.frame.FrameProvider; + +public interface VisionSource { + FrameProvider getFrameProvider(); + + VisionSourceSettables getSettables(); +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/processes/VisionSourceSettables.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/processes/VisionSourceSettables.java new file mode 100644 index 000000000..35f1d5eed --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/processes/VisionSourceSettables.java @@ -0,0 +1,24 @@ +package com.chameleonvision.common.vision.processes; + +import edu.wpi.cscore.VideoMode; +import java.util.Dictionary; + +public interface VisionSourceSettables { + int getExposure(); + + void setExposure(int exposure); + + int getBrightness(); + + void setBrightness(int brightness); + + int getGain(); + + void setGain(int gain); + + VideoMode getCurrentVideoMode(); + + void setCurrentVideoMode(VideoMode videoMode); + + Dictionary getAllVideoModes(); +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TargetModel.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TargetModel.java index cf073c37a..6f48d991e 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TargetModel.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TargetModel.java @@ -66,6 +66,16 @@ public class TargetModel implements Releasable { return new TargetModel(corners, 12); // TODO switch to meters } + public static TargetModel get2019Target() { + var corners = + List.of( + new Point3(-5.936, 2.662, 0), + new Point3(-7.313, -2.662, 0), + new Point3(7.313, -2.662, 0), + new Point3(5.936, 2.662, 0)); + return new TargetModel(corners, 4); + } + @Override public void release() { realWorldTargetCoordinates.release(); diff --git a/chameleon-server/src/main/java/com/chameleonvision/server/Main.java b/chameleon-server/src/main/java/com/chameleonvision/server/RequestHandler.java similarity index 54% rename from chameleon-server/src/main/java/com/chameleonvision/server/Main.java rename to chameleon-server/src/main/java/com/chameleonvision/server/RequestHandler.java index 8f49a8027..f886a48b4 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/server/Main.java +++ b/chameleon-server/src/main/java/com/chameleonvision/server/RequestHandler.java @@ -1,3 +1,3 @@ package com.chameleonvision.server; -public class Main {} +public class RequestHandler {} diff --git a/chameleon-server/src/main/java/com/chameleonvision/server/Server.java b/chameleon-server/src/main/java/com/chameleonvision/server/Server.java new file mode 100644 index 000000000..9f3f71b45 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/server/Server.java @@ -0,0 +1,41 @@ +package com.chameleonvision.server; + +import io.javalin.Javalin; + +public class Server { + + public static void main(int port) { + Javalin app = + Javalin.create( + javalinConfig -> { + javalinConfig.showJavalinBanner = false; + javalinConfig.addStaticFiles("web"); + javalinConfig.enableCorsForAllOrigins(); + }); + /*Web Socket Events */ + app.ws( + "/websocket", + ws -> { + ws.onConnect(SocketHandler::onConnect); + ws.onClose(SocketHandler::onClose); + ws.onBinaryMessage(SocketHandler::onBinaryMessage); + }); + /*API Events*/ + // app.post("/api/settings/general", + // com.chameleonvision._2.web.RequestHandler::onGeneralSettings); + // app.post("/api/settings/camera", + // com.chameleonvision._2.web.RequestHandler::onCameraSettings); + // app.post("/api/vision/duplicate", + // com.chameleonvision._2.web.RequestHandler::onDuplicatePipeline); + // app.post("/api/settings/startCalibration", + // com.chameleonvision._2.web.RequestHandler::onCalibrationStart); + // app.post("/api/settings/snapshot", + // com.chameleonvision._2.web.RequestHandler::onSnapshot); + // app.post("/api/settings/endCalibration", + // com.chameleonvision._2.web.RequestHandler::onCalibrationEnding); + // app.post("/api/vision/pnpModel", + // com.chameleonvision._2.web.RequestHandler::onPnpModel); + // app.post("/api/install", RequestHandler::onInstallOrUpdate); + app.start(port); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/server/SocketHandler.java b/chameleon-server/src/main/java/com/chameleonvision/server/SocketHandler.java new file mode 100644 index 000000000..a600150de --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/server/SocketHandler.java @@ -0,0 +1,53 @@ +package com.chameleonvision.server; + +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.core.type.TypeReference; +import com.fasterxml.jackson.databind.ObjectMapper; +import io.javalin.websocket.*; +import java.io.IOException; +import java.nio.ByteBuffer; +import java.util.ArrayList; +import java.util.List; +import java.util.Map; +import org.msgpack.jackson.dataformat.MessagePackFactory; + +public class SocketHandler { + static List users = new ArrayList<>(); + static ObjectMapper objectMapper = new ObjectMapper(new MessagePackFactory()); + + public static void onConnect(WsConnectContext context) { + users.add(context); + } + + static void onClose(WsCloseContext context) { + users.remove(context); + } + + public static void onBinaryMessage(WsBinaryMessageContext context) { + try { + Map data = + objectMapper.readValue(context.data(), new TypeReference>() {}); + // TODO pass data to ui data provider + } catch (IOException e) { + e.printStackTrace(); + } + } + + public static void sendMessage(Object message, WsContext user) throws JsonProcessingException { + ByteBuffer b = ByteBuffer.wrap(objectMapper.writeValueAsBytes(message)); + user.send(b); + } + + public static void broadcastMessage(Object message, WsContext userToSkip) + throws JsonProcessingException { + for (WsContext user : users) { + if (user != userToSkip) { + sendMessage(message, user); + } + } + } + + public static void broadcastMessage(Object message) throws JsonProcessingException { + broadcastMessage(message, null); + } +} diff --git a/chameleon-server/src/test/java/com/chameleonvision/common/vision/pipeline/ReflectivePipelineTest.java b/chameleon-server/src/test/java/com/chameleonvision/common/vision/pipeline/ReflectivePipelineTest.java index 08ad99c8b..9a9eb6e6f 100644 --- a/chameleon-server/src/test/java/com/chameleonvision/common/vision/pipeline/ReflectivePipelineTest.java +++ b/chameleon-server/src/test/java/com/chameleonvision/common/vision/pipeline/ReflectivePipelineTest.java @@ -47,11 +47,11 @@ public class ReflectivePipelineTest { TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes), TestUtils.WPI2019Image.FOV); - TestUtils.showImage(frameProvider.getFrame().image.getMat(), "Pipeline input", 1); + TestUtils.showImage(frameProvider.get().image.getMat(), "Pipeline input", 1); CVPipelineResult pipelineResult; - pipelineResult = pipeline.run(frameProvider.getFrame(), settings); + pipelineResult = pipeline.run(frameProvider.get()); printTestResults(pipelineResult); Assertions.assertTrue(pipelineResult.hasTargets()); @@ -76,7 +76,7 @@ public class ReflectivePipelineTest { TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_108in_Center), TestUtils.WPI2020Image.FOV); - CVPipelineResult pipelineResult = pipeline.run(frameProvider.getFrame(), settings); + CVPipelineResult pipelineResult = pipeline.run(frameProvider.get()); printTestResults(pipelineResult); TestUtils.showImage(pipelineResult.outputFrame.image.getMat(), "Pipeline output"); @@ -86,7 +86,7 @@ public class ReflectivePipelineTest { var pipeline = new ReflectivePipeline(); while (true) { - CVPipelineResult pipelineResult = pipeline.run(frame, settings); + CVPipelineResult pipelineResult = pipeline.run(frame); printTestResults(pipelineResult); int preRelease = CVMat.getMatCount(); pipelineResult.release(); @@ -113,7 +113,7 @@ public class ReflectivePipelineTest { settings.contourGroupingMode = ContourGroupingMode.Dual; settings.contourIntersection = ContourIntersectionDirection.Up; - continuouslyRunPipeline(frameProvider.getFrame(), settings); + continuouslyRunPipeline(frameProvider.get(), settings); } private static void printTestResults(CVPipelineResult pipelineResult) { diff --git a/chameleon-server/src/test/java/com/chameleonvision/common/vision/pipeline/SolvePNPTest.java b/chameleon-server/src/test/java/com/chameleonvision/common/vision/pipeline/SolvePNPTest.java index 7b63cbed0..56ec0f877 100644 --- a/chameleon-server/src/test/java/com/chameleonvision/common/vision/pipeline/SolvePNPTest.java +++ b/chameleon-server/src/test/java/com/chameleonvision/common/vision/pipeline/SolvePNPTest.java @@ -17,45 +17,39 @@ import edu.wpi.first.wpilibj.geometry.Rotation2d; import java.io.IOException; import java.nio.file.Path; import java.util.stream.Collectors; +import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; public class SolvePNPTest { - @Test - public void meme() throws IOException { + private static final String LIFECAM_240P_CAL_FILE = "lifecam240p.json"; + private static final String LIFECAM_480P_CAL_FILE = "lifecam480p.json"; + + @BeforeEach + public void Init() { TestUtils.loadLibraries(); - - var lowres = (Path.of(TestUtils.getCalibrationPath().toString(), "lifecamcal.json").toFile()); - var cal1 = new ObjectMapper().readValue(lowres, CameraCalibrationCoefficients.class); - - var highres = (Path.of(TestUtils.getCalibrationPath().toString(), "lifecamcal2.json").toFile()); - var cal2 = new ObjectMapper().readValue(highres, CameraCalibrationCoefficients.class); } - private CameraCalibrationCoefficients get640p() { + @Test + public void loadCameraIntrinsics() { + var lifecam240pCal = getCoeffs(LIFECAM_240P_CAL_FILE); + var lifecam480pCal = getCoeffs(LIFECAM_480P_CAL_FILE); + + assertNotNull(lifecam240pCal); + checkCameraCoefficients(lifecam240pCal); + assertNotNull(lifecam480pCal); + checkCameraCoefficients(lifecam480pCal); + } + + private CameraCalibrationCoefficients getCoeffs(String filename) { try { var cameraCalibration = new ObjectMapper() .readValue( - (Path.of(TestUtils.getCalibrationPath().toString(), "lifecam640p.json").toFile()), + (Path.of(TestUtils.getCalibrationPath().toString(), filename).toFile()), CameraCalibrationCoefficients.class); - assertEquals(3, cameraCalibration.cameraIntrinsics.rows); - assertEquals(3, cameraCalibration.cameraIntrinsics.cols); - assertEquals(1, cameraCalibration.cameraExtrinsics.rows); - assertEquals(5, cameraCalibration.cameraExtrinsics.cols); - assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMat().rows()); - assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMat().cols()); - assertEquals(1, cameraCalibration.cameraExtrinsics.getAsMat().rows()); - assertEquals(5, cameraCalibration.cameraExtrinsics.getAsMat().cols()); - assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMatOfDouble().rows()); - assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMatOfDouble().cols()); - assertEquals(1, cameraCalibration.cameraExtrinsics.getAsMatOfDouble().rows()); - assertEquals(5, cameraCalibration.cameraExtrinsics.getAsMatOfDouble().cols()); - assertEquals(3, cameraCalibration.getCameraIntrinsicsMat().rows()); - assertEquals(3, cameraCalibration.getCameraIntrinsicsMat().cols()); - assertEquals(1, cameraCalibration.getCameraExtrinsicsMat().rows()); - assertEquals(5, cameraCalibration.getCameraExtrinsicsMat().cols()); + checkCameraCoefficients(cameraCalibration); return cameraCalibration; } catch (IOException e) { @@ -64,9 +58,27 @@ public class SolvePNPTest { } } + private void checkCameraCoefficients(CameraCalibrationCoefficients cameraCalibration) { + assertEquals(3, cameraCalibration.cameraIntrinsics.rows); + assertEquals(3, cameraCalibration.cameraIntrinsics.cols); + assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMat().rows()); + assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMat().cols()); + assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMatOfDouble().rows()); + assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMatOfDouble().cols()); + assertEquals(3, cameraCalibration.getCameraIntrinsicsMat().rows()); + assertEquals(3, cameraCalibration.getCameraIntrinsicsMat().cols()); + assertEquals(1, cameraCalibration.cameraExtrinsics.rows); + assertEquals(5, cameraCalibration.cameraExtrinsics.cols); + assertEquals(1, cameraCalibration.cameraExtrinsics.getAsMat().rows()); + assertEquals(5, cameraCalibration.cameraExtrinsics.getAsMat().cols()); + assertEquals(1, cameraCalibration.cameraExtrinsics.getAsMatOfDouble().rows()); + assertEquals(5, cameraCalibration.cameraExtrinsics.getAsMatOfDouble().cols()); + assertEquals(1, cameraCalibration.getCameraExtrinsicsMat().rows()); + assertEquals(5, cameraCalibration.getCameraExtrinsicsMat().cols()); + } + @Test public void test2019() { - TestUtils.loadLibraries(); var pipeline = new ReflectivePipeline(); var settings = new ReflectivePipelineSettings(); @@ -80,6 +92,11 @@ public class SolvePNPTest { settings.contourIntersection = ContourIntersectionDirection.Up; settings.cornerDetectionUseConvexHulls = true; + settings.targetModel = TargetModel.get2019Target(); + settings.cameraCalibration = getCoeffs(LIFECAM_240P_CAL_FILE); + + pipeline.settings = settings; + var frameProvider = new FileFrameProvider( TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark48in), @@ -87,14 +104,20 @@ public class SolvePNPTest { CVPipelineResult pipelineResult; - pipelineResult = pipeline.run(frameProvider.getFrame(), settings); + pipelineResult = pipeline.run(frameProvider.get()); + printTestResults(pipelineResult); + + // these numbers are not *accurate*, but they are known and expected + var pose = pipelineResult.targets.get(0).getRobotRelativePose(); + assertEquals(41.96, pose.getTranslation().getX(), 0.05); + assertEquals(-1.03, pose.getTranslation().getY(), 0.05); + assertEquals(1.46, pose.getRotation().getDegrees(), 0.05); TestUtils.showImage(pipelineResult.outputFrame.image.getMat(), "Pipeline output", 1000 * 90); } @Test public void test2020() { - TestUtils.loadLibraries(); var pipeline = new ReflectivePipeline(); var settings = new ReflectivePipelineSettings(); @@ -105,55 +128,55 @@ public class SolvePNPTest { settings.solvePNPEnabled = true; settings.cornerDetectionAccuracyPercentage = 4; settings.cornerDetectionUseConvexHulls = true; - settings.cameraCalibration = get640p(); + settings.cameraCalibration = getCoeffs(LIFECAM_480P_CAL_FILE); + settings.targetModel = TargetModel.get2020Target(36); settings.cameraPitch = Rotation2d.fromDegrees(0.0); - assertNotNull(settings.cameraCalibration); - assertEquals(3, settings.cameraCalibration.cameraIntrinsics.rows); - assertEquals(3, settings.cameraCalibration.cameraIntrinsics.cols); - assertEquals(1, settings.cameraCalibration.cameraExtrinsics.rows); - assertEquals(5, settings.cameraCalibration.cameraExtrinsics.cols); - - assertEquals(3, settings.cameraCalibration.cameraIntrinsics.getAsMat().rows()); - assertEquals(3, settings.cameraCalibration.cameraIntrinsics.getAsMat().cols()); - assertEquals(1, settings.cameraCalibration.cameraExtrinsics.getAsMat().rows()); - assertEquals(5, settings.cameraCalibration.cameraExtrinsics.getAsMat().cols()); - - assertEquals(3, settings.cameraCalibration.cameraIntrinsics.getAsMatOfDouble().rows()); - assertEquals(3, settings.cameraCalibration.cameraIntrinsics.getAsMatOfDouble().cols()); - assertEquals(1, settings.cameraCalibration.cameraExtrinsics.getAsMatOfDouble().rows()); - assertEquals(5, settings.cameraCalibration.cameraExtrinsics.getAsMatOfDouble().cols()); - - assertEquals(3, settings.cameraCalibration.getCameraIntrinsicsMat().rows()); - assertEquals(3, settings.cameraCalibration.getCameraIntrinsicsMat().cols()); - assertEquals(1, settings.cameraCalibration.getCameraExtrinsicsMat().rows()); - assertEquals(5, settings.cameraCalibration.getCameraExtrinsicsMat().cols()); + pipeline.settings = settings; var frameProvider = new FileFrameProvider( TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_224in_Left), TestUtils.WPI2020Image.FOV); - // TestUtils.showImage(frameProvider.getFrame().image.getMat(), "Pipeline output", - // 999999); - - CVPipelineResult pipelineResult = pipeline.run(frameProvider.getFrame(), settings); + CVPipelineResult pipelineResult = pipeline.run(frameProvider.get()); printTestResults(pipelineResult); + // these numbers are not *accurate*, but they are known and expected var pose = pipelineResult.targets.get(0).getRobotRelativePose(); - // assertEquals(180, pose.getTranslation().getX(), 20); - // assertEquals(0, pose.getTranslation().getY(), 20); - // assertEquals(0, pose.getRotation().getDegrees(), 5); + assertEquals(260.26, pose.getTranslation().getX(), 0.05); + assertEquals(64.26, pose.getTranslation().getY(), 0.05); + assertEquals(36.88, pose.getRotation().getDegrees(), 0.05); TestUtils.showImage(pipelineResult.outputFrame.image.getMat(), "Pipeline output", 999999); } + // @Test + // public void junk() { + // var frameProvider = + // new FileFrameProvider( + // + // TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes), + // TestUtils.WPI2019Image.FOV); + // + // var settings = new ReflectivePipelineSettings(); + // settings.hsvHue.set(60, 100); + // settings.hsvSaturation.set(100, 255); + // settings.hsvValue.set(190, 255); + // settings.outputShowThresholded = true; + // settings.outputShowMultipleTargets = true; + // settings.contourGroupingMode = ContourGroupingMode.Dual; + // settings.contourIntersection = ContourIntersectionDirection.Up; + // + // continuouslyRunPipeline(frameProvider.getFrame(), settings); + // } + private static void continuouslyRunPipeline(Frame frame, ReflectivePipelineSettings settings) { var pipeline = new ReflectivePipeline(); - + pipeline.settings = settings; while (true) { - CVPipelineResult pipelineResult = pipeline.run(frame, settings); + CVPipelineResult pipelineResult = pipeline.run(frame); printTestResults(pipelineResult); int preRelease = CVMat.getMatCount(); pipelineResult.release(); @@ -163,7 +186,7 @@ public class SolvePNPTest { } } - // used to run VisualVM for profiling. It won't run on unit tests. + // used to run VisualVM for profiling, which won't run on unit tests. public static void main(String[] args) { TestUtils.loadLibraries(); var frameProvider = @@ -180,7 +203,7 @@ public class SolvePNPTest { settings.contourGroupingMode = ContourGroupingMode.Dual; settings.contourIntersection = ContourIntersectionDirection.Up; - continuouslyRunPipeline(frameProvider.getFrame(), settings); + continuouslyRunPipeline(frameProvider.get(), settings); } private static void printTestResults(CVPipelineResult pipelineResult) { diff --git a/chameleon-server/src/test/java/com/chameleonvision/common/vision/processes/VisionModuleManagerTest.java b/chameleon-server/src/test/java/com/chameleonvision/common/vision/processes/VisionModuleManagerTest.java new file mode 100644 index 000000000..c4bc68b13 --- /dev/null +++ b/chameleon-server/src/test/java/com/chameleonvision/common/vision/processes/VisionModuleManagerTest.java @@ -0,0 +1,130 @@ +package com.chameleonvision.common.vision.processes; + +import com.chameleonvision.common.datatransfer.DataConsumer; +import com.chameleonvision.common.util.TestUtils; +import com.chameleonvision.common.vision.frame.FrameProvider; +import com.chameleonvision.common.vision.frame.provider.FileFrameProvider; +import com.chameleonvision.common.vision.pipeline.CVPipelineResult; +import edu.wpi.cscore.VideoMode; +import java.util.ArrayList; +import java.util.Dictionary; +import org.junit.jupiter.api.*; + +public class VisionModuleManagerTest { + + @BeforeEach + public void init() { + TestUtils.loadLibraries(); + } + + private static class TestSource implements VisionSource { + + private final FrameProvider provider; + + public TestSource(FrameProvider provider) { + + this.provider = provider; + } + + @Override + public FrameProvider getFrameProvider() { + return provider; + } + + @Override + public VisionSourceSettables getSettables() { + return new TestSettables(); + } + } + + private static class TestSettables implements VisionSourceSettables { + + @Override + public int getExposure() { + return 0; + } + + @Override + public void setExposure(int exposure) {} + + @Override + public int getBrightness() { + return 0; + } + + @Override + public void setBrightness(int brightness) {} + + @Override + public int getGain() { + return 0; + } + + @Override + public void setGain(int gain) {} + + @Override + public VideoMode getCurrentVideoMode() { + return null; + } + + @Override + public void setCurrentVideoMode(VideoMode videoMode) {} + + @Override + public Dictionary getAllVideoModes() { + return null; + } + } + + private static class TestDataConsumer implements DataConsumer { + private Data data; + + @Override + public void accept(Data data) { + this.data = data; + } + + public Data getData() { + return data; + } + } + + @Test + public void setupManager() { + var sources = new ArrayList(); + sources.add( + new TestSource( + new FileFrameProvider( + TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes), + TestUtils.WPI2019Image.FOV))); + + var moduleManager = new VisionModuleManager(sources); + var module0DataConsumer = new TestDataConsumer(); + + moduleManager.visionModules.get(0).addDataConsumer(module0DataConsumer); + + moduleManager.startModules(); + + sleep(500); + + Assertions.assertNotNull(module0DataConsumer.data); + Assertions.assertNotNull(module0DataConsumer.data.result); + printTestResults(module0DataConsumer.data.result); + } + + private static void printTestResults(CVPipelineResult pipelineResult) { + double fps = 1000 / pipelineResult.getLatencyMillis(); + System.out.print( + "Pipeline ran in " + pipelineResult.getLatencyMillis() + "ms (" + fps + " fps), "); + System.out.println("Found " + pipelineResult.targets.size() + " valid targets"); + } + + private void sleep(int millis) { + try { + Thread.sleep(millis); + } catch (InterruptedException e) { + // ignored + } + } +} diff --git a/chameleon-server/src/test/resources/calibration/lifecam320p.json b/chameleon-server/src/test/resources/calibration/lifecam240p.json similarity index 100% rename from chameleon-server/src/test/resources/calibration/lifecam320p.json rename to chameleon-server/src/test/resources/calibration/lifecam240p.json diff --git a/chameleon-server/src/test/resources/calibration/lifecam640p.json b/chameleon-server/src/test/resources/calibration/lifecam480p.json similarity index 100% rename from chameleon-server/src/test/resources/calibration/lifecam640p.json rename to chameleon-server/src/test/resources/calibration/lifecam480p.json