OVirtualRobotComm.cc

Go to the documentation of this file.

/*
 * This file is part of openSDK.
 *
 * Copyright (C) 2006-2007 openSDK team
 *
 * openSDK is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; version 2.
 *
 * openSDK is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with openSDK; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *     $Id: OVirtualRobotComm.cc,v 1.30 2007/07/15 15:32:07 nuno-lopes Exp $
 */

#include <iostream>
#include <sys/time.h>
#include <unistd.h>
#include <errno.h>
#include <OPENR/ObjcommMessages.h>
#include <OPENR/OObserver.h>
#include <OPENR/OSubject.h>
#include <ModuleData.h>
#include <Platform.h>
#include <Primitives.h>
#include "main.h"
#include "helper.h"
#include "opensdkAPI.h"

using namespace std;

static void* VirtualRobotComm(void* PTS);
static void* VirtualRobotComm_sensor(void* PTS);
static void* VirtualRobotComm_image(void* PTS);
static void sigalrm_handler(int);
static void* process_sensor_readings(void*);
static void* process_images(void*);
static void processVectorData(OCommandInfo *info, OCommandData *data);

#define NEW_SYSTEM_OBJ(func) \
        id = ThreadsList.size(); \
        data = new perThreadStruct(id); /* deleted in cleanup() */ \
        \
        if (pthread_create(&(data->threadId), NULL, func, data)) { \
                cerr << "Couldn't create a new thread for the " << #func << " system object" << endl; \
                exit(0x77); \
        } \
        ThreadsList.push_back(data);


void init_virtual_robot_comm(void)
{
        size_t id;
        perThreadStruct *data;

        NEW_SYSTEM_OBJ(VirtualRobotComm)
        NEW_SYSTEM_OBJ(VirtualRobotComm_sensor)
        NEW_SYSTEM_OBJ(VirtualRobotComm_image);

        pthread_t thread;
        if (pthread_create(&thread, NULL, process_sensor_readings, NULL)) {
                cerr << "Couldn't create a new thread to connect to the sensor socket" << endl;
                exit(0x78);
        }

        if (pthread_create(&thread, NULL, process_images, NULL)) {
                cerr << "Couldn't create a new thread to connect to the images socket" << endl;
                exit(0x79);
        }
}

#undef MOD_DATA
#define MOD_DATA(x) (PTS->x)

static void* VirtualRobotComm(void *data)
{
        BLOCK_SIGNALS()
        pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);

        perThreadStruct *PTS = (perThreadStruct*)data;
        pthread_setspecific(perThreadKey, data);

        // register this as a standard OPEN-R object
        OObserver observer;
        {
        OServiceEntry entry((void*)MOD_DATA(OID), 0);
        const char name[]="OVirtualRobotComm.Effector.OCommandVectorData.O";
        RegisterServiceEntry(entry, name);
        observer.SetNotifyEntry(entry);
        }

        MOD_DATA(DoInitCompleted) = true;

        // DoStart can only be called after all DoInit()s
        while (__DoInitCompleted == false) {
                opensdk_yield();
        }

        // process connect messages
        pthread_mutex_lock(&MOD_DATA(queue_mutex));
        while (!MOD_DATA(queue).empty()) {
                sem_wait(&MOD_DATA(sem)); // just to decrement the counter

                if (__in_shutdown) {
                        MOD_DATA(DoStopCompleted) = true;
                        goto exit_and_signal;
                }

                struct message m = MOD_DATA(queue).front();
                MOD_DATA(queue).pop();

                OConnectMessage *msg = (OConnectMessage*)m.msg;
                observer.ConnectHandler(*msg);

                delete msg;
        }
        pthread_mutex_unlock(&MOD_DATA(queue_mutex));

        observer.AssertReady();

        // proccess messages
        while(1) {
                sem_wait(&MOD_DATA(sem));

                if (__in_shutdown) {
                        MOD_DATA(DoStopCompleted) = true;
                        break;
                }

                pthread_mutex_lock(&MOD_DATA(queue_mutex));
                struct message m = MOD_DATA(queue).front();
                MOD_DATA(queue).pop();
                pthread_mutex_unlock(&MOD_DATA(queue_mutex));

                ONotifyMessage *msg = (ONotifyMessage*)m.msg;

                // sanity check
                if (msg->msgType != omsgNOTIFY_V22) {
                        cerr << "strange message received in OVirtualRobotComm(2). type=" << msg->msgType << endl;
                        delete msg;
                        continue;
                }

                for (int i=0; i < msg->numOfData; ++i) {
                        RCRegion *RCR = msg->data[i].srcRCR;
                        OCommandVectorData *data = (OCommandVectorData*)RCR->Base();

                        for (unsigned int j=0; j < data->vectorInfo.numData; ++j) {
                                processVectorData(data->GetInfo(j), data->GetData(j));
                        }

                        RCR->RemoveReference();
                }

                usleep(USECONDS_TICK/64);

                observer.AssertReady(msg->subjectID);

                delete msg;
        }

exit_and_signal:
        MOD_DATA(exited) = true;
        pthread_cond_signal(&MOD_DATA(cond));
        return NULL;
}


static OSubject *subject; 
static long frameNumber = 0; 
static unsigned int tick_number = 0; 
static OSensorFrameVectorData *SensorFrameData;
static pthread_mutex_t sensorFrameData_mutex = PTHREAD_MUTEX_INITIALIZER;
static const unsigned int numVectorData = num_of_primitives;
static const size_t SensorFrameData_size = sizeof(OSensorFrameVectorData) + (sizeof(OSensorFrameInfo) + sizeof(OSensorFrameData)) * numVectorData;

slongword VirtualRobotComm_get_sensor_reading(int id)
{
        pthread_mutex_lock(&sensorFrameData_mutex);
        slongword tmp = SensorFrameData->GetData(id)->frame[tick_number].value;
        pthread_mutex_unlock(&sensorFrameData_mutex);
        return tmp;
}

static void* VirtualRobotComm_sensor(void *data)
{
        BLOCK_SIGNALS_EXCEPT(SIGALRM)
        pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);

        perThreadStruct *PTS = (perThreadStruct*)data;
        pthread_setspecific(perThreadKey, data);

        subject = new OSubject;

        // create the data that will be sent to observers
        SensorFrameData = (OSensorFrameVectorData*)calloc(1, SensorFrameData_size);
        SensorFrameData->SetNumData(numVectorData);

        for (unsigned int i=0; i < numVectorData; ++i) {
                OSensorFrameInfo *info = SensorFrameData->GetInfo(i);
                info->primitiveID = i;
                info->numFrames   = 4;
                info->frameSize   = 16; //all frames are 16 bytes long
                info->dataOffset = sizeof(ODataVectorInfo) + sizeof(OSensorFrameInfo)*numVectorData + sizeof(OSensorFrameData)*i;
        }

        // register this as a standard OPEN-R object
        {
        OServiceEntry entry((void*)MOD_DATA(OID), 0);
        const char name[]="OVirtualRobotComm.Sensor.OSensorFrameVectorData.S";
        RegisterServiceEntry(entry, name);
        subject->SetReadyEntry(entry);
        }

        MOD_DATA(DoInitCompleted) = true;

        {
        struct itimerval timer = {
                {0, USECONDS_TICK}, {0, USECONDS_TICK}
        };
        signal(SIGALRM, sigalrm_handler);

        if (setitimer(ITIMER_REAL, &timer, NULL) < 0) {
                perror("setitimer() failed");
                exit(0x41);
        }
        }

        // DoStart can only be called after all DoInit()s
        while (__DoInitCompleted == false) {
                opensdk_yield();
        }

        // proccess messages
        while(1) {
                if (sem_wait(&MOD_DATA(sem)) < 0) continue;

                if (__in_shutdown) {
                        MOD_DATA(DoStopCompleted) = true;
                        break;
                }

                pthread_mutex_lock(&MOD_DATA(queue_mutex));
                struct message m = MOD_DATA(queue).front();
                MOD_DATA(queue).pop();
                pthread_mutex_unlock(&MOD_DATA(queue_mutex));

                OReadyMessage *msg = (OReadyMessage*)m.msg;

                if (msg->msgType == omsgCONTROL || msg->msgType == omsgCONTROL_V21) {
                        OControlMessage *msg = (OControlMessage*)m.msg;
                        subject->ControlHandler(*msg);
                } else {
                        subject->ReadyHandler(*msg);
                }

                delete msg;
        }

        MOD_DATA(exited) = true;
        pthread_cond_signal(&MOD_DATA(cond));
        return NULL;
}


static void sigalrm_handler(int)
{
        pthread_mutex_lock(&sensorFrameData_mutex);
        unsigned int old_frame = tick_number;

        if (++tick_number == NUM_TICKS_TO_SEND_DATA) {
                tick_number = 0;

                for (unsigned int i=0; i < numVectorData; ++i) {
                        SensorFrameData->GetInfo(i)->frameNumber = frameNumber;
                }

                frameNumber = (frameNumber + NUM_TICKS_TO_SEND_DATA) % oframeMAX_NUMBER;

                subject->SetData(SensorFrameData, SensorFrameData_size);
                subject->NotifyObservers();
        }

        // copy the data from the previous frame to the new one, so that it can be used
        // if the simulator doesn't send anything in the short sensor frame period
        for (unsigned int i=0; i < numVectorData; ++i) {
                SensorFrameData->GetData(i)->frame[tick_number].value = SensorFrameData->GetData(i)->frame[old_frame].value;
        }

        pthread_mutex_unlock(&sensorFrameData_mutex);
}


#define GET_DOUBLE(str) n = strtod(buf+sizeof(str)-1, NULL)
#define SET_VALUE(id, val) { \
                pthread_mutex_lock(&sensorFrameData_mutex); \
                SensorFrameData->GetData(id)->frame[tick_number].value = (slongword)(val * 1000000.0); \
                pthread_mutex_unlock(&sensorFrameData_mutex); \
        }

static void* process_sensor_readings(void*)
{
        FILE **fp = &socketList[SENSORS].fp_client;
        char buf[2048];

        BLOCK_SIGNALS()

        while (true) {
                if (!*fp) {
                        sleep(2);
                        continue;
                }

                if (!fgets(buf, sizeof(buf), *fp)) {
                        continue;
                }

                int joint_id;
                double n;

                if (!strncmp(buf, "AX:", sizeof("AX:")-1)) {
                        GET_DOUBLE("AX:");
                        SET_VALUE(1, n)

                } else if (!strncmp(buf, "AY:", sizeof("AY:")-1)) {
                        GET_DOUBLE("AY:");
                        SET_VALUE(0, n)

                } else if (!strncmp(buf, "AZ:", sizeof("AZ:")-1)) {
                        GET_DOUBLE("AZ:");
                        SET_VALUE(2, n)

                } else if (!strncmp(buf, "EDG:", sizeof("EDG:")-1)) {
                        GET_DOUBLE("EDG:");
                        SET_VALUE(10, n)

                } else if (!strncmp(buf, "IRF:", sizeof("IRF:")-1)) {
                        GET_DOUBLE("IRF:");
                        SET_VALUE(40, n)

                } else if (!strncmp(buf, "IRN:", sizeof("IRN:")-1)) {
                        GET_DOUBLE("IRN:");
                        SET_VALUE(39, n)

                } else if (sscanf(buf, "J%d:%lf", &joint_id, &n) == 2) { // joint values
                        SET_VALUE(joint_id, n)

                } else {
                        cerr << "bogus message received from the sensor socket: '" << buf << "'" << endl;
                }
        }

        return NULL;
}


static void send_server(char *str)
{
        int sock = socketList[ACTUATORS].fd_client;
        if (sock < 1) return;

        ssize_t ret;
        do {
                ret = write(sock, str, strlen(str));
        } while (ret < 0 && errno == EINTR);
}


static OSubject *img_subject;
static longword img_frame_number = 0;


static void* VirtualRobotComm_image(void *data)
{
        BLOCK_SIGNALS()
        pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);

        perThreadStruct *PTS = (perThreadStruct*)data;
        pthread_setspecific(perThreadKey, data);

        img_subject = new OSubject();

        // register this as a standard OPEN-R object
        {
        OServiceEntry entry((void*)MOD_DATA(OID), 0);
        const char name[]="OVirtualRobotComm.FbkImageSensor.OFbkImageVectorData.S";
        RegisterServiceEntry(entry, name);
        img_subject->SetReadyEntry(entry);
        }

        MOD_DATA(DoInitCompleted) = true;

        // DoStart can only be called after all DoInit()s
        while (__DoInitCompleted == false) {
                opensdk_yield();
        }

        // proccess messages
        while(1) {
                sem_wait(&MOD_DATA(sem));

                if (__in_shutdown) {
                        MOD_DATA(DoStopCompleted) = true;
                        break;
                }

                pthread_mutex_lock(&MOD_DATA(queue_mutex));
                struct message m = MOD_DATA(queue).front();
                MOD_DATA(queue).pop();
                pthread_mutex_unlock(&MOD_DATA(queue_mutex));

                OReadyMessage *msg = (OReadyMessage*)m.msg;

                if (msg->msgType == omsgCONTROL || msg->msgType == omsgCONTROL_V21) {
                        OControlMessage *msg = (OControlMessage*)m.msg;
                        img_subject->ControlHandler(*msg);
                } else {
                        img_subject->ReadyHandler(*msg);
                }

                delete msg;
        }

        MOD_DATA(exited) = true;
        pthread_cond_signal(&MOD_DATA(cond));
        return NULL;
}



#define FILL_INFO(idx,s,sprev) \
        { \
        OFbkImageInfo *info = data->GetInfo(idx); \
        info->primitiveID = 18; \
        info->type        = odataFBK_YCrCb; \
        info->dataSize    = IMG_SIZE/((s)*(s)); \
        info->width       = IMG_WIDTH/(s); \
        info->height      = IMG_HEIGHT/(s); \
        info->dataOffset  = sizeof(ODataVectorInfo) + sizeof(OFbkImageInfo)*IMG_LAYERS + (sprev); \
        }

static void* process_images(void*)
{
        FILE **fp = &socketList[IMAGE].fp_client;
        const size_t data_size = sizeof(OFbkImageVectorData) + sizeof(OFbkImageInfo)*IMG_LAYERS + (IMG_SIZE+IMG_SIZE/4+IMG_SIZE/16+IMG_SIZE);
        OFbkImageVectorData *data;

        BLOCK_SIGNALS()

        data = (OFbkImageVectorData*)malloc(data_size);

        data->vectorInfo.numData = IMG_LAYERS;


        FILL_INFO(ofbkimageLAYER_H, 1, 0)
        FILL_INFO(ofbkimageLAYER_M, 2, IMG_SIZE)
        FILL_INFO(ofbkimageLAYER_L, 4, IMG_SIZE+IMG_SIZE/4)
        FILL_INFO(ofbkimageLAYER_C, 1, IMG_SIZE+IMG_SIZE/4+IMG_SIZE/16)

        while (true) {
                if (!*fp) {
                        sleep(2);
                        continue;
                }

                if (!fread(data->GetData(ofbkimageLAYER_H), IMG_SIZE, 1, *fp) ||
                !fread(data->GetData(ofbkimageLAYER_M), IMG_SIZE/4, 1, *fp) ||
                !fread(data->GetData(ofbkimageLAYER_L), IMG_SIZE/16, 1, *fp)
                ) {
                        continue;
                }

                for (unsigned int i=0; i < IMG_LAYERS; ++i) {
                        data->GetInfo(i)->frameNumber = img_frame_number;
                }

                ++img_frame_number;

                // TODO: copy the image to other buffer with the necessary transformations

                // send the imgs to observers
                img_subject->SetData(data, data_size);
                img_subject->NotifyObservers();

        }

        return NULL;
}


static void processVectorData(OCommandInfo *info, OCommandData *data_)
{
        OPrimitiveID primitiveID = info->primitiveID;
        size_t numFrames = info->numFrames;
        char buffer[2048], *str = buffer;

        str += sprintf(str, "%ld", primitiveID);

        switch (info->type) {
                case odataLED_COMMAND2:
                        {
                        const OLEDCommandValue2 *data = (const OLEDCommandValue2*)data_->value;
                        //TODO
                        break;
                        }

                case odataLED_COMMAND3:
                        {
                        const OLEDCommandValue3 *data = (const OLEDCommandValue3*)data_->value;
                        //TODO
                        break;
                        }

                case odataJOINT_COMMAND2:
                        {
                        const OJointCommandValue2 *data = (const OJointCommandValue2*)data_->value;
                        for (unsigned int i=0; i < numFrames; ++i) {
                                double val = data[i].value; // 10^-6 rad
                                val /= 1000000.0; // rad

                                str += sprintf(str, " %lf", val);
                        }

                        sprintf(str, "\n");
                        send_server(buffer);
                        break;
                        }

                case odataJOINT_COMMAND4:
                        {
                        const OJointCommandValue4 *data = (const OJointCommandValue4*)data_->value;
                        //TODO
                        break;
                        }

                default:
                        cerr << "processVectorData: structure format not implemented: " << info->type << endl;
        }
}

---