PCVideoServer.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 <vxWorks.h>

#include "PCVideoServer.h"

#include <errnoLib.h>
#include <hostLib.h>
#include <inetLib.h>
#include <sockLib.h>

#include "Vision/AxisCamera.h"
#include "Task.h"
#include "Timer.h"
#include "WPIErrors.h"

class ScopedSocket {
public:
        ScopedSocket(int camSock)
                : m_camSock(camSock)
        {
        }

        ~ScopedSocket() {
                if (m_camSock != ERROR) {
                        close(m_camSock);
                }
        }
        // Cast to int allows you to pass this to any function that
        // takes the socket as an int.
        operator int() const {
                return m_camSock;
        }

private:
        int m_camSock;
};

//============================================================================
// PCVideoServer
//============================================================================

PCVideoServer::PCVideoServer()
        : m_serverTask("PCVideoServer", (FUNCPTR)s_ServerTask)
        , m_newImageSem (NULL)
        , m_stopServer (false)
{
        AxisCamera &cam = AxisCamera::GetInstance();
        m_newImageSem = cam.GetNewImageSem();
        if (!cam.StatusIsFatal())
        {
                StartServerTask();
        }
        else
        {
                CloneError(&cam);
        }
}

PCVideoServer::~PCVideoServer()
{
        // Stop the images to PC server.
        Stop();
        // Clear the error so that you can use this object to make a connection to
        // the VIDEO_TO_PC_PORT to stop the ImageToPCServer if it is waiting to
        // accept connections from a PC.
        ClearError();
        // Open a socket.
        int camSock = socket(AF_INET, SOCK_STREAM, 0);
        if (camSock == ERROR)
        {
                wpi_setErrnoError();
                return;
        }
        ScopedSocket scopedCamSock(camSock);
        // If successful
        if (!StatusIsFatal())
        {
                //  Create a connection to the localhost.
                struct sockaddr_in selfAddr;
                int sockAddrSize = sizeof(selfAddr);
                bzero ((char *) &selfAddr, sockAddrSize);
                selfAddr.sin_family = AF_INET;
                selfAddr.sin_len = (u_char) sockAddrSize;
                selfAddr.sin_port = htons (VIDEO_TO_PC_PORT);

                if (( (int)(selfAddr.sin_addr.s_addr = inet_addr (const_cast<char*>("localhost")) ) != ERROR) ||
                        ( (int)(selfAddr.sin_addr.s_addr = hostGetByName (const_cast<char*>("localhost")) ) != ERROR))
                {
                        struct timeval connectTimeout;
                        connectTimeout.tv_sec = 1;
                        connectTimeout.tv_usec = 0;
                        connectWithTimeout(camSock, (struct sockaddr *) &selfAddr, sockAddrSize, &connectTimeout);
                }
        }
}

int PCVideoServer::StartServerTask()
{
        if (StatusIsFatal())
        {
                return -1;
        }
        int id = 0;
        m_stopServer = false;
        // Check for prior copy of running task
        int oldId = taskNameToId((char*)m_serverTask.GetName());
        if(oldId != ERROR)
        {
                // TODO: Report error. You are in a bad state.
                taskDelete(oldId);
        }

        // spawn video server task
        // this is done to ensure that the task is spawned with the
        // floating point context save parameter.
        bool started = m_serverTask.Start((int)this);

        id = m_serverTask.GetID();

        if (!started)
        {
                wpi_setWPIError(TaskError);
                return id;
        }
        taskDelay(1);
        return id;
}

void PCVideoServer::Start()
{
        StartServerTask();
}

void PCVideoServer::Stop()
{
        m_stopServer = true;
}

int PCVideoServer::s_ServerTask(PCVideoServer *thisPtr)
{
        return thisPtr->ServerTask();
}

int PCVideoServer::ServerTask()
{
        /* Setup to PC sockets */
        struct sockaddr_in serverAddr;
        int sockAddrSize = sizeof(serverAddr);
        int pcSock = ERROR;
        bzero ((char *) &serverAddr, sockAddrSize);
        serverAddr.sin_len = (u_char) sockAddrSize;
        serverAddr.sin_family = AF_INET;
        serverAddr.sin_port = htons (VIDEO_TO_PC_PORT);
        serverAddr.sin_addr.s_addr = htonl (INADDR_ANY);

        int success;
        while (true)
        {
                taskSafe();
                // Create the socket.
                if ((pcSock = socket (AF_INET, SOCK_STREAM, 0)) == ERROR)
                {
                        wpi_setErrnoErrorWithContext("Failed to create the PCVideoServer socket");
                        continue;
                }
                // Set the TCP socket so that it can be reused if it is in the wait state.
                int reuseAddr = 1;
                setsockopt(pcSock, SOL_SOCKET, SO_REUSEADDR, reinterpret_cast<char*>(&reuseAddr), sizeof(reuseAddr));
                // Bind socket to local address.
                if (bind (pcSock, (struct sockaddr *) &serverAddr, sockAddrSize) == ERROR)
                {
                        wpi_setErrnoErrorWithContext("Failed to bind the PCVideoServer port");
                        close (pcSock);
                        continue;
                }
                // Create queue for client connection requests.
                if (listen (pcSock, 1) == ERROR)
                {
                        wpi_setErrnoErrorWithContext("Failed to listen on the PCVideoServer port");
                        close (pcSock);
                        continue;
                }

                struct sockaddr_in clientAddr;
                int clientAddrSize;
                int newPCSock = accept (pcSock, reinterpret_cast<sockaddr*>(&clientAddr), &clientAddrSize);
                if (newPCSock  == ERROR)
                {
                        close(pcSock);
                        continue;
                }
                //TODO: check camera error

                int numBytes = 0;
                int imageDataSize = 0;
                char* imageData = NULL;

                while(!m_stopServer)
                {
                        success = semTake(m_newImageSem, 1000);
                        if (success == ERROR)
                        {
                                // If the semTake timed out, there are no new images from the camera.
                                continue;
                        }
                        success = AxisCamera::GetInstance().CopyJPEG(&imageData, numBytes, imageDataSize);
                        if (!success)
                        {
                                // No point in running too fast -
                                Wait(1.0);
                                // If camera is not initialzed you will get failure and
                                // the timestamp is invalid. Reset this value and try again.
                                continue;
                        }

                        // Write header to PC
                        static const char header[4]={1,0,0,0};
                        int headerSend = write(newPCSock, const_cast<char*>(header), 4);

                        // Write image length to PC
                        int lengthSend = write(newPCSock, reinterpret_cast<char*>(&numBytes), 4);

                        // Write image to PC
                        int sent = write (newPCSock, imageData, numBytes);

                        // The PC probably closed connection. Get out of here
                        // and try listening again.
                        if (headerSend == ERROR || lengthSend == ERROR || sent == ERROR)
                        {
                                break;
                        }
                }
                // Clean up
                delete [] imageData;
                close (newPCSock);
                newPCSock = ERROR;
                close (pcSock);
                pcSock = ERROR;
                taskUnsafe();
                Wait(0.1);
        }
        return (OK);
}