loader/main.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: main.cc,v 1.56 2007/07/16 13:09:00 nuno-lopes Exp $
 */

#include <iostream>
#include <stack>
#include <vector>

#include <cstring>
#include <cstdlib>
#include <cstdio>
#include <time.h>
#include <unistd.h>
#include <dlfcn.h>
#include <pthread.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <signal.h>

#include <Platform.h>
#include <ModuleData.h>
#include <OpenSdkThread.h>
#include <utils.h>
#include "main.h"
#include "helper.h"
#include "gunzip.h"
#include "OVirtualRobotComm.h"
#include "opensdkAPI.h"

#include <OPENR/ObjcommMessages.h>
#include <OPENR/OPENRAPI.h>

using namespace std;

#define CHECK_SYSCALL(val, msg) if ((val) == -1) { perror(msg); exit(4); }
#define CLOSE_SOCKET(sock) if (sock != -1) { close(sock); sock = -1; }
#define CREATE_SOCKET_THREAD(sock) \
        { \
                pthread_t myThread; \
                if (pthread_create(&myThread, NULL, listen_socket, sock)) { \
                        cerr << "Couldn't create a new thread to load servers" << endl; \
                        return 5; \
                } \
                skThreadsList.push(myThread); \
        }

static stack<void*> dlopenHandlers;     // dlopen()'ed librarys
vector<perThreadStruct*> ThreadsList; // modules threads
static stack<pthread_t> skThreadsList;  // socket related threads

socketList_t socketList[]= {
        {-1, -1, NULL, ACTUATORS_SERVER_SOCKET},
        {-1, -1, NULL, SENSORS_SERVER_SOCKET},
        {-1, -1, NULL, IMAGE_SERVER_SOCKET},
};

pthread_key_t perThreadKey;

bool __in_shutdown = false;

bool __DoInitCompleted = false;

bool __DoStopCompleted = false;

map<string, OServiceEntry> _objectsEntryList;

static list< pair<string, string> > connectionStrings;


static bool load_module(const char* name)
{
        char *path = resolve_case_insensitive_path(name);

        string tmp_mod_name = path;
        tmp_mod_name += ".tmp";

        int ret = gunzip_module(path, tmp_mod_name.c_str());
        free(path);

        if (ret != Z_OK) {
                cerr << "Couldn't gunzip the module properly. Error " << ret << ": " << zError(ret) << endl;
                return false;
        }

        // compatibility with GDB: prepend the current working dir so that it is able to load the debug symbols
        char cwd[PATH_MAX];
        if (getcwd(cwd, sizeof(cwd))) {
                tmp_mod_name = cwd + string("/") + tmp_mod_name;
        }

        size_t id = ThreadsList.size();
        perThreadStruct* data = new perThreadStruct(id); // deleted in cleanup()
        executorArg* arg      = new executorArg(tmp_mod_name, data); // deleted in helper.cc

        if (pthread_create(&(data->threadId), NULL, module_executor, arg)) {
                cerr << "Couldn't create a new thread to load the module" << endl;
                return false;
        }

        cout << "Loaded module: " << name << " (thread id: " << data->threadId << "::" << id << ")" << endl;

        // for future cleanup
//      dlopenHandlers.push(handle);
        ThreadsList.push_back(data);

        return true;
}


static bool parse_object_cfg(void)
{
        char *path = resolve_case_insensitive_path("MS/OPEN-R/MW/CONF/OBJECT.CFG");
        fstream objectsFile(path);
        free(path);

        if (!objectsFile) {
                cerr << "Couldn't open the OBJECT.CFG file" << endl;
                return false;
        }

        //parse the file and load the objects
        string line;
        bool mySection = true; // starts on a top level section

        while(getNonEmptyLine(objectsFile, line, true, &mySection)) {
                if (!load_module(line.c_str() + 1)) {
                        return false;
                }
        }
        return true;
}


static bool parse_connect_cfg(void)
{
        char *path = resolve_case_insensitive_path("MS/OPEN-R/MW/CONF/CONNECT.CFG");
        fstream file(path);
        free(path);

        if (!file) {
                // the file is optional
                return true;
        }

        string line;
        while(getNonEmptyLine(file, line)) {
                size_t i = line.find_first_of(" \t");
                size_t j = line.find_first_not_of(" \t", i);
                if (i == string::npos || j == string::npos) {
                        cerr << "malformed CONNECT.CFG file" << endl;
                        return false;
                }

                string subject(line, 0, i);
                string observer(line, j);
                cout << "connect: " << subject << " --> " << observer << endl;
                connectionStrings.push_back(pair<string,string>(subject, observer));
        }

        return true;
}


static void sendConnectMessages(void)
{
        for(list< pair<string, string> >::iterator it = connectionStrings.begin(); it != connectionStrings.end(); ++it) {

                if (_objectsEntryList.find(it->first) == _objectsEntryList.end() ||
                _objectsEntryList.find(it->second) == _objectsEntryList.end()) {
                        continue;
                }

                OServiceEntry subject      = _objectsEntryList[it->first];
                OServiceEntry observer     = _objectsEntryList[it->second];
                OServiceEntry subjectFrom  = subject;
                OServiceEntry observerFrom = observer;

                list< std::pair<int,int> >& l = ThreadsList[(size_t)subject.GetOID().GetAddress()]->ConnectHandlers;
                list< std::pair<int,int> >& l2 = ThreadsList[(size_t)observer.GetOID().GetAddress()]->ConnectHandlers;

                // find the right selectors
                for (list< std::pair<int,int> >::iterator it2 = l.begin(); it2 != l.end(); ++it2) {
                        if (it2->first == (int)subject.GetSelector()) {
                                subjectFrom.selector = it2->second;
                                break;
                        }
                }

                for (list< std::pair<int,int> >::iterator it2 = l2.begin(); it2 != l2.end(); ++it2) {
                        if (it2->first == (int)observer.GetSelector()) {
                                observerFrom.selector = it2->second;
                                break;
                        }
                }


                // send msg to the subject
                OControlMessage *msg1 = new OControlMessage(objcommCMD_ADDOBSERVER_V22, observerFrom, 0, 1, 1, it->first.c_str()); // TODO: fix min/max/skip
                strcpy(msg1->subName, it->first.c_str());
                _sendMessage(subject, msg1);

                // send msg to the observer
                OConnectMessage *msg2 = new OConnectMessage(oconnectcmdCONNECT);
                msg2->entry = subjectFrom;
                strcpy(msg2->subName, it->second.c_str());
                _sendMessage(observer, msg2);
        }
}


static void* listen_socket(void* _sock)
{
        BLOCK_SIGNALS()

        int *fd_main = &(((socketList_t*)_sock)->fd_main);
        int *fd_client = &(((socketList_t*)_sock)->fd_client);
        FILE **fp_client = &(((socketList_t*)_sock)->fp_client);
        const char *name = ((socketList_t*)_sock)->name;

        CHECK_SYSCALL(*fd_main = socket(AF_UNIX, SOCK_STREAM, 0), "socket() failed")

        struct sockaddr_un addr;
        socklen_t sizeof_addr = sizeof(addr);
        addr.sun_family = AF_UNIX;
        strcpy(addr.sun_path, name);

        unlink(name);
        CHECK_SYSCALL(bind(*fd_main, (const struct sockaddr*)&addr, sizeof(addr)), "bind() failed")
        CHECK_SYSCALL(listen(*fd_main, 1), "listen() failed")

        // we only use one socket at a time. so we close the existing socket and keep the most recent
        do {
                int tmp = *fd_client;
                CHECK_SYSCALL(*fd_client = accept(*fd_main, (struct sockaddr*)&addr, &sizeof_addr), "accept() failed")
                if (*fp_client) fclose(*fp_client);
                CLOSE_SOCKET(tmp)
                *fp_client = fdopen(*fd_client, "r+");
        } while(1);

        return NULL;
}


static void connect_to_actuators_server()
{
        struct sockaddr_un addr;
        addr.sun_family = AF_UNIX;
        strcpy(addr.sun_path, ACTUATORS_SERVER_SOCKET);

        int s = socket(AF_UNIX, SOCK_STREAM, 0);

        if (connect(s, (const sockaddr*)&addr, sizeof(addr)) < 0) {
                cerr << "not connected to the actuators server" << endl;
        } else {
                socketList[ACTUATORS].fd_client = s;
                cout << "connected to the actuators server" << endl;
        }
}


static void cleanup(void)
{
        cout << "Start Shutdown procedures..." << endl;

        __in_shutdown = true;
        __DoInitCompleted = true; // fake it if it didn't complete

        // update the robot boot condition
        OPENR::Shutdown(OBootCondition(obcbPAUSE_SW));

        // let modules know that they should start exiting
        for(vector<perThreadStruct*>::iterator it = ThreadsList.begin(); it != ThreadsList.end(); ++it) {
                sem_post(&((*it)->sem));
        }

        // check if all modules have already run its DoInit() method
        for(vector<perThreadStruct*>::iterator it = ThreadsList.begin(); it != ThreadsList.end(); ++it) {
                while ((*it)->DoStopCompleted == false && (*it)->exited == false) {
                        opensdk_yield();
                }
        }
        __DoStopCompleted = true;

        // shutdown modules (max 30 seconds)
        struct timespec maxTime;
        maxTime.tv_sec  = time(NULL) + MAX_SHUTDOWN_TIME;
        maxTime.tv_nsec = 0;

        for(vector<perThreadStruct*>::iterator it = ThreadsList.begin(); it != ThreadsList.end(); ++it) {
                (*it)->exited or pthread_cond_timedwait(&((*it)->cond), &((*it)->cond_mutex), &maxTime);

                // we call both _cancel and _join, because _cancel will fail if the thread has already exited
                if (pthread_cancel((*it)->threadId) && pthread_join((*it)->threadId, NULL)) {
                        cerr << "failed to stop module thread " << (*it)->threadId << endl;
                }
                delete (*it);
        }

        // ask each one of the threads of the pool to quit
        OpenSdkThread::cleanThreadPool();

        // cleanup socket threads resources
        while(!skThreadsList.empty()) {
                if (pthread_cancel(skThreadsList.top())) {
                        cerr << "failed to stop socket thread " << (void*)skThreadsList.top() << endl;
                }
                skThreadsList.pop();
        }

        // unload modules
        while(!dlopenHandlers.empty()) {
                // compatibility with valgrind: don't close dlopen'ed libraries so that it is able to generate
                // useful memory leakage reports
                /* if (dlclose(dlopenHandlers.top())) {
                        cerr << "failed to close library " << dlopenHandlers.top() << endl;
                } */
                dlopenHandlers.pop();
        }

        for (unsigned int i=0; i<sizeof(socketList)/sizeof(*socketList); ++i) {
                CLOSE_SOCKET(socketList[i].fd_main)
                CLOSE_SOCKET(socketList[i].fd_client)
                unlink(socketList[i].name);
        }
}


static void sig_handler(int sig)
{
        //cleanup();
        int exit_code;
        switch(sig) {
                case SIGINT:
                case SIGQUIT:
                        exit_code = 0;
                        break;
                default:
                        exit_code = sig + 100;
        }
        exit(exit_code);
}


int main(int argc, char *argv[])
{
        bool run_with_chroot = true;

        while (--argc) {
                ++argv;

                if (!strcmp(*argv, "--help") || !strcmp(*argv, "-h")) {
                        cout << "Usage: ./OPENRloader [options] [Directory Root]" << endl << endl
                        << "Available options:" << endl
                        << "--chroot=[yes|no]\tRun with or without chroot. defaults to yes" << endl
                        << endl;
                        return 0;
                }

                if (!strncmp(*argv, "--chroot=", sizeof("--chroot=")-1)) {
                        run_with_chroot = (argv[0][sizeof("--chroot=")-1] == 'y');

                } else if (chdir(*argv) == -1) {
                        perror("Couldn't change the working directory");
                        return 1;
                }
        }

        if (run_with_chroot) {
                // try to chroot, so that fopen() calls in the client code work correctly
                if (chroot(".") == 0) {
                        cout << "Running with chroot()" << endl;
                } else {
                        cerr << "chroot failed" << endl;
                }
        } else {
                cout << "not running with chroot()" << endl;
        }

        // now we are ready to start loading the files
        cout << "Starting boot procedures for " ROBOTDESIGN "..." << endl << endl;

        // register handler to cleanup the mess as shutdown
        atexit(cleanup);
        signal(SIGINT,  sig_handler);
        signal(SIGQUIT, sig_handler);
        signal(SIGTERM, sig_handler);

        BLOCK_SIGNAL(SIGALRM);

        //Initialize OpenSdkThread pool
        OpenSdkThread::initThreadPool();

        // create per-thread data
        if (pthread_key_create(&perThreadKey, NULL)) {
                cerr << "pthread_key_create() failed" << endl;
                return 2;
        }

        // create the sockets for communications with servers
        CREATE_SOCKET_THREAD((void*)&socketList[SENSORS])
        CREATE_SOCKET_THREAD((void*)&socketList[IMAGE])
        connect_to_actuators_server();
        opensdk_yield(); // really create the servers before loading modules

        if (!parse_connect_cfg()) {
                return 3;
        }

        // init the thread that implements the OVirtualRobotComm system object
        init_virtual_robot_comm();

        if (!parse_object_cfg()) {
                return 4;
        }

        // check if all modules have already run its DoInit() method
        for(vector<perThreadStruct*>::iterator it = ThreadsList.begin(); it != ThreadsList.end(); ++it) {
                while ((*it)->DoInitCompleted == false && (*it)->exited == false) {
                        opensdk_yield();
                }
        }

        // connect objects
        sendConnectMessages();

        // now, objects can run DoStart() safely
        __DoInitCompleted = true;

        // lock this thread, as it isn't needed anymore
        pthread_mutex_t dummy = PTHREAD_MUTEX_INITIALIZER;
        pthread_mutex_lock(&dummy);
        pthread_mutex_lock(&dummy);

        return 0;
}

---