USB Camera Vision Processing with OpenCV (VMX-pi)

In this example, we are going to use a USB camera connected to the VMX-pi and display its video feed on the dashboard. However, this is not just a normal camera stream. In this program, we are also doing a small amount of vision processing using OpenCV. We begin by setting up the values in Constants.h. In this file, we store the camera index and the camera resolution. The camera index tells the robot which USB camera to use. In most cases, if only one USB camera is connected, its index will be 0. The width and height define the size of the video image that will be captured and displayed.

Constants.h

#pragma once
namespace constant
{
 // USB camera index
 static constexpr int CAMERA_INDEX = 0;
 // Camera resolution
 static constexpr int CAMERA_WIDTH = 640;
 static constexpr int CAMERA_HEIGHT = 480;
}

In Robot.h, we create the normal robot class and declare a function called VisionThread(). This function is where all of the camera processing will happen.

#pragma once
#include <frc/TimedRobot.h>
class Robot : public frc::TimedRobot {
public:
 void RobotInit() override;
 void RobotPeriodic() override;
 void DisabledInit() override;

 void DisabledPeriodic() override;
 void AutonomousInit() override;
 void AutonomousPeriodic() override;
 void TeleopInit() override;
 void TeleopPeriodic() override;
 void TestPeriodic() override;
private:
 static void VisionThread();
};

In Robot.cpp, inside VisionThread(), the robot starts the USB camera using CameraServer. This allows the robot to begin capturing live video from the camera. We then set the resolution so that the camera uses the width and height we selected in Constants.h. Next, we create two important OpenCV camera objects:  CvSink, which grabs video frames from the camera  CvSource, which sends processed video frames back to the dashboard We also create a cv::Mat object. A Mat is an OpenCV image container that stores the current frame from the camera. Inside the while (true) loop, the robot continuously grabs new frames from the camera. If a frame cannot be captured, the program sends an error to the output stream and skips that cycle. If the frame is captured successfully, the robot draws a white rectangle on the image using OpenCV. After that, the processed image is sent to the dashboard. This means the driver does not just see the original camera image — they see the image with the rectangle drawn on top of it. In RobotInit(), we start the vision code in a separate thread. This is very important. Camera processing can take time, and if it runs in the main robot loop, it can slow down or interfere with the rest of the robot code. By running it in its own thread, the robot can continue driving and responding normally while the camera processing happens in the background.

#include "Robot.h"
#include "Constants.h"
#include <thread>
#include <cameraserver/CameraServer.h>
#include <opencv2/core/core.hpp>
#include <opencv2/core/types.hpp>

#include <opencv2/imgproc/imgproc.hpp>
#include <wpi/raw_ostream.h>
void Robot::VisionThread()
{
#if defined(__linux__)
 // Start capturing video from the USB camera
 cs::UsbCamera camera =
 frc::CameraServer::GetInstance()-
>StartAutomaticCapture(constant::CAMERA_INDEX);
 // Set camera resolution
 camera.SetResolution(constant::CAMERA_WIDTH, constant::CAMERA_HEIGHT);
 // Get video from the camera
 cs::CvSink cvSink = frc::CameraServer::GetInstance()->GetVideo();
 // Create an output stream to send processed images to the dashboard
 cs::CvSource outputStream =
 frc::CameraServer::GetInstance()->PutVideo(
 "Rectangle",
 constant::CAMERA_WIDTH,
 constant::CAMERA_HEIGHT
 );
 // Create a Mat object to store each image frame
 cv::Mat mat;
 while (true)
 {
 // Grab a frame from the camera
 if (cvSink.GrabFrame(mat) == 0)
 {
 outputStream.NotifyError(cvSink.GetError());
 continue;
 }
 // Draw a white rectangle on the image
 rectangle(
 mat,
 cv::Point(100, 100),
 cv::Point(400, 400),
 cv::Scalar(255, 255, 255),
 5
 );

 // Send the processed frame to the dashboard
 outputStream.PutFrame(mat);
 }
#endif
}
void Robot::RobotInit()
{
#if defined(__linux__)
 // Run the camera processing in a separate thread
 std::thread visionThread(VisionThread);
 visionThread.detach();
#else
 wpi::errs() << "Vision only available on Linux.\n";
 wpi::errs().flush();
#endif
}
void Robot::RobotPeriodic() {}
void Robot::DisabledInit() {}
void Robot::DisabledPeriodic() {}
void Robot::AutonomousInit() {}
void Robot::AutonomousPeriodic() {}
void Robot::TeleopInit() {}
void Robot::TeleopPeriodic() {}
void Robot::TestPeriodic() {}
#ifndef RUNNING_FRC_TESTS
int main()
{
 return frc::StartRobot<Robot>();
}
#endif

To view the camera feed on Shuffleboard, first open the side panel by pulling out the tab on the left or right side of the screen.

Next, click on CameraServer.

Under CameraServer, you will see either USB Camera 0 or Rectangle, depending on which camera stream your code is sending. Drag that camera stream onto the dashboard. If everything has been set up correctly and the camera is connected to the USB port on the VMX-pi, you should now be able to see the live image or video feed from the camera on Shuffleboard.