An example use case 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.vision.VisionPipeline}.
+ * // For this example, assume that it's perfect and will always see the tote.
+ * private MyFindTotePipeline findTotePipeline;
+ * private {@link edu.wpi.first.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.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;
+ * }
+ * }
+ *
+ * }
+ * You should call this method to see a camera feed on the dashboard. + * If you also want to perform vision processing on the roboRIO, use + * getVideo() to get access to the camera images. + * + *
The first time this overload is called, it calls + * {@link #startAutomaticCapture(int)} with device 0, creating a camera + * named "USB Camera 0". Subsequent calls increment the device number + * (e.g. 1, 2, etc). + */ + public UsbCamera startAutomaticCapture() { + UsbCamera camera = startAutomaticCapture(m_defaultUsbDevice.getAndIncrement()); + CameraServerSharedStore.getCameraServerShared().reportUsbCamera(camera.getHandle()); + return camera; + } + + /** + * Start automatically capturing images to send to the dashboard. + * + *
This overload calls {@link #startAutomaticCapture(String, int)} with + * a name of "USB Camera {dev}". + * + * @param dev The device number of the camera interface + */ + public UsbCamera startAutomaticCapture(int dev) { + UsbCamera camera = new UsbCamera("USB Camera " + dev, dev); + startAutomaticCapture(camera); + CameraServerSharedStore.getCameraServerShared().reportUsbCamera(camera.getHandle()); + return camera; + } + + /** + * Start automatically capturing images to send to the dashboard. + * + * @param name The name to give the camera + * @param dev The device number of the camera interface + */ + public UsbCamera startAutomaticCapture(String name, int dev) { + UsbCamera camera = new UsbCamera(name, dev); + startAutomaticCapture(camera); + CameraServerSharedStore.getCameraServerShared().reportUsbCamera(camera.getHandle()); + return camera; + } + + /** + * Start automatically capturing images to send to the dashboard. + * + * @param name The name to give the camera + * @param path The device path (e.g. "/dev/video0") of the camera + */ + public UsbCamera startAutomaticCapture(String name, String path) { + UsbCamera camera = new UsbCamera(name, path); + startAutomaticCapture(camera); + CameraServerSharedStore.getCameraServerShared().reportUsbCamera(camera.getHandle()); + return camera; + } + + /** + * Start automatically capturing images to send to the dashboard from + * an existing camera. + * + * @param camera Camera + */ + public void startAutomaticCapture(VideoSource camera) { + addCamera(camera); + VideoSink server = addServer("serve_" + camera.getName()); + server.setSource(camera); + } + + /** + * Adds an Axis IP camera. + * + *
This overload calls {@link #addAxisCamera(String, String)} with + * name "Axis Camera". + * + * @param host Camera host IP or DNS name (e.g. "10.x.y.11") + */ + public AxisCamera addAxisCamera(String host) { + return addAxisCamera("Axis Camera", host); + } + + /** + * Adds an Axis IP camera. + * + *
This overload calls {@link #addAxisCamera(String, String[])} with + * name "Axis Camera". + * + * @param hosts Array of Camera host IPs/DNS names + */ + public AxisCamera addAxisCamera(String[] hosts) { + return addAxisCamera("Axis Camera", hosts); + } + + /** + * Adds an Axis IP camera. + * + * @param name The name to give the camera + * @param host Camera host IP or DNS name (e.g. "10.x.y.11") + */ + public AxisCamera addAxisCamera(String name, String host) { + AxisCamera camera = new AxisCamera(name, host); + // Create a passthrough MJPEG server for USB access + startAutomaticCapture(camera); + CameraServerSharedStore.getCameraServerShared().reportAxisCamera(camera.getHandle()); + return camera; + } + + /** + * Adds an Axis IP camera. + * + * @param name The name to give the camera + * @param hosts Array of Camera host IPs/DNS names + */ + public AxisCamera addAxisCamera(String name, String[] hosts) { + AxisCamera camera = new AxisCamera(name, hosts); + // Create a passthrough MJPEG server for USB access + startAutomaticCapture(camera); + CameraServerSharedStore.getCameraServerShared().reportAxisCamera(camera.getHandle()); + return camera; + } + + /** + * Get OpenCV access to the primary camera feed. This allows you to + * get images from the camera for image processing on the roboRIO. + * + *
This is only valid to call after a camera feed has been added + * with startAutomaticCapture() or addServer(). + */ + public CvSink getVideo() { + VideoSource source; + synchronized (this) { + if (m_primarySourceName == null) { + throw new VideoException("no camera available"); + } + source = m_sources.get(m_primarySourceName); + } + if (source == null) { + throw new VideoException("no camera available"); + } + return getVideo(source); + } + + /** + * Get OpenCV access to the specified camera. This allows you to get + * images from the camera for image processing on the roboRIO. + * + * @param camera Camera (e.g. as returned by startAutomaticCapture). + */ + public CvSink getVideo(VideoSource camera) { + String name = "opencv_" + camera.getName(); + + synchronized (this) { + VideoSink sink = m_sinks.get(name); + if (sink != null) { + VideoSink.Kind kind = sink.getKind(); + if (kind != VideoSink.Kind.kCv) { + throw new VideoException("expected OpenCV sink, but got " + kind); + } + return (CvSink) sink; + } + } + + CvSink newsink = new CvSink(name); + newsink.setSource(camera); + addServer(newsink); + return newsink; + } + + /** + * Get OpenCV access to the specified camera. This allows you to get + * images from the camera for image processing on the roboRIO. + * + * @param name Camera name + */ + public CvSink getVideo(String name) { + VideoSource source; + synchronized (this) { + source = m_sources.get(name); + if (source == null) { + throw new VideoException("could not find camera " + name); + } + } + return getVideo(source); + } + + /** + * Create a MJPEG stream with OpenCV input. This can be called to pass custom + * annotated images to the dashboard. + * + * @param name Name to give the stream + * @param width Width of the image being sent + * @param height Height of the image being sent + */ + public CvSource putVideo(String name, int width, int height) { + CvSource source = new CvSource(name, VideoMode.PixelFormat.kMJPEG, width, height, 30); + startAutomaticCapture(source); + return source; + } + + /** + * Adds a MJPEG server at the next available port. + * + * @param name Server name + */ + public MjpegServer addServer(String name) { + int port; + synchronized (this) { + port = m_nextPort; + m_nextPort++; + } + return addServer(name, port); + } + + /** + * Adds a MJPEG server. + * + * @param name Server name + */ + public MjpegServer addServer(String name, int port) { + MjpegServer server = new MjpegServer(name, port); + addServer(server); + return server; + } + + /** + * Adds an already created server. + * + * @param server Server + */ + public void addServer(VideoSink server) { + synchronized (this) { + m_sinks.put(server.getName(), server); + } + } + + /** + * Removes a server by name. + * + * @param name Server name + */ + public void removeServer(String name) { + synchronized (this) { + m_sinks.remove(name); + } + } + + /** + * Get server for the primary camera feed. + * + *
This is only valid to call after a camera feed has been added + * with startAutomaticCapture() or addServer(). + */ + public VideoSink getServer() { + synchronized (this) { + if (m_primarySourceName == null) { + throw new VideoException("no camera available"); + } + return getServer("serve_" + m_primarySourceName); + } + } + + /** + * Gets a server by name. + * + * @param name Server name + */ + public VideoSink getServer(String name) { + synchronized (this) { + return m_sinks.get(name); + } + } + + /** + * Adds an already created camera. + * + * @param camera Camera + */ + public void addCamera(VideoSource camera) { + String name = camera.getName(); + synchronized (this) { + if (m_primarySourceName == null) { + m_primarySourceName = name; + } + m_sources.put(name, camera); + } + } + + /** + * Removes a camera by name. + * + * @param name Camera name + */ + public void removeCamera(String name) { + synchronized (this) { + m_sources.remove(name); + } + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java index 304595a090..ed978492e0 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java @@ -15,6 +15,8 @@ import java.nio.file.Files; import java.util.function.Supplier; import edu.wpi.cscore.CameraServerJNI; +import edu.wpi.first.cameraserver.CameraServerShared; +import edu.wpi.first.cameraserver.CameraServerSharedStore; import edu.wpi.first.hal.FRCNetComm.tInstances; import edu.wpi.first.hal.FRCNetComm.tResourceType; import edu.wpi.first.hal.HAL; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionPipeline.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionPipeline.java new file mode 100644 index 0000000000..2c0f8f2904 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionPipeline.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2016-2018 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.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 + * + * @deprecated Replaced with edu.wpi.first.vision.VisionPipeline + */ +@Deprecated +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/main/java/edu/wpi/first/wpilibj/vision/VisionRunner.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionRunner.java new file mode 100644 index 0000000000..701b86ea9c --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionRunner.java @@ -0,0 +1,131 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2016-2018 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.vision; + +import org.opencv.core.Mat; + +import edu.wpi.cscore.CvSink; +import edu.wpi.cscore.VideoSource; +import edu.wpi.first.cameraserver.CameraServerSharedStore; + +/** + * 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 vision + * + * @deprecated Replaced with edu.wpi.first.vision.VisionRunner + */ +@Deprecated +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; + private volatile boolean m_enabled = true; + + /** + * 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() { + Long id = CameraServerSharedStore.getCameraServerShared().getRobotMainThreadId(); + + if (id != null && Thread.currentThread().getId() == id.longValue()) { + throw new IllegalStateException( + "VisionRunner.runOnce() cannot be called from the main robot thread"); + } + runOnceInternal(); + } + + private void runOnceInternal() { + long frameTime = m_cvSink.grabFrame(m_image); + if (frameTime == 0) { + // There was an error, report it + String error = m_cvSink.getError(); + CameraServerSharedStore.getCameraServerShared().reportDriverStationError(error); + } 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() { + Long id = CameraServerSharedStore.getCameraServerShared().getRobotMainThreadId(); + + if (id != null && Thread.currentThread().getId() == id.longValue()) { + throw new IllegalStateException( + "VisionRunner.runForever() cannot be called from the main robot thread"); + } + while (m_enabled && !Thread.interrupted()) { + runOnceInternal(); + } + } + + /** + * Stop a RunForever() loop. + */ + public void stop() { + m_enabled = false; + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionThread.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionThread.java new file mode 100644 index 0000000000..5184ba0fb3 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionThread.java @@ -0,0 +1,50 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2016-2018 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.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) + * + * @deprecated Replaced with edu.wpi.first.vision.VisionThread + */ +@Deprecated +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/cameraserver/src/main/java/edu/wpi/first/wpilibj/vision/package-info.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/package-info.java similarity index 99% rename from cameraserver/src/main/java/edu/wpi/first/wpilibj/vision/package-info.java rename to wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/package-info.java index 94d7101a38..01c8d73be9 100644 --- a/cameraserver/src/main/java/edu/wpi/first/wpilibj/vision/package-info.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/package-info.java @@ -85,4 +85,5 @@ * } * */ +@java.lang.Deprecated package edu.wpi.first.wpilibj.vision; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/axiscamera/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/axiscamera/Robot.java index c860cf080f..ee4b42451c 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/axiscamera/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/axiscamera/Robot.java @@ -15,7 +15,7 @@ import org.opencv.imgproc.Imgproc; import edu.wpi.cscore.AxisCamera; import edu.wpi.cscore.CvSink; import edu.wpi.cscore.CvSource; -import edu.wpi.first.wpilibj.CameraServer; +import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj.TimedRobot; /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/intermediatevision/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/intermediatevision/Robot.java index 8910eb2ae8..526e849e1f 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/intermediatevision/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/intermediatevision/Robot.java @@ -15,7 +15,7 @@ import org.opencv.imgproc.Imgproc; import edu.wpi.cscore.CvSink; import edu.wpi.cscore.CvSource; import edu.wpi.cscore.UsbCamera; -import edu.wpi.first.wpilibj.CameraServer; +import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj.TimedRobot; /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/quickvision/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/quickvision/Robot.java index 1b40754e2c..9816f137e9 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/quickvision/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/quickvision/Robot.java @@ -7,7 +7,7 @@ package edu.wpi.first.wpilibj.examples.quickvision; -import edu.wpi.first.wpilibj.CameraServer; +import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj.TimedRobot; /**