diff --git a/src/main/java/org/hyperonline/hyperlib/HYPERVision.java b/src/main/java/org/hyperonline/hyperlib/HYPERVision.java new file mode 100644 index 0000000..0d16ad8 --- /dev/null +++ b/src/main/java/org/hyperonline/hyperlib/HYPERVision.java @@ -0,0 +1,102 @@ +package org.hyperonline.hyperlib; + +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.networktables.NetworkTableInstance; + +/** + * HYPERVision provide code which is mostly the same in every vision application. + * + * @author Chris McGroarty + */ +public abstract class HYPERVision { + + protected CameraServer m_cameraServer; + protected NetworkTableInstance m_tableInstance; + + // override this property to true in a child class that runs on the robot + protected boolean m_isRobot = false; + + /** + * Initialize the HYPERVision application with components initialized in a specific order + */ + public final void visionInit() { + initNetworkTable(); + initCameraServer(); + initCameras(); + initConnectors(); + initProcessors(); + initPipelines(); + initModules(); + } + + /** + * Method runs throughout the application, allowing for + * the scheduling of runnable tasks in the PeriodicScheduler + */ + private void visionPeriodic() { + PeriodicScheduler.getInstance().run(); + } + + /** + * Run the Vision application + * Matches startCompetition of a Robot + */ + public void startCompetition() { + visionInit(); + startModules(); + + // only run periodic when not running on the robot + // robot has its own periodics + while (!m_isRobot) { + visionPeriodic(); + } + } + + /** + * Initialize the NetworkTableInstance and start our team's client + */ + protected void initNetworkTable() { + m_tableInstance = NetworkTableInstance.getDefault(); + m_tableInstance.startClientTeam(69); + } + + /** + * Initialize the cameras used in this HYPERVision + */ + protected abstract void initCameras(); + + /** + * Initialize the VisionConnectors used in this HYPERVision + */ + protected abstract void initConnectors(); + + /** + * Initialize the TargetProcessors used in this HYPERVision + */ + protected abstract void initProcessors(); + /** + * Initialize the VIsionGUIPipelines used in this HYPERVision + */ + protected abstract void initPipelines(); + /** + * Initialize the VisionModules used in this HYPERVision + */ + protected abstract void initModules(); + /** + * Start the VisionModules used in this HYPERVision + */ + protected abstract void startModules(); + + /** + * Initialize the CameraServer used in this HYPERVision + */ + protected void initCameraServer() { + m_cameraServer = CameraServer.getInstance(); + } + + /** + * Update properties when Preferences have changed + */ + protected abstract void onPreferencesUpdated(); +} + diff --git a/src/main/java/org/hyperonline/hyperlib/vision/AbstractTargetProcessor.java b/src/main/java/org/hyperonline/hyperlib/vision/AbstractTargetProcessor.java index 42d264a..48557aa 100644 --- a/src/main/java/org/hyperonline/hyperlib/vision/AbstractTargetProcessor.java +++ b/src/main/java/org/hyperonline/hyperlib/vision/AbstractTargetProcessor.java @@ -1,80 +1,42 @@ package org.hyperonline.hyperlib.vision; -import java.util.List; - -import org.hyperonline.hyperlib.pid.DisplacementPIDSource; import org.opencv.core.Rect; -import edu.wpi.first.wpilibj.PIDSource; +import java.util.List; /** * Base class with common functionality used by TargetProcessors. In particular, * this provides methods to store a result, and access it via PID sources. - * - * @author James Hagborg * - * @param - * VisionResult type of this AbstractTargetProcessor + * @param VisionResult type of this AbstractTargetProcessor + * @author James Hagborg */ public abstract class AbstractTargetProcessor implements TargetProcessor { - private volatile T m_lastResult; - + private AbstractVisionConnector m_visionConnector; + + protected AbstractTargetProcessor(AbstractVisionConnector connector) { + m_visionConnector = connector; + } + /** - * - * @param targets - * list of targets to compute + * @param targets list of targets to compute * @return {T} */ public abstract T computeResult(List targets); - - /** - * - * @return {T} - */ - public abstract T getDefaultValue(); - - @Override - public final void process(List targets) { - m_lastResult = computeResult(targets); - } - /** - * Get the most recent result of this processor. If no result has been - * produced yet, returns the default value. - * - * @return The most recent result. - */ - public T getLastResult() { - T res = m_lastResult; - return res == null ? getDefaultValue() : res; - } - - /** - * Get a PID source which tracks the x-coordinate of the target. - * - * @return A PID source tracking the x-coordinate of the target. - */ - public PIDSource xPID() { - return new DisplacementPIDSource() { - @Override - public double pidGet() { - return getLastResult().xError(); - } - }; + protected T getLastResult(){ + return m_visionConnector.getLastResult(); } /** - * Get a PID source which tracks the y-coordinate of the target. - * - * @return A PID source tracking the y-coordinate of the target. + * + * @param targets */ - public PIDSource yPID() { - return new DisplacementPIDSource() { - @Override - public double pidGet() { - return getLastResult().yError(); - } - }; + @Override + public final void process(List targets) { + T res = computeResult(targets); + m_visionConnector.updateResult(res); } } + diff --git a/src/main/java/org/hyperonline/hyperlib/vision/AbstractVisionConnector.java b/src/main/java/org/hyperonline/hyperlib/vision/AbstractVisionConnector.java new file mode 100644 index 0000000..ed01b42 --- /dev/null +++ b/src/main/java/org/hyperonline/hyperlib/vision/AbstractVisionConnector.java @@ -0,0 +1,164 @@ +package org.hyperonline.hyperlib.vision; + +import edu.wpi.first.networktables.*; +import edu.wpi.first.wpilibj.PIDSource; +import org.hyperonline.hyperlib.pid.DisplacementPIDSource; + +import java.util.Objects; + +/** + * Base class with common functionality used by VisionConnectors. In particular, + * this provides methods to store, publish, and subscribe to a result, + * and access it via PID sources. + * + * @param VisionResult type of this AbstractVisionConnector + * @author Chris McGroarty + */ +public abstract class AbstractVisionConnector implements VisionConnector { + + protected final NetworkTableEntry m_xError; + protected final NetworkTableEntry m_yError; + protected final NetworkTableEntry m_xAbs; + protected final NetworkTableEntry m_yAbs; + protected final NetworkTableEntry m_foundTarget; + protected final NetworkTableEntry m_lastResultTimestamp; + protected T m_lastResult; + protected NetworkTable m_table; + private String m_mainTableName = "hypervision"; + private String m_subTableName; + private NetworkTableInstance m_inst; + private int m_listenerID; + + protected AbstractVisionConnector(String subTableName, NetworkTableInstance inst) { + m_subTableName = Objects.requireNonNull(subTableName); + + m_inst = Objects.requireNonNull(inst); + m_table = m_inst.getTable(getTableName()); + m_lastResult = getDefaultValue(); + + m_xError = m_table.getEntry("xError"); + m_yError = m_table.getEntry("yError"); + m_xAbs = m_table.getEntry("xAbsolute"); + m_yAbs = m_table.getEntry("yAbsolute"); + m_foundTarget = m_table.getEntry("foundTarget"); + m_lastResultTimestamp = m_table.getEntry("lastResultTimestamp"); + } + + private String getTableName() { + return "/" + m_mainTableName + "/" + m_subTableName; + } + + /** + * add listener for the lastResultTimestamp entry in NetworkTables + * process the next result on an update + */ + public void subscribe() { + m_listenerID = m_table.addEntryListener("lastResultTimestamp", this::next, EntryListenerFlags.kUpdate); + } + + /** + * remove listener for the lastResultTimestamp entry in NetworkTables + */ + public void unsubscribe() { + m_table.removeEntryListener(m_listenerID); + } + + /** + * @return {T} + */ + public abstract T getDefaultValue(); + + /** + * @param result {T} + */ + public void updateResult(T result) { + if (m_lastResult == null) { + m_lastResult = getDefaultValue(); + } + if (!m_lastResult.equals(result)) { + m_lastResult = (result == null ? getDefaultValue() : result); + if (m_inst.isConnected()) { + this.publish(); + } + } else { + System.out.println("NetworkTableInstance(69) not connected"); + } + } + + /** + * publish VisionResult values to corresponding NetworkTable Entries + * should be overriden and supered by any children of type T extends VisionResult + */ + public void publish() { + m_lastResultTimestamp.setDouble(System.currentTimeMillis()); + m_xError.setDouble(m_lastResult.xError()); + m_yError.setDouble(m_lastResult.yError()); + m_xAbs.setDouble(m_lastResult.xAbsolute()); + m_yAbs.setDouble(m_lastResult.yAbsolute()); + m_foundTarget.setBoolean(m_lastResult.foundTarget()); + } + + /** + * retrieve a VisionResult from NetworkTable Entries + * should be overriden and supered by any children of type T extends VisionResult + * + * @return {VisionResult} + */ + protected VisionResult retrieve() { + return new VisionResult( + m_xError.getDouble(0), + m_yError.getDouble(0), + m_xAbs.getDouble(0), + m_yAbs.getDouble(0), + m_foundTarget.getBoolean(false) + ); + } + + /** + * evaluate and process the results in NetworkTables to my lastResult + * + * @param table + * @param key + * @param entry + * @param value + * @param flags + */ + protected abstract void next(NetworkTable table, String key, NetworkTableEntry entry, + NetworkTableValue value, int flags); + + + /** + * @return {T} + */ + public T getLastResult() { + return m_lastResult; + } + + /** + * Get a PID source which tracks the x-coordinate of the target. + * + * @return A PID source tracking the x-coordinate of the target. + */ + public PIDSource xPID() { + return new DisplacementPIDSource() { + @Override + public double pidGet() { + return getLastResult().xError(); + } + }; + } + + /** + * Get a PID source which tracks the y-coordinate of the target. + * + * @return A PID source tracking the y-coordinate of the target. + */ + public PIDSource yPID() { + return new DisplacementPIDSource() { + @Override + public double pidGet() { + return getLastResult().yError(); + } + }; + } +} diff --git a/src/main/java/org/hyperonline/hyperlib/vision/BasicVisionConnector.java b/src/main/java/org/hyperonline/hyperlib/vision/BasicVisionConnector.java new file mode 100644 index 0000000..0f76479 --- /dev/null +++ b/src/main/java/org/hyperonline/hyperlib/vision/BasicVisionConnector.java @@ -0,0 +1,43 @@ +package org.hyperonline.hyperlib.vision; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.NetworkTableValue; + +/** + * NetworkTables Connector for a AbstractTargetProcessor of VisionResult + * + * @author Chris McGroarty + */ +public class BasicVisionConnector extends AbstractVisionConnector { + + /** + * + * @param subTableName name of the subtable to use in the connector + * @param inst instance of NetworkTables + */ + public BasicVisionConnector(String subTableName, NetworkTableInstance inst) { + super(subTableName, inst); + } + + /** + * @param table + * @param key + * @param entry + * @param value + * @param flags + */ + @Override + protected void next(NetworkTable table, String key, NetworkTableEntry entry, NetworkTableValue value, int flags) { + m_lastResult = retrieve(); + } + + /** + * {@inheritDoc} + */ + @Override + public VisionResult getDefaultValue() { + return new VisionResult(0, 0, 0, 0, false); + } +} diff --git a/src/main/java/org/hyperonline/hyperlib/vision/ClosestPairTargetProcessor.java b/src/main/java/org/hyperonline/hyperlib/vision/ClosestPairTargetProcessor.java index 57ba819..158165f 100644 --- a/src/main/java/org/hyperonline/hyperlib/vision/ClosestPairTargetProcessor.java +++ b/src/main/java/org/hyperonline/hyperlib/vision/ClosestPairTargetProcessor.java @@ -1,52 +1,47 @@ package org.hyperonline.hyperlib.vision; -import java.util.Comparator; -import java.util.List; -import java.util.Objects; -import java.util.function.IntSupplier; - +import edu.wpi.first.wpilibj.PIDSource; import org.hyperonline.hyperlib.pid.DisplacementPIDSource; import org.hyperonline.hyperlib.pref.IntPreference; -import org.opencv.core.Mat; -import org.opencv.core.Point; -import org.opencv.core.Point3; -import org.opencv.core.Rect; -import org.opencv.core.Scalar; +import org.opencv.core.*; import org.opencv.imgproc.Imgproc; -import edu.wpi.first.wpilibj.PIDSource; +import java.util.Comparator; +import java.util.List; +import java.util.Objects; +import java.util.function.IntSupplier; /** * Target processor which finds the closest two targets to the crosshairs, and * averages their position. This is the algorithm that was used to track the * boiler in the 2017 steamworks game. It can more generally be applied to * anything that's two pieces of tape. - * + *

* This processor uses the height of the targets to measure distance. However, * it computes x, y, and height all as simple averages, rather than weighting by * depth. This is a good approximation as long as one target is not * significantly closer than the other, meaning the camera is not viewing the * target from a steep angle. - * - * @author James Hagborg * + * @author James Hagborg */ public class ClosestPairTargetProcessor extends AbstractTargetProcessor { + private static final Scalar MARKER_COLOR = new Scalar(0, 0, 255); + private static final double MARKER_WIDTH = 6; private final IntSupplier m_xCrosshairs, m_yCrosshairs; /** * Construct a new target processor with the given fixed crosshairs * position. - * - * @param xCrosshairs - * X coordinate for the crosshairs - * @param yCrosshairs - * Y coordinate for the crosshairs + * + * @param connector VisionConnector for retreiving/publishing the result + * @param xCrosshairs X coordinate for the crosshairs + * @param yCrosshairs Y coordinate for the crosshairs */ - public ClosestPairTargetProcessor(int xCrosshairs, int yCrosshairs) { - this(() -> xCrosshairs, () -> yCrosshairs); + public ClosestPairTargetProcessor(TargetWithHeightConnector connector, int xCrosshairs, int yCrosshairs) { + this(connector, () -> xCrosshairs, () -> yCrosshairs); } /** @@ -56,14 +51,14 @@ public ClosestPairTargetProcessor(int xCrosshairs, int yCrosshairs) { * the vision thread, so it should not reference the internals of commands, * subsystems, or other robot code. Note that preferences are safe to access * from any thread. - * - * @param xCrosshairs - * X coordinate for the crosshairs - * @param yCrosshairs - * Y coordinate for the crosshairs + * + * @param connector VisionConnector for retreiving/publishing the result + * @param xCrosshairs X coordinate for the crosshairs + * @param yCrosshairs Y coordinate for the crosshairs */ - public ClosestPairTargetProcessor(IntSupplier xCrosshairs, - IntSupplier yCrosshairs) { + public ClosestPairTargetProcessor(TargetWithHeightConnector connector, IntSupplier xCrosshairs, + IntSupplier yCrosshairs) { + super(connector); m_xCrosshairs = Objects.requireNonNull(xCrosshairs); m_yCrosshairs = Objects.requireNonNull(yCrosshairs); } @@ -71,9 +66,8 @@ public ClosestPairTargetProcessor(IntSupplier xCrosshairs, /** * Compute the Euclidean distance (actually the distance squared) of a * target from the crosshairs. - * - * @param result - * The target to check + * + * @param result The target to check * @return The distance from the crosshairs */ private double targetDistance(Point3 result) { @@ -84,9 +78,8 @@ private double targetDistance(Point3 result) { /** * Find the center of a rectangle. - * - * @param rect - * The target rectangle. + * + * @param rect The target rectangle. * @return The point at the center of the rectangle. */ private Point3 targetToPoint(Rect rect) { @@ -101,9 +94,8 @@ private Point3 targetToPoint(Rect rect) { * crosshairs. Since this method is only called when the point given is * actually the chosen target, this also saves the point for drawing a * marker later. - * - * @param point - * The point at the center of the target, in absolute pixels. + * + * @param point The point at the center of the target, in absolute pixels. * @return The new VisionResult */ private TargetWithHeightResult pointToResult(Point3 point) { @@ -115,7 +107,7 @@ private TargetWithHeightResult pointToResult(Point3 point) { /** * Given two Points, find the average of their positions. - * + * * @param a * @param b * @return @@ -133,28 +125,11 @@ public TargetWithHeightResult computeResult(List targets) { .sorted(Comparator.comparingDouble(this::targetDistance)) .limit(2).toArray(Point3[]::new); if (result.length < 2) { - return getDefaultValue(); + return null; } else { return pointToResult(averagePoints(result[0], result[1])); } } - - /** - * Get a PID source that returns the height of the target. - * - * @return A PID source returning the height of the target. - */ - public PIDSource heightPID() { - return new DisplacementPIDSource() { - @Override - public double pidGet() { - return getLastResult().height(); - } - }; - } - - private static final Scalar MARKER_COLOR = new Scalar(0, 0, 255); - private static final double MARKER_WIDTH = 6; /** * {@inheritDoc} @@ -174,13 +149,4 @@ public void writeOutput(Mat mat) { Imgproc.line(mat, bl, br, MARKER_COLOR); } } - - /** - * {@inheritDoc} - */ - @Override - public TargetWithHeightResult getDefaultValue() { - return new TargetWithHeightResult(0, 0, 0, 0, 0, false); - } - } diff --git a/src/main/java/org/hyperonline/hyperlib/vision/ClosestTargetProcessor.java b/src/main/java/org/hyperonline/hyperlib/vision/ClosestTargetProcessor.java index 79e7ee4..e58ea3c 100644 --- a/src/main/java/org/hyperonline/hyperlib/vision/ClosestTargetProcessor.java +++ b/src/main/java/org/hyperonline/hyperlib/vision/ClosestTargetProcessor.java @@ -1,10 +1,5 @@ package org.hyperonline.hyperlib.vision; -import java.util.Comparator; -import java.util.List; -import java.util.Objects; -import java.util.function.IntSupplier; - import org.hyperonline.hyperlib.pref.IntPreference; import org.opencv.core.Mat; import org.opencv.core.Point; @@ -12,16 +7,20 @@ import org.opencv.core.Scalar; import org.opencv.imgproc.Imgproc; +import java.util.Comparator; +import java.util.List; +import java.util.Objects; +import java.util.function.IntSupplier; + /** * Target processor class which picks out the closest target to the crosshairs. - * - * @author James Hagborg * + * @author James Hagborg */ public class ClosestTargetProcessor extends AbstractTargetProcessor { + private static final Scalar MARKER_COLOR = new Scalar(0, 0, 255); private final IntSupplier m_xCrosshairs, m_yCrosshairs; - /* * The most recent point where a target was found. This is stored in * absolute pixels, rather than relative to the crosshairs, like the result @@ -32,14 +31,13 @@ public class ClosestTargetProcessor extends AbstractTargetProcessor xCrosshairs, () -> yCrosshairs); + public ClosestTargetProcessor(BasicVisionConnector connector, int xCrosshairs, int yCrosshairs) { + this(connector, () -> xCrosshairs, () -> yCrosshairs); } /** @@ -49,13 +47,13 @@ public ClosestTargetProcessor(int xCrosshairs, int yCrosshairs) { * the vision thread, so it should not reference the internals of commands, * subsystems, or other robot code. Note that preferences are safe to access * from any thread. - * - * @param xCrosshairs - * X coordinate for the crosshairs - * @param yCrosshairs - * Y coordinate for the crosshairs + * + * @param connector VisionConnector for retreiving/publishing the result + * @param xCrosshairs X coordinate for the crosshairs + * @param yCrosshairs Y coordinate for the crosshairs */ - public ClosestTargetProcessor(IntSupplier xCrosshairs, IntSupplier yCrosshairs) { + public ClosestTargetProcessor(BasicVisionConnector connector, IntSupplier xCrosshairs, IntSupplier yCrosshairs) { + super(connector); m_xCrosshairs = Objects.requireNonNull(xCrosshairs); m_yCrosshairs = Objects.requireNonNull(yCrosshairs); } @@ -63,9 +61,8 @@ public ClosestTargetProcessor(IntSupplier xCrosshairs, IntSupplier yCrosshairs) /** * Compute the Euclidean distance (actually the distance squared) of a * target from the crosshairs. - * - * @param result - * The target to check + * + * @param result The target to check * @return The distance from the crosshairs */ private double targetDistance(Point result) { @@ -76,9 +73,8 @@ private double targetDistance(Point result) { /** * Find the center of a rectangle. - * - * @param rect - * The target rectangle. + * + * @param rect The target rectangle. * @return The point at the center of the rectangle. */ private Point targetToPoint(Rect rect) { @@ -92,7 +88,7 @@ private Point targetToPoint(Rect rect) { * crosshairs. Since this method is only called when the point given is * actually the chosen target, this also saves the point for drawing a marker * later. - * + * * @param point The point at the center of the target, in absolute pixels. * @return The new VisionResult */ @@ -100,7 +96,7 @@ private VisionResult pointToResult(Point point) { m_lastPoint = point; return new VisionResult( point.x - m_xCrosshairs.getAsInt(), - point.y - m_yCrosshairs.getAsInt(), + point.y - m_yCrosshairs.getAsInt(), point.x, point.y, true); } @@ -113,11 +109,9 @@ public VisionResult computeResult(List targets) { .map(this::targetToPoint) .min(Comparator.comparingDouble(this::targetDistance)) .map(this::pointToResult) - .orElse(getDefaultValue()); + .orElse(null); } - private static final Scalar MARKER_COLOR = new Scalar(0, 0, 255); - /** * {@inheritDoc} */ @@ -128,12 +122,4 @@ public void writeOutput(Mat mat) { } } - /** - * {@inheritDoc} - */ - @Override - public VisionResult getDefaultValue() { - return new VisionResult(0, 0, 0, 0, false); - } - } diff --git a/src/main/java/org/hyperonline/hyperlib/vision/SkewPairTargetProcessor.java b/src/main/java/org/hyperonline/hyperlib/vision/SkewPairTargetProcessor.java new file mode 100644 index 0000000..c5b6c2a --- /dev/null +++ b/src/main/java/org/hyperonline/hyperlib/vision/SkewPairTargetProcessor.java @@ -0,0 +1,110 @@ +package org.hyperonline.hyperlib.vision; + +import edu.wpi.first.wpilibj.PIDSource; +import org.hyperonline.hyperlib.pid.DisplacementPIDSource; +import org.opencv.core.Mat; +import org.opencv.core.Point; +import org.opencv.core.Rect; +import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; + +import java.util.Comparator; +import java.util.List; +import java.util.Objects; +import java.util.function.IntSupplier; + +/** + * Target processor class which picks out the pair of targets skewed towards each other. + * + * @author James Hagborg + */ +public class SkewPairTargetProcessor extends AbstractTargetProcessor { + + private final IntSupplier m_xCrosshairs, m_yCrosshairs; + + /** + * + * @param connector VisionConnector for retreiving/publishing the result + * @param xCrosshairs X coordinate for the crosshairs + * @param yCrosshairs Y coordinate for the crosshairs + */ + public SkewPairTargetProcessor(SkewVisionConnector connector, int xCrosshairs, int yCrosshairs) { + this(connector, () -> xCrosshairs, () -> yCrosshairs); + } + + /** + * + * @param connector VisionConnector for retreiving/publishing the result + * @param xCrosshairs X coordinate for the crosshairs + * @param yCrosshairs Y coordinate for the crosshairs + */ + public SkewPairTargetProcessor(SkewVisionConnector connector, IntSupplier xCrosshairs, + IntSupplier yCrosshairs) { + super(connector); + m_xCrosshairs = Objects.requireNonNull(xCrosshairs); + m_yCrosshairs = Objects.requireNonNull(yCrosshairs); + } + + private double targetDistance(Rect result) { + Point center = centerOfTarget(result); + double xError = center.x - m_xCrosshairs.getAsInt(); + double yError = center.y - m_yCrosshairs.getAsInt(); + return xError * xError + yError * yError; + } + + private Point centerOfTarget(Rect rect) { + int xCenter = rect.x + rect.width / 2; + int yCenter = rect.y + rect.height / 2; + return new Point(xCenter, yCenter); + } + + private Point averagePoints(Point a, Point b) { + return new Point((a.x + b.x) / 2, (a.y + b.y) / 2); + } + + private SkewVisionResult pairToResult(Rect r1, Rect r2) { + // Swap order so r1 is on the left + if (r1.x > r2.x) { + Rect t = r1; + r1 = r2; + r2 = t; + } + final double skew = r1.height / r2.height - r2.height / r1.height; + final Point center = averagePoints(centerOfTarget(r1), centerOfTarget(r2)); + return new SkewVisionResult(center.x - m_xCrosshairs.getAsInt(), + center.y - m_yCrosshairs.getAsInt(), + center.x, center.y, skew, true); + } + + /** + * {@inheritDoc} + */ + @Override + public SkewVisionResult computeResult(List targets) { + Rect[] result = targets.stream() + .sorted(Comparator.comparingDouble(this::targetDistance)) + .limit(2).toArray(Rect[]::new); + if (result.length < 2) { + return null; + } else { + return pairToResult(result[0], result[1]); + } + } + + private static final Scalar MARKER_COLOR = new Scalar(0, 0, 255); + private static final double MARKER_SCALE = 20; + + /** + * {@inheritDoc} + */ + @Override + public void writeOutput(Mat mat) { + SkewVisionResult result = getLastResult(); + if (result.foundTarget()) { + Point center = new Point(result.xAbsolute(), result.yAbsolute()); + Point skewMarker = new Point(center.x + result.skew() * MARKER_SCALE, center.y); + Imgproc.circle(mat, center, 6, MARKER_COLOR); + Imgproc.line(mat, center, skewMarker, MARKER_COLOR); + } + } +} \ No newline at end of file diff --git a/src/main/java/org/hyperonline/hyperlib/vision/SkewVisionConnector.java b/src/main/java/org/hyperonline/hyperlib/vision/SkewVisionConnector.java new file mode 100644 index 0000000..8be9d4b --- /dev/null +++ b/src/main/java/org/hyperonline/hyperlib/vision/SkewVisionConnector.java @@ -0,0 +1,75 @@ +package org.hyperonline.hyperlib.vision; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.NetworkTableValue; +import edu.wpi.first.wpilibj.PIDSource; +import org.hyperonline.hyperlib.pid.DisplacementPIDSource; + +/** + * NetworkTables Connector for a AbstractTargetProcessor of SkewVisionResult + * + * @author Chris McGroarty + */ +public class SkewVisionConnector extends AbstractVisionConnector { + + private final NetworkTableEntry m_skew; + + /** + * + * @param subTableName name of the subtable to use in the connector + * @param inst instance of NetworkTables + */ + public SkewVisionConnector(String subTableName, NetworkTableInstance inst) { + super(subTableName, inst); + m_skew = m_table.getEntry("skew"); + } + + /** + * @return A PID source tracking the skew of the target + */ + public PIDSource skewPID() { + return new DisplacementPIDSource() { + @Override + public double pidGet() { + return getLastResult().skew(); + } + }; + } + + /** + * retrieve a SkewVisionResult from NetworkTable Entries + * + * @return {SkewVisionResult} + */ + @Override + protected SkewVisionResult retrieve() { + return new SkewVisionResult(super.retrieve(), m_skew.getDouble(0)); + } + + /** + * {@inheritDoc} + */ + @Override + public void publish() { + super.publish(); + m_skew.setDouble(m_lastResult.skew()); + } + + /** + * {@inheritDoc} + */ + @Override + protected void next(NetworkTable table, String key, NetworkTableEntry entry, NetworkTableValue value, int flags) { + m_lastResult = retrieve(); + } + + /** + * {@inheritDoc} + */ + @Override + public SkewVisionResult getDefaultValue() { + return new SkewVisionResult(0, 0, 0, 0, 0, false); + } +} diff --git a/src/main/java/org/hyperonline/hyperlib/vision/SkewVisionResult.java b/src/main/java/org/hyperonline/hyperlib/vision/SkewVisionResult.java new file mode 100644 index 0000000..36149ae --- /dev/null +++ b/src/main/java/org/hyperonline/hyperlib/vision/SkewVisionResult.java @@ -0,0 +1,40 @@ +package org.hyperonline.hyperlib.vision; + +/** + * Vision result which stores the skew of the target found. + * + * @author James Hagoborg + */ +public class SkewVisionResult extends VisionResult { + + private final double m_skew; + + /** + * @param result base result to add to + * @param skew skew of the target + */ + public SkewVisionResult(VisionResult result, double skew) { + this(result.xError(), result.yError(), result.xAbsolute(), result.yAbsolute(), skew, result.foundTarget()); + } + + /** + * @param xError error on x-coordinate + * @param yError error on y-coordinate + * @param xAbs absolute x-coordinate + * @param yAbs absolute y-coordinate + * @param skew skew of the target + * @param foundTarget whether a target was actually found + */ + public SkewVisionResult(double xError, double yError, double xAbs, double yAbs, double skew, boolean foundTarget) { + super(xError, yError, xAbs, yAbs, foundTarget); + m_skew = skew; + } + + /** + * @return the skew of the target + */ + public double skew() { + return m_skew; + } + +} diff --git a/src/main/java/org/hyperonline/hyperlib/vision/TargetWithHeightConnector.java b/src/main/java/org/hyperonline/hyperlib/vision/TargetWithHeightConnector.java new file mode 100644 index 0000000..2b83035 --- /dev/null +++ b/src/main/java/org/hyperonline/hyperlib/vision/TargetWithHeightConnector.java @@ -0,0 +1,77 @@ +package org.hyperonline.hyperlib.vision; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.NetworkTableValue; +import edu.wpi.first.wpilibj.PIDSource; +import org.hyperonline.hyperlib.pid.DisplacementPIDSource; + +/** + * NetworkTables Connector for an AbstractTargetProcessor of TargetWithHeightResult + * + * @author Chris McGroarty + */ +public class TargetWithHeightConnector extends AbstractVisionConnector { + + private final NetworkTableEntry m_height; + + /** + * + * @param subTableName name of the subtable to use in the connector + * @param inst instance of NetworkTables + */ + public TargetWithHeightConnector(String subTableName, NetworkTableInstance inst) { + super(subTableName, inst); + m_height = m_table.getEntry("height"); + } + + /** + * retrieve a TargetWithHeightResult from NetworkTable Entries + * + * @return {TargetWithHeightResult} + */ + @Override + protected TargetWithHeightResult retrieve() { + return new TargetWithHeightResult(super.retrieve(), m_height.getDouble(0)); + } + + /** + * {@inheritDoc} + */ + @Override + public void publish() { + super.publish(); + m_height.setDouble(m_lastResult.height()); + } + + /** + * Get a PID source that returns the height of the target. + * + * @return A PID source returning the height of the target. + */ + public PIDSource heightPID() { + return new DisplacementPIDSource() { + @Override + public double pidGet() { + return getLastResult().height(); + } + }; + } + + /** + * {@inheritDoc} + */ + @Override + public TargetWithHeightResult getDefaultValue() { + return new TargetWithHeightResult(0, 0, 0, 0, 0, false); + } + + /** + * {@inheritDoc} + */ + @Override + protected void next(NetworkTable table, String key, NetworkTableEntry entry, NetworkTableValue value, int flags) { + m_lastResult = retrieve(); + } +} diff --git a/src/main/java/org/hyperonline/hyperlib/vision/TargetWithHeightResult.java b/src/main/java/org/hyperonline/hyperlib/vision/TargetWithHeightResult.java index 5cabe4b..5075ef4 100644 --- a/src/main/java/org/hyperonline/hyperlib/vision/TargetWithHeightResult.java +++ b/src/main/java/org/hyperonline/hyperlib/vision/TargetWithHeightResult.java @@ -2,36 +2,44 @@ /** * Vision result which stores the height of the target found. - * - * @author James Hagoborg * + * @author James Hagoborg */ public class TargetWithHeightResult extends VisionResult { private final double m_height; + /** + * + * @param result base result to add to + * @param height height of the target + */ + public TargetWithHeightResult(VisionResult result, double height) { + this(result.xError(), result.yError(), result.xAbsolute(), result.yAbsolute(), height, result.foundTarget()); + } + /** * Construct a result with the given parameters - * - * @param xError - * error on x-coordinate - * @param yError - * error on y-coordinate - * @param xAbs - * absolute x-coordinate - * @param yAbs - * absolute y-coordinate - * @param height - * height of the target - * @param foundTarget - * whether a target was actually found + * + * @param xError error on x-coordinate + * @param yError error on y-coordinate + * @param xAbs absolute x-coordinate + * @param yAbs absolute y-coordinate + * @param height height of the target + * @param foundTarget whether a target was actually found */ public TargetWithHeightResult(double xError, double yError, double xAbs, - double yAbs, double height, boolean foundTarget) { + double yAbs, double height, boolean foundTarget) { super(xError, yError, xAbs, yAbs, foundTarget); m_height = height; } - public double height() { return m_height; } + /** + * + * @return height of the target + */ + public double height() { + return m_height; + } } diff --git a/src/main/java/org/hyperonline/hyperlib/vision/VisionConnector.java b/src/main/java/org/hyperonline/hyperlib/vision/VisionConnector.java new file mode 100644 index 0000000..6a34dad --- /dev/null +++ b/src/main/java/org/hyperonline/hyperlib/vision/VisionConnector.java @@ -0,0 +1,22 @@ +package org.hyperonline.hyperlib.vision; + +/** + * A Connector that allows for the subscribing of and publishing to NetworkTables + * @author Chris McGroarty + */ +public interface VisionConnector { + /** + * subscribes to a VisionConnector's VisionResult in NetworkTable + */ + void subscribe(); + + /** + * cancel the subscription/listener to NetworkTable + */ + void unsubscribe(); + + /** + * publishes a VisionConnector's VisionResult to NetworkTable + */ + void publish(); +}