servers/usarsim/main.cc

Go to the documentation of this file.

/*
 * This file is part of openSDK.
 *
 * Copyright (C) 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.27 2007/08/13 11:19:31 nuno-lopes Exp $
 */

#include <iostream>
#include <iomanip>
#include <cstring>
#include <cstdlib>
#include <cstdio>
#include <cstdarg>
#include <unistd.h>
#include <netdb.h>
#include <limits.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <arpa/inet.h>
#include <netinet/tcp.h>
#include <pthread.h>
#include <signal.h>
#include "config.h"
#include "images.h"
#include <Platform.h>
#define USARSIM
#include <Primitives.h>
#include <OPENR/ODataFormats.h>

#define CHECK_SYSCALL(val, msg) if ((val) < 0) { perror(msg); exit(4); }

using namespace std;

enum team_type {
        NO_TEAM,
        TEAM_BLUE,
        TEAM_RED
};

static team_type team                    = DEFAULT_USARSIM_ROBOT_TEAM;
static const char *usarsim_host          = DEFAULT_USARSIM_HOST;
static short usarsim_port                = DEFAULT_USARSIM_PORT;
static short usarsim_imgserver_port      = DEFAULT_USARSIM_IMGSERVER_PORT;
static const char *usarsim_robot         = DEFAULT_USARSIM_ROBOT;
static const char *usarsim_location      = DEFAULT_USARSIM_LOCATION;
static const char *usarsim_ball_location = DEFAULT_USARSIM_BALL_LOCATION;
static bool display_fps                  = DEFAULT_DISPLAY_IMAGE_FPS;

static int usarsim_socket;
static int usarsim_ball_socket;
static FILE *usarsim_imgserver_socket;
static int actuators_socket;
static FILE *sensors_socket;

static double joint_values[num_of_primitives] = {INT_MIN};


static const char* team_str(void)
{
        switch (team) {
                case NO_TEAM:   return "NO_TEAM";
                case TEAM_BLUE: return "BLUE";
                case TEAM_RED:  return "RED";
        }

        return NULL; // keep gcc happy..
}


static bool connect_to_usarsim(int *sock, int port = usarsim_port, bool read_init_msg = true)
{
        char buffer[BUFFER_SIZE];
        struct sockaddr_in addr;
        struct hostent *h = gethostbyname(usarsim_host);

        if (!h) {
                cerr << "Could not resolve the USARSim host: '" << usarsim_host << "'" << endl;
                return false;
        }

        memset(&addr, 0, sizeof(struct sockaddr_in));
        memcpy(&addr.sin_addr, h->h_addr, h->h_length);
        addr.sin_family = AF_INET;
        addr.sin_port = htons(port);

        *sock = socket(AF_INET, SOCK_STREAM, 0);
        if (*sock < 0) {
                perror("couldn't create a new socket");
                return false;
        }

        if (connect(*sock, (struct sockaddr*)&addr, sizeof(addr)) < 0) {
                perror("error connecting to USARSim server");
                close(*sock);
                return false;
        }

        // read the initial message from the server
        if (read_init_msg) {
                ssize_t ret = read(*sock, buffer, sizeof(buffer));
                if (ret <= 0) {
                        cerr << "error reading the initial message from the USARSim server" << endl;
                        close(*sock);
                        return false;
                }

                fwrite(buffer, ret, 1, stdout);
        }

#if defined(TCP_NODELAY) && defined(IPPROTO_TCP)
        // disable buffering of joint actions (creates more packets, but improves sense of realism)
        const int opt = 1;
        if (setsockopt(*sock, IPPROTO_TCP, TCP_NODELAY, &opt, sizeof(opt)) < 0) {
                perror("could not set TCP_NODELAY");
        }
#endif

        return true;
}


static int connect_to_opensdk_sensor(const char *MSdir, const char *socket_name)
{
        char filename[PATH_MAX];
        snprintf(filename, sizeof(filename), "%s/%s", MSdir, socket_name);

        struct sockaddr_un addr;
        addr.sun_family = AF_UNIX;
        strcpy(addr.sun_path, filename);

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

        for (int i=0; i < 1000; ++i) {
                if (connect(s, (const sockaddr*)&addr, sizeof(addr)) < 0) {
                        opensdk_yield();
                        continue;
                }
                goto ok;
        }

        perror("Couldn't connect to the openSDK sensor server socket");

ok:
        return s;
}


static int connect_to_opensdk_actuator(const char *MSdir)
{
        char filename[PATH_MAX];
        snprintf(filename, sizeof(filename), "%s/" ACTUATORS_SERVER_SOCKET, MSdir);

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

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

        unlink(filename);
        CHECK_SYSCALL(bind(s, (const struct sockaddr*)&addr, sizeof(addr)), "bind() failed")
        CHECK_SYSCALL(listen(s, 1), "listen() failed")
        CHECK_SYSCALL(sock = accept(s, (struct sockaddr*)&addr, &sizeof_addr), "accept() failed")

        close(s);
        return sock;
}


static void send_usarsim(int sock, const char *format, ...) PRINTF_FORMAT;
static void send_usarsim(int sock, const char *format, ...)
{
        va_list ap;
        char buffer[BUFFER_SIZE];
        int bufsize;

        va_start(ap, format);
        bufsize = vsprintf(buffer, format, ap);
        va_end(ap);

        if (DEBUG_USARSIM_PROTOCOL) {
                cout << "[->USARSim] " << buffer;
        }

        if (write(sock, buffer, bufsize) < 0) {
                perror("error writing to the USARSim socket");
                exit(0x31);
        }
}


static void* read_usarsim_messages(void*)
{
        char buffer[BUFFER_SIZE];
        FILE *fp = fdopen(usarsim_socket, "r");

        while (true) {
                if (!fgets(buffer, sizeof(buffer), fp)) {
                        cerr << "error reading from USARSim socket" << endl;
                        exit(0xdd);
                }

                if (!strncmp(buffer, "STA ", sizeof("STA ")-1)) {
                        // state and mission Package messages
                        // these messages aren't useful for us, so just skip them (5 per second)
                        continue;
                }

                if (DEBUG_USARSIM_PROTOCOL) {
                        buffer[strlen(buffer)-1] = '\0'; // remove newline char
                        cout << "[<-USARSim] " << buffer << endl;
                }

                if (!strncmp(buffer, "SEN ", sizeof("SEN ")-1)) {
                        // Sensor readings
                        char *msg = buffer + sizeof("SEN ")-1;

                        if (!strncmp("{Type IR} ", msg, sizeof("{Type IR} ")-1)) {
                                msg += sizeof("{Type IR} ")-1;
                                double IRN, IRF, EDG; // near, far and edge IR sensors
                                if (sscanf(msg, "{Name IRN Range %lf} {Name IRF Range %lf} {Name EDG Range %lf}", &IRN, &IRF, &EDG) != 3) {
                                        goto sensors_error;
                                }

                                fprintf(sensors_socket, "IRN:%lf\nIRF:%lf\nEDG:%lf\n", IRN, IRF, EDG);
                                fflush(sensors_socket);

                        } else if (!strncmp("{Type Accel} ", msg, sizeof("{Type Accel} ")-1)) {
                                msg += sizeof("{Type Accel} ")-1;
                                double ax, ay, az;
                                if (sscanf(msg, "{Name ACC} {Acceleration %lf,%lf,%lf}", &ax, &ay, &az) != 3) {
                                        goto sensors_error;
                                }

                                fprintf(sensors_socket, "AX:%lf\nAY:%lf\nAZ:%lf\n", ax, ay, az);
                                fflush(sensors_socket);

                        } else if (!strncmp("{Type Helper}", msg, sizeof("{Type Helper}")-1)) {
                                continue; // this message has no interest for us

                        } else {
sensors_error:
                                cerr << "bogus sensor readings message received from USARSim: '" << msg << "'" << endl;
                        }

                } else {
                        cerr << "bogus message received: '" << buffer << "'" << endl;
                }
        }

        return NULL;
}


static void* read_usarsim_images(void *MSdir)
{
        // connect to the image server
        int img_socket = connect_to_opensdk_sensor((const char*)MSdir, IMAGE_SERVER_SOCKET);
        FILE *opensdk_socket = fdopen(img_socket, "w");
        if (!opensdk_socket) {
                exit(0x08);
        }

        if (connect_to_usarsim(&img_socket, usarsim_imgserver_port, false)) {
                usarsim_imgserver_socket = fdopen(img_socket, "r+");
                if (!usarsim_imgserver_socket) {
                        exit(0x43);
                }

        } else {
                cout << "Not connected to USARSim image server" << endl;
                return NULL;
        }


        unsigned char buffer[IMG_SIZE], newimg[IMG_SIZE];

        byte *Y  = newimg;
        byte *Cb = newimg+(IMG_SIZE*2/3);
        byte *Cr = newimg+IMG_SIZE/3;

        struct timeval last_tv = {0};
        unsigned int num_of_images = 0;

        while (true) {
                if (fread(buffer, 1, 1, usarsim_imgserver_socket) != 1) {
                        cerr << "error reading image type from USARSim" << endl;
                        return NULL;
                }

                if (*buffer != 0) { // check the image type. for now accept only RAW images
                        cerr << "Invalid image type received: " << (char)(*buffer + '0') << endl;
                        return NULL;
                }

                uint32_t size;
                uint16_t width, height;

                // we do several calls because of memory alignment issues on some platforms
                if (!fread(&size, 4, 1, usarsim_imgserver_socket) || !fread(&width, 2, 1, usarsim_imgserver_socket) || !fread(&height, 2, 1, usarsim_imgserver_socket)) {
                        cerr << "error reading image info from USARSim" << endl;
                }

                size   = ntohl(size);
                width  = ntohs(width);
                height = ntohs(height);

                if (width != IMG_WIDTH || height != IMG_HEIGHT) {
                        cerr << "the image received doesn't match the required dimensions (" << IMG_WIDTH << 'x' << IMG_HEIGHT <<
                        "). received: " << width << 'x' << height << endl;
                        return NULL;
                }

                if (size-4 != IMG_SIZE) { // 4 bytes were already fetched (the dimensions)
                        cerr << "the image received doesn't match the required size (" << IMG_SIZE << "). received: " << size << endl;
                        return NULL;
                }

                if (!fread(buffer, IMG_SIZE, 1, usarsim_imgserver_socket)) {
                        cerr << "error reading data from USARSim image server" << endl;
                        return NULL;
                }


                if (display_fps) {
                        struct timeval tv;
                        gettimeofday(&tv, NULL);

                        ++num_of_images;

                        if (tv.tv_sec != last_tv.tv_sec) {
                                float seconds = (tv.tv_sec + tv.tv_usec / 1000000.0) - (last_tv.tv_sec + last_tv.tv_usec / 1000000.0);
                                float fps = num_of_images / seconds;
                                cout << "\r" << fixed << setprecision(2) << fps << " fps" << flush;
                                num_of_images = 0;
                                last_tv = tv;
                        }
                }


                RGB_to_YCbCr(buffer, IMG_WIDTH, IMG_HEIGHT, Y, Cb, Cr);

                if (!fwrite(newimg, IMG_SIZE, 1, opensdk_socket)) {
                        cerr << "error writing to openSDK image socket" << endl;
                        return NULL;
                }

                shrink_frame(IMG_WIDTH, IMG_HEIGHT, 2, Y, Cb, Cr, buffer, buffer+(IMG_SIZE/4*2/3), buffer+(IMG_SIZE/4/3));

                if (!fwrite(buffer, IMG_SIZE/4, 1, opensdk_socket)) {
                        cerr << "error writing to openSDK image socket" << endl;
                        return NULL;
                }

                shrink_frame(IMG_WIDTH, IMG_HEIGHT, 4, Y, Cb, Cr, buffer, buffer+(IMG_SIZE/16*2/3), buffer+(IMG_SIZE/16/3));

                if (!fwrite(buffer, IMG_SIZE/16, 1, opensdk_socket)) {
                        cerr << "error writing to openSDK image socket" << endl;
                        return NULL;
                }

/*FILE *fp = fopen("sample-aibo.raw", "w");
                if (!fwrite(buffer, IMG_SIZE, 1, fp)) {
                        cerr << "error writing to openSDK image socket" << endl;
                        return NULL;
                }
exit(0);*/

                // ask for another image
                fwrite("OK", 2, 1, usarsim_imgserver_socket);
        }

        return NULL;
}


static void read_opensdk_messages(void)
{
        char buffer[BUFFER_SIZE];
        FILE *fp = fdopen(actuators_socket, "r");

        while (true) {
                if (!fgets(buffer, sizeof(buffer), fp)) {
                        cerr << "error reading from openSDK socket" << endl;
                        exit(0x23);
                }

                char *str = buffer;
                long primitiveId = strtol(str, &str, 10);
                double *table = &joint_values[primitiveId];

                do {
                        double val = strtod(str, &str);
                        *table = val;
                } while (*str != '\n' && *str);
        }
}


static void sigalrm_handler(int)
{
        char buffer[BUFFER_SIZE], *str = buffer + sizeof("MULTIDRIVE")-1;
        char buffer_opensdk[BUFFER_SIZE], *str_opensdk = buffer_opensdk;
        *str = '\0';

        for (unsigned int i=0; i < num_of_primitives; ++i) {
                double val = joint_values[i];
                if (val != INT_MIN) {
                        if (usarsim_primitives[i]) { // ignore primitives that USARSim doesn't support
                                str += sprintf(str, " {%s %lf}", usarsim_primitives[i], val);
                                str_opensdk += sprintf(str_opensdk, "J%d:%lf\n", i, val);
                        }
                        joint_values[i] = INT_MIN; // reset the value
                }
        }

        if (*(buffer + sizeof("MULTIDRIVE")-1)) {
                memcpy(buffer, "MULTIDRIVE", sizeof("MULTIDRIVE")-1);
                sprintf(str, "\r\n");
                send_usarsim(usarsim_socket, buffer);
                fwrite(buffer_opensdk, strlen(buffer_opensdk), 1, sensors_socket);
                fflush(sensors_socket);
        }
}


int main(int argc, char *argv[])
{
        const char *MSdir = NULL;

        while (--argc) {
                const char *arg = *++argv;

                if (!strncmp(arg, "--team=", sizeof("--team=")-1)) {
                        arg += sizeof("--team=")-1;

                        if (!strcasecmp(arg, "blue")) {
                                team = TEAM_BLUE;
                        } else if (!strcasecmp(arg, "red")) {
                                team = TEAM_RED;
                        } else if (!strcasecmp(arg, "no_team")) {
                                team = NO_TEAM;
                        } else {
                                cerr << "Warning: team '" << arg << "' not supported!" << endl;
                        }

                } else if (!strncmp(arg, "--usarsim-host=", sizeof("--usarsim-host=")-1)) {
                        usarsim_host = arg + sizeof("--usarsim-host=")-1;

                } else if (!strncmp(arg, "--usarsim-port=", sizeof("--usarsim-port=")-1)) {
                        usarsim_port = atoi(arg + sizeof("--usarsim-port=")-1);

                } else if (!strncmp(arg, "--usarsim-robot=", sizeof("--usarsim-robot=")-1)) {
                        usarsim_robot = arg + sizeof("--usarsim-robot=")-1;

                } else if (!strncmp(arg, "--usarsim-location=", sizeof("--usarsim-location=")-1)) {
                        usarsim_location = arg + sizeof("--usarsim-location=")-1;

                } else if (!strncmp(arg, "--usarsim-ball=", sizeof("--usarsim-ball=")-1)) {
                        usarsim_ball_location = arg + sizeof("--usarsim-ball=")-1;

                } else if (!strncmp(arg, "--display-fps=", sizeof("--display-fps=")-1)) {
                        display_fps = !strncmp(arg + sizeof("--display-fps=")-1, "yes", sizeof("yes")-1);

                } else if (!strcmp(arg, "--help") || !strcmp(arg, "-h")) {
                        cout << "./USARSimServer [options] <MS dir>" << endl << endl
                        << "Available options:" << endl
                        << "--team=TEAM\t\t\tTeam of the robot (either BLUE, RED or NO_TEAM). defaults to '" << team_str() << "'" << endl
                        << "--usarsim-host=HOST\t\tHost of the USARSim server. defaults to '" << DEFAULT_USARSIM_HOST << "'" << endl
                        << "--usarsim-port=PORT\t\tPort of the USARSim server. defaults to " << DEFAULT_USARSIM_PORT << endl
                        << "--usarsim-robot=MODEL\t\tUSARSim Robot Model. defaults to '" << DEFAULT_USARSIM_ROBOT << "'" << endl
                        << "--usarsim-location=X,Y,Z\tCoordinates of the start position. defaults to '" << DEFAULT_USARSIM_LOCATION << "'" << endl
                        << "--usarsim-ball=[no|location]\tIf and where to place the ball. defaults to '" << DEFAULT_USARSIM_BALL_LOCATION << "'" << endl
                        << "--display-fps=[no|yes]\t\tDisplay image FPS received from the image server. defaults to '" << (DEFAULT_DISPLAY_IMAGE_FPS ? "yes" : "no") << "'" << endl
                        << endl;
                        return 0;

                } else if (argc == 1) { //MS dir
                        MSdir = arg;

                } else {
                        cerr << "option not recognised: '" << arg << "'" << endl;
                }
        }


        if (!MSdir) {
                cerr << "MS directory not set. exiting.." << endl;
                return 0x02;
        }

        cout << "OpenSDK USARSim connector loading..." << endl;

        actuators_socket = connect_to_opensdk_actuator(MSdir);
        int sensors_fd   = connect_to_opensdk_sensor(MSdir, SENSORS_SERVER_SOCKET);

        if (!connect_to_usarsim(&usarsim_socket)) {
                return 0x01;
        }

        sensors_socket = fdopen(sensors_fd, "w");
        if (!sensors_socket) {
                return 0x04;
        }

        // create a ball only if asked
        if (strcasecmp(usarsim_ball_location, "no")) {
                if (!connect_to_usarsim(&usarsim_ball_socket)) {
                        return 0x02;
                }

                send_usarsim(usarsim_ball_socket, "INIT {ClassName USARBot.SoccerBall} {Name Ball} {Location %s}\r\n", usarsim_ball_location);
        }

        {
        char skin[64];
        if (team == NO_TEAM) {
                *skin = '\0';
        } else {
                sprintf(skin, "{Skin %s} ", team_str());
        }

        // spawn a new robot
        send_usarsim(usarsim_socket, "INIT {ClassName USARBot.%s} %s{Location %s}\r\n", usarsim_robot, skin, usarsim_location);
        }

        // create a thread to receive data from USARSim
        pthread_t thread;
        pthread_create(&thread, NULL, read_usarsim_messages, NULL);

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

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

        pthread_create(&thread, NULL, read_usarsim_images, (void*)MSdir);


        // alloc the main thread to the other task
        read_opensdk_messages();

        return 0;
}

---