From 2282a11a79023522cde9dbac42e3f04385e53ebc Mon Sep 17 00:00:00 2001 From: Fredric Silberberg Date: Mon, 5 Jan 2015 13:46:12 -0500 Subject: [PATCH] Added the C++ implementation of Peter Johnson's UsbCamera. Rewrote CameraServer to use the new USBCamera implementation and get rid of some unnecessary copying of the entire image. Change-Id: I877750e990b6159c0aaf829df62b253a171fbada --- .../wpilibC++Devices/include/CameraServer.h | 90 ++-- wpilibc/wpilibC++Devices/include/USBCamera.h | 111 +++++ wpilibc/wpilibC++Devices/src/CameraServer.cpp | 407 +++++++++--------- wpilibc/wpilibC++Devices/src/USBCamera.cpp | 328 ++++++++++++++ 4 files changed, 696 insertions(+), 240 deletions(-) create mode 100644 wpilibc/wpilibC++Devices/include/USBCamera.h create mode 100644 wpilibc/wpilibC++Devices/src/USBCamera.cpp diff --git a/wpilibc/wpilibC++Devices/include/CameraServer.h b/wpilibc/wpilibC++Devices/include/CameraServer.h index 67b78ccc25..8359ca366a 100644 --- a/wpilibc/wpilibC++Devices/include/CameraServer.h +++ b/wpilibc/wpilibC++Devices/include/CameraServer.h @@ -6,54 +6,76 @@ /*----------------------------------------------------------------------------*/ #pragma once +#include "USBCamera.h" #include "ErrorBase.h" #include "nivision.h" #include "NIIMAQdx.h" -#include -#include #include +#include +#include #include +#include +#include -/** - * 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 m_camera; + std::thread m_serverThread; + std::thread m_captureThread; + std::recursive_mutex m_imageMutex; + std::condition_variable_any m_newImageVariable; + std::vector m_dataPool; + unsigned int m_quality; + bool m_autoCaptureStarted; + bool m_hwClient; + std::tuple 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 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 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 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); }; - diff --git a/wpilibc/wpilibC++Devices/include/USBCamera.h b/wpilibc/wpilibC++Devices/include/USBCamera.h new file mode 100644 index 0000000000..ac8bfe4e9d --- /dev/null +++ b/wpilibc/wpilibC++Devices/include/USBCamera.h @@ -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 +#include + +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); +}; diff --git a/wpilibc/wpilibC++Devices/src/CameraServer.cpp b/wpilibc/wpilibC++Devices/src/CameraServer.cpp index 5fd03573e3..d8547bf052 100644 --- a/wpilibc/wpilibC++Devices/src/CameraServer.cpp +++ b/wpilibc/wpilibC++Devices/src/CameraServer.cpp @@ -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 -#include -#include +#include #include - #include #include #include @@ -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 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 imageData) { + if (std::get<3>(imageData)) imaqDispose(std::get<0>(imageData)); + else if (std::get<0>(imageData) != nullptr) { + std::unique_lock 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 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 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 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 camera = std::make_shared(cameraName, true); + camera->OpenCamera(); + StartAutomaticCapture(camera); +} + +void CameraServer::StartAutomaticCapture(std::shared_ptr camera) { + std::unique_lock 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 lock(m_imageMutex); + return m_autoCaptureStarted; +} + +void CameraServer::SetSize(unsigned int size) { + std::unique_lock 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 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 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 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 lock(m_imageMutex); + auto period = std::chrono::microseconds(1000000) / req.fps; + while (true) { + auto startTime = std::chrono::steady_clock::now(); + std::tuple imageData; + { + std::unique_lock 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); } diff --git a/wpilibc/wpilibC++Devices/src/USBCamera.cpp b/wpilibc/wpilibC++Devices/src/USBCamera.cpp new file mode 100644 index 0000000000..8d36be1cb1 --- /dev/null +++ b/wpilibc/wpilibC++Devices/src/USBCamera.cpp @@ -0,0 +1,328 @@ +#include "USBCamera.h" + +#include "Utility.h" + +#include +#include +#include +#include +#include + +// 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 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 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 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 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 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, ¤tMode); + IMAQdxVideoMode modes[count]; + SAFE_IMAQ_CALL(IMAQdxEnumerateVideoModes, m_id, modes, &count, ¤tMode); + + // 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 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 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 lock(m_mutex); + if (m_brightness != brightness) { + m_needSettingsUpdate = true; + m_brightness = brightness; + } +} + +unsigned int USBCamera::GetBrightness() { + std::unique_lock lock(m_mutex); + return m_brightness; +} + +void USBCamera::SetWhiteBalanceAuto() { + std::unique_lock lock(m_mutex); + m_whiteBalance = AUTO; + m_whiteBalanceValue = 0; + m_whiteBalanceValuePresent = false; + m_needSettingsUpdate = true; +} + +void USBCamera::SetWhiteBalanceHoldCurrent() { + std::unique_lock lock(m_mutex); + m_whiteBalance = MANUAL; + m_whiteBalanceValue = 0; + m_whiteBalanceValuePresent = false; + m_needSettingsUpdate = true; +} + +void USBCamera::SetWhiteBalanceManual(unsigned int whiteBalance) { + std::unique_lock lock(m_mutex); + m_whiteBalance = MANUAL; + m_whiteBalanceValue = whiteBalance; + m_whiteBalanceValuePresent = true; + m_needSettingsUpdate = true; +} + +void USBCamera::SetExposureAuto() { + std::unique_lock lock(m_mutex); + m_exposure = AUTO; + m_exposureValue = 0; + m_exposureValuePresent = false; + m_needSettingsUpdate = true; +} + +void USBCamera::SetExposureHoldCurrent() { + std::unique_lock lock(m_mutex); + m_exposure = MANUAL; + m_exposureValue = 0; + m_exposureValuePresent = false; + m_needSettingsUpdate = true; +} + +void USBCamera::SetExposureManual(unsigned int level) { + std::unique_lock 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 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 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); +}