Merge "Add AxisCamera C++ class"

wpilibc/wpilibC++Devices/include/Vision/AxisCamera.h

@@ -0,0 +1,127 @@
+/*----------------------------------------------------------------------------*/
+/* 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.  */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <thread>
+#include <string>
+#include <mutex>
+
+#include "ErrorBase.h"
+#include "Vision/ColorImage.h"
+#include "Vision/HSLImage.h"
+#include "nivision.h"
+
+/**
+ * Axis M1011 network camera
+ */
+class AxisCamera: public ErrorBase
+{
+public:
+	enum WhiteBalance
+	{
+		kWhiteBalance_Automatic,
+		kWhiteBalance_Hold,
+		kWhiteBalance_FixedOutdoor1,
+		kWhiteBalance_FixedOutdoor2,
+		kWhiteBalance_FixedIndoor,
+		kWhiteBalance_FixedFluorescent1,
+		kWhiteBalance_FixedFluorescent2
+	};
+
+	enum ExposureControl
+	{
+		kExposureControl_Automatic,
+		kExposureControl_Hold,
+		kExposureControl_FlickerFree50Hz,
+		kExposureControl_FlickerFree60Hz
+	};
+
+	enum Resolution
+	{
+		kResolution_640x480,
+		kResolution_480x360,
+		kResolution_320x240,
+		kResolution_240x180,
+		kResolution_176x144,
+		kResolution_160x120,
+	};
+
+	enum Rotation
+	{
+		kRotation_0, kRotation_180
+	};
+
+	explicit AxisCamera(std::string const& cameraHost);
+	virtual ~AxisCamera();
+
+	bool IsFreshImage() const;
+
+	int GetImage(Image *image);
+	int GetImage(ColorImage *image);
+	HSLImage *GetImage();
+	int CopyJPEG(char **destImage, unsigned int &destImageSize, unsigned int &destImageBufferSize);
+
+	void WriteBrightness(int brightness);
+	int GetBrightness();
+
+	void WriteWhiteBalance(WhiteBalance whiteBalance);
+	WhiteBalance GetWhiteBalance();
+
+	void WriteColorLevel(int colorLevel);
+	int GetColorLevel();
+
+	void WriteExposureControl(ExposureControl exposureControl);
+	ExposureControl GetExposureControl();
+
+	void WriteExposurePriority(int exposurePriority);
+	int GetExposurePriority();
+
+	void WriteMaxFPS(int maxFPS);
+	int GetMaxFPS();
+
+	void WriteResolution(Resolution resolution);
+	Resolution GetResolution();
+
+	void WriteCompression(int compression);
+	int GetCompression();
+
+	void WriteRotation(Rotation rotation);
+	Rotation GetRotation();
+
+private:
+	std::thread m_captureThread;
+	std::string m_cameraHost;
+	int m_cameraSocket;
+	std::mutex m_captureMutex;
+
+	std::mutex m_imageDataMutex;
+	std::vector<uint8_t> m_imageData;
+	bool m_freshImage;
+
+	int m_brightness;
+	WhiteBalance m_whiteBalance;
+	int m_colorLevel;
+	ExposureControl m_exposureControl;
+	int m_exposurePriority;
+	int m_maxFPS;
+	Resolution m_resolution;
+	int m_compression;
+	Rotation m_rotation;
+	bool m_parametersDirty;
+	bool m_streamDirty;
+	std::mutex m_parametersMutex;
+
+	bool m_done;
+
+	void Capture();
+	void ReadImagesFromCamera();
+	bool WriteParameters();
+
+	int CreateCameraSocket(std::string const& requestString, bool setError);
+
+	DISALLOW_COPY_AND_ASSIGN(AxisCamera);
+};

wpilibc/wpilibC++Devices/include/WPILib.h

@@ -85,6 +85,7 @@
 #include "Utility.h"
 #include "Victor.h"
 #include "VictorSP.h"
+#include "Vision/AxisCamera.h"
 #include "Vision/BinaryImage.h"
 #include "Vision/ColorImage.h"
 #include "Vision/HSLImage.h"

wpilibc/wpilibC++Devices/src/Vision/AxisCamera.cpp

@@ -0,0 +1,639 @@
+/*----------------------------------------------------------------------------*/
+/* 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>
+
+/** Private NI function to decode JPEG */
+IMAQ_FUNC int Priv_ReadJPEGString_C(Image* _image, const unsigned char* _string,
+		uint32_t _stringLength);
+
+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_cameraSocket(-1)
+		, m_freshImage(false)
+		, m_brightness(50)
+		, m_whiteBalance(kWhiteBalance_Automatic)
+		, m_colorLevel(50)
+		, m_exposureControl(kExposureControl_Automatic)
+		, m_exposurePriority(50)
+		, m_maxFPS(0)
+		, m_resolution(kResolution_640x480)
+		, m_compression(50), m_rotation(kRotation_0)
+		, m_parametersDirty(true)
+		, m_streamDirty(true)
+		, m_done(false)
+{
+	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<std::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()
+{
+	HSLImage *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<std::mutex> lock(m_imageDataMutex);
+	if (destImage == NULL)
+		wpi_setWPIErrorWithContext(NullParameter, "destImage must not be NULL");
+
+	if (m_imageData.size() == 0)
+		return 0; // if no source image
+
+	if (destImageBufferSize < m_imageData.size()) // if current destination buffer too small
+	{
+		if (*destImage != NULL) delete [] *destImage;
+		destImageBufferSize = m_imageData.size() + kImageBufferAllocationIncrement;
+		*destImage = new char[destImageBufferSize];
+		if (*destImage == NULL) return 0;
+	}
+	// copy this image into destination buffer
+	if (*destImage == NULL)
+	{
+		wpi_setWPIErrorWithContext(NullParameter, "*destImage must not be NULL");
+	}
+	
+	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<std::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<std::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<std::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<std::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<std::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<std::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<std::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<std::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<std::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<std::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<std::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<std::mutex> lock(m_parametersMutex);
+	return m_maxFPS;
+}
+
+/**
+ * Write resolution value to camera.
+ * @param resolution The camera resolution value to write to the camera.  Use the Resolution_t enum.
+ */
+void AxisCamera::WriteResolution(AxisCamera::Resolution resolution)
+{
+	std::lock_guard<std::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<std::mutex> lock(m_parametersMutex);
+	return m_resolution;
+}
+
+/**
+ * @return The configured rotation mode of the camera
+ */
+AxisCamera::Rotation AxisCamera::GetRotation()
+{
+	std::lock_guard<std::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<std::mutex> lock(m_parametersMutex);
+
+	if (m_compression != compression)
+	{
+		m_compression = compression;
+		m_parametersDirty = true;
+		m_streamDirty = true;
+	}
+}
+
+/**
+ * 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 image from the Rotation_t enum in AxisCameraParams (kRotation_0 or kRotation_180)
+ */
+void AxisCamera::WriteRotation(AxisCamera::Rotation rotation)
+{
+	std::lock_guard<std::mutex> lock(m_parametersMutex);
+
+	if (m_rotation != rotation)
+	{
+		m_rotation = rotation;
+		m_parametersDirty = true;
+		m_streamDirty = true;
+	}
+}
+
+/**
+ * Thread spawned by AxisCamera constructor to receive images from cam
+ * If setNewImageSem has been called, this function does a semGive on each new image
+ * Images can be accessed by calling getImage()
+ */
+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 = NULL;
+	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 (NULL != strstr(trailingPtr, "\r\n\r\n"))
+			{
+				--counter;
+			}
+			if (++trailingCounter >= 4)
+			{
+				trailingPtr++;
+			}
+		}
+		counter = 1;
+		char *contentLength = strstr(initialReadBuffer, "Content-Length: ");
+		if (contentLength == NULL)
+		{
+			wpi_setWPIErrorWithContext(IncompatibleMode,
+					"No content-length token found in packet");
+			close(m_cameraSocket);
+			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 == NULL)
+			{
+				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<std::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 by both AxisCameraParams and AxisCamera.
+ * @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 = 0;
+	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", 0, &address) == -1)
+	{
+		if (setError) wpi_setErrnoErrorWithContext("Failed to create the camera socket");
+		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");
+		close(camSocket);
+		return -1;
+	}
+
+	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;
+}