The roboRIO can forward video to the driver station to allow a human driver to see while driving even when line of sight is blocked. Even better, we can process the video, either to assist the human driver (e.g. by placing cross hairs where a shooter is pointed) or by automatically finding a game piece of target during autonomous operation.

Most video processing is done using the free open-source library: OpenCV which is included with WPILib.

The example below displays the video from a USB web camera connected to the RoboRIO on the dashboard and overlays a red rectangle on each received video frame. Overlays such as cross-hairs, circles, etc. can be used to show the driver where the robot must be pointed/positioned to reach a target (e.g. where a shooter is pointing or a carried game-piece must be positioned. Overlays often help the driver line up the robot as needed.

package frc.robot;

import org.opencv.core.Mat;
import org.opencv.imgproc.Imgproc;
import org.opencv.core.Scalar;
import org.opencv.core.Point;

import edu.wpi.cscore.CvSink;
import edu.wpi.cscore.CvSource;
import edu.wpi.cscore.UsbCamera;
import edu.wpi.first.cameraserver.*;
import edu.wpi.first.wpilibj.TimedRobot;

public class Robot extends TimedRobot {
    public void robotInit() {
            // do camera processing in its own thread so if anything goes wrong,
            // we don't take down the entire robot
            new Thread(() -> {
                // get a handle to the camera server that accesses the USB camera
                UsbCamera camera = CameraServer.getInstance().startAutomaticCapture();
                camera.setResolution(640, 480);
                // CvSink receives video from the camera               
                CvSink cvSink = CameraServer.getInstance().getVideo();
                // CvSource sends video stream (named "Rectangle") to the smart dashboard
                CvSource outputStream = CameraServer.getInstance().putVideo("Rectangle", 640, 480);
                // We store video frames in a Mat for processing
                Mat mat = new Mat();
                // Continuously grab frames from the USB camera, 
                // add a rectangle overlay onto them, and
                // then stream the combined image to the dashboard
                while(!Thread.interrupted()) {

                  // grab a frame from the video stream
                  if (cvSink.grabFrame(mat) == 0) {
                    // skip the rest of the current iteration

                  // Draw a red rectangle on the frame image
                  Imgproc.rectangle(mat,                                    // mat to operate on
                                    new Point(160,120), new Point(480,360), // rectangle coords
                                    new Scalar(0,0,255),                    // OpenCV uses BGR
                                    5);                                     // line thickness

                  // Give the output stream the new image to display

Extra Credit:

  • Draw a crosshair on the video stream and change its position based on an xbox controller joystick input

Last modified 3 years ago Last modified on Nov 29, 2019, 9:51:45 AM