From bf9f0a9e6d82773213dc651172da4bbda943a02b Mon Sep 17 00:00:00 2001 From: Sam Carlberg Date: Fri, 23 Dec 2016 11:48:56 -0500 Subject: [PATCH] Add vision pipeline API to make it easier to run OpenCV pipelines (#388) --- .../java/edu/wpi/first/wpilibj/RobotBase.java | 6 + .../first/wpilibj/vision/VisionPipeline.java | 27 ++++ .../first/wpilibj/vision/VisionRunner.java | 115 ++++++++++++++++++ .../first/wpilibj/vision/VisionThread.java | 48 ++++++++ .../first/wpilibj/vision/package-info.java | 89 ++++++++++++++ 5 files changed, 285 insertions(+) create mode 100644 wpilibj/src/athena/java/edu/wpi/first/wpilibj/vision/VisionPipeline.java create mode 100644 wpilibj/src/athena/java/edu/wpi/first/wpilibj/vision/VisionRunner.java create mode 100644 wpilibj/src/athena/java/edu/wpi/first/wpilibj/vision/VisionThread.java create mode 100644 wpilibj/src/athena/java/edu/wpi/first/wpilibj/vision/package-info.java diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/RobotBase.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/RobotBase.java index 99f0d98e83..5a6a3ca6a8 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/RobotBase.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/RobotBase.java @@ -37,6 +37,12 @@ public abstract class RobotBase { */ public static final int ROBOT_TASK_PRIORITY = 101; + /** + * The ID of the main Java thread. + */ + // This is usually 1, but it is best to make sure + public static final long MAIN_THREAD_ID = Thread.currentThread().getId(); + protected final DriverStation m_ds; /** diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/vision/VisionPipeline.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/vision/VisionPipeline.java new file mode 100644 index 0000000000..76f44e8b3a --- /dev/null +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/vision/VisionPipeline.java @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* 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 org.opencv.core.Mat; + +/** + * A vision pipeline is responsible for running a group of + * OpenCV algorithms to extract data from an image. + * + * @see VisionRunner + * @see VisionThread + */ +public interface VisionPipeline { + + /** + * Processes the image input and sets the result objects. + * Implementations should make these objects accessible. + */ + void process(Mat image); + +} diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/vision/VisionRunner.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/vision/VisionRunner.java new file mode 100644 index 0000000000..7a016e695f --- /dev/null +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/vision/VisionRunner.java @@ -0,0 +1,115 @@ +/*----------------------------------------------------------------------------*/ +/* 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.CvSink; +import edu.wpi.cscore.VideoSource; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotBase; +import org.opencv.core.Mat; + +/** + * A vision runner is a convenient wrapper object to make it easy to run vision pipelines + * from robot code. The easiest way to use this is to run it in a {@link VisionThread} + * and use the listener to take snapshots of the pipeline's outputs. + * + * @see VisionPipeline + * @see VisionThread + * @see edu.wpi.first.wpilibj.vision + */ +public class VisionRunner

{ + + 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 + * @param

the 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;
+ *          }
+ *      }
+ *
+ * }
+ *
+ *

+ */ +package edu.wpi.first.wpilibj.vision;