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

Added the C++ implementation of Peter Johnson's UsbCamera.

Browse Source 
Rewrote CameraServer to use the new USBCamera implementation and get rid
of some unnecessary copying of the entire image.

Change-Id: I877750e990b6159c0aaf829df62b253a171fbada
This commit is contained in:
Fredric Silberberg
2015-01-05 13:46:12 -05:00
parent ae9a7d19f1
commit 2282a11a79
4 changed files with 696 additions and 240 deletions
Download Patch File Download Diff File Expand all files Collapse all files

407
wpilibc/wpilibC++Devices/src/CameraServer.cpp
View File

@@ -1,21 +1,10 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2014. 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. */
/*----------------------------------------------------------------------------*/
#include "CameraServer.h"
#include "ErrorBase.h"
#include "WPIErrors.h"
#include "Utility.h"
#include "Timer.h"
#include <iostream>
#include <algorithm>
#include <algorithm>
#include <chrono>
#include <cstring>
#include <sys/socket.h>
#include <unistd.h>
#include <netdb.h>
@@ -23,228 +12,234 @@
constexpr uint8_t CameraServer::kMagicNumber[];
CameraServer *CameraServer::s_instance = nullptr;
/**
* Singleton getter.
*/
CameraServer *CameraServer::GetInstance() {
if(s_instance == nullptr) {
s_instance = new CameraServer;
}
return s_instance;
CameraServer* CameraServer::GetInstance() {
if (s_instance == NULL) {
s_instance = new CameraServer;
}
return s_instance;
}
CameraServer::CameraServer() :
m_serverThread(&CameraServer::serve, this),
m_quality(50),
m_autoCaptureStarted(false) {
m_camera(),
m_serverThread(&CameraServer::Serve, this),
m_captureThread(),
m_imageMutex(),
m_newImageVariable(),
m_dataPool(3),
m_quality(50),
m_autoCaptureStarted(false),
m_hwClient(true),
m_imageData(nullptr, 0, 0, false) {
for (int i = 0; i < 3; i++)
m_dataPool.push_back(new uint8_t[kMaxImageSize]);
}
/**
* Manually change the image that is served by the MJPEG stream. This can
* be called to pass custom annotated images to the dashboard. Note that, for
* 640x480 video, this method could take between 40 and 50 milliseconds to
* complete.
*
* This shouldn't be called if {@link #StartAutomaticCapture} is called.
*
* @param image The IMAQ image to show on the dashboard
*/
void CameraServer::SetImage(Image const *image) {std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
/* Flatten the IMAQ image to a JPEG */
uint32_t dataSize;
uint8_t *data = (uint8_t *)imaqFlatten(image, IMAQ_FLATTEN_IMAGE, IMAQ_COMPRESSION_JPEG, 10 * m_quality, &dataSize);
/* Find the start of the JPEG data */
uint8_t *jpegData = data;
while(jpegData[0] != 0xff || jpegData[1] != 0xd8) {
jpegData++;
dataSize--;
wpi_assert(dataSize >= 2);
}
m_imageData.assign(dataSize, '\0');
std::copy(jpegData, jpegData + dataSize, m_imageData.begin());
/* Release the data from IMAQ */
imaqDispose(data);
m_newImageReady.notify_all();
void CameraServer::FreeImageData(std::tuple<uint8_t*, unsigned int, unsigned int, bool> imageData) {
if (std::get<3>(imageData)) imaqDispose(std::get<0>(imageData));
else if (std::get<0>(imageData) != nullptr) {
std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
m_dataPool.push_back(std::get<0>(imageData));
}
}
void CameraServer::SetImageData(uint8_t* data, unsigned int size, unsigned int start, bool imaqData) {
std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
FreeImageData(m_imageData);
m_imageData = std::make_tuple(data, size, start, imaqData);
m_newImageVariable.notify_all();
}
void CameraServer::SetImage(Image const *image) {
unsigned int dataSize = 0;
uint8_t* data = (uint8_t*) imaqFlatten(image, IMAQ_FLATTEN_IMAGE, IMAQ_COMPRESSION_JPEG, 10 * m_quality, &dataSize);
// If we're using a HW camera, then find the start of the data
bool hwClient;
{
// Make a local copy of the hwClient variable so that we can safely use it.
std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
hwClient = m_hwClient;
}
unsigned int start = 0;
if (hwClient) {
while (start < dataSize - 1) {
if (data[start] == 0xFF && data[start + 1] == 0xD8) break;
else start++;
}
}
dataSize -= start;
wpi_assert(dataSize > 2);
SetImageData(data, dataSize, start, true);
}
void CameraServer::AutoCapture() {
Image* frame = imaqCreateImage(IMAQ_IMAGE_RGB, 0);
while (true) {
bool hwClient;
uint8_t* data = nullptr;
{
std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
hwClient = m_hwClient;
if (hwClient) {
data = m_dataPool.back();
m_dataPool.pop_back();
}
}
if (hwClient) {
unsigned int size = m_camera->GetImageData(data, kMaxImageSize);
SetImageData(data, size);
} else {
m_camera->GetImage(frame);
SetImage(frame);
}
}
}
/**
* Start automatically capturing images to send to the dashboard.
*
* You should call this method to just see a camera feed on the dashboard
* without doing any vision processing on the roboRIO. {@link #SetImage}
* shouldn't be called after this is called.
*
* @param cameraName The name of the camera interface (e.g. "cam1")
*/
void CameraServer::StartAutomaticCapture(char const *cameraName) {
if(m_autoCaptureStarted) {
wpi_setWPIErrorWithContext(IncompatibleState, "Automatic capture has already been started");
return;
}
/* Capturing images takes a lot of CPU time, since JPEG compression is
done in software, so this is done in a new thread. */
std::thread captureThread([cameraName, this] {
IMAQdxSession session;
IMAQdxError error;
Image *frame = imaqCreateImage(IMAQ_IMAGE_RGB, 0);
error = IMAQdxOpenCamera(cameraName, IMAQdxCameraControlModeController, &session);
if(error != IMAQdxErrorSuccess) {
wpi_setImaqErrorWithContext(error, "IMAQdxOpenCamera");
}
error = IMAQdxConfigureGrab(session);
if(error != IMAQdxErrorSuccess) {
wpi_setImaqErrorWithContext(error, "IMAQdxConfigureGrab");
}
error = IMAQdxStartAcquisition(session);
if(error != IMAQdxErrorSuccess) {
wpi_setImaqErrorWithContext(error, "IMAQdxStartAcquisition");
}
/* In an infinite loop, wait for an image from the camera, then sent
it to the dashboard. */
for(;;) {
error = IMAQdxGrab(session, frame, true, NULL);
if(error != IMAQdxErrorSuccess) {
wpi_setImaqErrorWithContext(error, "IMAQdxGrab");
break;
}
SetImage(frame);
}
/* If something went wrong, close the session */
IMAQdxStopAcquisition(session);
IMAQdxCloseCamera(session);
});
captureThread.detach();
std::shared_ptr<USBCamera> camera = std::make_shared<USBCamera>(cameraName, true);
camera->OpenCamera();
StartAutomaticCapture(camera);
}
void CameraServer::StartAutomaticCapture(std::shared_ptr<USBCamera> camera) {
std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
if (m_autoCaptureStarted) return;
m_camera = camera;
m_camera->StartCapture();
m_captureThread = std::thread(&CameraServer::AutoCapture, this);
m_captureThread.detach();
m_autoCaptureStarted = true;
}
bool CameraServer::IsAutoCaptureStarted() {
std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
return m_autoCaptureStarted;
}
void CameraServer::SetSize(unsigned int size) {
std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
if (!m_camera) return;
if (size == kSize160x120) m_camera->SetSize(160, 120);
else if (size == kSize320x240) m_camera->SetSize(320, 240);
else if (size == kSize640x480) m_camera->SetSize(640, 480);
}
/**
* Set the quality of the compressed image sent to the dashboard
*
* @param quality The quality of the JPEG image, from 0 to 100
*/
void CameraServer::SetQuality(unsigned int quality) {
if(quality > 100) {
wpi_setWPIErrorWithContext(ParameterOutOfRange, "JPEG quality should be from 0 to 100");
return;
}
m_quality = quality;
std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
m_quality = quality > 100 ? 100 : quality;
}
/**
* Get the quality of the compressed image sent to the dashboard
*
* @return The quality, from 0 to 100
*/
unsigned int CameraServer::GetQuality() const {
return m_quality;
unsigned int CameraServer::GetQuality() {
std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
return m_quality;
}
/**
* Run the M-JPEG server.
*
* This function listens for a connection from the dashboard in a background
* thread, then sends back the M-JPEG stream.
*/
void CameraServer::serve() {
int sock = socket(AF_INET, SOCK_STREAM, 0);
void CameraServer::Serve() {
int sock = socket(AF_INET, SOCK_STREAM, 0);
if(sock == -1) {
wpi_setErrnoError();
}
if (sock == -1)
wpi_setErrnoError();
int reuseAddr = 1;
if(setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &reuseAddr, sizeof(reuseAddr)) == -1) {
wpi_setErrnoError();
}
int reuseAddr = 1;
if (setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &reuseAddr, sizeof(reuseAddr)) == -1)
wpi_setErrnoError();
sockaddr_in address, clientAddress;
sockaddr_in address, clientAddress;
memset(&address, 0, sizeof(address));
address.sin_family = AF_INET;
address.sin_addr.s_addr = htonl(INADDR_ANY);
address.sin_port = htons(kPort);
memset(&address, 0, sizeof(address));
address.sin_family = AF_INET;
address.sin_addr.s_addr = htonl(INADDR_ANY);
address.sin_port = htons(kPort);
if(bind(sock, (struct sockaddr *)&address, sizeof(address)) == -1) {
wpi_setErrnoError();
}
if (bind(sock, (struct sockaddr *)&address, sizeof(address)) == -1)
wpi_setErrnoError();
if(listen(sock, 10) == -1) {
wpi_setErrnoError();
}
if (listen(sock, 10) == -1)
wpi_setErrnoError();
for(;;) {
socklen_t clientaddresslen = sizeof(clientAddress);
int conn = accept(sock, (struct sockaddr *)&clientAddress, &clientaddresslen);
while(true) {
socklen_t clientAddressLen = sizeof(clientAddress);
/* Read the request from the dashboard */
Request req;
if(read(conn, &req, sizeof req) == -1) {
wpi_setErrnoError();
close(conn);
continue;
} else {
req.fps = ntohl(req.fps);
req.compression = ntohl(req.compression);
req.size = ntohl(req.size);
}
int conn = accept(sock, (struct sockaddr*)&clientAddress, &clientAddressLen);
if (conn == -1) {
wpi_setErrnoError();
continue;
}
/* Only the "hardware compression" mode is supported from C++ */
if(req.compression != kHardwareCompression) {
wpi_setWPIErrorWithContext(IncompatibleState, "Choose \"USB Camera HW\" on the dashboard");
close(conn);
continue;
}
Request req;
if (read(conn, &req, sizeof(req)) == -1) {
wpi_setErrnoError();
close(conn);
continue;
} else {
req.fps = ntohl(req.fps);
req.compression = ntohl(req.compression);
req.size = ntohl(req.size);
}
/* The period that frames are sent is 1/FPS */
auto period = std::chrono::microseconds(1000000) / req.fps;
// TODO: Support the SW Compression. The rest of the code below will work as though this
// check isn't here
if (req.compression != kHardwareCompression) {
wpi_setWPIErrorWithContext(IncompatibleState, "Choose \"USB Camera HW\" on the dashboard");
close(conn);
continue;
}
for(;;) {
auto startTime = std::chrono::steady_clock::now();
{
// Wait for the camera to be setw
std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
if (!m_camera) {
std::cout << "Camera not yet ready, awaiting first image" << std::endl;
m_newImageVariable.wait(lock);
}
m_hwClient = req.compression == kHardwareCompression;
if (!m_hwClient) SetQuality(100 - req.compression);
else if (m_camera) m_camera->SetFPS(req.fps);
SetSize(req.size);
}
{
std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
auto period = std::chrono::microseconds(1000000) / req.fps;
while (true) {
auto startTime = std::chrono::steady_clock::now();
std::tuple<uint8_t*, unsigned int, unsigned int, bool> imageData;
{
std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
m_newImageVariable.wait(lock);
imageData = m_imageData;
m_imageData = std::make_tuple(nullptr, 0, 0, false);
}
m_newImageReady.wait(lock);
unsigned int size = std::get<1>(imageData);
unsigned int netSize = htonl(size);
unsigned int start = std::get<2>(imageData);
uint8_t *data = std::get<0>(imageData);
/* Send the magic number that indicates the beginning of a new image */
if(write(conn, kMagicNumber, sizeof kMagicNumber) == -1) {
break;
}
if (data == nullptr) continue;
/* Send the size of this image */
uint32_t size = htonl(m_imageData.size());
if(write(conn, &size, sizeof size) == -1) {
m_imageMutex.unlock();
break;
}
/* Send the image data itself */
if(write(conn, m_imageData.data(), m_imageData.size()) == -1) {
break;
}
}
/* Sleep long enough to maintain a constant framerate */
std::this_thread::sleep_until(startTime + period);
}
close(conn);
}
close(sock);
if (write(conn, kMagicNumber, sizeof(kMagicNumber)) == -1) {
wpi_setErrnoErrorWithContext("[CameraServer] Error sending magic number");
FreeImageData(imageData);
break;
}
if (write(conn, &netSize, sizeof(netSize)) == -1) {
wpi_setErrnoErrorWithContext("[CameraServer] Error sending image size");
FreeImageData(imageData);
break;
}
if (write(conn, &data[start], sizeof(uint8_t) * size) == -1) {
wpi_setErrnoErrorWithContext("[CameraServer] Error sending image data");
FreeImageData(imageData);
break;
}
FreeImageData(imageData);
std::this_thread::sleep_until(startTime + period);
}
close(conn);
}
close(sock);
}