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

2.2 (#52)

Browse Source 
## features
-- added pi cam support (from wpilib v2020.2.2)
-- added gain slider for ps3 eye
-- added custom pnp model upload
-- network tables will try to reconnect if no connection
-- re did network tables key and added new values 


## bug fixes
-- fixed solve pnp not detection
-- lowered minimum fps for some camera
-- fixed vision hang bug
This commit is contained in:
Banks T
2020-01-22 14:53:11 -05:00
committed by oriagranat9
parent 0e2e950d18
commit a0b89168b4
30 changed files with 719 additions and 198 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -2,20 +2,17 @@ package com.chameleonvision;
import com.chameleonvision.config.ConfigManager;
import com.chameleonvision.network.NetworkManager;
import com.chameleonvision.networktables.NetworkTablesManager;
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;
import edu.wpi.cscore.CameraServerCvJNI;
import edu.wpi.cscore.CameraServerJNI;
import edu.wpi.first.networktables.LogMessage;
import edu.wpi.first.networktables.NetworkTableInstance;
import java.io.IOException;
import java.util.function.Consumer;
import static com.chameleonvision.util.Platform.CurrentPlatform;
@@ -26,6 +23,7 @@ public class Main {
private static final String NETWORK_MANAGE_KEY = "--unmanage-network"; // no args for this setting
private static final String IGNORE_ROOT_KEY = "--ignore-root"; // no args for this setting
private static final String TEST_MODE_KEY = "--cv-development";
private static final String UI_PORT_KEY = "--ui-port";
private static final int DEFAULT_PORT = 5800;
@@ -34,21 +32,7 @@ public class Main {
private static boolean ignoreRoot = false;
private static String ntClientModeServer = null;
public static boolean testMode = false;
private static class NTLogger implements Consumer<LogMessage> {
private boolean hasReportedConnectionFailure = false;
@Override
public void accept(LogMessage logMessage) {
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);
}
}
}
public static int uiPort = DEFAULT_PORT;
private static void handleArgs(String[] args) {
for (int i = 0; i < args.length; i++) {
@@ -65,6 +49,13 @@ public class Main {
}
i++; // increment to skip an 'arg' next go-around of for loop, as that would be this value
break;
case UI_PORT_KEY:
var potentialPort = args[i + 1];
if (potentialPort != null && !potentialPort.isBlank() && !potentialPort.startsWith("-") & !potentialPort.startsWith("--")) {
value = potentialPort;
}
i++;
break;
case NT_SERVERMODE_KEY:
case NETWORK_MANAGE_KEY:
case IGNORE_ROOT_KEY:
@@ -101,6 +92,15 @@ public class Main {
case TEST_MODE_KEY:
testMode = true;
break;
case UI_PORT_KEY:
if (value != null) {
try {
uiPort = Integer.parseInt(value);
} catch (NumberFormatException e){
System.err.println("ui Port was not a valid number using port 5800");
}
}
break;
}
}
}
@@ -124,7 +124,7 @@ public class Main {
System.out.println("Ignoring root, network will not be managed!");
} else {
System.err.println("This program must be run as root!");
return;
return;
}
}
@@ -133,7 +133,7 @@ public class Main {
CameraServerJNI.forceLoad();
CameraServerCvJNI.forceLoad();
} catch (UnsatisfiedLinkError | IOException e) {
if(CurrentPlatform.isWindows()) {
if (CurrentPlatform.isWindows()) {
System.err.println("Try to download the VC++ Redistributable, https://aka.ms/vs/16/release/vc_redist.x64.exe");
}
throw new RuntimeException("Failed to load JNI Libraries!");
@@ -147,20 +147,12 @@ public class Main {
System.out.println("Scripts not yet supported on Windows. ScriptEvents will be ignored.");
}
NetworkManager.initialize(manageNetwork);
if (ntServerMode) {
System.out.println("Starting NT Server");
NetworkTableInstance.getDefault().startServer();
NetworkTablesManager.setServerMode();
} else {
NetworkTableInstance.getDefault().addLogger(new NTLogger(), 0, 255); // to hide error messages
if (ntClientModeServer != null) {
NetworkTableInstance.getDefault().startClient(ntClientModeServer);
} else {
NetworkTableInstance.getDefault().startClientTeam(ConfigManager.settings.teamNumber);
}
// NetworkTableInstance.getDefault().startClient("localhost");
NetworkTablesManager.setClientMode(ntClientModeServer);
}
ScriptManager.queueEvent(ScriptEventType.kProgramInit);
@@ -179,7 +171,7 @@ public class Main {
VisionManager.startProcesses();
System.out.printf("Starting Web server at port %d\n", DEFAULT_PORT);
Server.main(DEFAULT_PORT);
System.out.printf("Starting Web server at port %d\n", uiPort);
Server.main(uiPort);
}
}

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

@@ -51,7 +51,7 @@ public class CameraConfig {
private CameraJsonConfig loadConfig() {
CameraJsonConfig config = preliminaryConfig;
try {
config = JacksonHelper.deserializer(configPath, CameraJsonConfig.class);
config = JacksonHelper.deserialize(configPath, CameraJsonConfig.class);
} catch (IOException e) {
System.err.printf("Failed to load camera config: %s - using default.\n", configPath.toString());
}
@@ -61,7 +61,7 @@ public class CameraConfig {
private CVPipelineSettings loadDriverMode() {
CVPipelineSettings driverMode = new CVPipelineSettings();
try {
driverMode = JacksonHelper.deserializer(driverModePath, CVPipelineSettings.class);
driverMode = JacksonHelper.deserialize(driverModePath, CVPipelineSettings.class);
} catch (IOException e) {
System.err.println("Failed to load camera drivermode: " + driverModePath.toString());
}
@@ -75,7 +75,7 @@ public class CameraConfig {
private List<CameraCalibrationConfig> loadCalibration() {
List<CameraCalibrationConfig> calibrations = new ArrayList<>();
try {
calibrations = List.of(Objects.requireNonNull(JacksonHelper.deserializer(calibrationPath, CameraCalibrationConfig[].class)));
calibrations = List.of(Objects.requireNonNull(JacksonHelper.deserialize(calibrationPath, CameraCalibrationConfig[].class)));
} catch (Exception e) {
System.err.println("Failed to load camera calibration: " + driverModePath.toString());
}

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

@@ -1,6 +1,5 @@
package com.chameleonvision.config;
import com.chameleonvision.Main;
import com.chameleonvision.util.*;
import com.chameleonvision.vision.pipeline.CVPipelineSettings;
@@ -54,7 +53,7 @@ public class ConfigManager {
}
} else {
try {
settings = JacksonHelper.deserializer(settingsFilePath, GeneralSettings.class);
settings = JacksonHelper.deserialize(settingsFilePath, GeneralSettings.class);
} catch (IOException e) {
System.err.println("Failed to load settings.json, using defaults.");
}

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

@@ -1,5 +1,7 @@
package com.chameleonvision.config;
import com.chameleonvision.config.serializers.StandardCVPipelineSettingsDeserializer;
import com.chameleonvision.config.serializers.StandardCVPipelineSettingsSerializer;
import com.chameleonvision.util.FileHelper;
import com.chameleonvision.util.JacksonHelper;
import com.chameleonvision.vision.pipeline.*;
@@ -71,11 +73,20 @@ public class PipelineConfig {
var path = getPipelinePath(settings);
try {
JacksonHelper.serializer(path, settings);
FileHelper.setFilePerms(path);
} catch (IOException e) {
e.printStackTrace();
if (settings instanceof StandardCVPipelineSettings) {
try {
JacksonHelper.serialize(path, (StandardCVPipelineSettings)settings, StandardCVPipelineSettings.class, new StandardCVPipelineSettingsSerializer());
FileHelper.setFilePerms(path);
} catch (IOException e) {
e.printStackTrace();
}
} else {
try {
JacksonHelper.serializer(path, settings);
FileHelper.setFilePerms(path);
} catch (IOException e) {
e.printStackTrace();
}
}
}
@@ -119,7 +130,7 @@ public class PipelineConfig {
} else {
for(File pipelineFile : pipelineFiles) {
try {
var pipe = JacksonHelper.deserializer(Paths.get(pipelineFile.getPath()), StandardCVPipelineSettings.class);
var pipe = JacksonHelper.deserialize(Paths.get(pipelineFile.getPath()), StandardCVPipelineSettings.class, new StandardCVPipelineSettingsDeserializer());
deserializedList.add(pipe);
} catch (IOException e) {
System.err.println("couldn't load cvpipeline2d");

110
chameleon-server/src/main/java/com/chameleonvision/config/serializers/BaseDeserializer.java Normal file
View File

@@ -0,0 +1,110 @@
package com.chameleonvision.config.serializers;
import com.fasterxml.jackson.core.JsonProcessingException;
import com.fasterxml.jackson.core.type.TypeReference;
import com.fasterxml.jackson.databind.JsonNode;
import com.fasterxml.jackson.databind.ObjectMapper;
import com.fasterxml.jackson.databind.deser.std.StdDeserializer;
import com.fasterxml.jackson.databind.type.CollectionType;
import com.fasterxml.jackson.databind.type.TypeFactory;
import org.jetbrains.annotations.NotNull;
import org.opencv.core.MatOfPoint3f;
import org.opencv.core.Point3;
import java.io.IOException;
import java.util.ArrayList;
import java.util.List;
public abstract class BaseDeserializer<T> extends StdDeserializer<T> {
protected BaseDeserializer(Class<?> vc) {
super(vc);
}
JsonNode baseNode;
private static final CollectionType numberListColType = TypeFactory.defaultInstance().constructCollectionType(List.class, Number.class);
private CollectionType pointListColType = TypeFactory.defaultInstance().constructCollectionType(List.class, Object.class);
private static final ObjectMapper mapper = new ObjectMapper();
private static boolean nodeGood(JsonNode node) {
return node != null && !node.toString().equals("");
}
List<Number> getNumberList(String name, List<Number> defaultValue) throws JsonProcessingException {
JsonNode node = baseNode.get(name);
if (nodeGood(node)) {
return mapper.readValue(node.toString(), numberListColType);
}
return defaultValue;
}
boolean getBoolean(String name, boolean defaultValue) {
JsonNode node = baseNode.get(name);
if (nodeGood(node)) {
return node.booleanValue();
}
return defaultValue;
}
int getInt(String name, int defaultValue) {
return (int) getDouble(name, defaultValue);
}
double getDouble(String name, double defaultValue) {
JsonNode node = baseNode.get(name);
if (nodeGood(node)) {
return node.numberValue().doubleValue();
}
return defaultValue;
}
String getString(String name, String defaultValue) {
JsonNode node = baseNode.get(name);
if (nodeGood(node)) {
return node.asText();
}
return defaultValue;
}
<E extends Enum<E>> E getEnum(String name, Class<E> enumClass, E defaultValue) throws IOException {
JsonNode node = baseNode.get(name);
if (nodeGood(node)) {
E[] possibleVals = enumClass.getEnumConstants();
String jsonVal = baseNode.get(name).asText();
for (E val : possibleVals) {
if (val.name().equals(jsonVal)) {
return val;
}
}
}
return defaultValue;
}
MatOfPoint3f getMatOfPoint3f(String name, MatOfPoint3f defaultValue) throws JsonProcessingException {
JsonNode node = baseNode.get(name);
if (nodeGood(node)){
List<List<Number>> numberList = mapper.readValue(node.toString(), pointListColType);
List<Point3> point3List = new ArrayList<>();
for (List<Number> tmp : numberList){
Point3 p = new Point3();
p.x = tmp.get(0).doubleValue();
p.y = tmp.get(1).doubleValue();
p.z = tmp.get(2).doubleValue();
point3List.add(p);
}
MatOfPoint3f mat = new MatOfPoint3f();
mat.fromList(point3List);
return mat;
}
return defaultValue;
}
}

43
chameleon-server/src/main/java/com/chameleonvision/config/serializers/BaseSerializer.java Normal file
View File

@@ -0,0 +1,43 @@
package com.chameleonvision.config.serializers;
import com.fasterxml.jackson.core.JsonGenerator;
import com.fasterxml.jackson.databind.ser.std.StdSerializer;
import org.opencv.core.MatOfPoint3f;
import org.opencv.core.Point;
import org.opencv.core.Point3;
import java.io.IOException;
import java.util.ArrayList;
import java.util.List;
public abstract class BaseSerializer<T> extends StdSerializer<T> {
protected BaseSerializer(Class<T> t) {
super(t);
}
JsonGenerator generator;
void writeNumberListAsNumberArray(String name, List<Number> list) throws IOException {
generator.writeArrayFieldStart(name);
for (Number i : list) {
generator.writeObject(i);
}
generator.writeEndArray();
}
<E extends Enum<E>> void writeEnum(String name, E num) throws IOException {
generator.writeFieldName(name);
generator.writeString(num.name());
}
void writeMatOfPoint3f(String name, MatOfPoint3f mat) throws IOException {
List<Point3> point3List = mat.toList();
generator.writeArrayFieldStart(name);
for (Point3 point3 : point3List) {
double[] tmp = {point3.x, point3.y, point3.z};
generator.writeObject(tmp);
}
generator.writeEndArray();
}
}

80
chameleon-server/src/main/java/com/chameleonvision/config/serializers/StandardCVPipelineSettingsDeserializer.java Normal file
View File

@@ -0,0 +1,80 @@
package com.chameleonvision.config.serializers;
import com.chameleonvision.vision.enums.*;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipelineSettings;
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.node.IntNode;
import java.io.IOException;
public class StandardCVPipelineSettingsDeserializer extends BaseDeserializer<StandardCVPipelineSettings> {
public StandardCVPipelineSettingsDeserializer() {
this(null);
}
private StandardCVPipelineSettingsDeserializer(Class<?> vc) {
super(vc);
}
@Override
public StandardCVPipelineSettings deserialize(JsonParser jsonParser, DeserializationContext ctxt) throws IOException, JsonProcessingException {
// set BaseDeserializer parser reference.
baseNode = jsonParser.getCodec().readTree(jsonParser);
StandardCVPipelineSettings pipeline = new StandardCVPipelineSettings();
pipeline.index = getInt("index", pipeline.index);
pipeline.flipMode = getEnum("flipMode", ImageFlipMode.class, pipeline.flipMode);
pipeline.rotationMode = getEnum("rotationMode", ImageRotationMode.class, pipeline.rotationMode);
pipeline.nickname = getString("nickname", pipeline.nickname);
pipeline.exposure = getDouble("exposure", pipeline.exposure);
pipeline.brightness = getDouble("brightness", pipeline.brightness);
pipeline.gain = getDouble("gain", pipeline.gain);
pipeline.videoModeIndex = getInt("videoModeIndex", pipeline.videoModeIndex);
pipeline.streamDivisor = getEnum("streamDivisor", StreamDivisor.class, pipeline.streamDivisor);
pipeline.hue = getNumberList("hue", pipeline.hue);
pipeline.saturation = getNumberList("saturation", pipeline.saturation);
pipeline.value = getNumberList("value", pipeline.value);
pipeline.erode = getBoolean("erode", pipeline.erode);
pipeline.dilate = getBoolean("dilate", pipeline.dilate);
pipeline.area = getNumberList("area", pipeline.area);
pipeline.ratio = getNumberList("ratio", pipeline.ratio);
pipeline.extent = getNumberList("extent", pipeline.extent);
pipeline.speckle = getInt("speckle", (Integer) pipeline.speckle);
pipeline.isBinary = getBoolean("isBinary", pipeline.isBinary);
pipeline.sortMode = getEnum("sortMode", SortMode.class, pipeline.sortMode);
pipeline.targetRegion = getEnum("targetRegion", TargetRegion.class, pipeline.targetRegion);
pipeline.targetOrientation = getEnum("targetOrientation", TargetOrientation.class, pipeline.targetOrientation);
pipeline.multiple = getBoolean("multiple", pipeline.multiple);
pipeline.targetGroup = getEnum("targetGroup", TargetGroup.class, pipeline.targetGroup);
pipeline.targetIntersection = getEnum("targetIntersection", TargetIntersection.class, pipeline.targetIntersection);
pipeline.point = getNumberList("point", pipeline.point);
pipeline.calibrationMode = getEnum("calibrationMode", CalibrationMode.class, pipeline.calibrationMode);
pipeline.dualTargetCalibrationM = getDouble("dualTargetCalibrationM", pipeline.dualTargetCalibrationM);
pipeline.dualTargetCalibrationB = getDouble("dualTargetCalibrationB", pipeline.dualTargetCalibrationB);
pipeline.is3D = getBoolean("is3D", pipeline.is3D);
pipeline.targetCornerMat = getMatOfPoint3f("targetCornerMat", pipeline.targetCornerMat);
return pipeline;
}
}

82
chameleon-server/src/main/java/com/chameleonvision/config/serializers/StandardCVPipelineSettingsSerializer.java Normal file
View File

@@ -0,0 +1,82 @@
package com.chameleonvision.config.serializers;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipelineSettings;
import com.fasterxml.jackson.core.JsonGenerator;
import com.fasterxml.jackson.databind.SerializerProvider;
import java.io.IOException;
public class StandardCVPipelineSettingsSerializer extends BaseSerializer<StandardCVPipelineSettings> {
public StandardCVPipelineSettingsSerializer() {
this(null);
}
private StandardCVPipelineSettingsSerializer(Class<StandardCVPipelineSettings> t) {
super(t);
}
@Override
public void serialize(StandardCVPipelineSettings pipeline, JsonGenerator gen, SerializerProvider provider) throws IOException {
// set BaseSerializer generator reference.
generator = gen;
gen.writeStartObject();
gen.writeNumberField("index", pipeline.index);
writeEnum("flipMode", pipeline.flipMode);
writeEnum("rotationMode", pipeline.rotationMode);
gen.writeStringField("nickname", pipeline.nickname);
gen.writeNumberField("exposure", pipeline.exposure);
gen.writeNumberField("brightness", pipeline.brightness);
gen.writeNumberField("gain", pipeline.gain);
gen.writeNumberField("videoModeIndex", pipeline.videoModeIndex);
writeEnum("streamDivisor", pipeline.streamDivisor);
writeNumberListAsNumberArray("hue", pipeline.hue);
writeNumberListAsNumberArray("saturation", pipeline.saturation);
writeNumberListAsNumberArray("value", pipeline.value);
gen.writeBooleanField("erode", pipeline.erode);
gen.writeBooleanField("dilate", pipeline.dilate);
writeNumberListAsNumberArray("area", pipeline.area);
writeNumberListAsNumberArray("ratio", pipeline.ratio);
writeNumberListAsNumberArray("extent", pipeline.extent);
// speckle rejection
gen.writeNumberField("speckle", (Integer) pipeline.speckle);
// stream output (camera feed, or thresholded feed)
gen.writeBooleanField("isBinary", pipeline.isBinary);
writeEnum("sortMode", pipeline.sortMode);
writeEnum("targetRegion", pipeline.targetRegion);
writeEnum("targetOrientation", pipeline.targetOrientation);
// show multiple targets when drawing
gen.writeBooleanField("multiple", pipeline.multiple);
writeEnum("targetGroup", pipeline.targetGroup);
writeEnum("targetIntersection", pipeline.targetIntersection);
// single calibration point
writeNumberListAsNumberArray("point", pipeline.point);
// target X/Y calibration
writeEnum("calibrationMode", pipeline.calibrationMode);
// TODO: better names? or use an array?
gen.writeNumberField("dualTargetCalibrationM", pipeline.dualTargetCalibrationM);
gen.writeNumberField("dualTargetCalibrationB", pipeline.dualTargetCalibrationB);
gen.writeBooleanField("is3D", pipeline.is3D);
writeMatOfPoint3f("targetCornerMat", pipeline.targetCornerMat);
gen.writeEndObject();
}
}

69
chameleon-server/src/main/java/com/chameleonvision/networktables/NetworkTablesManager.java Normal file
View File

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

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

@@ -1,24 +1,18 @@
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 {
@@ -94,7 +88,7 @@ public class ScriptManager {
static List<ScriptConfig> loadConfig() {
try {
var raw = JacksonHelper.deserializer(scriptConfigPath, ScriptConfig[].class);
var raw = JacksonHelper.deserialize(scriptConfigPath, ScriptConfig[].class);
if (raw != null) {
return List.of(raw);
}

4
chameleon-server/src/main/java/com/chameleonvision/util/FileHelper.java
View File

@@ -1,5 +1,7 @@
package com.chameleonvision.util;
import com.chameleonvision.Debug;
import java.io.File;
import java.io.IOException;
import java.nio.file.Files;
@@ -20,7 +22,7 @@ public class FileHelper {
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());
Debug.printInfo("Setting perms on" + path.toString());
Files.setPosixFilePermissions(path, perms);
if (thisFile.isDirectory()) {
for (File subfile : thisFile.listFiles()) {

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

@@ -12,7 +12,7 @@ public class Helpers {
}
public static Scalar colorToScalar(Color color) {
return new Scalar(color.getRed(), color.getGreen(), color.getBlue());
return new Scalar(color.getBlue(), color.getGreen(), color.getRed());
}
public static HashMap VideoModeToHashMap(VideoMode videoMode) {

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

@@ -1,9 +1,14 @@
package com.chameleonvision.util;
import com.chameleonvision.config.serializers.StandardCVPipelineSettingsDeserializer;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipelineSettings;
import com.fasterxml.jackson.databind.ObjectMapper;
import com.fasterxml.jackson.databind.deser.std.StdDeserializer;
import com.fasterxml.jackson.databind.json.JsonMapper;
import com.fasterxml.jackson.databind.jsontype.BasicPolymorphicTypeValidator;
import com.fasterxml.jackson.databind.jsontype.PolymorphicTypeValidator;
import com.fasterxml.jackson.databind.module.SimpleModule;
import com.fasterxml.jackson.databind.ser.std.StdSerializer;
import java.io.File;
import java.io.IOException;
@@ -18,7 +23,7 @@ public class JacksonHelper {
objectMapper.writerWithDefaultPrettyPrinter().writeValue(new File(path.toString()), object);
}
public static <T> T deserializer(Path path, Class<T> ref) throws IOException {
public static <T> T deserialize(Path path, Class<T> ref) throws IOException {
PolymorphicTypeValidator ptv = BasicPolymorphicTypeValidator.builder().allowIfBaseType(ref).build();
ObjectMapper objectMapper = JsonMapper.builder().activateDefaultTyping(ptv, ObjectMapper.DefaultTyping.JAVA_LANG_OBJECT).build();
File jsonFile = new File(path.toString());
@@ -27,4 +32,26 @@ public class JacksonHelper {
}
return null;
}
public static <T> T deserialize(Path path, Class<T> ref, StdDeserializer<T> deserializer) throws IOException {
ObjectMapper objectMapper = new ObjectMapper();
SimpleModule module = new SimpleModule();
module.addDeserializer(ref, deserializer);
objectMapper.registerModule(module);
File jsonFile = new File(path.toString());
if (jsonFile.exists() && jsonFile.length() > 0) {
return objectMapper.readValue(jsonFile, ref);
}
return null;
}
public static <T> void serialize(Path path, T object, Class<T> ref, StdSerializer<T> serializer) throws IOException {
ObjectMapper objectMapper = new ObjectMapper();
SimpleModule module = new SimpleModule();
module.addSerializer(ref, serializer);
objectMapper.registerModule(module);
objectMapper.writerWithDefaultPrettyPrinter().writeValue(new File(path.toString()), object);
}
}

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

@@ -15,9 +15,9 @@ import org.opencv.videoio.VideoCapture;
import java.util.*;
import java.util.stream.Collectors;
@SuppressWarnings("rawtypes")
public class VisionManager {
private VisionManager() {
}
private VisionManager() {}
private static final LinkedHashMap<String, UsbCameraInfo> usbCameraInfosByCameraName = new LinkedHashMap<>();
private static final LinkedList<FullCameraConfiguration> loadedCameraConfigs = new LinkedList<>();

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

@@ -4,6 +4,7 @@ import com.chameleonvision.Debug;
import com.chameleonvision.config.CameraCalibrationConfig;
import com.chameleonvision.config.CameraConfig;
import com.chameleonvision.config.ConfigManager;
import com.chameleonvision.networktables.NetworkTablesManager;
import com.chameleonvision.scripting.ScriptEventType;
import com.chameleonvision.scripting.ScriptManager;
import com.chameleonvision.config.FullCameraConfiguration;
@@ -32,10 +33,10 @@ import java.util.concurrent.LinkedBlockingDeque;
import java.util.stream.Collectors;
@SuppressWarnings("rawtypes")
public class VisionProcess {
private final USBCameraCapture cameraCapture;
// private final CameraStreamerRunnable streamRunnable;
public final USBCameraCapture cameraCapture;
private final VisionProcessRunnable visionRunnable;
private final CameraConfig fileConfig;
public final CameraStreamer cameraStreamer;
@@ -43,8 +44,6 @@ public class VisionProcess {
private volatile CVPipelineResult lastPipelineResult;
private BlockingQueue<Mat> streamFrameQueue = new LinkedBlockingDeque<>(1);
// network table stuff
private final NetworkTable defaultTable;
private NetworkTableInstance tableInstance;
@@ -59,6 +58,12 @@ public class VisionProcess {
private NetworkTableEntry ntLatencyEntry;
private NetworkTableEntry ntValidEntry;
private NetworkTableEntry ntPoseEntry;
private NetworkTableEntry ntFittedHeightEntry;
private NetworkTableEntry ntFittedWidthEntry;
private NetworkTableEntry ntBoundingHeightEntry;
private NetworkTableEntry ntBoundingWidthEntry;
private NetworkTableEntry ntTargetRotation;
private ObjectMapper objectMapper = new ObjectMapper();
private long lastUIUpdateMs = 0;
@@ -72,7 +77,6 @@ public class VisionProcess {
// Thread to put frames on the dashboard
this.cameraStreamer = new CameraStreamer(cameraCapture, config.cameraConfig.name, pipelineManager.getCurrentPipeline().settings.streamDivisor);
// this.streamRunnable = new CameraStreamerRunnable(30, cameraStreamer);
// Thread to process vision data
this.visionRunnable = new VisionProcessRunnable();
@@ -110,21 +114,26 @@ public class VisionProcess {
public void setCameraNickname(String newName) {
getCamera().getProperties().setNickname(newName);
var newTable = NetworkTableInstance.getDefault().getTable("/chameleon-vision/" + newName);
resetNT(newTable);
NetworkTable camTable = NetworkTablesManager.kRootTable.getSubTable(newName);
resetNT(camTable);
}
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");
ntLatencyEntry = newTable.getEntry("latency");
ntValidEntry = newTable.getEntry("is_valid");
ntAuxListEntry = newTable.getEntry("aux_targets");
ntPoseEntry = newTable.getEntry("pose");
private void initNT(NetworkTable camTable) {
tableInstance = camTable.getInstance();
ntPipelineEntry = camTable.getEntry("pipeline");
ntDriverModeEntry = camTable.getEntry("driverMode");
ntPitchEntry = camTable.getEntry("targetPitch");
ntYawEntry = camTable.getEntry("targetYaw");
ntAreaEntry = camTable.getEntry("targetArea");
ntLatencyEntry = camTable.getEntry("latency");
ntValidEntry = camTable.getEntry("isValid");
ntAuxListEntry = camTable.getEntry("auxTargets");
ntPoseEntry = camTable.getEntry("targetPose");
ntFittedHeightEntry = camTable.getEntry("targetFittedHeight");
ntFittedWidthEntry = camTable.getEntry("targetFittedWidth");
ntBoundingHeightEntry = camTable.getEntry("targetBoundingHeight");
ntBoundingWidthEntry = camTable.getEntry("targetBoundingWidth");
ntTargetRotation = camTable.getEntry("targetRotation");
ntDriveModeListenerID = ntDriverModeEntry.addListener(this::setDriverMode, EntryListenerFlags.kUpdate);
ntPipelineListenerID = ntPipelineEntry.addListener(this::setPipeline, EntryListenerFlags.kUpdate);
ntDriverModeEntry.setBoolean(false);
@@ -157,7 +166,6 @@ public class VisionProcess {
}
public void setDriverModeEntry(boolean isDriverMode) {
// if it's null, we haven't even started the program yet, so just return
// otherwise, set it.
if (ntDriverModeEntry != null) {
@@ -176,7 +184,7 @@ public class VisionProcess {
HashMap<String, Object> WebSend = new HashMap<>();
HashMap<String, Object> point = new HashMap<>();
HashMap<String, Object> pointMap = new HashMap<>();
ArrayList<Object> webTargets = new ArrayList<Object>();
ArrayList<Object> webTargets = new ArrayList<>();
List<Double> center = new ArrayList<>();
@@ -203,9 +211,8 @@ public class VisionProcess {
}
center.add(bestTarget.minAreaRect.center.x);
center.add(bestTarget.minAreaRect.center.y);
} catch (ClassCastException ignored) {
}
} else {
pointMap.put("pitch", null);
@@ -236,17 +243,23 @@ public class VisionProcess {
//noinspection unchecked
List<StandardCVPipeline.TrackedTarget> targets = (List<StandardCVPipeline.TrackedTarget>) data.targets;
StandardCVPipeline.TrackedTarget bestTarget = targets.get(0);
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);
ntPitchEntry.setDouble(bestTarget.pitch);
ntYawEntry.setDouble(bestTarget.yaw);
ntAreaEntry.setDouble(bestTarget.area);
ntBoundingHeightEntry.setDouble(bestTarget.boundingRect.height);
ntBoundingWidthEntry.setDouble(bestTarget.boundingRect.width);
ntFittedHeightEntry.setDouble(bestTarget.minAreaRect.size.height);
ntFittedWidthEntry.setDouble(bestTarget.minAreaRect.size.width);
ntTargetRotation.setDouble(bestTarget.minAreaRect.angle);
try {
Pose2d targetPose = targets.get(0).cameraRelativePose;
double[] targetArray = {targetPose.getTranslation().getX(), targetPose.getTranslation().getY(), targetPose.getRotation().getDegrees()};
ntPoseEntry.setDoubleArray(targetArray);
// ntPoseEntry.setString(objectMapper.writeValueAsString(targets.get(0).cameraRelativePose));
ntAuxListEntry.setString(objectMapper.writeValueAsString(targets.stream()
.map(it -> List.of(it.pitch, it.yaw, it.area, it.cameraRelativePose))
.map(it -> List.of(it.pitch, it.yaw, it.area, it.boundingRect.width, it.boundingRect.height, it.minAreaRect.size.width, it.minAreaRect.size.height, it.minAreaRect.angle, it.cameraRelativePose))
.collect(Collectors.toList())));
} catch (JsonProcessingException e) {
e.printStackTrace();
@@ -260,7 +273,6 @@ public class VisionProcess {
}
}
tableInstance.flush();
}
public void setVideoMode(VideoMode newMode) {
@@ -343,8 +355,6 @@ public class VisionProcess {
}
try {
// streamFrameQueue.clear();
// streamFrameQueue.add(lastPipelineResult.outputMat);
var currentTime = System.currentTimeMillis();
if ((currentTime - lastStreamTimeMs) / 1000d > 1.0 / 30.0) {
cameraStreamer.runStream(lastPipelineResult.outputMat);
@@ -372,38 +382,5 @@ public class VisionProcess {
temp /= 7.0;
return temp;
}
}
private class CameraStreamerRunnable extends LoopingRunnable {
final CameraStreamer streamer;
private Mat bufferMat = new Mat();
private CameraStreamerRunnable(int cameraFPS, CameraStreamer streamer) {
// add 2 FPS to allow for a bit of overhead
super(1000L / (cameraFPS + 2));
this.streamer = streamer;
}
@Override
protected void process() {
if (!streamFrameQueue.isEmpty()) {
try {
bufferMat = streamFrameQueue.take();
try {
streamer.runStream(bufferMat);
bufferMat.release();
} catch (Exception e) {
// do nothing
}
} catch (InterruptedException e) {
e.printStackTrace();
}
}
}
}
}

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

@@ -2,6 +2,7 @@ package com.chameleonvision.vision.camera;
import com.chameleonvision.config.CameraCalibrationConfig;
import com.chameleonvision.config.FullCameraConfiguration;
import com.chameleonvision.util.Helpers;
import edu.wpi.cscore.CvSink;
import edu.wpi.cscore.UsbCamera;
import edu.wpi.cscore.VideoException;
@@ -13,6 +14,7 @@ import org.opencv.core.Size;
import org.opencv.imgcodecs.Imgcodecs;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
public class USBCameraCapture implements CameraCapture {
@@ -30,6 +32,12 @@ public class USBCameraCapture implements CameraCapture {
cvSink = CameraServer.getInstance().getVideo(baseCamera);
properties = new USBCaptureProperties(baseCamera, config);
var videoModes = properties.getVideoModes();
if(videoModes.size() < 1) {
throw new VideoException("0 video modes are valid! Full list provided by camera: \n\n"
+ Arrays.stream(baseCamera.enumerateVideoModes()).map(Helpers::VideoModeToHashMap).toString() );
}
int videoMode = properties.videoModes.size() - 1 <= config.videomode ? config.videomode : 0;
setVideoMode(videoMode);
}

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

@@ -17,15 +17,15 @@ 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;
private static final int MINIMUM_FPS = 30;
private static final int MINIMUM_FPS = 21;
private static final int MINIMUM_WIDTH = 320;
private static final int MINIMUM_HEIGHT = 200;
private static final int MAX_INIT_MS = 1500;
private static final int PS3EYE_VID = 1415;
private static final int PS3EYE_PID = 2000;
private static final int PS3EYE_VID = 0x1415;
private static final int PS3EYE_PID = 0x2000;
private static final List<VideoMode.PixelFormat> ALLOWED_PIXEL_FORMATS = Arrays.asList(VideoMode.PixelFormat.kYUYV, VideoMode.PixelFormat.kMJPEG);
private static final List<VideoMode.PixelFormat> ALLOWED_PIXEL_FORMATS = Arrays.asList(VideoMode.PixelFormat.kYUYV, VideoMode.PixelFormat.kMJPEG, VideoMode.PixelFormat.kBGR);
private static final Predicate<VideoMode> kMinFPSPredicate = (videoMode -> videoMode.fps >= MINIMUM_FPS);
private static final Predicate<VideoMode> kMinSizePredicate = (videoMode -> videoMode.width >= MINIMUM_WIDTH && videoMode.height >= MINIMUM_HEIGHT);

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

@@ -118,7 +118,8 @@ public class PipelineManager {
}
public void setCurrentPipeline(int index) {
CVPipeline newPipeline=null;
CVPipeline newPipeline = null;
if (index == DRIVERMODE_INDEX) {
newPipeline = driverModePipeline;
@@ -165,6 +166,11 @@ public class PipelineManager {
ntIndexEntry.setDouble(index);
}
}
// gain setting quirk
if (!parentProcess.cameraCapture.getProperties().isPS3Eye) {
getCurrentPipeline().settings.gain = -1;
}
}
public void addPipeline(CVPipelineSettings setting) {

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

@@ -235,7 +235,7 @@ public class StandardCVPipeline extends CVPipeline<StandardCVPipelineResult, Sta
System.out.printf("full pipeline run time was %.3fms (%.2fFPS)\n", truePipelineTimeMillis, truePipelineFPS);
}
memManager.run();
// memManager.run();
resultCache = new StandardCVPipelineResult(collect2dTargetsResult.getLeft(), outputMat, totalPipelineTimeNanos);
return resultCache;
@@ -260,6 +260,7 @@ public class StandardCVPipeline extends CVPipeline<StandardCVPipelineResult, Sta
public double area = 0.0;
public Point point = new Point();
public RotatedRect minAreaRect;
public Rect boundingRect;
// 3d stuff
public Pose2d cameraRelativePose = new Pose2d();

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

@@ -4,10 +4,7 @@ import com.chameleonvision.vision.enums.*;
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 org.opencv.core.*;
import java.util.Arrays;
import java.util.List;
@@ -20,7 +17,7 @@ public class StandardCVPipelineSettings extends CVPipelineSettings {
public boolean dilate = false;
public List<Number> area = Arrays.asList(0.0, 100.0);
public List<Number> ratio = Arrays.asList(0.0, 20.0);
public List<Number> extent = Arrays.asList(0, 100);
public List<Number> extent = Arrays.asList(0.0, 100.0);
public Number speckle = 5;
public boolean isBinary = false;
public SortMode sortMode = SortMode.Largest;
@@ -35,7 +32,22 @@ public class StandardCVPipelineSettings extends CVPipelineSettings {
public double dualTargetCalibrationB = 0;
// 3d stuff
public double targetWidth = 15.5, targetHeight = 6.0;
public MatOfPoint3f targetCornerMat = new MatOfPoint3f();
private static MatOfPoint3f hexTargetMat = new MatOfPoint3f();
static {
hexTargetMat.fromList(List.of(
new Point3(-19.625, 0, 0),
new Point3(-9.819867, -17, 0),
new Point3(9.819867, -17, 0),
new Point3(19.625, 0, 0)));
}
public StandardCVPipelineSettings() {
super();
hexTargetMat.copyTo(targetCornerMat);
}
public boolean is3D = false;
}

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

@@ -16,10 +16,12 @@ import java.util.List;
public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.TrackedTarget>>, Mat> {
private MatOfPoint3f boxCornerMat = new MatOfPoint3f();
private MatOfPoint tempMatOfPoints = new MatOfPoint();
public Scalar green = Helpers.colorToScalar(Color.GREEN);
public Scalar blue = Helpers.colorToScalar(Color.BLUE);
public Scalar red = Helpers.colorToScalar(Color.RED);
public Scalar orange = Helpers.colorToScalar(Color.orange);
public DrawSolvePNPPipe(CameraCalibrationConfig settings) {
setConfig(settings);
@@ -46,14 +48,14 @@ public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.
public void set2020Box() {
boxCornerMat.release();
boxCornerMat = new MatOfPoint3f(
new Point3(-16.25, 0, 0),
new Point3(-19.625, 0, 0),
new Point3(-9.819867, -17, 0),
new Point3(9.819867, -17, 0),
new Point3(16.25, 0, 0),
new Point3(-16.25, 0, -6),
new Point3(19.625, 0, 0),
new Point3(-19.625, 0, -6),
new Point3(-9.819867, -17, -6),
new Point3(9.819867, -17, -6),
new Point3(16.25, 0, -6)
new Point3(19.625, 0, -6)
);
}
@@ -97,17 +99,20 @@ public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.
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, green, 5);
}
// draw poly dp
var list = it.approxPoly.toList();
for(int i = 0; i < list.size(); i++) {
var next = (i == list.size() - 1) ? list.get(0) : list.get(i + 1);
Imgproc.line(image, list.get(i), next, red, 3);
Imgproc.line(image, list.get(i), next, red, 2);
}
// draw center
Imgproc.circle(image, it.minAreaRect.center, 5, red);
// 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, green, 5);
}
// sketch out floor

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

@@ -7,6 +7,7 @@ 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.core.Point;
import org.opencv.imgproc.Imgproc;
import org.opencv.imgproc.Moments;
@@ -60,9 +61,11 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<StandardCV
contourBuffer.fromArray(c.toArray());
if (contourBuffer.cols() != 0 && contourBuffer.rows() != 0) {
RotatedRect rect = Imgproc.minAreaRect(contourBuffer);
Rect boundingRect = Imgproc.boundingRect(contourBuffer);
var target = new StandardCVPipeline.TrackedTarget();
target.minAreaRect = rect;
target.rawContour = contourBuffer;
target.boundingRect = boundingRect;
groupedContours.add(target);
}
});
@@ -90,9 +93,10 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<StandardCV
if (contourBuffer.cols() != 0 && contourBuffer.rows() != 0) {
RotatedRect rect = Imgproc.minAreaRect(contourBuffer);
Rect boundingRect = Imgproc.boundingRect(contourBuffer);
var target = new StandardCVPipeline.TrackedTarget();
target.minAreaRect = rect;
target.boundingRect = boundingRect;
// find left and right bouding rectangles
target.leftRightDualTargetPair =
Pair.of(Imgproc.boundingRect(firstContour),

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

@@ -48,10 +48,11 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
if(isHighGoal) {
// tl, bl, br, tr is the order
List<Point3> corners = List.of(
new Point3(-16.25, 0, 0),
new Point3(-19.625, 0, 0),
new Point3(-9.819867, -17, 0),
new Point3(9.819867, -17, 0),
new Point3(16.25, 0, 0));
new Point3(19.625, 0, 0));
setObjectCorners(corners);
} else {
setBoundingBoxTarget(7, 11);
@@ -81,6 +82,7 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
// setBoundingBoxTarget(settings.targetWidth, settings.targetHeight);
// TODO add proper year differentiation
tilt_angle = tilt.getRadians();
this.objPointsMat = settings.targetCornerMat;
}
private void setCameraCoeffs(CameraCalibrationConfig settings) {
@@ -137,6 +139,16 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
return points;
}
MatOfPoint2f target2020ResultMat = new MatOfPoint2f();
private double distanceBetween(Point a, Point b) {
return FastMath.sqrt(FastMath.pow(a.x - b.x, 2) + FastMath.pow(a.y - b.y, 2));
}
MatOfInt tempInt = new MatOfInt();
MatOfPoint2f tempMat2f = new MatOfPoint2f();
MatOfPoint tempMatOfPoint = new MatOfPoint();
/**
* Find the target using the outermost tape corners and a 2020 target.
* @param target the target.
@@ -149,25 +161,46 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
Comparator<Point> distanceProvider = Comparator.comparingDouble((Point point) -> FastMath.sqrt(FastMath.pow(centroid.x - point.x, 2) + FastMath.pow(centroid.y - point.y, 2)));
var contour = target.rawContour;
var convex = tempInt;
tempMatOfPoint.fromList(contour.toList());
Imgproc.convexHull(tempMatOfPoint, convex);
var combinedList = contour.toList();
// approx poly dp time
Imgproc.approxPolyDP(contour, polyOutput, 3, true);
Point[] contourArray = contour.toArray();
Point[] hullPoints = new Point[convex.rows()];
List<Integer> hullContourIdxList = convex.toList();
for (int i = 0; i < hullContourIdxList.size(); i++) {
hullPoints[i] = contourArray[hullContourIdxList.get(i)];
}
tempMat2f.fromArray(hullPoints);
Imgproc.approxPolyDP(tempMat2f, polyOutput, 5, true);
var polyList = polyOutput.toList();
polyOutput.copyTo(target.approxPoly);
// start looking in the top left quadrant
// left top, left bottom, right bottom, right top
var boundingBoxCorners = findBoundingBoxCorners(target).toList();
try {
var tl = polyList.stream().filter(point -> point.x < centroid.x && point.y < centroid.y).max(distanceProvider).get();
var tr = polyList.stream().filter(point -> point.x > centroid.x && point.y < centroid.y).max(distanceProvider).get();
// top left and top right are the poly corners closest to the bouding box tl and tr
Point tl = polyList.stream().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(0)))).get();
Point tr = polyList.stream().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(3)))).get();
// var tl = polyList.stream().filter(point -> point.x < centroid.x && point.y < centroid.y).max(distanceProvider).get();
// var tr = polyList.stream().filter(point -> point.x > centroid.x && point.y < centroid.y).max(distanceProvider).get();
var bl = polyList.stream().filter(point -> point.x < centroid.x && point.y > centroid.y).max(distanceProvider).get();
var br = polyList.stream().filter(point -> point.x > centroid.x && point.y > centroid.y).max(distanceProvider).get();
boundingBoxResultMat.release();
boundingBoxResultMat.fromList(List.of(tl, bl, br, tr));
target2020ResultMat.release();
target2020ResultMat.fromList(List.of(tl, bl, br, tr));
return boundingBoxResultMat;
return target2020ResultMat;
} catch (NoSuchElementException e) {
return null;
}
@@ -215,6 +248,11 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
return boundingBoxResultMat;
}
/**
*
* @param target the target to find the corners of.
* @return the corners. left top, left bottom, right bottom, right top
*/
private MatOfPoint2f findBoundingBoxCorners(StandardCVPipeline.TrackedTarget target) {
// List<Pair<MatOfPoint2f, CVPipeline2d.Target2d>> list = new ArrayList<>();
@@ -300,8 +338,6 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
@Deprecated
private MatOfPoint2f findGoodFeaturesToTrack2019(StandardCVPipeline.TrackedTarget target, Mat srcImage) {
// Imgproc.approxPolyDP(new MatOfPoint2f(max_contour.toArray()),approx,epsilon,true);
// start by looking for corners
var points__ = findBoundingBoxCorners(target).toList();
var xList = points__.stream().map(it -> it.x).sorted(Double::compare).collect(Collectors.toList());
@@ -313,7 +349,6 @@ public class SolvePNPPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTa
var boundingBr = new Point (
xList.get(2), yList.get(2)
);
System.out.println("tl/br:\n" + boundingTl.toString() + "\n" + boundingBr.toString());
var slightlyBiggerTl = new Point(

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

@@ -3,6 +3,7 @@ package com.chameleonvision.web;
import com.chameleonvision.Exceptions.DuplicatedKeyException;
import com.chameleonvision.config.ConfigManager;
import com.chameleonvision.network.NetworkIPMode;
import com.chameleonvision.networktables.NetworkTablesManager;
import com.chameleonvision.vision.VisionManager;
import com.chameleonvision.vision.VisionProcess;
import com.chameleonvision.vision.camera.USBCameraCapture;
@@ -18,8 +19,12 @@ import edu.wpi.cscore.VideoMode;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import io.javalin.http.Context;
import io.javalin.http.Handler;
import org.apache.commons.math3.ml.neuralnet.Network;
import org.opencv.core.Point;
import org.opencv.core.Point3;
import java.io.IOException;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
@@ -34,7 +39,11 @@ public class RequestHandler {
Map map = objectMapper.readValue(ctx.body(), Map.class);
// TODO: change to function, to restart NetworkTables
ConfigManager.settings.teamNumber = (int) map.get("teamNumber");
int newTeamNumber = (int) map.get("teamNumber");
if (newTeamNumber != ConfigManager.settings.teamNumber && !NetworkTablesManager.isServer) {
NetworkTablesManager.setTeamClientMode();
}
ConfigManager.settings.teamNumber = newTeamNumber;
ConfigManager.settings.connectionType = NetworkIPMode.values()[(int) map.get("connectionType")];
ConfigManager.settings.ip = (String) map.get("ip");
@@ -71,9 +80,9 @@ public class RequestHandler {
if (cam.getCamera().getProperties().videoModes.size() < newPipeline.videoModeIndex) {
newPipeline.videoModeIndex = cam.getCamera().getProperties().videoModes.size() - 1;
}
if (newPipeline.is3D){
if (newPipeline.is3D) {
var calibration = cam.getCamera().getCalibration(cam.getCamera().getProperties().getVideoMode(newPipeline.videoModeIndex));
if (calibration == null){
if (calibration == null) {
newPipeline.is3D = false;
}
}
@@ -176,9 +185,22 @@ public class RequestHandler {
}
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);
//noinspection unchecked
List<List<Number>> points = kObjectMapper.readValue(ctx.body(), List.class);
// each entry should be an xy pair
var pointsList = new ArrayList<Point3>();
for (List<Number> point : points) {
double x, y;
x = point.get(0).doubleValue();
y = point.get(1).doubleValue();
var pointToAdd = new Point3(x, y, 0.0);
pointsList.add(pointToAdd);
}
System.out.println(pointsList.toString());
if (VisionManager.getCurrentUIVisionProcess().pipelineManager.getCurrentPipeline().settings instanceof StandardCVPipelineSettings) {
var settings = (StandardCVPipelineSettings) VisionManager.getCurrentUIVisionProcess().pipelineManager.getCurrentPipeline().settings;
settings.targetCornerMat.fromList(pointsList);
}
}
}