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

Robot offset point (#98)

Browse Source 
* Add offset point calculation in backend

* Add pipeline result caching

* Add dual offset unit test
This commit is contained in:
Banks T
2020-08-27 14:41:03 -04:00
committed by GitHub
parent 9f0e89ea29
commit b6d9fe216a
22 changed files with 415 additions and 218 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
photon-server/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java Normal file
View File

@@ -0,0 +1,27 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2015-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.trajectory;
// This is a stub from WPILib
public class Trajectory {
public static class State {
/**
*
* Linearly interpolates between two values.
*
* @param startValue The start value.
* @param endValue The end value.
* @param t The fraction for interpolation.
* @return The interpolated value.
*/
@SuppressWarnings("ParameterName")
public static double lerp(double startValue, double endValue, double t) {
return startValue + (endValue - startValue) * t;
}
}
}

9
photon-server/src/main/java/org/photonvision/common/util/math/MathUtils.java
View File

@@ -47,4 +47,13 @@ public class MathUtils {
public static double nanosToMillis(long nanos) {
return nanos / 1000000.0;
}
public static double map(
double value, double in_min, double in_max, double out_min, double out_max) {
return (value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
public static int map(int value, int inMin, int inMax, int outMin, int outMax) {
return (int) Math.floor(map((double) value, inMin, inMax, outMin, outMax) + 0.5);
}
}

46
photon-server/src/main/java/org/photonvision/server/SocketHandler.java
View File

@@ -52,8 +52,6 @@ public class SocketHandler {
public static class UIMap extends HashMap<String, Object> {}
abstract static class SelectiveBroadcastPair extends Pair<UIMap, WsContext> {}
private static class ThreadSafeSingleton {
private static final SocketHandler INSTANCE = new SocketHandler();
}
@@ -199,30 +197,28 @@ public class SocketHandler {
dcService.publishEvent(newPipelineEvent);
break;
}
case SMT_COMMAND:
case SMT_DELETECURRENTPIPELINE:
{
var cmd = SocketMessageCommandType.fromEntryKey((String) entryValue);
switch (cmd) {
case SMCT_DELETECURRENTPIPELINE:
{
var deleteCurrentPipelineEvent =
new IncomingWebSocketEvent<>(
DataChangeDestination.DCD_ACTIVEMODULE,
"deleteCurrPipeline",
0,
cameraIndex,
context);
dcService.publishEvent(deleteCurrentPipelineEvent);
break;
}
case SMCT_SAVE:
{
var saveEvent =
new IncomingWebSocketEvent<>(DataChangeDestination.DCD_OTHER, "save", 0);
dcService.publishEvent(saveEvent);
break;
}
}
var deleteCurrentPipelineEvent =
new IncomingWebSocketEvent<>(
DataChangeDestination.DCD_ACTIVEMODULE,
"deleteCurrPipeline",
0,
cameraIndex,
context);
dcService.publishEvent(deleteCurrentPipelineEvent);
break;
}
case SMT_ROBOTOFFSETPOINT:
{
var robotOffsetPointEvent =
new IncomingWebSocketEvent<>(
DataChangeDestination.DCD_ACTIVEMODULE,
"robotOffsetPoint",
(Integer) entryValue,
cameraIndex,
null);
dcService.publishEvent(robotOffsetPointEvent);
break;
}
case SMT_CURRENTCAMERA:

5
photon-server/src/main/java/org/photonvision/server/SocketMessageType.java
View File

@@ -27,13 +27,14 @@ public enum SocketMessageType {
SMT_CHANGECAMERANAME("changeCameraName"),
SMT_CHANGEPIPELINENAME("changePipelineName"),
SMT_ADDNEWPIPELINE("addNewPipeline"),
SMT_COMMAND("command"),
SMT_DELETECURRENTPIPELINE("deleteCurrentPipeline"),
SMT_CURRENTCAMERA("currentCamera"),
SMT_PIPELINESETTINGCHANGE("changePipelineSetting"),
SMT_CURRENTPIPELINE("currentPipeline"),
SMT_STARTPNPCALIBRATION("startPnpCalibration"),
SMT_TAKECALIBRATIONSNAPSHOT("takeCalibrationSnapshot"),
SMT_DUPLICATEPIPELINE("duplicatePipeline");
SMT_DUPLICATEPIPELINE("duplicatePipeline"),
SMT_ROBOTOFFSETPOINT("robotOffsetPoint");
public final String entryKey;

48
photon-server/src/main/java/org/photonvision/vision/opencv/DualOffsetValues.java Normal file
View File

@@ -0,0 +1,48 @@
/*
* Copyright (C) 2020 Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package org.photonvision.vision.opencv;
import org.opencv.core.Point;
import org.photonvision.common.util.numbers.DoubleCouple;
import org.photonvision.vision.target.TargetCalculations;
public class DualOffsetValues {
public final Point firstPoint;
public final double firstPointArea;
public final Point secondPoint;
public final double secondPointArea;
public DualOffsetValues() {
firstPoint = new Point();
firstPointArea = 0;
secondPoint = new Point();
secondPointArea = 0;
}
public DualOffsetValues(
Point firstPoint, double firstPointArea, Point secondPoint, double secondPointArea) {
this.firstPoint = firstPoint;
this.firstPointArea = firstPointArea;
this.secondPoint = secondPoint;
this.secondPointArea = secondPointArea;
}
public DoubleCouple getLineValues() {
return TargetCalculations.getLineFromPoints(firstPoint, secondPoint);
}
}

84
photon-server/src/main/java/org/photonvision/vision/pipe/impl/Collect2dTargetsPipe.java
View File

@@ -20,8 +20,8 @@ package org.photonvision.vision.pipe.impl;
import java.util.ArrayList;
import java.util.List;
import org.opencv.core.Point;
import org.photonvision.common.util.numbers.DoubleCouple;
import org.photonvision.vision.frame.FrameStaticProperties;
import org.photonvision.vision.opencv.DualOffsetValues;
import org.photonvision.vision.pipe.CVPipe;
import org.photonvision.vision.target.*;
@@ -42,15 +42,12 @@ public class Collect2dTargetsPipe
var calculationParams =
new TrackedTarget.TargetCalculationParameters(
params.getOrientation() == TargetOrientation.Landscape,
params.getOffsetPointRegion(),
params.getUserOffsetPoint(),
params.getFrameStaticProperties().centerPoint,
new DoubleCouple(params.getCalibrationB(), params.getCalibrationM()),
params.getOffsetMode(),
params.getFrameStaticProperties().horizontalFocalLength,
params.getFrameStaticProperties().verticalFocalLength,
params.getFrameStaticProperties().imageArea);
params.targetOrientation == TargetOrientation.Landscape,
params.targetOffsetPointEdge,
params.robotOffsetPointMode,
params.robotOffsetSinglePoint,
params.dualOffsetValues,
params.frameStaticProperties);
for (PotentialTarget target : in) {
targets.add(new TrackedTarget(target, calculationParams));
@@ -60,57 +57,26 @@ public class Collect2dTargetsPipe
}
public static class Collect2dTargetsParams {
private final FrameStaticProperties m_frameStaticProperties;
private final RobotOffsetPointMode m_offsetMode;
private final double m_calibrationM;
private final double m_calibrationB;
private final Point m_userOffsetPoint;
private final TargetOffsetPointEdge m_region;
private final TargetOrientation m_orientation;
private final RobotOffsetPointMode robotOffsetPointMode;
private final Point robotOffsetSinglePoint;
private final DualOffsetValues dualOffsetValues;
private final TargetOffsetPointEdge targetOffsetPointEdge;
private final TargetOrientation targetOrientation;
private final FrameStaticProperties frameStaticProperties;
public Collect2dTargetsParams(
FrameStaticProperties frameStaticProperties,
RobotOffsetPointMode offsetMode,
double calibrationM,
double calibrationB,
Point calibrationPoint,
TargetOffsetPointEdge region,
TargetOrientation orientation) {
m_frameStaticProperties = frameStaticProperties;
m_offsetMode = offsetMode;
m_calibrationM = calibrationM;
m_calibrationB = calibrationB;
m_userOffsetPoint = calibrationPoint;
m_region = region;
m_orientation = orientation;
}
public FrameStaticProperties getFrameStaticProperties() {
return m_frameStaticProperties;
}
public RobotOffsetPointMode getOffsetMode() {
return m_offsetMode;
}
public double getCalibrationM() {
return m_calibrationM;
}
public double getCalibrationB() {
return m_calibrationB;
}
public Point getUserOffsetPoint() {
return m_userOffsetPoint;
}
public TargetOffsetPointEdge getOffsetPointRegion() {
return m_region;
}
public TargetOrientation getOrientation() {
return m_orientation;
RobotOffsetPointMode robotOffsetPointMode,
Point robotOffsetSinglePoint,
DualOffsetValues dualOffsetValues,
TargetOffsetPointEdge targetOffsetPointEdge,
TargetOrientation orientation,
FrameStaticProperties frameStaticProperties) {
this.frameStaticProperties = frameStaticProperties;
this.robotOffsetPointMode = robotOffsetPointMode;
this.robotOffsetSinglePoint = robotOffsetSinglePoint;
this.dualOffsetValues = dualOffsetValues;
this.targetOffsetPointEdge = targetOffsetPointEdge;
targetOrientation = orientation;
}
}
}

58
photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw2dCrosshairPipe.java
View File

@@ -24,9 +24,11 @@ import org.opencv.core.Mat;
import org.opencv.core.Point;
import org.opencv.imgproc.Imgproc;
import org.photonvision.common.util.ColorHelper;
import org.photonvision.common.util.numbers.DoubleCouple;
import org.photonvision.vision.frame.FrameStaticProperties;
import org.photonvision.vision.opencv.DualOffsetValues;
import org.photonvision.vision.pipe.MutatingPipe;
import org.photonvision.vision.target.RobotOffsetPointMode;
import org.photonvision.vision.target.TargetCalculations;
import org.photonvision.vision.target.TrackedTarget;
public class Draw2dCrosshairPipe
@@ -37,22 +39,32 @@ public class Draw2dCrosshairPipe
protected Void process(Pair<Mat, List<TrackedTarget>> in) {
if (!params.shouldDraw) return null;
Mat image = in.getLeft();
var camCenterPoint = params.frameStaticProperties.centerPoint;
var image = in.getLeft();
if (params.showCrosshair) {
double x = image.cols() / 2.0;
double y = image.rows() / 2.0;
double scale = image.cols() / 32.0;
double x = params.frameStaticProperties.centerX;
double y = params.frameStaticProperties.centerY;
double scale = params.frameStaticProperties.imageWidth / 32.0;
switch (params.calibrationMode) {
switch (params.robotOffsetPointMode) {
case Single:
if (!params.calibrationPoint.isEmpty()) {
x = params.calibrationPoint.getFirst();
y = params.calibrationPoint.getSecond();
if (params.singleOffsetPoint.x != 0 && params.singleOffsetPoint.y != 0) {
x = params.singleOffsetPoint.x;
y = params.singleOffsetPoint.y;
}
break;
case Dual:
// TODO: draw crosshair based on dual calibration
if (in.getRight().size() >= 1) {
var target = in.getRight().get(0);
if (target != null) {
var area = target.getArea();
var offsetCrosshair =
TargetCalculations.calculateDualOffsetCrosshair(params.dualOffsetValues, area);
x = offsetCrosshair.x;
y = offsetCrosshair.y;
}
}
break;
}
@@ -72,14 +84,30 @@ public class Draw2dCrosshairPipe
public Color crosshairColor = Color.GREEN;
public final boolean shouldDraw;
public final RobotOffsetPointMode calibrationMode;
public final DoubleCouple calibrationPoint;
public final FrameStaticProperties frameStaticProperties;
public final RobotOffsetPointMode robotOffsetPointMode;
public final Point singleOffsetPoint;
public final DualOffsetValues dualOffsetValues;
public Draw2dCrosshairParams(FrameStaticProperties frameStaticProperties) {
shouldDraw = true;
this.frameStaticProperties = frameStaticProperties;
robotOffsetPointMode = RobotOffsetPointMode.None;
singleOffsetPoint = new Point();
dualOffsetValues = new DualOffsetValues();
}
public Draw2dCrosshairParams(
boolean shouldDraw, RobotOffsetPointMode calibrationMode, DoubleCouple calibrationPoint) {
boolean shouldDraw,
RobotOffsetPointMode robotOffsetPointMode,
Point singleOffsetPoint,
DualOffsetValues dualOffsetValues,
FrameStaticProperties frameStaticProperties) {
this.shouldDraw = shouldDraw;
this.calibrationMode = calibrationMode;
this.calibrationPoint = calibrationPoint;
this.frameStaticProperties = frameStaticProperties;
this.robotOffsetPointMode = robotOffsetPointMode;
this.singleOffsetPoint = singleOffsetPoint;
this.dualOffsetValues = dualOffsetValues;
}
}
}

6
photon-server/src/main/java/org/photonvision/vision/pipe/impl/SortContoursPipe.java
View File

@@ -60,14 +60,14 @@ public class SortContoursPipe
public static class SortContoursParams {
private final ContourSortMode m_sortMode;
private final FrameStaticProperties m_frameStaticProperties;
private final int m_maxTargets;
private final FrameStaticProperties m_frameStaticProperties;
public SortContoursParams(
ContourSortMode sortMode, FrameStaticProperties camProperties, int maxTargets) {
ContourSortMode sortMode, int maxTargets, FrameStaticProperties camProperties) {
m_sortMode = sortMode;
m_frameStaticProperties = camProperties;
m_maxTargets = maxTargets;
m_frameStaticProperties = camProperties;
}
public ContourSortMode getSortMode() {

27
photon-server/src/main/java/org/photonvision/vision/pipeline/AdvancedPipelineSettings.java
View File

@@ -18,6 +18,7 @@
package org.photonvision.vision.pipeline;
import java.util.Objects;
import org.opencv.core.Point;
import org.photonvision.common.util.numbers.DoubleCouple;
import org.photonvision.common.util.numbers.IntegerCouple;
import org.photonvision.vision.opencv.ContourSortMode;
@@ -62,11 +63,13 @@ public class AdvancedPipelineSettings extends CVPipelineSettings {
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();
public Point offsetSinglePoint = new Point();
// 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;
public Point offsetDualPointA = new Point();
public double offsetDualPointAArea = 0;
public Point offsetDualPointB = new Point();
public double offsetDualPointBArea = 0;
@Override
public boolean equals(Object o) {
@@ -79,8 +82,12 @@ public class AdvancedPipelineSettings extends CVPipelineSettings {
&& erode == that.erode
&& dilate == that.dilate
&& contourSpecklePercentage == that.contourSpecklePercentage
&& Double.compare(that.offsetDualLineM, offsetDualLineM) == 0
&& Double.compare(that.offsetDualLineB, offsetDualLineB) == 0
&& Double.compare(that.offsetDualPointA.x, offsetDualPointA.x) == 0
&& Double.compare(that.offsetDualPointA.y, offsetDualPointA.y) == 0
&& Double.compare(that.offsetDualPointAArea, offsetDualPointAArea) == 0
&& Double.compare(that.offsetDualPointB.x, offsetDualPointB.x) == 0
&& Double.compare(that.offsetDualPointB.y, offsetDualPointB.y) == 0
&& Double.compare(that.offsetDualPointBArea, offsetDualPointBArea) == 0
&& hsvHue.equals(that.hsvHue)
&& hsvSaturation.equals(that.hsvSaturation)
&& hsvValue.equals(that.hsvValue)
@@ -91,7 +98,7 @@ public class AdvancedPipelineSettings extends CVPipelineSettings {
&& contourTargetOffsetPointEdge == that.contourTargetOffsetPointEdge
&& contourTargetOrientation == that.contourTargetOrientation
&& offsetRobotOffsetMode == that.offsetRobotOffsetMode
&& offsetCalibrationPoint.equals(that.offsetCalibrationPoint);
&& offsetSinglePoint.equals(that.offsetSinglePoint);
}
@Override
@@ -113,8 +120,10 @@ public class AdvancedPipelineSettings extends CVPipelineSettings {
contourTargetOffsetPointEdge,
contourTargetOrientation,
offsetRobotOffsetMode,
offsetCalibrationPoint,
offsetDualLineM,
offsetDualLineB);
offsetSinglePoint,
offsetDualPointA,
offsetDualPointAArea,
offsetDualPointB,
offsetDualPointBArea);
}
}

24
photon-server/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java
View File

@@ -71,6 +71,13 @@ public class ColoredShapePipeline
protected void setPipeParams(
FrameStaticProperties frameStaticProperties, ColoredShapePipelineSettings settings) {
DualOffsetValues dualOffsetValues =
new DualOffsetValues(
settings.offsetDualPointA,
settings.offsetDualPointAArea,
settings.offsetDualPointB,
settings.offsetDualPointBArea);
RotateImagePipe.RotateImageParams rotateImageParams =
new RotateImagePipe.RotateImageParams(settings.inputImageRotationMode);
rotateImagePipe.setParams(rotateImageParams);
@@ -126,19 +133,18 @@ public class ColoredShapePipeline
SortContoursPipe.SortContoursParams sortContoursParams =
new SortContoursPipe.SortContoursParams(
settings.contourSortMode,
frameStaticProperties,
settings.outputShowMultipleTargets ? 5 : 1); // TODO don't hardcode?
settings.outputShowMultipleTargets ? 5 : 1,
frameStaticProperties); // TODO don't hardcode?
sortContoursPipe.setParams(sortContoursParams);
Collect2dTargetsPipe.Collect2dTargetsParams collect2dTargetsParams =
new Collect2dTargetsPipe.Collect2dTargetsParams(
frameStaticProperties,
settings.offsetRobotOffsetMode,
settings.offsetDualLineM,
settings.offsetDualLineB,
settings.offsetCalibrationPoint.toPoint(),
settings.offsetSinglePoint,
dualOffsetValues,
settings.contourTargetOffsetPointEdge,
settings.contourTargetOrientation);
settings.contourTargetOrientation,
frameStaticProperties);
collect2dTargetsPipe.setParams(collect2dTargetsParams);
var params =
@@ -167,7 +173,9 @@ public class ColoredShapePipeline
new Draw2dCrosshairPipe.Draw2dCrosshairParams(
settings.outputShouldDraw,
settings.offsetRobotOffsetMode,
settings.offsetCalibrationPoint);
settings.offsetSinglePoint,
dualOffsetValues,
frameStaticProperties);
draw2dCrosshairPipe.setParams(draw2dCrosshairParams);
var draw3dContoursParams =

3
photon-server/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java
View File

@@ -45,8 +45,7 @@ public class DriverModePipeline
rotateImagePipe.setParams(rotateImageParams);
Draw2dCrosshairPipe.Draw2dCrosshairParams draw2dCrosshairParams =
new Draw2dCrosshairPipe.Draw2dCrosshairParams(
true, settings.offsetPointMode, settings.offsetPoint);
new Draw2dCrosshairPipe.Draw2dCrosshairParams(frameStaticProperties);
draw2dCrosshairPipe.setParams(draw2dCrosshairParams);
}

25
photon-server/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java
View File

@@ -26,6 +26,7 @@ import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.frame.FrameStaticProperties;
import org.photonvision.vision.opencv.CVMat;
import org.photonvision.vision.opencv.Contour;
import org.photonvision.vision.opencv.DualOffsetValues;
import org.photonvision.vision.pipe.CVPipe.CVPipeResult;
import org.photonvision.vision.pipe.impl.*;
import org.photonvision.vision.pipeline.result.CVPipelineResult;
@@ -68,6 +69,13 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
protected void setPipeParams(
FrameStaticProperties frameStaticProperties, ReflectivePipelineSettings settings) {
DualOffsetValues dualOffsetValues =
new DualOffsetValues(
settings.offsetDualPointA,
settings.offsetDualPointAArea,
settings.offsetDualPointB,
settings.offsetDualPointBArea);
RotateImagePipe.RotateImageParams rotateImageParams =
new RotateImagePipe.RotateImageParams(settings.inputImageRotationMode);
rotateImagePipe.setParams(rotateImageParams);
@@ -105,19 +113,18 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
SortContoursPipe.SortContoursParams sortContoursParams =
new SortContoursPipe.SortContoursParams(
settings.contourSortMode,
frameStaticProperties,
settings.outputShowMultipleTargets ? 5 : 1); // TODO don't hardcode?
settings.outputShowMultipleTargets ? 5 : 1, // TODO don't hardcode?
frameStaticProperties);
sortContoursPipe.setParams(sortContoursParams);
Collect2dTargetsPipe.Collect2dTargetsParams collect2dTargetsParams =
new Collect2dTargetsPipe.Collect2dTargetsParams(
frameStaticProperties,
settings.offsetRobotOffsetMode,
settings.offsetDualLineM,
settings.offsetDualLineB,
settings.offsetCalibrationPoint.toPoint(),
settings.offsetSinglePoint,
dualOffsetValues,
settings.contourTargetOffsetPointEdge,
settings.contourTargetOrientation);
settings.contourTargetOrientation,
frameStaticProperties);
collect2dTargetsPipe.setParams(collect2dTargetsParams);
var params =
@@ -138,7 +145,9 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
new Draw2dCrosshairPipe.Draw2dCrosshairParams(
settings.outputShouldDraw,
settings.offsetRobotOffsetMode,
settings.offsetCalibrationPoint);
settings.offsetSinglePoint,
dualOffsetValues,
frameStaticProperties);
draw2dCrosshairPipe.setParams(draw2dCrosshairParams);
var draw3dContoursParams =

5
photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java
View File

@@ -42,6 +42,7 @@ import org.photonvision.vision.pipeline.ReflectivePipelineSettings;
import org.photonvision.vision.pipeline.UICalibrationData;
import org.photonvision.vision.pipeline.result.CVPipelineResult;
import org.photonvision.vision.target.TargetModel;
import org.photonvision.vision.target.TrackedTarget;
/**
* This is the God Class
@@ -65,6 +66,7 @@ public class VisionModule {
protected final QuirkyCamera cameraQuirks;
private long lastFrameConsumeMillis;
protected TrackedTarget lastPipelineResultBestTarget;
MJPGFrameConsumer dashboardInputStreamer;
MJPGFrameConsumer dashboardOutputStreamer;
@@ -112,6 +114,9 @@ public class VisionModule {
uiDataConsumer = new UIDataPublisher(index);
addResultConsumer(ntConsumer);
addResultConsumer(uiDataConsumer);
addResultConsumer(
(result) ->
lastPipelineResultBestTarget = result.hasTargets() ? result.targets.get(0) : null);
setPipeline(visionSource.getSettables().getConfiguration().currentPipelineIndex);

45
photon-server/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java
View File

@@ -20,6 +20,7 @@ package org.photonvision.vision.processes;
import java.util.ArrayList;
import java.util.Map;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Point;
import org.photonvision.common.dataflow.DataChangeSubscriber;
import org.photonvision.common.dataflow.events.DataChangeEvent;
import org.photonvision.common.dataflow.events.IncomingWebSocketEvent;
@@ -29,8 +30,10 @@ import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.numbers.DoubleCouple;
import org.photonvision.common.util.numbers.IntegerCouple;
import org.photonvision.vision.camera.CameraQuirk;
import org.photonvision.vision.pipeline.AdvancedPipelineSettings;
import org.photonvision.vision.pipeline.PipelineType;
import org.photonvision.vision.pipeline.UICalibrationData;
import org.photonvision.vision.target.RobotOffsetPointOperation;
@SuppressWarnings("unchecked")
public class VisionModuleChangeSubscriber extends DataChangeSubscriber {
@@ -136,6 +139,47 @@ public class VisionModuleChangeSubscriber extends DataChangeSubscriber {
case "takeCalSnapshot":
parentModule.takeCalibrationSnapshot();
return;
case "robotOffsetPoint":
if (currentSettings instanceof AdvancedPipelineSettings) {
var curAdvSettings = (AdvancedPipelineSettings) currentSettings;
var offsetOperation = RobotOffsetPointOperation.fromIndex((int) newPropValue);
var latestTarget = parentModule.lastPipelineResultBestTarget;
if (latestTarget != null) {
var newPoint = latestTarget.getTargetOffsetPoint();
switch (curAdvSettings.offsetRobotOffsetMode) {
case Single:
if (offsetOperation == RobotOffsetPointOperation.ROPO_CLEAR) {
curAdvSettings.offsetSinglePoint = new Point();
} else if (offsetOperation == RobotOffsetPointOperation.ROPO_TAKESINGLE) {
curAdvSettings.offsetSinglePoint = newPoint;
}
break;
case Dual:
if (offsetOperation == RobotOffsetPointOperation.ROPO_CLEAR) {
curAdvSettings.offsetDualPointA = new Point();
curAdvSettings.offsetDualPointAArea = 0;
curAdvSettings.offsetDualPointB = new Point();
curAdvSettings.offsetDualPointBArea = 0;
} else {
// update point and area
switch (offsetOperation) {
case ROPO_TAKEFIRSTDUAL:
curAdvSettings.offsetDualPointA = newPoint;
curAdvSettings.offsetDualPointAArea = latestTarget.getArea();
break;
case ROPO_TAKESECONDDUAL:
curAdvSettings.offsetDualPointB = newPoint;
curAdvSettings.offsetDualPointBArea = latestTarget.getArea();
break;
}
}
break;
}
}
}
return;
}
// special case for camera settables
@@ -204,7 +248,6 @@ public class VisionModuleChangeSubscriber extends DataChangeSubscriber {
logger.error("Unknown exception when setting PSC prop!", e);
}
// parentModule.saveModule();
parentModule.saveAndBroadcastSelective(wsEvent.originContext, propName, newPropValue);
}
}

32
photon-server/src/main/java/org/photonvision/server/SocketMessageCommandType.java → photon-server/src/main/java/org/photonvision/vision/target/RobotOffsetPointOperation.java
View File

@@ -15,20 +15,32 @@
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package org.photonvision.server;
package org.photonvision.vision.target;
public enum SocketMessageCommandType {
SMCT_DELETECURRENTPIPELINE("deleteCurrentPipeline"),
SMCT_SAVE("save");
public enum RobotOffsetPointOperation {
ROPO_CLEAR(0),
ROPO_TAKESINGLE(1),
ROPO_TAKEFIRSTDUAL(2),
ROPO_TAKESECONDDUAL(3);
public final String entryValue;
public final int index;
SocketMessageCommandType(String entryValue) {
this.entryValue = entryValue;
RobotOffsetPointOperation(int index) {
this.index = index;
}
public static SocketMessageCommandType fromEntryKey(String entryValue) {
if (entryValue.equalsIgnoreCase(SMCT_SAVE.entryValue)) return SMCT_SAVE;
else return SMCT_DELETECURRENTPIPELINE;
public static RobotOffsetPointOperation fromIndex(int index) {
switch (index) {
case 0:
return ROPO_CLEAR;
case 1:
return ROPO_TAKESINGLE;
case 2:
return ROPO_TAKEFIRSTDUAL;
case 3:
return ROPO_TAKESECONDDUAL;
default:
return ROPO_CLEAR;
}
}
}

42
photon-server/src/main/java/org/photonvision/vision/target/TargetCalculations.java
View File

@@ -17,10 +17,13 @@
package org.photonvision.vision.target;
import edu.wpi.first.wpilibj.trajectory.Trajectory;
import org.apache.commons.math3.util.FastMath;
import org.opencv.core.Point;
import org.opencv.core.RotatedRect;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.common.util.numbers.DoubleCouple;
import org.photonvision.vision.opencv.DualOffsetValues;
public class TargetCalculations {
public static double calculateYaw(
@@ -84,7 +87,7 @@ public class TargetCalculations {
public static Point calculateRobotOffsetPoint(
Point offsetPoint,
Point camCenterPoint,
DoubleCouple offsetEquationValues,
DualOffsetValues dualOffsetValues,
RobotOffsetPointMode offsetMode) {
switch (offsetMode) {
case None:
@@ -97,12 +100,39 @@ public class TargetCalculations {
return offsetPoint;
}
case Dual:
Point resultPoint = new Point();
resultPoint.x =
(offsetPoint.x - offsetEquationValues.getFirst()) / offsetEquationValues.getSecond();
resultPoint.y =
(offsetPoint.y * offsetEquationValues.getSecond()) + offsetEquationValues.getFirst();
var resultPoint = new Point();
var lineValues = dualOffsetValues.getLineValues();
var offsetSlope = lineValues.getFirst();
var offsetIntercept = lineValues.getSecond();
resultPoint.x = (offsetPoint.x - offsetIntercept) / offsetSlope;
resultPoint.y = (offsetPoint.y * offsetSlope) + offsetIntercept;
return resultPoint;
}
}
public static Point calculateDualOffsetCrosshair(
DualOffsetValues dualOffsetValues, double currentArea) {
boolean firstLarger = dualOffsetValues.firstPointArea >= dualOffsetValues.secondPointArea;
double upperArea =
firstLarger ? dualOffsetValues.secondPointArea : dualOffsetValues.firstPointArea;
double lowerArea =
firstLarger ? dualOffsetValues.firstPointArea : dualOffsetValues.secondPointArea;
var areaFraction = MathUtils.map(currentArea, lowerArea, upperArea, 0, 1);
var xLerp =
Trajectory.State.lerp(
dualOffsetValues.firstPoint.x, dualOffsetValues.secondPoint.x, areaFraction);
var yLerp =
Trajectory.State.lerp(
dualOffsetValues.firstPoint.y, dualOffsetValues.secondPoint.y, areaFraction);
return new Point(xLerp, yLerp);
}
public static DoubleCouple getLineFromPoints(Point firstPoint, Point secondPoint) {
var offsetLineSlope = (secondPoint.y - firstPoint.y) / (secondPoint.x - firstPoint.x);
var offsetLineIntercept = firstPoint.y - (offsetLineSlope * firstPoint.x);
return new DoubleCouple(offsetLineSlope, offsetLineIntercept);
}
}

48
photon-server/src/main/java/org/photonvision/vision/target/TrackedTarget.java
View File

@@ -24,8 +24,9 @@ import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.Point;
import org.opencv.core.RotatedRect;
import org.photonvision.common.util.numbers.DoubleCouple;
import org.photonvision.vision.frame.FrameStaticProperties;
import org.photonvision.vision.opencv.Contour;
import org.photonvision.vision.opencv.DualOffsetValues;
import org.photonvision.vision.opencv.Releasable;
public class TrackedTarget implements Releasable {
@@ -103,9 +104,9 @@ public class TrackedTarget implements Releasable {
params.isLandscape, params.targetOffsetPointEdge, getMinAreaRect());
m_robotOffsetPoint =
TargetCalculations.calculateRobotOffsetPoint(
m_targetOffsetPoint,
params.robotOffsetSinglePoint,
params.cameraCenterPoint,
params.offsetEquationValues,
params.dualOffsetValues,
params.robotOffsetPointMode);
// order of this stuff doesnt matter though
@@ -193,10 +194,12 @@ public class TrackedTarget implements Releasable {
final TargetOffsetPointEdge targetOffsetPointEdge;
// RobotOffset calculation values
final Point userOffsetPoint;
final Point cameraCenterPoint;
final DoubleCouple offsetEquationValues;
final RobotOffsetPointMode robotOffsetPointMode;
final Point robotOffsetSinglePoint;
final DualOffsetValues dualOffsetValues;
// center point of image
final Point cameraCenterPoint;
// yaw calculation values
final double horizontalFocalLength;
@@ -210,22 +213,43 @@ public class TrackedTarget implements Releasable {
public TargetCalculationParameters(
boolean isLandscape,
TargetOffsetPointEdge targetOffsetPointEdge,
Point userOffsetPoint,
Point cameraCenterPoint,
DoubleCouple offsetEquationValues,
RobotOffsetPointMode robotOffsetPointMode,
Point robotOffsetSinglePoint,
DualOffsetValues dualOffsetValues,
Point cameraCenterPoint,
double horizontalFocalLength,
double verticalFocalLength,
double imageArea) {
this.isLandscape = isLandscape;
this.targetOffsetPointEdge = targetOffsetPointEdge;
this.userOffsetPoint = userOffsetPoint;
this.cameraCenterPoint = cameraCenterPoint;
this.offsetEquationValues = offsetEquationValues;
this.robotOffsetPointMode = robotOffsetPointMode;
this.robotOffsetSinglePoint = robotOffsetSinglePoint;
this.dualOffsetValues = dualOffsetValues;
this.cameraCenterPoint = cameraCenterPoint;
this.horizontalFocalLength = horizontalFocalLength;
this.verticalFocalLength = verticalFocalLength;
this.imageArea = imageArea;
}
public TargetCalculationParameters(
boolean isLandscape,
TargetOffsetPointEdge targetOffsetPointEdge,
RobotOffsetPointMode robotOffsetPointMode,
Point robotOffsetSinglePoint,
DualOffsetValues dualOffsetValues,
FrameStaticProperties frameStaticProperties) {
this.isLandscape = isLandscape;
this.targetOffsetPointEdge = targetOffsetPointEdge;
this.robotOffsetPointMode = robotOffsetPointMode;
this.robotOffsetSinglePoint = robotOffsetSinglePoint;
this.dualOffsetValues = dualOffsetValues;
this.cameraCenterPoint = frameStaticProperties.centerPoint;
this.horizontalFocalLength = frameStaticProperties.horizontalFocalLength;
this.verticalFocalLength = frameStaticProperties.verticalFocalLength;
this.imageArea = frameStaticProperties.imageArea;
}
}
}