AxisCamera.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/AxisCamera.h"

#include <string.h>
#include "Synchronized.h"
#include "Vision/PCVideoServer.h"
#include "WPIErrors.h"

IMAQ_FUNC int Priv_ReadJPEGString_C(Image* _image, const unsigned char* _string, UINT32 _stringLength);

// Max packet without jumbo frames is 1500... add 36 because??
#define kMaxPacketSize 1536
#define kImageBufferAllocationIncrement 1000

AxisCamera *AxisCamera::_instance = NULL;

AxisCamera::AxisCamera(const char *ipAddress)
        : AxisCameraParams(ipAddress)
        , m_cameraSocket(ERROR)
        , m_protectedImageBuffer(NULL)
        , m_protectedImageBufferLength(0)
        , m_protectedImageSize(0)
        , m_protectedImageSem(NULL)
        , m_freshImage(false)
        , m_imageStreamTask("cameraTask", (FUNCPTR)s_ImageStreamTaskFunction)
        , m_videoServer(NULL)
{
        m_protectedImageSem = semMCreate(SEM_Q_PRIORITY | SEM_INVERSION_SAFE | SEM_DELETE_SAFE);

        if (!StatusIsFatal())
                m_imageStreamTask.Start((int)this); 
}

AxisCamera::~AxisCamera()
{
        delete m_videoServer;
        m_videoServer = NULL;

        m_imageStreamTask.Stop();
        close(m_cameraSocket);

        SemSet_t::iterator it = m_newImageSemSet.begin();
        SemSet_t::iterator end = m_newImageSemSet.end();
        for (;it != end; it++)
        {
                semDelete(*it);
        }
        m_newImageSemSet.clear();

        semDelete(m_protectedImageSem);
}

AxisCamera &AxisCamera::GetInstance(const char *cameraIP)
{
        if (NULL == _instance)
        {
                _instance = new AxisCamera(cameraIP);

                _instance->m_videoServer = new PCVideoServer();
        }

        return *_instance;
}

void AxisCamera::DeleteInstance()
{
        delete _instance;
        _instance = NULL;
}

bool AxisCamera::IsFreshImage()
{
        return m_freshImage;
}

SEM_ID AxisCamera::GetNewImageSem()
{
        SEM_ID sem = semBCreate (SEM_Q_PRIORITY, SEM_EMPTY);
        m_newImageSemSet.insert(sem);
        return sem;
}

int AxisCamera::GetImage(Image* imaqImage)
{
        if (m_protectedImageBuffer == NULL)
                return 0;
        Synchronized sync(m_protectedImageSem);
        Priv_ReadJPEGString_C(imaqImage,
                (unsigned char*)m_protectedImageBuffer, m_protectedImageSize);
        m_freshImage = false;
        return 1;
}

#if JAVA_CAMERA_LIB != 1

int AxisCamera::GetImage(ColorImage* image)
{
        return GetImage(image->GetImaqImage());
}

HSLImage* AxisCamera::GetImage()
{
        HSLImage *image = new HSLImage();
        GetImage(image);
        return image;
}
#endif

int AxisCamera::CopyJPEG(char **destImage, int &destImageSize, int &destImageBufferSize)
{
        Synchronized sync(m_protectedImageSem);
        if (destImage == NULL)
                wpi_setWPIErrorWithContext(NullParameter, "destImage must not be NULL");

        if (m_protectedImageBuffer == NULL || m_protectedImageSize <= 0)
                return 0; // if no source image

        if (destImageBufferSize < m_protectedImageSize) // if current destination buffer too small
        {
                if (*destImage != NULL) delete [] *destImage;
                destImageBufferSize = m_protectedImageSize + 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");
        }
        // TODO: Is this copy realy necessary... perhaps we can simply transmit while holding the protected buffer
        memcpy(*destImage, m_protectedImageBuffer, m_protectedImageSize);
        destImageSize = m_protectedImageSize;
        return 1;
}

int AxisCamera::s_ImageStreamTaskFunction(AxisCamera *thisPtr)
{
        return thisPtr->ImageStreamTaskFunction();
}

int AxisCamera::ImageStreamTaskFunction()
{
        // 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 (1)
        {
                const char *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";
                semTake(m_socketPossessionSem, WAIT_FOREVER);
                m_cameraSocket = CreateCameraSocket(requestString);
                if (m_cameraSocket == ERROR)
                {
                        // Don't hammer the camera if it isn't ready.
                        semGive(m_socketPossessionSem);
                        taskDelay(1000);
                }
                else
                {
                        ReadImagesFromCamera();
                }
        }
}

int AxisCamera::ReadImagesFromCamera()
{
        char *imgBuffer = NULL;
        int imgBufferLength = 0;
        //Infinite loop, task deletion handled by taskDeleteHook
        // Socket cleanup handled by destructor

        // 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 (1)
        {
                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) == ERROR)
                        {
                                wpi_setErrnoErrorWithContext("Failed to read image header");
                                close (m_cameraSocket);
                                return ERROR;
                        }
                        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 ERROR;
                }
                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
                UpdatePublicImageFromCamera(imgBuffer, readLength);
                if (semTake(m_paramChangedSem, NO_WAIT) == OK)
                {
                        // params need to be updated: close the video stream; release the camera.
                        close(m_cameraSocket);
                        semGive(m_socketPossessionSem);
                        return 0;
                }
        }
}

void AxisCamera::UpdatePublicImageFromCamera(char *imgBuffer, int imgSize)
{
        {
                Synchronized sync(m_protectedImageSem);

                // Adjust the buffer size if current destination buffer is too small.
                if (m_protectedImageBufferLength < imgSize)
                {
                        if (m_protectedImageBuffer != NULL) delete [] m_protectedImageBuffer;
                        m_protectedImageBufferLength = imgSize + kImageBufferAllocationIncrement;
                        m_protectedImageBuffer = new char[m_protectedImageBufferLength];
                        if (m_protectedImageBuffer == NULL)
                        {
                                m_protectedImageBufferLength = 0;
                                return;
                        }
                }

                memcpy(m_protectedImageBuffer, imgBuffer, imgSize);
                m_protectedImageSize = imgSize;
        }

        m_freshImage = true;
        // Notify everyone who is interested.
        SemSet_t::iterator it = m_newImageSemSet.begin();
        SemSet_t::iterator end = m_newImageSemSet.end();
        for (;it != end; it++)
        {
                semGive(*it);
        }
}

void AxisCamera::RestartCameraTask()
{
        m_imageStreamTask.Stop();
        m_imageStreamTask.Start((int)this);
}

#if JAVA_CAMERA_LIB == 1

// C bindings used by Java
// These need to stay as is or Java has to change

void AxisCameraStart(const char *IPAddress)
{
#ifdef SVN_REV
        if (strlen(SVN_REV))
        {
                printf("JavaCameraLib was compiled from SVN revision %s\n", SVN_REV);
        }
        else
        {
                printf("JavaCameraLib was compiled from a location that is not source controlled.\n");
        }
#else
        printf("JavaCameraLib was compiled without -D'SVN_REV=nnnn'\n");
#endif
        AxisCamera::GetInstance(IPAddress);
}

int AxisCameraGetImage (Image* image)
{
        return AxisCamera::GetInstance().GetImage(image);
}

void AxisCameraWriteBrightness(int brightness)
{
        AxisCamera::GetInstance().WriteBrightness(brightness);
}

int AxisCameraGetBrightness()
{
        return AxisCamera::GetInstance().GetBrightness();
}

void AxisCameraWriteWhiteBalance(AxisCameraParams::WhiteBalance_t whiteBalance)
{
        AxisCamera::GetInstance().WriteWhiteBalance(whiteBalance);
}

AxisCameraParams::WhiteBalance_t AxisCameraGetWhiteBalance()
{
        return AxisCamera::GetInstance().GetWhiteBalance();
}

void AxisCameraWriteColorLevel(int colorLevel)
{
        AxisCamera::GetInstance().WriteColorLevel(colorLevel);
}

int AxisCameraGetColorLevel()
{
        return AxisCamera::GetInstance().GetColorLevel();
}

void AxisCameraWriteExposureControl(AxisCameraParams::Exposure_t exposure)
{
        AxisCamera::GetInstance().WriteExposureControl(exposure);
}

AxisCameraParams::Exposure_t AxisCameraGetExposureControl()
{
        return AxisCamera::GetInstance().GetExposureControl();
}

void AxisCameraWriteExposurePriority(int exposure)
{
        AxisCamera::GetInstance().WriteExposurePriority(exposure);
}

int AxisCameraGetExposurePriority()
{
        return AxisCamera::GetInstance().GetExposurePriority();
}

void AxisCameraWriteMaxFPS(int maxFPS)
{
        AxisCamera::GetInstance().WriteMaxFPS(maxFPS);
}

int AxisCameraGetMaxFPS()
{
        return AxisCamera::GetInstance().GetMaxFPS();
}

void AxisCameraWriteResolution(AxisCameraParams::Resolution_t resolution)
{
        AxisCamera::GetInstance().WriteResolution(resolution);
}

AxisCameraParams::Resolution_t AxisCameraGetResolution()
{
        return AxisCamera::GetInstance().GetResolution();
}

void AxisCameraWriteCompression(int compression)
{
        AxisCamera::GetInstance().WriteCompression(compression);
}

int AxisCameraGetCompression()
{
        return AxisCamera::GetInstance().GetCompression();
}

void AxisCameraWriteRotation(AxisCameraParams::Rotation_t rotation)
{
        AxisCamera::GetInstance().WriteRotation(rotation);
}

AxisCameraParams::Rotation_t AxisCameraGetRotation()
{
        return AxisCamera::GetInstance().GetRotation();
}

void AxisCameraDeleteInstance()
{
        AxisCamera::DeleteInstance();
}

int AxisCameraFreshImage()
{
        return AxisCamera::GetInstance().IsFreshImage();
}

#endif // JAVA_CAMERA_LIB == 1