{ + + private final CvSink m_cvSink = new CvSink("VisionRunner CvSink"); + private final P m_pipeline; + private final Mat m_image = new Mat(); + private final Listener m_listener; + + /** + * Listener interface for a callback that should run after a pipeline has processed its input. + * + * @param
the type of the pipeline this listener is for + */ + @FunctionalInterface + public interface Listener
{ + + /** + * Called when the pipeline has run. This shouldn't take much time to run because it will delay + * later calls to the pipeline's {@link VisionPipeline#process process} method. Copying the + * outputs and code that uses the copies should be synchronized on the same mutex to + * prevent multiple threads from reading and writing to the same memory at the same time. + * + * @param pipeline the vision pipeline that ran + */ + void copyPipelineOutputs(P pipeline); + + } + + /** + * Creates a new vision runner. It will take images from the {@code videoSource}, send them to + * the {@code pipeline}, and call the {@code listener} when the pipeline has finished to alert + * user code when it is safe to access the pipeline's outputs. + * + * @param videoSource the video source to use to supply images for the pipeline + * @param pipeline the vision pipeline to run + * @param listener a function to call after the pipeline has finished running + */ + public VisionRunner(VideoSource videoSource, P pipeline, Listener listener) { + this.m_pipeline = pipeline; + this.m_listener = listener; + m_cvSink.setSource(videoSource); + } + + /** + * Runs the pipeline one time, giving it the next image from the video source specified + * in the constructor. This will block until the source either has an image or throws an error. + * If the source successfully supplied a frame, the pipeline's image input will be set, + * the pipeline will run, and the listener specified in the constructor will be called to notify + * it that the pipeline ran. + * + *
This method is exposed to allow teams to add additional functionality or have their own + * ways to run the pipeline. Most teams, however, should just use {@link #runForever} in its own + * thread using a {@link VisionThread}.
+ */ + public void runOnce() { + if (Thread.currentThread().getId() == RobotBase.MAIN_THREAD_ID) { + throw new IllegalStateException( + "VisionRunner.runOnce() cannot be called from the main robot thread"); + } + long frameTime = m_cvSink.grabFrame(m_image); + if (frameTime == 0) { + // There was an error, report it + String error = m_cvSink.getError(); + DriverStation.reportError(error, true); + } else { + // No errors, process the image + m_pipeline.process(m_image); + m_listener.copyPipelineOutputs(m_pipeline); + } + } + + /** + * A convenience method that calls {@link #runOnce()} in an infinite loop. This must + * be run in a dedicated thread, and cannot be used in the main robot thread because + * it will freeze the robot program. + * + *Do not call this method directly from the main thread.
+ * + * @throws IllegalStateException if this is called from the main robot thread + * @see VisionThread + */ + public void runForever() { + if (Thread.currentThread().getId() == RobotBase.MAIN_THREAD_ID) { + throw new IllegalStateException( + "VisionRunner.runForever() cannot be called from the main robot thread"); + } + while (true) { + runOnce(); + } + } + +} diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/vision/VisionThread.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/vision/VisionThread.java new file mode 100644 index 0000000000..842acfb223 --- /dev/null +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/vision/VisionThread.java @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2016. 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.vision; + +import edu.wpi.cscore.VideoSource; + +/** + * A vision thread is a special thread that runs a vision pipeline. It is a daemon thread; + * it does not prevent the program from exiting when all other non-daemon threads + * have finished running. + * + * @see VisionPipeline + * @see VisionRunner + * @see Thread#setDaemon(boolean) + */ +public class VisionThread extends Thread { + + /** + * Creates a vision thread that continuously runs a {@link VisionPipeline}. + * + * @param visionRunner the runner for a vision pipeline + */ + public VisionThread(VisionRunner visionRunner) { + super(visionRunner::runForever, "WPILib Vision Thread"); + setDaemon(true); + } + + /** + * Creates a new vision thread that continuously runs the given vision pipeline. This is + * equivalent to {@code new VisionThread(new VisionRunner<>(videoSource, pipeline, listener))}. + * + * @param videoSource the source for images the pipeline should process + * @param pipeline the pipeline to run + * @param listener the listener to copy outputs from the pipeline after it runs + * @paramthe type of the pipeline + */ + public
VisionThread(VideoSource videoSource, + P pipeline, + VisionRunner.Listener listener) { + this(new VisionRunner<>(videoSource, pipeline, listener)); + } + +} diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/vision/package-info.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/vision/package-info.java new file mode 100644 index 0000000000..a743345db7 --- /dev/null +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/vision/package-info.java @@ -0,0 +1,89 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2016. 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. */ +/*----------------------------------------------------------------------------*/ + +/** + * Classes in the {@code edu.wpi.first.wpilibj.vision} package are designed to + * simplify using OpenCV vision processing code from a robot program. + * + *
An example usecase for grabbing a yellow tote from 2015 in autonomous:
+ *
+ *
+ * public class Robot extends IterativeRobot
+ * implements VisionRunner.Listener<MyFindTotePipeline> {
+ *
+ * // A USB camera connected to the roboRIO.
+ * private {@link edu.wpi.cscore.VideoSource VideoSource} usbCamera;
+ *
+ * // A vision pipeline. This could be handwritten or generated by GRIP.
+ * // This has to implement {@link edu.wpi.first.wpilibj.vision.VisionPipeline}.
+ * // For this example, assume that it's perfect and will always see the tote.
+ * private MyFindTotePipeline findTotePipeline;
+ * private {@link edu.wpi.first.wpilibj.vision.VisionThread} findToteThread;
+ *
+ * // The object to synchronize on to make sure the vision thread doesn't
+ * // write to variables the main thread is using.
+ * private final Object visionLock = new Object();
+ *
+ * // The pipeline outputs we want
+ * private boolean pipelineRan = false; // lets us know when the pipeline has actually run
+ * private double angleToTote = 0;
+ * private double distanceToTote = 0;
+ *
+ * {@literal @}Override
+ * public void {@link edu.wpi.first.wpilibj.vision.VisionRunner.Listener#copyPipelineOutputs
+ * copyPipelineOutputs(MyFindTotePipeline pipeline)} {
+ * synchronized (visionLock) {
+ * // Take a snapshot of the pipeline's output because
+ * // it may have changed the next time this method is called!
+ * this.pipelineRan = true;
+ * this.angleToTote = pipeline.getAngleToTote();
+ * this.distanceToTote = pipeline.getDistanceToTote();
+ * }
+ * }
+ *
+ * {@literal @}Override
+ * public void robotInit() {
+ * usbCamera = CameraServer.getInstance().startAutomaticCapture(0);
+ * findTotePipeline = new MyFindTotePipeline();
+ * findToteThread = new VisionThread(usbCamera, findTotePipeline, this);
+ * }
+ *
+ * {@literal @}Override
+ * public void autonomousInit() {
+ * findToteThread.start();
+ * }
+ *
+ * {@literal @}Override
+ * public void autonomousPeriodic() {
+ * double angle;
+ * double distance;
+ * synchronized (visionLock) {
+ * if (!pipelineRan) {
+ * // Wait until the pipeline has run
+ * return;
+ * }
+ * // Copy the outputs to make sure they're all from the same run
+ * angle = this.angleToTote;
+ * distance = this.distanceToTote;
+ * }
+ * if (!aimedAtTote()) {
+ * turnToAngle(angle);
+ * } else if (!droveToTote()) {
+ * driveDistance(distance);
+ * } else if (!grabbedTote()) {
+ * grabTote();
+ * } else {
+ * // Tote was grabbed and we're done!
+ * return;
+ * }
+ * }
+ *
+ * }
+ *