AxisCameraParams.cpp

/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. 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/AxisCameraParams.h"

#include "Vision/AxisCamera.h"
#include <inetLib.h>
#include "pcre.h"
#include <sockLib.h>
#include <string.h>
#include "Synchronized.h"
#include "Timer.h"
#include "Utility.h"
#include "WPIErrors.h"

#if JAVA_CAMERA_LIB != 1
#include "DriverStation.h"
#endif

static const char *const kRotationChoices[] = {"0", "180"};
static const char *const kResolutionChoices[] = {"640x480", "640x360", "320x240", "160x120"};
static const char *const kExposureControlChoices[] = { "automatic", "hold", "flickerfree50", "flickerfree60" };
static const char *const kWhiteBalanceChoices[] = { "auto", "holdwb", "fixed_outdoor1",
                "fixed_outdoor2", "fixed_indoor", "fixed_fluor1", "fixed_fluor2" };

AxisCameraParams::AxisCameraParams(const char* ipAddress)
        : m_paramTask("paramTask", (FUNCPTR) s_ParamTaskFunction)
        , m_paramChangedSem (NULL)
        , m_socketPossessionSem (NULL)
        , m_brightnessParam (NULL)
        , m_compressionParam (NULL)
        , m_exposurePriorityParam (NULL)
        , m_colorLevelParam (NULL)
        , m_maxFPSParam (NULL)
        , m_rotationParam (NULL)
        , m_resolutionParam (NULL)
        , m_exposureControlParam (NULL)
        , m_whiteBalanceParam (NULL)
{
        if (ipAddress == NULL || strlen(ipAddress) == 0)
        {
#if JAVA_CAMERA_LIB == 1
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "IP Address must be specified");
                return;
#else
                DriverStation *ds = DriverStation::GetInstance();
                ds->WaitForData();
                UINT16 teamNumber = ds->GetTeamNumber();
                char cameraIP[16];
                snprintf(cameraIP, 16, "10.%d.%d.11", teamNumber / 100, teamNumber % 100);
                m_ipAddress = inet_addr(cameraIP);
#endif
        }
        else
        {
                m_ipAddress = inet_addr((char*)ipAddress);
        }

        if (m_ipAddress == (u_long)ERROR)
        {
                wpi_setErrnoError();
                return;
        }

        m_brightnessParam = new IntCameraParameter("ImageSource.I0.Sensor.Brightness=%i",
                        "root.ImageSource.I0.Sensor.Brightness=(.*)", false);
        m_parameters.push_back(m_brightnessParam);
        m_colorLevelParam = new IntCameraParameter("ImageSource.I0.Sensor.ColorLevel=%i",
                        "root.ImageSource.I0.Sensor.ColorLevel=(.*)", false);
        m_parameters.push_back(m_colorLevelParam);
        m_exposurePriorityParam = new IntCameraParameter("ImageSource.I0.Sensor.exposurePriority=%i",
                        "root.ImageSource.I0.Sensor.ExposurePriority=(.*)", false);
        m_parameters.push_back(m_exposurePriorityParam);
        m_compressionParam = new IntCameraParameter("Image.I0.Appearance.Compression=%i",
                        "root.Image.I0.Appearance.Compression=(.*)", true);
        m_parameters.push_back(m_compressionParam);
        m_maxFPSParam = new IntCameraParameter("Image.I0.Stream.FPS=%i",
                        "root.Image.I0.Stream.FPS=(.*)", false);
        m_parameters.push_back(m_maxFPSParam);
        m_rotationParam = new EnumCameraParameter("Image.I0.Appearance.Rotation=%s",
                        "root.Image.I0.Appearance.Rotation=(.*)", true, kRotationChoices, sizeof(kRotationChoices)/sizeof(kRotationChoices[0]));
        m_parameters.push_back(m_rotationParam);
        m_resolutionParam = new EnumCameraParameter("Image.I0.Appearance.Resolution=%s",
                        "root.Image.I0.Appearance.Resolution=(.*)", true, kResolutionChoices, sizeof(kResolutionChoices)/sizeof(kResolutionChoices[0]));
        m_parameters.push_back(m_resolutionParam);
        m_exposureControlParam = new EnumCameraParameter("ImageSource.I0.Sensor.Exposure=%s",
                        "root.ImageSource.I0.Sensor.Exposure=(.*)", false, kExposureControlChoices, sizeof(kExposureControlChoices)/sizeof(kExposureControlChoices[0]));
        m_parameters.push_back(m_exposureControlParam);
        m_whiteBalanceParam = new EnumCameraParameter("ImageSource.IO.Sensor.WhiteBalance=%s",
                        "root.ImageSource.I0.Sensor.WhiteBalance=(.*)", false, kWhiteBalanceChoices, sizeof(kWhiteBalanceChoices)/sizeof(kWhiteBalanceChoices[0]));
        m_parameters.push_back(m_whiteBalanceParam);

        m_paramChangedSem = semBCreate (SEM_Q_PRIORITY, SEM_EMPTY);
        m_socketPossessionSem = semBCreate (SEM_Q_PRIORITY, SEM_FULL);

        m_paramTask.Start((int)this);
}

AxisCameraParams::~AxisCameraParams()
{
        m_paramTask.Stop();

        semDelete(m_socketPossessionSem);
        semDelete(m_paramChangedSem);

        delete m_whiteBalanceParam;
        delete m_exposureControlParam;
        delete m_resolutionParam;
        delete m_rotationParam;
        delete m_maxFPSParam;
        delete m_compressionParam;
        delete m_exposurePriorityParam;
        delete m_colorLevelParam;
        delete m_brightnessParam;
}

int AxisCameraParams::s_ParamTaskFunction(AxisCameraParams* thisPtr)
{
        return thisPtr->ParamTaskFunction();
}

// TODO: need to synchronize the actual setting of parameters (the assignment statement)
int AxisCameraParams::ParamTaskFunction()
{
        static bool firstTime = true;

        while (true)
        {
                semTake(m_socketPossessionSem, WAIT_FOREVER);
                if (firstTime)
                {
                        while (ReadCamParams() == 0);
                        firstTime = false;
                }
                bool restartRequired = false;

                ParameterVector_t::iterator it = m_parameters.begin();
                ParameterVector_t::iterator end = m_parameters.end();
                for(; it != end; it++)
                {
                        bool changed = false;
                        char param[150];
                        restartRequired |= (*it)->CheckChanged(changed, param);
                        if (changed)
                        {
                                UpdateCamParam(param);
                        }
                }
                if (restartRequired)
                {
                        RestartCameraTask();
                }
                semGive(m_socketPossessionSem);
        }
        return 0;
}

void AxisCameraParams::WriteBrightness(int brightness)
{
        m_brightnessParam->SetValue(brightness);
        semGive(m_paramChangedSem);
}

int AxisCameraParams::GetBrightness()
{
        return m_brightnessParam->GetValue();
}

void AxisCameraParams::WriteWhiteBalance(WhiteBalance_t whiteBalance)
{
        m_whiteBalanceParam->SetValue(whiteBalance);
        semGive(m_paramChangedSem);
}

AxisCameraParams::WhiteBalance_t AxisCameraParams::GetWhiteBalance()
{
        return (WhiteBalance_t) m_whiteBalanceParam->GetValue();
}

void AxisCameraParams::WriteColorLevel(int colorLevel)
{
        m_colorLevelParam->SetValue(colorLevel);
        semGive(m_paramChangedSem);
}

int AxisCameraParams::GetColorLevel()
{
        return m_colorLevelParam->GetValue();
}

void AxisCameraParams::WriteExposureControl(Exposure_t exposureControl)
{
        m_exposureControlParam->SetValue(exposureControl);
        semGive(m_paramChangedSem);
}

AxisCameraParams::Exposure_t AxisCameraParams::GetExposureControl()
{
        return (Exposure_t) m_exposureControlParam->GetValue();
}

void AxisCameraParams::WriteResolution(Resolution_t resolution)
{
        m_resolutionParam->SetValue(resolution);
        semGive(m_paramChangedSem);
}

AxisCameraParams::Resolution_t AxisCameraParams::GetResolution()
{
        return (Resolution_t) m_resolutionParam->GetValue();
}

void AxisCameraParams::WriteExposurePriority(int exposurePriority)
{
        m_exposurePriorityParam->SetValue(exposurePriority);
        semGive(m_paramChangedSem);
}

int AxisCameraParams::GetExposurePriority()
{
        return m_exposurePriorityParam->GetValue();
}

void AxisCameraParams::WriteRotation(Rotation_t rotation)
{
        m_rotationParam->SetValue(rotation);
        semGive(m_paramChangedSem);
}

AxisCameraParams::Rotation_t AxisCameraParams::GetRotation()
{
        return (Rotation_t) m_rotationParam->GetValue();
}

void AxisCameraParams::WriteCompression(int compression)
{
        m_compressionParam->SetValue(compression);
        semGive(m_paramChangedSem);
}

int AxisCameraParams::GetCompression()
{
        return m_compressionParam->GetValue();
}

void AxisCameraParams::WriteMaxFPS(int maxFPS)
{
        m_maxFPSParam->SetValue(maxFPS);
        semGive(m_paramChangedSem);
}

int AxisCameraParams::GetMaxFPS()
{
        return m_maxFPSParam->GetValue();
}

int AxisCameraParams::UpdateCamParam(const char* param)
{
        char *requestString =
                "GET /axis-cgi/admin/param.cgi?action=update&%s HTTP/1.1\n\
User-Agent: HTTPStreamClient\n\
Connection: Keep-Alive\n\
Cache-Control: no-cache\n\
Authorization: Basic RlJDOkZSQw==\n\n";
        char completedRequest[1024];
        sprintf(completedRequest, requestString, param);
        // Send request
        int camSocket = CreateCameraSocket(completedRequest);
        if (camSocket == ERROR)
        {
                printf("UpdateCamParam failed: %s\n", param);
                return 0;
        }
        close(camSocket);
        return 1;
}

int AxisCameraParams::ReadCamParams()
{
        char * requestString =
                "GET /axis-cgi/admin/param.cgi?action=list HTTP/1.1\n\
User-Agent: HTTPStreamClient\n\
Connection: Keep-Alive\n\
Cache-Control: no-cache\n\
Authorization: Basic RlJDOkZSQw==\n\n";

        int camSocket = CreateCameraSocket(requestString);
        if (camSocket == ERROR)
        {
                return 0;
        }
        // Allocate on the heap since it is very large and only needed once
        char *readBuffer = new char[27000];
        int totalRead = 0;
        while (1)
        {
                wpi_assert(totalRead < 26000);
                int bytesRead = recv(camSocket, &readBuffer[totalRead], 1000, 0);
                if (bytesRead == ERROR)
                {
                        wpi_setErrnoErrorWithContext("Failed to read image header");
                        close(camSocket);
                        return 0;
                }
                else if (bytesRead <= 0)
                {
                        break;
                }
                totalRead += bytesRead;
        }
        readBuffer[totalRead] = '\0';

        ParameterVector_t::iterator it = m_parameters.begin();
        ParameterVector_t::iterator end = m_parameters.end();
        for(; it != end; it++)
        {
                (*it)->GetParamFromString(readBuffer, totalRead);
        }
        close(camSocket);
        delete [] readBuffer;
        return 1;
}

/*
 * 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.
 * @return ERROR if failed, socket handle if successful.
 */
int AxisCameraParams::CreateCameraSocket(const char *requestString)
{
        int sockAddrSize;
        struct sockaddr_in serverAddr;
        int camSocket;
        /* create socket */
        if ((camSocket = socket(AF_INET, SOCK_STREAM, 0)) == ERROR)
        {
                wpi_setErrnoErrorWithContext("Failed to create the camera socket");
                return ERROR;
        }

        sockAddrSize = sizeof(struct sockaddr_in);
        bzero((char *) &serverAddr, sockAddrSize);
        serverAddr.sin_family = AF_INET;
        serverAddr.sin_len = (u_char) sockAddrSize;
        serverAddr.sin_port = htons(80);

        serverAddr.sin_addr.s_addr = m_ipAddress;

        /* connect to server */
        struct timeval connectTimeout;
        connectTimeout.tv_sec = 5;
        connectTimeout.tv_usec = 0;
        if (connectWithTimeout(camSocket, (struct sockaddr *) &serverAddr, sockAddrSize, &connectTimeout) == ERROR)
        {
                wpi_setErrnoErrorWithContext("Failed to connect to the camera");
                close(camSocket);
                return ERROR;
        }
        int sent = send(camSocket, requestString, strlen(requestString), 0);
        if (sent == ERROR)
        {
                wpi_setErrnoErrorWithContext("Failed to send a request to the camera");
                close(camSocket);
                return ERROR;
        }
        return camSocket;
}