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

#include <OPENR/OPENRAPI.h>
#include <OPENR/OPENRMessages.h>
#include <iostream>
#include <fstream>
#include <map>
#include <cstdlib>
#include <cstring>
#include <cctype>
#include <pthread.h>
#include <boot.h>
#include <utils.h>
#include <Platform.h>
#include <Primitives.h>
#include <MemoryMapAgent.h>
#include <ModuleData.h>
#include <opensdkAPI.h>

using namespace std;

#define LOCK(m) pthread_mutex_lock(&m ##_mutex)
#define UNLOCK(m) pthread_mutex_unlock(&m ##_mutex)

// global variables that control the state of the robot
static longword myRobotStatus = orsbBATTERY_CONNECTED | orsbBATTERY_CAPACITY_FULL;
static word myBatteryStatus = obsbFULLY_CHARGED;
static word myRemainingCapacity = 100; // battery remaining capacity (percentage)
static word myTemperature = 300; // temperature (Kelvins)
static word myFullyChargedCapacity = 55555; // in mAh
static word myVoltage = 5000; // battery voltage (mV)
static word myCurrent = 1000; // battery current (mA)
static OVolumeSwitch myVolume = ovolumeSW3; //volume level
static OPower motorPower = opowerOFF;
static word myBootCondition = 0;

// list of observers of the power status changes
static map<OServiceEntry, OPowerStatus> powerStatusObservers;
static pthread_mutex_t powerStatusObservers_mutex = PTHREAD_MUTEX_INITIALIZER;

#define sendPowerStatusNotifications(property, bitmask) \
{ \
        LOCK(powerStatusObservers); \
\
        for(map<OServiceEntry, OPowerStatus>::iterator it = powerStatusObservers.begin(); it != powerStatusObservers.end(); ++it) { \
                if (it->second.property & bitmask) { \
                        OPowerStatusMessage *status = new OPowerStatusMessage(myRobotStatus, myBatteryStatus, myRemainingCapacity, myTemperature, myFullyChargedCapacity, myVoltage, myCurrent, ::GetTimeDifference(), myVolume); \
                        _sendMessage(it->first, status); \
                } \
        } \
\
        UNLOCK(powerStatusObservers); \
}


OStatus OPENR::OpenPrimitive(const char* locator, OPrimitiveID* primitiveID)
{
        // by default, the primitiveID should be set to the constant below
        *primitiveID = oprimitiveID_UNDEF;

        if (!locator) {
                return oINVALID_ARG;
        }

        //binary search to find the primitive ID
        int left = 0, right = num_of_primitives - 1;

        do {
                unsigned int mid = (left + right) / 2;
                int cmp = strcmp(locator, primitives[mid].locator);

                if (cmp < 0) {
                        right = mid - 1;
                } else if (cmp > 0) {
                        left = mid + 1;
                } else {
                        //TODO: mark the primitive as open
                        *primitiveID = mid;
                        return oSUCCESS;
                }
        } while(left <= right);

        return oNOT_FOUND;
}

OStatus OPENR::ClosePrimitive(OPrimitiveID primitiveID)
{
        CHECK_PRIMITIVEID()
        //TODO
        return oSUCCESS;
}

OStatus OPENR::ControlPrimitive(OPrimitiveID primitiveID, OPrimitiveRequest request, void* param, size_t paramSize, void* result, size_t resultSize)
{
        CHECK_PRIMITIVEID()

        switch (request) {



                default:
                        return oINVALID_ARG;
        }
}

OStatus OPENR::NewCommandVectorData(size_t numCommands, MemoryRegionID* memID, OCommandVectorData** baseAddr)
{
        const size_t size = sizeof(OCommandVectorData) + (sizeof(OCommandInfo) + sizeof(OCommandData))*numCommands;
        OCommandVectorData *data = *baseAddr = (OCommandVectorData*)malloc(size);

        if (!data) {
                return oNO_MEMORY;
        }

        data->vectorInfo.memRegionID = *memID = RegisterNewMemID(data);
        data->SetNumData(0);
        data->vectorInfo.offset = 0;
        data->vectorInfo.totalSize = size;

        for (size_t i=0; i < numCommands; ++i) {
                OCommandInfo *info = data->GetInfo(i);
                info->dataOffset = sizeof(ODataVectorInfo) + sizeof(OCommandInfo)*numCommands + sizeof(OCommandData)*i;
        }

        return oSUCCESS;
}

OStatus OPENR::DeleteCommandVectorData(MemoryRegionID memID)
{
        return FreeMemId(memID);
}

OStatus OPENR::NewSoundVectorData(size_t numSounds, size_t dataSize, MemoryRegionID* memID, OSoundVectorData** baseAddr)
{
        const size_t size = sizeof(OSoundVectorData) + (sizeof(OSoundInfo) + dataSize)*numSounds;
        OSoundVectorData *data = *baseAddr = (OSoundVectorData*)malloc(size);

        if (!data) {
                return oNO_MEMORY;
        }

        data->vectorInfo.memRegionID = *memID = RegisterNewMemID(data);
        data->SetNumData(0);
        data->vectorInfo.offset = 0;
        data->vectorInfo.totalSize = size;

        for (size_t i=0; i < numSounds; ++i) {
                OSoundInfo *info = data->GetInfo(i);
                info->dataOffset = sizeof(ODataVectorInfo) + sizeof(OSoundInfo)*numSounds + dataSize*i;
        }

        return oSUCCESS;
}

OStatus OPENR::DeleteSoundVectorData(MemoryRegionID memID)
{
        return FreeMemId(memID);
}

OStatus OPENR::EnableJointGain(OPrimitiveID primitiveID)
{
        CHECK_PRIMITIVEID_ALL()
        //TODO
        return oSUCCESS;
}

OStatus OPENR::DisableJointGain(OPrimitiveID primitiveID)
{
        CHECK_PRIMITIVEID_ALL()
        //TODO
        return oSUCCESS;
}

OStatus OPENR::SetJointGain(OPrimitiveID primitiveID, word pg, word ig, word dg, word ps, word is, word ds)
{
        CHECK_PRIMITIVEID_ALL()
        //TODO
        return oSUCCESS;
}

OStatus OPENR::RegisterDefaultJointGain(OPrimitiveID primitiveID, word pg, word ig, word dg, word ps, word is, word ds)
{
        CHECK_PRIMITIVEID_ALL()

        if (primitiveID == oprimitiveID_UNDEF) {
                //TODO
        }

        primitives[primitiveID].pg = pg;
        primitives[primitiveID].ig = ig;
        primitives[primitiveID].dg = dg;
        primitives[primitiveID].ps = ps;
        primitives[primitiveID].is = is;
        primitives[primitiveID].ds = ds;

        return oSUCCESS;
}

OStatus OPENR::GetJointGain(OPrimitiveID primitiveID, word* pg, word* ig, word* dg, word* ps, word* is, word* ds)
{
        CHECK_PRIMITIVEID()
        //TODO
        return oFAIL;
}

OStatus OPENR::GetDefaultJointGain(OPrimitiveID primitiveID, word* pg, word* ig, word* dg, word* ps, word* is, word* ds)
{
        if (primitiveID >= num_of_primitives || primitiveID < 0) {
                *pg = *ig = *dg = *ps = *is = *ds = 0;
                return oFAIL;
        }

        *pg = primitives[primitiveID].pg;
        *ig = primitives[primitiveID].ig;
        *dg = primitives[primitiveID].dg;
        *ps = primitives[primitiveID].ps;
        *is = primitives[primitiveID].is;
        *ds = primitives[primitiveID].ds;

        return oSUCCESS;
}

OStatus OPENR::SetDefaultJointGain(OPrimitiveID primitiveID)
{
        CHECK_PRIMITIVEID_ALL()
        //TODO
        return oFAIL;
}

OStatus OPENR::GetJointValue(OPrimitiveID primitiveID, OJointValue* value)
{
        CHECK_PRIMITIVEID()
        return ::GetJointValue(primitiveID, value);
}

OStatus OPENR::GetSensorValue(OPrimitiveID primitiveID, OSensorValue* value)
{
        CHECK_PRIMITIVEID()
        return ::GetSensorValue(primitiveID, value);
}

OStatus OPENR::NewSyncKey(OVRSyncKey* key)
{
        //TODO
        return oFAIL;
}

OStatus OPENR::CancelSyncKey(OVRSyncKey key)
{
        //TODO
        return oFAIL;
}

OStatus OPENR::DivideSyncKey(OVRSyncKey key, OVRSyncKey* key1, OVRSyncKey* key2)
{
        //TODO
        return oFAIL;
}

OStatus OPENR::NewCdtVectorData(MemoryRegionID* memID, OCdtVectorData** baseAddr)
{
        OCdtVectorData *data = *baseAddr = (OCdtVectorData*)malloc(sizeof(OCdtVectorData));
        data->vectorInfo.memRegionID = *memID = RegisterNewMemID(data);
        data->SetNumData(0);
        data->vectorInfo.offset = 0;
        data->vectorInfo.totalSize = sizeof(OCdtVectorData);

        return oSUCCESS;
}

OStatus OPENR::DeleteCdtVectorData(MemoryRegionID memID)
{
        return FreeMemId(memID);
}

OStatus OPENR::SetCdtVectorData(MemoryRegionID memID)
{
        //TODO
        return oFAIL;
}

OStatus OPENR::SetMotorPower(OPower power)
{
        bool changed = false;
        motorPower = power;

        if (power) {
                if (!(power & orsbMOTOR_POWER)) {
                        myRobotStatus |= orsbMOTOR_POWER;
                        changed = true;
                }
        } else {
                if (power & orsbMOTOR_POWER) {
                        myRobotStatus &= ~orsbMOTOR_POWER;
                        changed = true;
                }
        }

        if (changed) {
                sendPowerStatusNotifications(robotStatus, orsbMOTOR_POWER);
        }

        return oSUCCESS;
}

OStatus OPENR::Shutdown(const OBootCondition& bootCondition)
{
        //TODO: finish these if's
        if (bootCondition.bitmap & obcbPAUSE_SW) {
                myBootCondition |= obcbPAUSE_SW;
                myRobotStatus   |= orsbPAUSE;

                sendPowerStatusNotifications(robotStatus, orsbPAUSE);
                shutdown();
                return oSUCCESS;
        } /*else if (bootCondition.bitmap & obcbBOOT_TIMER) {
                if (set_shutdown_timer(bootCondition.bootTime, bootCondition.bootTimeType) == oSUCCESS) {
                        return oSUCCESS;
                }
        } */

        return oFAIL;
}

OStatus OPENR::GetBootCondition(OBootCondition* bootCondition)
{
        bootCondition->bitmap = myBootCondition;
        return oSUCCESS;
}

OStatus OPENR::GetPowerStatus(OPowerStatus* powerStatus)
{
        powerStatus->robotStatus = myRobotStatus;
        powerStatus->batteryStatus = myBatteryStatus;
        powerStatus->remainingCapacity = myRemainingCapacity;
        powerStatus->temperature = myTemperature;
        powerStatus->fullyChargedCapacity = myFullyChargedCapacity;
        powerStatus->voltage = myVoltage;
        powerStatus->current = myCurrent;
        powerStatus->timeDif = ::GetTimeDifference();
        powerStatus->volume = myVolume;
        return oSUCCESS;
}

OStatus OPENR::ObservePowerStatus(const OPowerStatus& notifyStatus, const OServiceEntry& entry)
{
        LOCK(powerStatusObservers);
        powerStatusObservers[entry] = notifyStatus;
        UNLOCK(powerStatusObservers);

        return oSUCCESS;
}

OStatus OPENR::UnobservePowerStatus(const OServiceEntry& entry)
{
        OStatus retval = oINVALID_ARG;

        LOCK(powerStatusObservers);

        map<OServiceEntry, OPowerStatus>::iterator it = powerStatusObservers.find(entry);
        if (it != powerStatusObservers.end()) {
                powerStatusObservers.erase(it);
                retval = oSUCCESS;
        }

        UNLOCK(powerStatusObservers);

        return retval;
}

OStatus OPENR::FindDesignData(const char* keyword, ODesignDataID* dataID, byte** data, size_t* size)
{
        if (!strcmp(keyword, odesignkeywordCPUINFO)) {
                //TODO
                return oFAIL;
        }

        char *path = resolve_case_insensitive_path("MS/OPEN-R/MW/CONF/DESIGNDB.CFG");
        fstream DB(path);
        free(path);

        if (!DB) {
                return oFAIL;
        }

        //parse the design DB file
        string line;
        bool mySection = true; // starts on a top level section

        const size_t keyword_len = strlen(keyword);

        while(getNonEmptyLine(DB, line, true, &mySection)) {
                if (!strncmp(keyword, line.c_str(), keyword_len) && isspace(line[keyword_len])) {
                        char *name = (char*)line.c_str() + keyword_len + 1;

                        while (isspace(*name)) ++name; // skip spaces between keywords and path
                        ++name;

                        *data = (byte*)file_get_contents(name, size);
                        if (!*data) {
                                return (size == 0 ? oDESIGNDATA_SIZE_ZERO : oNOT_FOUND);
                        }

                        // save the pointer+size for later unmaping
                        *dataID = MOD_DATA(lastDesignDataID)++;
                        DesignData tmp = {*data, *size};
                        MOD_DATA(designDataIDs)[*dataID] = tmp;

                        return oSUCCESS;
                }
        }
        return oNOT_FOUND;
}

OStatus OPENR::DeleteDesignData(ODesignDataID dataID)
{
        map<ODesignDataID, DesignData>::iterator it = MOD_DATA(designDataIDs).find(dataID);

        // dataID invalid/not found
        if (it == MOD_DATA(designDataIDs).end()) {
                return oINVALID_ARG;
        }

        DesignData data = it->second;

        if (!file_unmap_contents(data.base, data.size)) {
                return oFAIL;
        }

        MOD_DATA(designDataIDs).erase(it);
        return oSUCCESS;
}

OStatus OPENR::GetRobotDesign(char* robotDesign)
{
        strcpy(robotDesign, ROBOTDESIGN);
        return oSUCCESS;
}

OStatus OPENR::GetMemoryStickStatus(OMemoryStickStatus* status)
{
        //TODO
        return oFAIL;
}

OStatus OPENR::Fatal(OFatal fatal)
{
        //TODO
        return oFAIL;
}

OStatus OPENR::SetTime(const OTime& time)
{
        return ::SetTime(time);
}

OStatus OPENR::GetTime(OTime* time_)
{
        time_->Set(::getSystemTime().seconds, 0);
        return oSUCCESS;
}

OStatus OPENR::SetTimeDifference(sbyte timeDifference)
{
        if (::SetTimeDifference(timeDifference) == oSUCCESS) {
                sendPowerStatusNotifications(timeDif, opsoTIME_DIF_NOTIFY_EVERY_CHANGE);
                return oSUCCESS;
        }

        return oFAIL;
}

OStatus OPENR::GetTimeDifference(sbyte* timeDifference)
{
        *timeDifference = ::GetTimeDifference();
        return oSUCCESS;
}

OStatus OPENR::SetVolumeSwitch(OVolumeSwitch volSW)
{
        //TODO: this should report the new value to the sound server

        if (volSW != ovolumeSW0 && volSW != ovolumeSW1 && volSW != ovolumeSW2 && volSW != ovolumeSW3) {
                return oFAIL;
        }

        if (myVolume != volSW) {
                myVolume = volSW;
                sendPowerStatusNotifications(volume, opsoVOLUME_NOTIFY_EVERY_CHANGE);
        }
        return oSUCCESS;
}

OStatus OPENR::GetVolumeSwitch(OVolumeSwitch* volSW)
{
        *volSW = myVolume;
        return oSUCCESS;
}

---