Added enums, msgpack - new ui intergration

This commit is contained in:
Omer
2019-10-12 03:38:42 +03:00
parent 2eb4cbc950
commit 5f879f9d98
17 changed files with 239 additions and 193 deletions

View File

@@ -0,0 +1,5 @@
package com.chameleonvision.vision;
public enum Orientation {
Normal,Inverted//TODO add 90 and 270 deg rotation?
}

View File

@@ -6,20 +6,20 @@ import java.util.List;
public class Pipeline {
public int exposure = 50;
public int brightness = 50;
public String orientation = "Normal";
public Orientation orientation = Orientation.Normal;
public List<Integer> hue = Arrays.asList(50, 180);
public List<Integer> saturation = Arrays.asList(50, 255);
public List<Integer> value = Arrays.asList(50, 255);
public boolean erode = false;
public boolean dilate = false;
public List<Integer> area = Arrays.asList(0, 100);
public List<Double> ratio = Arrays.asList(0D, 20D);
public List<Float> area = Arrays.asList(0f, 100f);
public List<Float> ratio = Arrays.asList(0f, 20f);
public List<Integer> extent = Arrays.asList(0, 100);
public int is_binary = 0;
public String sort_mode = "Largest";
public String target_group = "Single";
public String target_intersection = "Up";
public boolean isBinary = false;
public SortMode sortMode = SortMode.Largest;
public TargetGroup targetGroup = TargetGroup.Single;
public TargetIntersection targetIntersection = TargetIntersection.Up;
public double M = 1;
public double B = 0;
public boolean is_calibrated = false;
public boolean isCalibrated = false;
}

View File

@@ -0,0 +1,5 @@
package com.chameleonvision.vision;
public enum SortMode {
Largest,Smallest,Highest,Lowest,Rightmost,Leftmost,Centermost
}

View File

@@ -0,0 +1,5 @@
package com.chameleonvision.vision;
public enum TargetGroup {
Single,Dual,Triple,Quadruple,Quintuple
}

View File

@@ -0,0 +1,5 @@
package com.chameleonvision.vision;
public enum TargetIntersection {
None,Up,Down,Left,Right
}

View File

@@ -13,7 +13,7 @@ import java.util.stream.IntStream;
public class Camera {
private static final double DEFAULT_FOV = 60.8;
private static final float DEFAULT_FOV = 60.8f;
private static final int MINIMUM_FPS = 30;
private static final int MINIMUM_WIDTH = 320;
private static final int MINIMUM_HEIGHT = 200;
@@ -29,7 +29,7 @@ public class Camera {
private final CvSink cvSink;
private final Object cvSourceLock = new Object();
private CvSource cvSource;
private double FOV;
private float FOV;
private CameraValues camVals;
private CamVideoMode camVideoMode;
private int currentPipelineIndex;
@@ -39,23 +39,23 @@ public class Camera {
this(cameraName, DEFAULT_FOV);
}
public Camera(String cameraName, double fov) {
public Camera(String cameraName, float fov) {
this(cameraName,CameraManager.AllUsbCameraInfosByName.get(cameraName), fov);
}
public Camera(String cameraName, UsbCameraInfo usbCamInfo, double fov) {
public Camera(String cameraName, UsbCameraInfo usbCamInfo, float fov) {
this(cameraName ,usbCamInfo, fov, new HashMap<>(), 0);
}
public Camera(String cameraName, double fov, int videoModeIndex) {
public Camera(String cameraName, float fov, int videoModeIndex) {
this(cameraName, fov, new HashMap<>(), videoModeIndex);
}
public Camera(String cameraName, double fov, HashMap<Integer, Pipeline> pipelines, int videoModeIndex) {
public Camera(String cameraName, float fov, HashMap<Integer, Pipeline> pipelines, int videoModeIndex) {
this(cameraName, CameraManager.AllUsbCameraInfosByName.get(cameraName), fov, pipelines, videoModeIndex);
}
public Camera(String cameraName, UsbCameraInfo usbCamInfo, double fov, HashMap<Integer, Pipeline> pipelines, int videoModeIndex) {
public Camera(String cameraName, UsbCameraInfo usbCamInfo, float fov, HashMap<Integer, Pipeline> pipelines, int videoModeIndex) {
FOV = fov;
name = cameraName;
path = usbCamInfo.path;
@@ -158,11 +158,11 @@ public class Camera {
.orElse(-1);
}
public double getFOV() {
public float getFOV() {
return FOV;
}
public void setFOV(double fov) {
public void setFOV(float fov) {
FOV = fov;
camVals = new CameraValues(this);
}

View File

@@ -14,7 +14,7 @@ public class CameraDeserializer implements JsonDeserializer<Camera> {
@Override
public Camera deserialize(JsonElement jsonElement, Type type, JsonDeserializationContext context) throws JsonParseException {
var jsonObj = jsonElement.getAsJsonObject();
var camFOV = jsonObj.get("FOV").getAsDouble();
var camFOV = jsonObj.get("FOV").getAsFloat();
var camName = jsonObj.get("name").getAsString();
var videoModeIndex = jsonObj.get("resolution").getAsInt();

View File

@@ -7,7 +7,7 @@ import org.apache.commons.math3.util.FastMath;
public class CameraValues {
public final int ImageWidth;
public final int ImageHeight;
public final double FOV;
public final float FOV;
public final double ImageArea;
public final double CenterX;
public final double CenterY;
@@ -24,7 +24,7 @@ public class CameraValues {
this(camera.getVideoMode().width, camera.getVideoMode().height, camera.getFOV());
}
public CameraValues(int imageWidth, int imageHeight, double fov) {
public CameraValues(int imageWidth, int imageHeight, float fov) {
ImageWidth = imageWidth;
ImageHeight = imageHeight;
FOV = fov;

View File

@@ -1,5 +1,8 @@
package com.chameleonvision.vision.process;
import com.chameleonvision.vision.SortMode;
import com.chameleonvision.vision.TargetGroup;
import com.chameleonvision.vision.TargetIntersection;
import com.chameleonvision.vision.camera.CameraValues;
import com.chameleonvision.util.MathHandler;
import org.apache.commons.math3.util.FastMath;
@@ -14,13 +17,6 @@ import java.util.*;
public class CVProcess {
private final CameraValues cameraValues;
private HashMap<String, Integer> targetGrouping = new HashMap<>() {{
put("Single", 1);
put("Dual", 2);
put("Triple", 3);
put("Quadruple", 4);
put("Quintuple", 5);
}};
private Mat kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(5, 5));
private Size blur = new Size(1,1);
private Mat hsvImage = new Mat();
@@ -58,7 +54,7 @@ public class CVProcess {
return foundContours;
}
List<MatOfPoint> filterContours(List<MatOfPoint> inputContours, List<Integer> area, List<Double> ratio, List<Integer> extent) {
List<MatOfPoint> filterContours(List<MatOfPoint> inputContours, List<Float> area, List<Float> ratio, List<Integer> extent) {
for (MatOfPoint Contour : inputContours) {
try {
double contourArea = Imgproc.contourArea(Contour);
@@ -97,34 +93,34 @@ public class CVProcess {
Moments m = Imgproc.moments(c);
return (m.get_m10()/m.get_m00());
}
RotatedRect sortTargetsToOne(List<RotatedRect> inputRects, String sortMode) {
RotatedRect sortTargetsToOne(List<RotatedRect> inputRects, SortMode sortMode) {
switch (sortMode) {
case "Largest":
case Largest:
return Collections.max(inputRects, Comparator.comparing(rect -> rect.size.area()));
case "Smallest":
case Smallest:
return Collections.min(inputRects, Comparator.comparing(rect -> rect.size.area()));
case "Highest":
case Highest:
return Collections.min(inputRects, Comparator.comparing(rect -> rect.center.y));
case "Lowest":
case Lowest:
return Collections.max(inputRects, Comparator.comparing(rect -> rect.center.y));
case "Leftmost":
case Leftmost:
return Collections.min(inputRects, Comparator.comparing(rect -> rect.center.x));
case "Rightmost":
case Rightmost:
return Collections.max(inputRects, Comparator.comparing(rect -> rect.center.x));
case "Centermost":
case Centermost:
return Collections.min(inputRects, sortByCentermostComparator);
default:
return inputRects.get(0); // default to whatever the first contour is, but this should never happen
}
}
List<RotatedRect> groupTargets(List<MatOfPoint> inputContours, String intersectionPoint, String targetGroup) {
List<RotatedRect> groupTargets(List<MatOfPoint> inputContours, TargetIntersection intersectionPoint, TargetGroup targetGroup) {
finalCountours.clear();
if (!targetGroup.equals("Single")) {
if (!targetGroup.equals(TargetGroup.Single)) {
inputContours.sort(sortByMomentsX);
for (var i = 0; i < inputContours.size(); i++) {
List<Point> FinalContourList = new ArrayList<>(inputContours.get(i).toList());
for (var c = 0; c < (targetGrouping.get(targetGroup) - 1); c++) {
for (var c = 0; c < targetGroup.ordinal(); c++) {
try {
MatOfPoint firstContour = inputContours.get(i + c);
MatOfPoint secondContour = inputContours.get(i + c + 1);
@@ -163,8 +159,8 @@ public class CVProcess {
return finalCountours;
}
private boolean isIntersecting(MatOfPoint ContourOne, MatOfPoint ContourTwo, String IntersectionPoint) {
if (IntersectionPoint.equals("None")) {
private boolean isIntersecting(MatOfPoint ContourOne, MatOfPoint ContourTwo, TargetIntersection intersectionPoint) {
if (intersectionPoint.equals(TargetIntersection.None)) {
return true;
}
try {
@@ -182,26 +178,26 @@ public class CVProcess {
double intersectionY = (mA * (intersectionX - x0A)) + y0A;
double massX = (x0A + x0B) / 2;
double massY = (y0A + y0B) / 2;
switch (IntersectionPoint) {
case "Up": {
switch (intersectionPoint) {
case Up: {
if (intersectionY < massY) {
return true;
}
break;
}
case "Down": {
case Down: {
if (intersectionY > massY) {
return true;
}
break;
}
case "Left": {
case Left: {
if (intersectionX < massX) {
return true;
}
break;
}
case "Right": {
case Right: {
if (intersectionX > massX) {
return true;
}

View File

@@ -1,6 +1,7 @@
package com.chameleonvision.vision.process;
import com.chameleonvision.settings.SettingsManager;
import com.chameleonvision.vision.Orientation;
import com.chameleonvision.vision.Pipeline;
import com.chameleonvision.vision.camera.Camera;
import com.chameleonvision.web.ServerHandler;
@@ -135,7 +136,7 @@ public class VisionProcess implements Runnable {
if (currentPipeline == null) {
return pipelineResult;
}
if (!currentPipeline.orientation.equals("Normal")) {
if (currentPipeline.orientation.equals(Orientation.Inverted)) {
Core.flip(inputImage, inputImage, -1);
}
if (ntDriverModeEntry.getBoolean(false)) {
@@ -147,7 +148,7 @@ public class VisionProcess implements Runnable {
cvProcess.hsvThreshold(inputImage, hsvThreshMat, hsvLower, hsvUpper, currentPipeline.erode, currentPipeline.dilate);
if (currentPipeline.is_binary == 1) {
if (currentPipeline.isBinary == true) {
Imgproc.cvtColor(hsvThreshMat, outputImage, Imgproc.COLOR_GRAY2BGR, 3);
} else {
inputImage.copyTo(outputImage);
@@ -156,13 +157,13 @@ public class VisionProcess implements Runnable {
if (foundContours.size() > 0) {
filteredContours = cvProcess.filterContours(foundContours, currentPipeline.area, currentPipeline.ratio, currentPipeline.extent);
if (filteredContours.size() > 0) {
groupedContours = cvProcess.groupTargets(filteredContours, currentPipeline.target_intersection, currentPipeline.target_group);
groupedContours = cvProcess.groupTargets(filteredContours, currentPipeline.targetIntersection, currentPipeline.targetGroup);
if (groupedContours.size() > 0) {
var finalRect = cvProcess.sortTargetsToOne(groupedContours, currentPipeline.sort_mode);
var finalRect = cvProcess.sortTargetsToOne(groupedContours, currentPipeline.sortMode);
// System.out.printf("Largest Contour Area: %.2f\n", finalRect.size.area());
pipelineResult.RawPoint = finalRect;
pipelineResult.IsValid = true;
if (!currentPipeline.is_calibrated) {
if (!currentPipeline.isCalibrated) {
pipelineResult.CalibratedX = camera.getCamVals().CenterX;
pipelineResult.CalibratedY = camera.getCamVals().CenterY;
} else {

View File

@@ -22,11 +22,10 @@ public class Server {
System.out.println("Socket Disconnected");
SettingsManager.saveSettings();
});
ws.onMessage(ctx -> {
// handler.onMessage(ctx);
ws.onBinaryMessage(ctx -> {
handler.onBinaryMessage(ctx);
});
ws.onBinaryMessage(ctx->handler.onMessage(ctx));
app.start(port);
});
app.start(port);
}
}
}

View File

@@ -1,33 +1,33 @@
package com.chameleonvision.web;
import com.chameleonvision.vision.Orientation;
import com.chameleonvision.vision.SortMode;
import com.chameleonvision.vision.TargetGroup;
import com.chameleonvision.vision.TargetIntersection;
import com.chameleonvision.vision.camera.CameraException;
import com.chameleonvision.settings.SettingsManager;
import com.chameleonvision.vision.camera.CameraManager;
import edu.wpi.cscore.VideoException;
import io.javalin.websocket.*;
import org.apache.commons.lang3.ArrayUtils;
import org.json.JSONArray;
import org.json.JSONObject;
import org.msgpack.MessagePack;
import org.msgpack.template.Templates;
import org.msgpack.type.ArrayValue;
import org.msgpack.type.BooleanValue;
import org.msgpack.type.IntegerValue;
import org.msgpack.type.MapValue;
import org.apache.commons.lang3.builder.ReflectionToStringBuilder;
import org.apache.commons.lang3.builder.ToStringStyle;
import org.msgpack.core.MessagePack;
import org.msgpack.core.MessageUnpacker;
import org.msgpack.value.ImmutableArrayValue;
import org.msgpack.value.ImmutableValue;
import org.msgpack.value.Value;
import org.springframework.beans.BeanUtils;
import java.io.IOException;
import java.lang.reflect.Field;
import java.util.*;
public class ServerHandler {
private static List<WsContext> users;
private MessagePack msgpack;
ServerHandler() {
users = new ArrayList<>();
msgpack = new MessagePack();
}
void onConnect(WsConnectContext context) {
@@ -39,103 +39,97 @@ public class ServerHandler {
users.remove(context);
}
void onMessage(WsBinaryMessageContext data) throws IOException {
byte[] b = ArrayUtils.toPrimitive(data.data());
System.out.println(msgpack.read(b).isMapValue());
Map entries = msgpack.read(b,Templates.tMap(Templates.TString,Templates.TValue));
System.out.println(Arrays.toString(entries.entrySet().toArray()));
entries.forEach((k, value) -> {
/*
To get int from value
((IntegerValue)value).getInt();
To get boolean from value
((BooleanValue)value).getBoolean();
To get Array from value
((ArrayValue) value).toArray();
*/
String key = k.toString();
System.out.printf("Got websocket msgpack data: [%s, %s]\n", key, value);
try{
if (hasField(CameraManager.getCurrentPipeline(), key)) {
//Special cases for exposure and brightness and aspect ratio
switch (key) {
case "exposure":
int newExposure = ((IntegerValue)value).getInt();
System.out.printf("Changing exposure to %d\n", newExposure);
try {
CameraManager.getCurrentCamera().setExposure(newExposure);
} catch (VideoException e) {
System.out.println("Exposure changes is not supported on your webcam/webcam's driver");
}
break;
case "brightness":
int newBrightness = (int) value;
System.out.printf("Changing brightness to %d\n", newBrightness);
CameraManager.getCurrentCamera().setBrightness(newBrightness);
break;
case "ratio":
//If there is any better to convert Integer to Double you're welcome to change it
List<Double> doubleRatio = CameraManager.getCurrentPipeline().ratio;
List<Object> newRatio = ((JSONArray) value).toList();
for (int i = 0; i < newRatio.size(); i++) {
doubleRatio.set(i, Double.parseDouble(newRatio.get(i).toString()));
}
break;
default:
//Any other field in CameraManager that doesn't need anything special
setField(CameraManager.getCurrentPipeline(), key, value);
System.out.println("CameraManager.getCurrentPipeline().hue = " + CameraManager.getCurrentPipeline().hue.get(0));
break;
void onBinaryMessage(WsBinaryMessageContext data) throws Exception {
MessageUnpacker unpacker = MessagePack.newDefaultUnpacker(ArrayUtils.toPrimitive(data.data()));
int length = unpacker.unpackMapHeader();
for (int mapIndex = 0; mapIndex < length; mapIndex++) {
String key = unpacker.unpackString(); // key
Object value = get(unpacker.unpackValue());
try {
if (hasField(CameraManager.getCurrentPipeline(), key)) {
//Special cases for exposure and brightness and aspect ratio
switch (key) {
case "exposure":
int newExposure = (int) value;
System.out.printf("Changing exposure to %d\n", newExposure);
try {
;
CameraManager.getCurrentCamera().setExposure(newExposure);
} catch (VideoException e) {
System.out.println("Exposure changes is not supported on your webcam/webcam's driver");
}
break;
case "brightness":
int newBrightness = (int) value;
System.out.printf("Changing brightness to %d\n", newBrightness);
CameraManager.getCurrentCamera().setBrightness(newBrightness);
break;
case "ratio":
CameraManager.getCurrentPipeline().ratio=getFloatList((ImmutableArrayValue) value);
break;
case "area":
CameraManager.getCurrentPipeline().area=getFloatList((ImmutableArrayValue) value);
break;
//Enums
case "targetIntersection":
setField(CameraManager.getCurrentPipeline(), key, TargetIntersection.values()[(int) value]);
break;
case "targetGroup":
setField(CameraManager.getCurrentPipeline(), key, TargetGroup.values()[(int) value]);
break;
case "sortMode":
setField(CameraManager.getCurrentPipeline(), key, SortMode.values()[(int) value]);
break;
case "orientation":
setField(CameraManager.getCurrentPipeline(), key, Orientation.values()[(int) value]);
break;
default:
//Any other field in CameraManager that doesn't need anything special
setField(CameraManager.getCurrentPipeline(), key, value);
break;
}
} else {
switch (key) {
case "change_general_settings_values":
Map<String, Object> map = (Map<String, Object>) value;
map.forEach((s, o) -> setField(SettingsManager.GeneralSettings, s, o));
SettingsManager.saveSettings();
break;
case "curr_camera":
String newCamera = (String) value;
System.out.printf("Changing camera to %s\n", newCamera);
CameraManager.setCurrentCamera(newCamera);
HashMap<String, Integer> portMap = new HashMap<>();
portMap.put("port", CameraManager.getCurrentCamera().getStreamPort());
sendFullSettings();
break;
case "curr_pipeline":
int newPipeline = (int) value;
System.out.printf("Changing pipeline to %s\n", newPipeline);
CameraManager.setCurrentPipeline(newPipeline);
CameraManager.getCurrentCameraProcess().ntPipelineEntry.setNumber(newPipeline);
broadcastMessage(allFieldsToMap(CameraManager.getCurrentPipeline()));
break;
case "resolution":
int newVideoMode = (int) value;
System.out.printf("Changing video mode to %d\n", newVideoMode);
CameraManager.getCurrentCamera().setCamVideoMode(newVideoMode, true);
break;
case "FOV":
float newFov = ((Integer) value) * 1F;//TODO check this
System.out.printf("Changing FOV to %f\n", newFov);
CameraManager.getCurrentCamera().setFOV(newFov);
break;
default:
System.out.printf("Unexpected value from websocket: [%s, %s]\n", key, value);
break;
}
}
} else {
switch (key) {
case "change_general_settings_values":
Map<String, Object> map = (Map<String, Object>) value;
map.forEach((s, o) -> setField(SettingsManager.GeneralSettings, s, o));
SettingsManager.saveSettings();
break;
case "curr_camera":
String newCamera = (String) value;
System.out.printf("Changing camera to %s\n", newCamera);
CameraManager.setCurrentCamera(newCamera);
HashMap<String, Integer> portMap = new HashMap<>();
portMap.put("port", CameraManager.getCurrentCamera().getStreamPort());
sendFullSettings();
break;
case "curr_pipeline":
int newPipeline = (int) value;
System.out.printf("Changing pipeline to %s\n", newPipeline);
CameraManager.setCurrentPipeline(newPipeline);
CameraManager.getCurrentCameraProcess().ntPipelineEntry.setNumber(newPipeline);
broadcastMessage(allFieldsToMap(CameraManager.getCurrentPipeline()));
break;
case "resolution":
int newVideoMode = (int) value;
System.out.printf("Changing video mode to %d\n", newVideoMode);
CameraManager.getCurrentCamera().setCamVideoMode(newVideoMode, true);
break;
case "FOV":
double newFov = Double.parseDouble(value.toString());//TODO check this
System.out.printf("Changing FOV to %f\n", newFov);
CameraManager.getCurrentCamera().setFOV(newFov);
break;
default:
System.out.printf("Unexpected value from websocket: [%s, %s]\n", key, value);
break;
}}
}
catch (CameraException e)
{
} catch (CameraException e) {
System.err.println("Camera error");
e.printStackTrace();
}
});
broadcastMessage(entries, data);
}
}
private void setField(Object obj, String fieldName, Object value) {
@@ -143,33 +137,64 @@ public class ServerHandler {
Field[] fields = obj.getClass().getFields();
for (Field f : fields) {
if (f.getName().equals(fieldName)) {
if (BeanUtils.isSimpleValueType(value.getClass())) {
if (BeanUtils.isSimpleValueType(f.getType())) {
f.set(obj, value);
} else if (value.getClass() == ArrayValue.class) {
f.set(obj, ((ArrayValue) value).toArray());
} else if (value instanceof ImmutableArrayValue) {
List<Value> valLst = ((ImmutableArrayValue) value).list();
List<Object> lst = new ArrayList<>();
for (Value val : valLst) {
lst.add(get((ImmutableValue) val));
}
f.set(obj, lst);
}
}
}
} catch (IllegalAccessException e) {
System.out.println("IllegalAccessException ");
} catch (Exception e) {
System.out.println("Exception setting field ");
e.printStackTrace();
}
}
private static void broadcastMessage(Object obj, WsContext userToSkip) {//TODO check if session id is a good way to differentiate users
for (var user : users) {
if (userToSkip != null && user.getSessionId().equals(userToSkip.getSessionId())) {
continue;
}
user.send(obj);
// if (obj.getClass() == String.class)
// user.send((String) obj);
// else if (obj.getClass() == HashMap.class)
// user.send(new JSONObject((HashMap<String, Object>) obj).toString());
// else
// user.send(new JSONObject(obj).toString());
private Object get(ImmutableValue v) throws Exception {
//TODO find a smarter way to write this
if (v.isIntegerValue())
return v.asIntegerValue().toInt();
if (v.isFloatValue())
return v.asFloatValue().toFloat();
if (v.isArrayValue()) {
return v.asArrayValue();
}
if (v.isBinaryValue())
return v.asBinaryValue().asByteArray();
if (v.isBooleanValue())
return v.asBooleanValue().getBoolean();
if (v.isMapValue())
return v.asMapValue().map();
if (v.isStringValue())
return v.asStringValue().asString();
throw new Exception("Value not recognized");
}
private List<Float> getFloatList(ImmutableArrayValue arrayValue) {
List<Float> output = new ArrayList<>();
List<Value> values = arrayValue.list();
for (Value v:values) {
if (v.isFloatValue())
output.add(v.asFloatValue().toFloat());
else
output.add((float) v.asIntegerValue().toInt());
}
return output;
}
private static void broadcastMessage(Object obj, WsContext userToSkip) {
if (users != null)
for (var user : users) {
if (userToSkip != null && user.getSessionId().equals(userToSkip.getSessionId())) {
continue;
}
user.send(obj);
}
}
public static void broadcastMessage(Object obj) {//TODO fix sending for msgpack