Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-28 02:11: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

90
wpilibc/wpilibC++Devices/include/CameraServer.h
View File

@@ -6,54 +6,76 @@
/*----------------------------------------------------------------------------*/
#pragma once
#include "USBCamera.h"
#include "ErrorBase.h"
#include "nivision.h"
#include "NIIMAQdx.h"
#include <vector>
#include <thread>
#include <mutex>
#include <thread>
#include <memory>
#include <condition_variable>
#include <tuple>
#include <vector>
/**
* Class that runs a TCP server that serves an M-JPEG stream to the dashboard.
*/
class CameraServer : public ErrorBase {
static constexpr uint16_t kPort = 1180;
static constexpr uint8_t kMagicNumber[] = { 0x01, 0x00, 0x00, 0x00 };
static constexpr uint32_t kSize640x480 = 0;
static constexpr uint32_t kSize320x240 = 1;
static constexpr uint32_t kSize160x120 = 2;
static constexpr int32_t kHardwareCompression = -1;
static constexpr char const *kDefaultCameraName = "cam0";
private:
static constexpr uint16_t kPort = 1180;
static constexpr uint8_t kMagicNumber[] = { 0x01, 0x00, 0x00, 0x00 };
static constexpr uint32_t kSize640x480 = 0;
static constexpr uint32_t kSize320x240 = 1;
static constexpr uint32_t kSize160x120 = 2;
static constexpr int32_t kHardwareCompression = -1;
static constexpr uint32_t kMaxImageSize = 200000;
public:
static CameraServer *GetInstance();
protected:
CameraServer();
std::shared_ptr<USBCamera> m_camera;
std::thread m_serverThread;
std::thread m_captureThread;
std::recursive_mutex m_imageMutex;
std::condition_variable_any m_newImageVariable;
std::vector<uint8_t*> m_dataPool;
unsigned int m_quality;
bool m_autoCaptureStarted;
bool m_hwClient;
std::tuple<uint8_t*, unsigned int, unsigned int, bool> m_imageData;
void SetImage(Image const *image);
void Serve();
void AutoCapture();
void SetImageData(uint8_t* data, unsigned int size, unsigned int start = 0, bool imaqData = false);
void FreeImageData(std::tuple<uint8_t*, unsigned int, unsigned int, bool> imageData);
void StartAutomaticCapture(char const *cameraName = kDefaultCameraName);
struct Request {
uint32_t fps;
int32_t compression;
uint32_t size;
};
void SetQuality(unsigned int quality);
unsigned int GetQuality() const;
static CameraServer* s_instance;
public:
static CameraServer* GetInstance();
void SetImage(Image const *image);
protected:
CameraServer();
void serve();
void StartAutomaticCapture(char const *cameraName = USBCamera::kDefaultCameraName);
std::thread m_serverThread;
std::recursive_mutex m_imageMutex;
std::condition_variable_any m_newImageReady;
std::vector<uint8_t> m_imageData;
unsigned int m_quality;
bool m_autoCaptureStarted;
/**
* 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} should not be called after this is called.
*
* @param camera The camera interface (eg. USBCamera)
*/
void StartAutomaticCapture(std::shared_ptr<USBCamera> camera);
struct Request {
uint32_t fps;
int32_t compression;
uint32_t size;
};
bool IsAutoCaptureStarted();
static CameraServer *s_instance;
void SetQuality(unsigned int quality);
unsigned int GetQuality();
void SetSize(unsigned int size);
};

111
wpilibc/wpilibC++Devices/include/USBCamera.h Normal file
View File

@@ -0,0 +1,111 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
#pragma once
#include "ErrorBase.h"
#include "nivision.h"
#include "NIIMAQdx.h"
#include <mutex>
#include <string>
typedef enum whiteBalance_enum {
kFixedIndoor = 3000,
kFixedOutdoor1 = 4000,
kFixedOutdoor2 = 5000,
kFixedFluorescent1 = 5100,
kFixedFlourescent2 = 5200
} whiteBalance;
class USBCamera : public ErrorBase {
private:
static constexpr char const *ATTR_WB_MODE = "CameraAttributes::WhiteBalance::Mode";
static constexpr char const *ATTR_WB_VALUE = "CameraAttributes::WhiteBalance::Value";
static constexpr char const *ATTR_EX_MODE = "CameraAttributes::Exposure::Mode";
static constexpr char const *ATTR_EX_VALUE = "CameraAttributes::Exposure::Value";
static constexpr char const *ATTR_BR_MODE = "CameraAttributes::Brightness::Mode";
static constexpr char const *ATTR_BR_VALUE = "CameraAttributes::Brightness::Value";
protected:
IMAQdxSession m_id;
std::string m_name;
bool m_useJpeg;
bool m_active;
bool m_open;
std::recursive_mutex m_mutex;
unsigned int m_width;
unsigned int m_height;
double m_fps;
std::string m_whiteBalance;
unsigned int m_whiteBalanceValue;
bool m_whiteBalanceValuePresent;
std::string m_exposure;
unsigned int m_exposureValue;
bool m_exposureValuePresent;
unsigned int m_brightness;
bool m_needSettingsUpdate;
unsigned int GetJpegSize(void* buffer, unsigned int buffSize);
public:
static constexpr char const *kDefaultCameraName = "cam0";
USBCamera(std::string name, bool useJpeg);
void OpenCamera();
void CloseCamera();
void StartCapture();
void StopCapture();
void SetFPS(double fps);
void SetSize(unsigned int width, unsigned int height);
void UpdateSettings();
/**
* Set the brightness, as a percentage (0-100).
*/
void SetBrightness(unsigned int brightness);
/**
* Get the brightness, as a percentage (0-100).
*/
unsigned int GetBrightness();
/**
* Set the white balance to auto
*/
void SetWhiteBalanceAuto();
/**
* Set the white balance to hold current
*/
void SetWhiteBalanceHoldCurrent();
/**
* Set the white balance to manual, with specified color temperature
*/
void SetWhiteBalanceManual(unsigned int wbValue);
/**
* Set the exposure to auto exposure
*/
void SetExposureAuto();
/**
* Set the exposure to hold current
*/
void SetExposureHoldCurrent();
/**
* Set the exposure to manual, with a given percentage (0-100)
*/
void SetExposureManual(unsigned int expValue);
void GetImage(Image* image);
unsigned int GetImageData(void* buffer, unsigned int bufferSize);
};

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

328
wpilibc/wpilibC++Devices/src/USBCamera.cpp Normal file
View File

@@ -0,0 +1,328 @@
#include "USBCamera.h"
#include "Utility.h"
#include <regex>
#include <chrono>
#include <thread>
#include <iostream>
#include <iomanip>
// This macro expands the given imaq function to ensure that it is called and
// properly checked for an error, calling the wpi_setImaqErrorWithContext
// macro
// To call it, just give the name of the function and the arguments
#define SAFE_IMAQ_CALL(funName, ...) { \
unsigned int error = funName(__VA_ARGS__); \
if (error != IMAQdxErrorSuccess) \
wpi_setImaqErrorWithContext(error, #funName); \
}
// Constants for the manual and auto types
static const std::string AUTO = "Auto";
static const std::string MANUAL = "Manual";
/**
* Helper function to determine the size of a jpeg. The general structure of
* how to parse a jpeg for length can be found in this stackoverflow article:
* http://stackoverflow.com/a/1602428. Be sure to also read the comments for
* the SOS flag explanation.
*/
unsigned int USBCamera::GetJpegSize(void* buffer, unsigned int buffSize) {
uint8_t* data = (uint8_t*) buffer;
if (!wpi_assert(data[0] == 0xff && data[1] == 0xd8)) return 0;
unsigned int pos = 2;
while (pos < buffSize) {
// All control markers start with 0xff, so if this isn't present,
// the JPEG is not valid
if (!wpi_assert(data[pos] == 0xff)) return 0;
unsigned char t = data[pos+1];
// These are RST markers. We just skip them and move onto the next marker
if (t == 0x01 || (t >= 0xd0 && t <= 0xd7)) {
pos += 2;
} else if (t == 0xd9) {
// End of Image, add 2 for this and 0-indexed
return pos + 2;
} else if (!wpi_assert(t != 0xd8)) {
// Another start of image, invalid image
return 0;
} else if (t == 0xda) {
// SOS marker. The next two bytes are a 16-bit big-endian int that is
// the length of the SOS header, skip that
unsigned int len = (((unsigned int) (data[pos+2] & 0xff)) << 8 | ((unsigned int) data[pos+3] & 0xff));
pos += len + 2;
// The next marker is the first marker that is 0xff followed by a non-RST
// element. 0xff followed by 0x00 is an escaped 0xff. 0xd0-0xd7 are RST
// markers
while (data[pos] != 0xff || data[pos+1] == 0x00 || (data[pos+1] >= 0xd0 && data[pos+1] <= 0xd7)) {
pos += 1;
if (pos >= buffSize) return 0;
}
} else {
// This is one of several possible markers. The next two bytes are a 16-bit
// big-endian int with the length of the marker header, skip that then
// continue searching
unsigned int len = (((unsigned int) (data[pos+2] & 0xff)) << 8 | ((unsigned int) data[pos+3] & 0xff));
pos += len + 2;
}
}
return 0;
}
USBCamera::USBCamera(std::string name, bool useJpeg) :
m_id(0),
m_name(name),
m_useJpeg(useJpeg),
m_active(false),
m_open(false),
m_mutex(),
m_width(320),
m_height(240),
m_fps(30),
m_whiteBalance(AUTO),
m_whiteBalanceValue(0),
m_whiteBalanceValuePresent(false),
m_exposure(MANUAL),
m_exposureValue(50),
m_exposureValuePresent(false),
m_brightness(80),
m_needSettingsUpdate(true) {
}
void USBCamera::OpenCamera() {
std::unique_lock<std::recursive_mutex> lock(m_mutex);
for (unsigned int i = 0; i < 3; i++) {
uInt32 id = 0;
// Can't use SAFE_IMAQ_CALL here because we only error on the third time
IMAQdxError error = IMAQdxOpenCamera(m_name.c_str(), IMAQdxCameraControlModeController, &id);
if (error != IMAQdxErrorSuccess) {
// Only error on the 3rd try
if (i >= 2)
wpi_setImaqErrorWithContext(error, "IMAQdxOpenCamera");
// Sleep for a few seconds to ensure the error has been dealt with
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
} else {
m_id = id;
m_open = true;
return;
}
}
}
void USBCamera::CloseCamera() {
std::unique_lock<std::recursive_mutex> lock(m_mutex);
if (!m_open) return;
SAFE_IMAQ_CALL(IMAQdxCloseCamera, m_id);
m_id = 0;
m_open = false;
}
void USBCamera::StartCapture() {
std::unique_lock<std::recursive_mutex> lock(m_mutex);
if (!m_open || m_active) return;
SAFE_IMAQ_CALL(IMAQdxConfigureGrab, m_id);
SAFE_IMAQ_CALL(IMAQdxStartAcquisition, m_id);
m_active = true;
}
void USBCamera::StopCapture() {
std::unique_lock<std::recursive_mutex> lock(m_mutex);
if (!m_open || !m_active) return;
SAFE_IMAQ_CALL(IMAQdxStopAcquisition, m_id);
SAFE_IMAQ_CALL(IMAQdxUnconfigureAcquisition, m_id);
m_active = false;
}
void USBCamera::UpdateSettings() {
std::unique_lock<std::recursive_mutex> lock(m_mutex);
bool wasActive = m_active;
if (wasActive)
StopCapture();
if (m_open)
CloseCamera();
OpenCamera();
uInt32 count = 0;
uInt32 currentMode = 0;
SAFE_IMAQ_CALL(IMAQdxEnumerateVideoModes, m_id, NULL, &count, &currentMode);
IMAQdxVideoMode modes[count];
SAFE_IMAQ_CALL(IMAQdxEnumerateVideoModes, m_id, modes, &count, &currentMode);
// Groups are:
// 0 - width
// 1 - height
// 2 - format
// 3 - fps
std::regex reMode("([0-9]+)\\s*x\\s*([0-9]+)\\s+(.*?)\\s+([0-9.]+)\\s*fps");
IMAQdxVideoMode* foundMode = nullptr;
IMAQdxVideoMode* currentModePtr = &modes[currentMode];
double foundFps = 1000.0;
// Loop through the modes, and find the match with the lowest fps
for (unsigned int i = 0; i < count; i++) {
std::cmatch m;
if (!std::regex_match(modes[i].Name, m, reMode))
continue;
unsigned int width = (unsigned int) std::stoul(m[1].str());
unsigned int height = (unsigned int) std::stoul(m[2].str());
if (width != m_width)
continue;
if (height != m_height)
continue;
double fps = atof(m[4].str().c_str());
if (fps < m_fps)
continue;
if (fps > foundFps)
continue;
bool isJpeg = m[3].str().compare("jpeg") == 0 || m[3].str().compare("JPEG") == 0;
if ((m_useJpeg && !isJpeg) || (!m_useJpeg && isJpeg))
continue;
foundMode = &modes[i];
foundFps = fps;
}
if (foundMode != nullptr) {
if (foundMode->Value != currentModePtr->Value) {
SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, IMAQdxAttributeVideoMode, IMAQdxValueTypeU32, foundMode->Value);
}
}
if (m_whiteBalance.compare(AUTO) == 0) {
SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_WB_MODE, IMAQdxValueTypeString, AUTO.c_str());
} else {
SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_WB_MODE, IMAQdxValueTypeString, MANUAL.c_str());
if (m_whiteBalanceValuePresent)
SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_WB_VALUE, IMAQdxValueTypeU32, m_whiteBalanceValue);
}
if (m_exposure.compare(AUTO) == 0) {
SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_EX_MODE, IMAQdxValueTypeString, std::string("AutoAperaturePriority").c_str());
} else {
SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_EX_MODE, IMAQdxValueTypeString, MANUAL.c_str());
if (m_exposureValuePresent) {
double minv = 0.0;
double maxv = 0.0;
SAFE_IMAQ_CALL(IMAQdxGetAttributeMinimum, m_id, ATTR_EX_VALUE, IMAQdxValueTypeF64, &minv);
SAFE_IMAQ_CALL(IMAQdxGetAttributeMaximum, m_id, ATTR_EX_VALUE, IMAQdxValueTypeF64, &maxv);
double val = minv + ((maxv - minv) * ((double) m_exposureValue / 100.0));
SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_EX_VALUE, IMAQdxValueTypeF64, val);
}
}
SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_BR_MODE, IMAQdxValueTypeString, MANUAL.c_str());
double minv = 0.0;
double maxv = 0.0;
SAFE_IMAQ_CALL(IMAQdxGetAttributeMinimum, m_id, ATTR_BR_VALUE, IMAQdxValueTypeF64, &minv);
SAFE_IMAQ_CALL(IMAQdxGetAttributeMaximum, m_id, ATTR_BR_VALUE, IMAQdxValueTypeF64, &maxv);
double val = minv + ((maxv - minv) * ((double) m_brightness / 100.0));
SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_BR_VALUE, IMAQdxValueTypeF64, val);
if (wasActive)
StartCapture();
}
void USBCamera::SetFPS(double fps) {
std::unique_lock<std::recursive_mutex> lock(m_mutex);
if (m_fps != fps) {
m_needSettingsUpdate = true;
m_fps = fps;
}
}
void USBCamera::SetSize(unsigned int width, unsigned int height) {
std::unique_lock<std::recursive_mutex> lock(m_mutex);
if (m_width != width || m_height != height) {
m_needSettingsUpdate = true;
m_width = width;
m_height = height;
}
}
void USBCamera::SetBrightness(unsigned int brightness) {
std::unique_lock<std::recursive_mutex> lock(m_mutex);
if (m_brightness != brightness) {
m_needSettingsUpdate = true;
m_brightness = brightness;
}
}
unsigned int USBCamera::GetBrightness() {
std::unique_lock<std::recursive_mutex> lock(m_mutex);
return m_brightness;
}
void USBCamera::SetWhiteBalanceAuto() {
std::unique_lock<std::recursive_mutex> lock(m_mutex);
m_whiteBalance = AUTO;
m_whiteBalanceValue = 0;
m_whiteBalanceValuePresent = false;
m_needSettingsUpdate = true;
}
void USBCamera::SetWhiteBalanceHoldCurrent() {
std::unique_lock<std::recursive_mutex> lock(m_mutex);
m_whiteBalance = MANUAL;
m_whiteBalanceValue = 0;
m_whiteBalanceValuePresent = false;
m_needSettingsUpdate = true;
}
void USBCamera::SetWhiteBalanceManual(unsigned int whiteBalance) {
std::unique_lock<std::recursive_mutex> lock(m_mutex);
m_whiteBalance = MANUAL;
m_whiteBalanceValue = whiteBalance;
m_whiteBalanceValuePresent = true;
m_needSettingsUpdate = true;
}
void USBCamera::SetExposureAuto() {
std::unique_lock<std::recursive_mutex> lock(m_mutex);
m_exposure = AUTO;
m_exposureValue = 0;
m_exposureValuePresent = false;
m_needSettingsUpdate = true;
}
void USBCamera::SetExposureHoldCurrent() {
std::unique_lock<std::recursive_mutex> lock(m_mutex);
m_exposure = MANUAL;
m_exposureValue = 0;
m_exposureValuePresent = false;
m_needSettingsUpdate = true;
}
void USBCamera::SetExposureManual(unsigned int level) {
std::unique_lock<std::recursive_mutex> lock(m_mutex);
m_exposure = MANUAL;
if (level > 100) m_exposureValue = 100;
else m_exposureValue = level;
m_exposureValuePresent = true;
m_needSettingsUpdate = true;
}
void USBCamera::GetImage(Image* image) {
std::unique_lock<std::recursive_mutex> lock(m_mutex);
if (m_needSettingsUpdate || m_useJpeg) {
m_needSettingsUpdate = false;
m_useJpeg = false;
UpdateSettings();
}
// BufNum is not actually used for anything at our level, since we are
// waiting until the next image is ready anyway
uInt32 bufNum;
SAFE_IMAQ_CALL(IMAQdxGrab, m_id, image, 1, &bufNum);
}
unsigned int USBCamera::GetImageData(void* buffer, unsigned int bufferSize) {
std::unique_lock<std::recursive_mutex> lock(m_mutex);
if (m_needSettingsUpdate || !m_useJpeg) {
m_needSettingsUpdate = false;
m_useJpeg = true;
UpdateSettings();
}
// BufNum is not actually used for anything at our level
uInt32 bufNum;
SAFE_IMAQ_CALL(IMAQdxGetImageData, m_id, buffer, bufferSize, IMAQdxBufferNumberModeLast, 0, &bufNum);
return GetJpegSize(buffer, bufferSize);
}