Add support for vision in C++

Add IMAQdx and its dependencies

Change-Id: I6befa563e96db224db83fb90985c86eb3e8d4f3e

Add a "CameraServer" class for C++

This class allows the driver station's camera viewer to interact with
a C++ program.  It includes both an automatic mode to send images from
a webcam to the dashboard in a background thread, and an option to
manually feed it IMAQ images.

Change-Id: I54fdb164c00dce165859c22f435be647dc9927cc

wpilibc/wpilibC++Devices/src/CameraServer.cpp

@@ -0,0 +1,250 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <cstring>
+
+#include <sys/socket.h>
+#include <unistd.h>
+#include <netdb.h>
+
+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() :
+		m_serverThread(&CameraServer::serve, this),
+		m_quality(50),
+		m_autoCaptureStarted(false) {
+}
+
+/**
+ * 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();
+}
+
+/**
+ * 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();
+}
+
+/**
+ * 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;
+}
+
+/**
+ * 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;
+}
+
+/**
+ * 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);
+
+	if(sock == -1) {
+		wpi_setErrnoError();
+	}
+
+	int reuseAddr = 1;
+	if(setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &reuseAddr, sizeof(reuseAddr)) == -1) {
+		wpi_setErrnoError();
+	}
+
+	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);
+
+	if(bind(sock, (struct sockaddr *)&address, sizeof(address)) == -1) {
+		wpi_setErrnoError();
+	}
+
+	if(listen(sock, 10) == -1) {
+		wpi_setErrnoError();
+	}
+
+	for(;;) {
+		socklen_t clientaddresslen = sizeof(clientAddress);
+		int conn = accept(sock, (struct sockaddr *)&clientAddress, &clientaddresslen);
+
+		/* 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);
+		}
+
+		/* 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;
+		}
+
+		/* The period that frames are sent is 1/FPS */
+		auto period = std::chrono::microseconds(1000000) / req.fps;
+
+		for(;;) {
+			auto startTime = std::chrono::steady_clock::now();
+
+			{
+				std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
+
+				m_newImageReady.wait(lock);
+
+				/* Send the magic number that indicates the beginning of a new image */
+				if(write(conn, kMagicNumber, sizeof kMagicNumber) == -1) {
+					break;
+				}
+
+				/* 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);
+}