Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-27 02:01:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add vision pipeline API to make it easier to run OpenCV pipelines (#388)

Browse Source
This commit is contained in:
Sam Carlberg
2016-12-23 11:48:56 -05:00
committed by Peter Johnson
parent c3160bad44
commit bf9f0a9e6d
5 changed files with 285 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
wpilibj/src/athena/java/edu/wpi/first/wpilibj/RobotBase.java
View File

@@ -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;
/**

27
wpilibj/src/athena/java/edu/wpi/first/wpilibj/vision/VisionPipeline.java Normal file
View File

@@ -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);
}

115
wpilibj/src/athena/java/edu/wpi/first/wpilibj/vision/VisionRunner.java Normal file
View File

@@ -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<P extends VisionPipeline> {
private final CvSink m_cvSink = new CvSink("VisionRunner CvSink");
private final P m_pipeline;
private final Mat m_image = new Mat();
private final Listener<? super P> m_listener;
/**
* Listener interface for a callback that should run after a pipeline has processed its input.
*
* @param <P> the type of the pipeline this listener is for
*/
@FunctionalInterface
public interface Listener<P extends VisionPipeline> {
/**
* 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 <i>synchronized</i> 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<? super P> 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.
*
* <p>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}.</p>
*/
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.
*
* <p><strong>Do not call this method directly from the main thread.</strong></p>
*
* @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();
}
}
}

48
wpilibj/src/athena/java/edu/wpi/first/wpilibj/vision/VisionThread.java Normal file
View File

@@ -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 <i>daemon</i> 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 <P> the type of the pipeline
*/
public <P extends VisionPipeline> VisionThread(VideoSource videoSource,
P pipeline,
VisionRunner.Listener<? super P> listener) {
this(new VisionRunner<>(videoSource, pipeline, listener));
}
}

89
wpilibj/src/athena/java/edu/wpi/first/wpilibj/vision/package-info.java Normal file
View File

@@ -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.
*
* <p>An example usecase for grabbing a yellow tote from 2015 in autonomous:
* <br>
* <pre><code>
* public class Robot extends IterativeRobot
* implements VisionRunner.Listener&lt;MyFindTotePipeline&gt; {
*
* // 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;
* }
* }
*
* }
* </code></pre>
* </p>
*/
package edu.wpi.first.wpilibj.vision;