Squashed 'third_party/allwpilib_2016/' content from commit 7f61816
Change-Id: If9d9245880859cdf580f5d7f77045135d0521ce7
git-subtree-dir: third_party/allwpilib_2016
git-subtree-split: 7f618166ed253a24629934fcf89c3decb0528a3b
diff --git a/wpilibc/Athena/src/Vision/AxisCamera.cpp b/wpilibc/Athena/src/Vision/AxisCamera.cpp
new file mode 100644
index 0000000..990c68a
--- /dev/null
+++ b/wpilibc/Athena/src/Vision/AxisCamera.cpp
@@ -0,0 +1,597 @@
+/*----------------------------------------------------------------------------*/
+/* 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 $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Vision/AxisCamera.h"
+
+#include "WPIErrors.h"
+
+#include <cstring>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <unistd.h>
+#include <netdb.h>
+#include <Timer.h>
+#include <iostream>
+#include <sstream>
+
+static const unsigned int kMaxPacketSize = 1536;
+static const unsigned int kImageBufferAllocationIncrement = 1000;
+
+static const std::string kWhiteBalanceStrings[] = {
+ "auto", "hold", "fixed_outdoor1", "fixed_outdoor2",
+ "fixed_indoor", "fixed_fluor1", "fixed_fluor2",
+};
+
+static const std::string kExposureControlStrings[] = {
+ "auto", "hold", "flickerfree50", "flickerfree60",
+};
+
+static const std::string kResolutionStrings[] = {
+ "640x480", "480x360", "320x240", "240x180", "176x144", "160x120",
+};
+
+static const std::string kRotationStrings[] = {
+ "0", "180",
+};
+
+/**
+ * AxisCamera constructor
+ * @param cameraHost The host to find the camera at, typically an IP address
+ */
+AxisCamera::AxisCamera(std::string const &cameraHost)
+ : m_cameraHost(cameraHost) {
+ m_captureThread = std::thread(&AxisCamera::Capture, this);
+}
+
+AxisCamera::~AxisCamera() {
+ m_done = true;
+ m_captureThread.join();
+}
+
+/*
+ * Return true if the latest image from the camera has not been retrieved by
+ * calling GetImage() yet.
+ * @return true if the image has not been retrieved yet.
+ */
+bool AxisCamera::IsFreshImage() const { return m_freshImage; }
+
+/**
+ * Get an image from the camera and store it in the provided image.
+ * @param image The imaq image to store the result in. This must be an HSL or
+ * RGB image.
+ * @return 1 upon success, zero on a failure
+ */
+int AxisCamera::GetImage(Image *image) {
+ if (m_imageData.size() == 0) {
+ return 0;
+ }
+
+ std::lock_guard<priority_mutex> lock(m_imageDataMutex);
+
+ Priv_ReadJPEGString_C(image, m_imageData.data(), m_imageData.size());
+
+ m_freshImage = false;
+
+ return 1;
+}
+
+/**
+ * Get an image from the camera and store it in the provided image.
+ * @param image The image to store the result in. This must be an HSL or RGB
+ * image
+ * @return 1 upon success, zero on a failure
+ */
+int AxisCamera::GetImage(ColorImage *image) {
+ return GetImage(image->GetImaqImage());
+}
+
+/**
+ * Instantiate a new image object and fill it with the latest image from the
+ * camera.
+ *
+ * The returned pointer is owned by the caller and is their responsibility to
+ * delete.
+ * @return a pointer to an HSLImage object
+ */
+HSLImage *AxisCamera::GetImage() {
+ auto image = new HSLImage();
+ GetImage(image);
+ return image;
+}
+
+/**
+ * Copy an image into an existing buffer.
+ * This copies an image into an existing buffer rather than creating a new image
+ * in memory. That way a new image is only allocated when the image being copied
+ * is
+ * larger than the destination.
+ * This method is called by the PCVideoServer class.
+ * @param imageData The destination image.
+ * @param numBytes The size of the destination image.
+ * @return 0 if failed (no source image or no memory), 1 if success.
+ */
+int AxisCamera::CopyJPEG(char **destImage, unsigned int &destImageSize,
+ unsigned int &destImageBufferSize) {
+ std::lock_guard<priority_mutex> lock(m_imageDataMutex);
+ if (destImage == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "destImage must not be nullptr");
+ return 0;
+ }
+
+ if (m_imageData.size() == 0) return 0; // if no source image
+
+ if (destImageBufferSize <
+ m_imageData.size()) // if current destination buffer too small
+ {
+ if (*destImage != nullptr) delete[] * destImage;
+ destImageBufferSize = m_imageData.size() + kImageBufferAllocationIncrement;
+ *destImage = new char[destImageBufferSize];
+ if (*destImage == nullptr) return 0;
+ }
+ // copy this image into destination buffer
+ if (*destImage == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "*destImage must not be nullptr");
+ }
+
+ std::copy(m_imageData.begin(), m_imageData.end(), *destImage);
+ destImageSize = m_imageData.size();
+ ;
+ return 1;
+}
+
+/**
+ * Request a change in the brightness of the camera images.
+ * @param brightness valid values 0 .. 100
+ */
+void AxisCamera::WriteBrightness(int brightness) {
+ if (brightness < 0 || brightness > 100) {
+ wpi_setWPIErrorWithContext(ParameterOutOfRange,
+ "Brightness must be from 0 to 100");
+ return;
+ }
+
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+
+ if (m_brightness != brightness) {
+ m_brightness = brightness;
+ m_parametersDirty = true;
+ }
+}
+
+/**
+ * @return The configured brightness of the camera images
+ */
+int AxisCamera::GetBrightness() {
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+ return m_brightness;
+}
+
+/**
+ * Request a change in the white balance on the camera.
+ * @param whiteBalance Valid values from the <code>WhiteBalance</code> enum.
+ */
+void AxisCamera::WriteWhiteBalance(AxisCamera::WhiteBalance whiteBalance) {
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+
+ if (m_whiteBalance != whiteBalance) {
+ m_whiteBalance = whiteBalance;
+ m_parametersDirty = true;
+ }
+}
+
+/**
+ * @return The configured white balances of the camera images
+ */
+AxisCamera::WhiteBalance AxisCamera::GetWhiteBalance() {
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+ return m_whiteBalance;
+}
+
+/**
+ * Request a change in the color level of the camera images.
+ * @param colorLevel valid values are 0 .. 100
+ */
+void AxisCamera::WriteColorLevel(int colorLevel) {
+ if (colorLevel < 0 || colorLevel > 100) {
+ wpi_setWPIErrorWithContext(ParameterOutOfRange,
+ "Color level must be from 0 to 100");
+ return;
+ }
+
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+
+ if (m_colorLevel != colorLevel) {
+ m_colorLevel = colorLevel;
+ m_parametersDirty = true;
+ }
+}
+
+/**
+ * @return The configured color level of the camera images
+ */
+int AxisCamera::GetColorLevel() {
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+ return m_colorLevel;
+}
+
+/**
+ * Request a change in the camera's exposure mode.
+ * @param exposureControl A mode to write in the <code>Exposure</code> enum.
+ */
+void AxisCamera::WriteExposureControl(
+ AxisCamera::ExposureControl exposureControl) {
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+
+ if (m_exposureControl != exposureControl) {
+ m_exposureControl = exposureControl;
+ m_parametersDirty = true;
+ }
+}
+
+/**
+ * @return The configured exposure control mode of the camera
+ */
+AxisCamera::ExposureControl AxisCamera::GetExposureControl() {
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+ return m_exposureControl;
+}
+
+/**
+ * Request a change in the exposure priority of the camera.
+ * @param exposurePriority Valid values are 0, 50, 100.
+ * 0 = Prioritize image quality
+ * 50 = None
+ * 100 = Prioritize frame rate
+ */
+void AxisCamera::WriteExposurePriority(int exposurePriority) {
+ if (exposurePriority != 0 && exposurePriority != 50 &&
+ exposurePriority != 100) {
+ wpi_setWPIErrorWithContext(ParameterOutOfRange,
+ "Exposure priority must be from 0, 50, or 100");
+ return;
+ }
+
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+
+ if (m_exposurePriority != exposurePriority) {
+ m_exposurePriority = exposurePriority;
+ m_parametersDirty = true;
+ }
+}
+
+/**
+ * @return The configured exposure priority of the camera
+ */
+int AxisCamera::GetExposurePriority() {
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+ return m_exposurePriority;
+}
+
+/**
+ * Write the maximum frames per second that the camera should send
+ * Write 0 to send as many as possible.
+ * @param maxFPS The number of frames the camera should send in a second,
+ * exposure permitting.
+ */
+void AxisCamera::WriteMaxFPS(int maxFPS) {
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+
+ if (m_maxFPS != maxFPS) {
+ m_maxFPS = maxFPS;
+ m_parametersDirty = true;
+ m_streamDirty = true;
+ }
+}
+
+/**
+ * @return The configured maximum FPS of the camera
+ */
+int AxisCamera::GetMaxFPS() {
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+ return m_maxFPS;
+}
+
+/**
+ * Write resolution value to camera.
+ * @param resolution The camera resolution value to write to the camera.
+ */
+void AxisCamera::WriteResolution(AxisCamera::Resolution resolution) {
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+
+ if (m_resolution != resolution) {
+ m_resolution = resolution;
+ m_parametersDirty = true;
+ m_streamDirty = true;
+ }
+}
+
+/**
+ * @return The configured resolution of the camera (not necessarily the same
+ * resolution as the most recent image, if it was changed recently.)
+ */
+AxisCamera::Resolution AxisCamera::GetResolution() {
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+ return m_resolution;
+}
+
+/**
+ * Write the rotation value to the camera.
+ * If you mount your camera upside down, use this to adjust the image for you.
+ * @param rotation The angle to rotate the camera
+ * (<code>AxisCamera::Rotation::k0</code>
+ * or <code>AxisCamera::Rotation::k180</code>)
+ */
+void AxisCamera::WriteRotation(AxisCamera::Rotation rotation) {
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+
+ if (m_rotation != rotation) {
+ m_rotation = rotation;
+ m_parametersDirty = true;
+ m_streamDirty = true;
+ }
+}
+
+/**
+ * @return The configured rotation mode of the camera
+ */
+AxisCamera::Rotation AxisCamera::GetRotation() {
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+ return m_rotation;
+}
+
+/**
+ * Write the compression value to the camera.
+ * @param compression Values between 0 and 100.
+ */
+void AxisCamera::WriteCompression(int compression) {
+ if (compression < 0 || compression > 100) {
+ wpi_setWPIErrorWithContext(ParameterOutOfRange,
+ "Compression must be from 0 to 100");
+ return;
+ }
+
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+
+ if (m_compression != compression) {
+ m_compression = compression;
+ m_parametersDirty = true;
+ m_streamDirty = true;
+ }
+}
+
+/**
+ * @return The configured compression level of the camera
+ */
+int AxisCamera::GetCompression() {
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+ return m_compression;
+}
+
+/**
+ * Method called in the capture thread to receive images from the camera
+ */
+void AxisCamera::Capture() {
+ int consecutiveErrors = 0;
+
+ // Loop on trying to setup the camera connection. This happens in a background
+ // thread so it shouldn't effect the operation of user programs.
+ while (!m_done) {
+ std::string requestString =
+ "GET /mjpg/video.mjpg HTTP/1.1\n"
+ "User-Agent: HTTPStreamClient\n"
+ "Connection: Keep-Alive\n"
+ "Cache-Control: no-cache\n"
+ "Authorization: Basic RlJDOkZSQw==\n\n";
+ m_captureMutex.lock();
+ m_cameraSocket = CreateCameraSocket(requestString, consecutiveErrors > 5);
+ if (m_cameraSocket != -1) {
+ ReadImagesFromCamera();
+ consecutiveErrors = 0;
+ } else {
+ consecutiveErrors++;
+ }
+ m_captureMutex.unlock();
+ Wait(0.5);
+ }
+}
+
+/**
+ * This function actually reads the images from the camera.
+ */
+void AxisCamera::ReadImagesFromCamera() {
+ char *imgBuffer = nullptr;
+ int imgBufferLength = 0;
+
+ // TODO: these recv calls must be non-blocking. Otherwise if the camera
+ // fails during a read, the code hangs and never retries when the camera comes
+ // back up.
+
+ int counter = 2;
+ while (!m_done) {
+ char initialReadBuffer[kMaxPacketSize] = "";
+ char intermediateBuffer[1];
+ char *trailingPtr = initialReadBuffer;
+ int trailingCounter = 0;
+ while (counter) {
+ // TODO: fix me... this cannot be the most efficient way to approach this,
+ // reading one byte at a time.
+ if (recv(m_cameraSocket, intermediateBuffer, 1, 0) == -1) {
+ wpi_setErrnoErrorWithContext("Failed to read image header");
+ close(m_cameraSocket);
+ return;
+ }
+ strncat(initialReadBuffer, intermediateBuffer, 1);
+ // trailingCounter ensures that we start looking for the 4 byte string
+ // after
+ // there is at least 4 bytes total. Kind of obscure.
+ // look for 2 blank lines (\r\n)
+ if (nullptr != strstr(trailingPtr, "\r\n\r\n")) {
+ --counter;
+ }
+ if (++trailingCounter >= 4) {
+ trailingPtr++;
+ }
+ }
+ counter = 1;
+ char *contentLength = strstr(initialReadBuffer, "Content-Length: ");
+ if (contentLength == nullptr) {
+ wpi_setWPIErrorWithContext(IncompatibleMode,
+ "No content-length token found in packet");
+ close(m_cameraSocket);
+ if (imgBuffer) delete[] imgBuffer;
+ return;
+ }
+ contentLength = contentLength + 16; // skip past "content length"
+ int readLength = atol(contentLength); // get the image byte count
+
+ // Make sure buffer is large enough
+ if (imgBufferLength < readLength) {
+ if (imgBuffer) delete[] imgBuffer;
+ imgBufferLength = readLength + kImageBufferAllocationIncrement;
+ imgBuffer = new char[imgBufferLength];
+ if (imgBuffer == nullptr) {
+ imgBufferLength = 0;
+ continue;
+ }
+ }
+
+ // Read the image data for "Content-Length" bytes
+ int bytesRead = 0;
+ int remaining = readLength;
+ while (bytesRead < readLength) {
+ int bytesThisRecv =
+ recv(m_cameraSocket, &imgBuffer[bytesRead], remaining, 0);
+ bytesRead += bytesThisRecv;
+ remaining -= bytesThisRecv;
+ }
+
+ // Update image
+ {
+ std::lock_guard<priority_mutex> lock(m_imageDataMutex);
+
+ m_imageData.assign(imgBuffer, imgBuffer + imgBufferLength);
+ m_freshImage = true;
+ }
+
+ if (WriteParameters()) {
+ break;
+ }
+ }
+
+ close(m_cameraSocket);
+}
+
+/**
+ * Send a request to the camera to set all of the parameters. This is called
+ * in the capture thread between each frame. This strategy avoids making lots
+ * of redundant HTTP requests, accounts for failed initial requests, and
+ * avoids blocking calls in the main thread unless necessary.
+ *
+ * This method does nothing if no parameters have been modified since it last
+ * completely successfully.
+ *
+ * @return <code>true</code> if the stream should be restarted due to a
+ * parameter changing.
+ */
+bool AxisCamera::WriteParameters() {
+ if (m_parametersDirty) {
+ std::stringstream request;
+ request << "GET /axis-cgi/admin/param.cgi?action=update";
+
+ m_parametersMutex.lock();
+ request << "&ImageSource.I0.Sensor.Brightness=" << m_brightness;
+ request << "&ImageSource.I0.Sensor.WhiteBalance="
+ << kWhiteBalanceStrings[m_whiteBalance];
+ request << "&ImageSource.I0.Sensor.ColorLevel=" << m_colorLevel;
+ request << "&ImageSource.I0.Sensor.Exposure="
+ << kExposureControlStrings[m_exposureControl];
+ request << "&ImageSource.I0.Sensor.ExposurePriority=" << m_exposurePriority;
+ request << "&Image.I0.Stream.FPS=" << m_maxFPS;
+ request << "&Image.I0.Appearance.Resolution="
+ << kResolutionStrings[m_resolution];
+ request << "&Image.I0.Appearance.Compression=" << m_compression;
+ request << "&Image.I0.Appearance.Rotation=" << kRotationStrings[m_rotation];
+ m_parametersMutex.unlock();
+
+ request << " HTTP/1.1" << std::endl;
+ request << "User-Agent: HTTPStreamClient" << std::endl;
+ request << "Connection: Keep-Alive" << std::endl;
+ request << "Cache-Control: no-cache" << std::endl;
+ request << "Authorization: Basic RlJDOkZSQw==" << std::endl;
+ request << std::endl;
+
+ int socket = CreateCameraSocket(request.str(), false);
+ if (socket == -1) {
+ wpi_setErrnoErrorWithContext("Error setting camera parameters");
+ } else {
+ close(socket);
+ m_parametersDirty = false;
+
+ if (m_streamDirty) {
+ m_streamDirty = false;
+ return true;
+ }
+ }
+ }
+
+ return false;
+}
+
+/**
+ * Create a socket connected to camera
+ * Used to create a connection to the camera for both capturing images and
+ * setting parameters.
+ * @param requestString The initial request string to send upon successful
+ * connection.
+ * @param setError If true, rais an error if there's a problem creating the
+ * connection.
+ * This is only enabled after several unsucessful connections, so a single one
+ * doesn't
+ * cause an error message to be printed if it immediately recovers.
+ * @return -1 if failed, socket handle if successful.
+ */
+int AxisCamera::CreateCameraSocket(std::string const &requestString,
+ bool setError) {
+ struct addrinfo *address = nullptr;
+ int camSocket;
+
+ /* create socket */
+ if ((camSocket = socket(AF_INET, SOCK_STREAM, 0)) == -1) {
+ if (setError)
+ wpi_setErrnoErrorWithContext("Failed to create the camera socket");
+ return -1;
+ }
+
+ if (getaddrinfo(m_cameraHost.c_str(), "80", nullptr, &address) == -1) {
+ if (setError) {
+ wpi_setErrnoErrorWithContext("Failed to create the camera socket");
+ close(camSocket);
+ }
+ return -1;
+ }
+
+ /* connect to server */
+ if (connect(camSocket, address->ai_addr, address->ai_addrlen) == -1) {
+ if (setError)
+ wpi_setErrnoErrorWithContext("Failed to connect to the camera");
+ freeaddrinfo(address);
+ close(camSocket);
+ return -1;
+ }
+
+ freeaddrinfo(address);
+
+ int sent = send(camSocket, requestString.c_str(), requestString.size(), 0);
+ if (sent == -1) {
+ if (setError)
+ wpi_setErrnoErrorWithContext("Failed to send a request to the camera");
+ close(camSocket);
+ return -1;
+ }
+
+ return camSocket;
+}