opensdkAPI.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: opensdkAPI.cc,v 1.6 2007/07/16 15:21:43 nuno-lopes Exp $
 */

#include <opensdkAPI.h>
#include <OVirtualRobotComm.h>
#include <main.h>
#include <Platform.h>
#include <pthread.h>
#include <ctype.h>
#include <glob.h>
#include <sys/time.h>

static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;

OStatus RegisterServiceEntry(const OServiceEntry& entry, const char *name)
{
        OStatus ret = oALREADY_EXIST;

        pthread_mutex_lock(&mutex);
        if (_objectsEntryList.find(name) == _objectsEntryList.end()) {
                _objectsEntryList[name] = entry;
                MOD_DATA(ConnectHandlers).push_back(std::pair<int,int>(entry.GetSelector(), 0));
                ret = oSUCCESS;
        }
        pthread_mutex_unlock(&mutex);

        return ret;
}


OStatus _sendMessage(OServiceEntry entry, void *msg) {
        if ((size_t)entry.GetOID().GetAddress() >= ThreadsList.size()) {
                return oFAIL; // no such OID
        }

        perThreadStruct* data = ThreadsList[(size_t)entry.GetOID().GetAddress()];

        pthread_mutex_lock(&(data->queue_mutex));
        data->queue.push(message(entry, msg));
        pthread_mutex_unlock(&(data->queue_mutex));

        sem_post(&(data->sem));
        return oSUCCESS;
}


OStatus GetJointValue(OPrimitiveID id, OJointValue* value)
{
        memset(value, 0, sizeof(OJointValue));
        value->value = VirtualRobotComm_get_sensor_reading(id);
        return oSUCCESS;
}


OStatus GetSensorValue(OPrimitiveID id, OSensorValue* value)
{
        memset(value, 0, sizeof(OSensorValue));
        value->value = VirtualRobotComm_get_sensor_reading(id);
        return oSUCCESS;
}


char* resolve_case_insensitive_path(const char *path)
{
        char *pattern = (char*)malloc(strlen(path) * 4 + 1); // allocate enough space for the worst case
        char *tmp = pattern;

        while (*path) {
                if (!isalpha(*path)) {
                        *tmp++ = *path++;
                        continue;
                }

                *tmp++ = '[';
                *tmp++ = tolower(*path);
                *tmp++ = toupper(*path);
                *tmp++ = ']';

                ++path;
        }

        *tmp = '\0';

        glob_t globbuf;
        if (glob(pattern, GLOB_NOSORT, NULL, &globbuf) == 0) {
                tmp = strdup(globbuf.gl_pathv[0]);
        } else {
                tmp = strdup(path);
        }

        globfree(&globbuf);
        return tmp;
}


// --------------------------
// time/date handling
// --------------------------

static time_t diffToSystem = 0; // difference to the system clock
static sbyte myTimeDif     = 0; // TZ offset (in hours)


sbyte GetTimeDifference(void)
{
        return myTimeDif;
}


OStatus SetTimeDifference(sbyte timeDifference)
{
        if (timeDifference >= -12 && timeDifference <= 12) {
                myTimeDif = timeDifference;
                return oSUCCESS;
        }

        return oFAIL;
}


OStatus SetTime(const OTime& time)
{
        if (SetTimeDifference(time.GetTimeDif()) != oSUCCESS) {
                return oFAIL;
        }

        diffToSystem = time.GetClock() - (getSystemTime().seconds - diffToSystem);

        return oSUCCESS;
}


SystemTime getSystemTime(void)
{
        struct timeval tv;
        gettimeofday(&tv, NULL);

        return SystemTime(tv.tv_sec - TIME_DIFF_SYSTEM + diffToSystem, tv.tv_usec);
}

---