vos/corelibs/tracking/trackerd.cc

Go to the documentation of this file.

/* $Id: trackerd.cc,v 1.14 2003/04/24 00:21:13 reed Exp $
 *
    Copyright (C) 2002 Reed Hedges

    This software was written at the University of Massachusetts with
    support from NSF grant #EIA 9703217

    This program 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; either version 2 of the License, or
    (at your option) any later version.

    This program 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 this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

/** @file trackerd.cc 
 * This is a simple tracker server: it listens on a socket for commands, reads
 * data from the a position or orientation tracker, and returns the data.
 * This allows multiple programs to use data from the same tracker.
 *
 * @author Reed Hedges, August 2002
 *
 * Currently supported position trackers are:
 *  GPSD (Remco Treffkorn's GPS daemon: <http://russnelson.com/gpsd/>)
 *
 * Currently supported orientaiton trackers are:
 *  Intersense IS300 on a serial port 
 *  (soon GPS compass and GPS derived)
 *
 * Commands are:
 *  om:0  Read orientation matrix from tracker #0. 
 *      Returns: om:0=T11,T12,T13;T21,T22,T23;T31,T32,T33
 *  oa:0  Read orientation angles from tracker #0.
 *      Returns: oa:0=YAW,PITCH,ROLL 
 *  p:0  Read position from tracker #0.
 *      Returns: p:0=X,Y,Z
 *
 *  All values are double-precision floating point numbers, as 
 *  output by printf("%.8f"). (use "%lf" with scanf)
 *
 *  Currently only one orientation tracker (o?:0) and one position
 *  tracker (p:0) are possible.
 *
 * @TODO  support multiple trackers
 * @TODO  add commands to set tracker calibrations (offsets and scale factors)
 * @TODO  read tracker info from config file
 * @BUG   no timeout if intersense does not respond (bug in
 * IntersenseOrientation)
 */


#include <sys/types.h>
#include <netdb.h>
#include <unistd.h>
#include <sys/time.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <signal.h>
#include <errno.h>
#include <set>
#include <iostream>
#include "orientation.hh"
#include "intersenseorientation.hh"
#include "position.hh"
#include "gpsd_position.hh"


using namespace std;

#ifdef __APPLE__
typedef unsigned int socklen_t;
#endif


/** Default socket port */
#define DEFAULT_PORT 2948


/** Return current time as seconds since unix epoc */
double currentTime() {
    static struct timeval tv;
    gettimeofday(&tv, NULL);
    return  ( tv.tv_sec + (tv.tv_usec / 1000000.0) );
}

/** Print summary of command-line options. */
void usage() {
    cout << "usage: trackerd [-P PORT] [-o OTYPE] [-p PTYPE] [-od DEV] [-os SPEED] [-ph HOST] [-pp PORT]\n" <<
        "\t-P PORT\tuse PORT for socket port (default is " << DEFAULT_PORT << "\n" <<
        "\t-o TYPE\tuse TYPE for orientation device 0 (intersense, fake)\n" <<
        "\t-p TYPE\tuse TYPE for position device 0 (gpsd, fake)\n" <<
        "\t-od DEV\tuse DEV for orientation device name (Default /dev/isense)\n" <<
        "\t-os SPEED\tuse SPEED baud for orientation device. (Default 115200)\n" <<
        "\t-of FREQ\tminimum tracker read frequency in seconds (Default 0.04)\n" << 
        "\t-ph HOST\tuse HOST for for position server host\n" <<
        "\t-pp PORT\tuse PORT for position server port\n" <<
        "\t-pf FREQ\tmininum tracker read frequency in seconds (Default is 0.04)\n" <<
        endl;
}

/** Testing */
//@{
class FakeOrientation : public Orientation {
public:
    virtual void getOrientationMatrix(double m[3][3]) {
        m[0][0] = m[0][1] = m[0][2] =
        m[1][0] = m[1][1] = m[1][2] = 
        m[2][0] = m[2][1] = m[2][2] = 42.0;
    }

    virtual void getEulerAngles(double* y, double* p, double* r) {
        *y = *p = *r = 23.0;
    }
};

class FakePosition : public Position {
public:
    virtual void getPosition(double* x, double* y, double* z) {
        *x = *y = *z = 0;
    }
};
//@}



/** Main */
int main(int argc, char** argv) {
    int port = DEFAULT_PORT;
    char* ori_type = 0;
    char* ori_dev = "/dev/isense";
    int ori_devspeed = 115200;  // baud
    Orientation* ori = NULL;
    char* pos_host = "localhost";
    int pos_port = GPSD_DEFAULT_PORT;
    char* pos_type = 0;
    Position* pos = NULL;
    double ori_freq = 0.04;     // seconds
    double pos_freq = 0.04;     // seconds
    double last_ori_read = 0;   // sec
    double last_pos_read = 0;   // sec


    /* Check parameters */

    for(int i = 1; i < argc; i++) {
        if(!strcmp(argv[i], "-o")) {
            ori_type = strdup(argv[++i]);
        } else if(!strcmp(argv[i], "-od")) {
            ori_dev = strdup(argv[++i]);
        } else if(!strcmp(argv[i], "-os")) {
            ori_devspeed = atoi(argv[++i]);
        } else if (!strcmp(argv[i], "-P")) {
           port = atoi( argv[++i] ); 
        } else if (!strcmp(argv[i], "-p")) {
            pos_type = strdup(argv[++i]);
        } else if (!strcmp(argv[i], "-ph")) {
            pos_host = strdup(argv[++i]);
        } else if (!strcmp(argv[i], "-pp")) {
            pos_port = atoi(argv[++i]);
        } else if (!strcmp(argv[i], "-of")) {
            ori_freq = atof(argv[++i]);
        } else if (!strcmp(argv[i], "-pf")) {
            pos_freq = atof(argv[++i]);
        } else {
            usage();
            exit(0);
        }
    }

    cerr << "creating tracking interfaces... ori_type = " << string( ori_type?ori_type:"NULL" ) << "  pos_type = " << string( pos_type?pos_type:"NULL" ) << endl;

    /* Create tracker objects */

    try {
        if(ori_type == 0)
            ori = 0;
        else if(strcmp(ori_type, "fake")==0)
            ori = new FakeOrientation();
#ifdef HAVE_INTERSENSE
        else if(strcmp(ori_type, "intersense") == 0)
            ori = new IntersenseOrientation(ori_dev, ori_devspeed);
#endif
        else {
            cerr << "Unrecognized orientation tracker type. Use \"-o intersense\" or \"-o fake\".\n";
            exit(-1);
        }
    } catch(exception& e) {
        cerr << "trackerd: Error connecting to " << ori_type << " orientation tracker device " << ori_dev << " at " << ori_devspeed << " baud! " << e.what();
        exit(-2);
    }


    try {
        if(pos_type == 0)
            pos = 0;
        else if(strcmp(pos_type, "fake")==0)
            pos = new FakePosition();
        else if(strcmp(pos_type, "gpsd") == 0)
            pos = new GPSDPosition(pos_host, pos_port);
        else {
            cerr << "Unrecognized position tracker type.  Use \"-p gpsd\" or \"-p fake\".\n";
            exit(-1);
        }
    } catch(exception& e) {
        cerr << "trackerd: Error connecting to " << pos_type << " position tracker server at " << pos_host << " port " << pos_port << "! " << e.what();
        exit(-2);
    }

    /* Start server */

    cout << "trackerd: Opening tcp socket at port " << port << "...\n";
    int server_socket = socket(AF_INET, SOCK_STREAM,  getprotobyname("tcp")->p_proto);

    cerr << "socket is: " << server_socket << endl;

    if(server_socket < 0) {
        cerr << "trackerd: error creating socket: " << strerror(errno) << "\n";
        exit(-1);
    }

    int one = 1;
    if(setsockopt(server_socket, SOL_SOCKET, SO_REUSEADDR, (char *) &one, sizeof(int)) < 0) {
        cerr << "trackerd: setting socket options: " << strerror(errno) << endl ; exit(-1);
    }
    struct sockaddr_in addr;
    memset(&addr, 0, sizeof(addr));
    addr.sin_addr.s_addr = htonl(INADDR_ANY);
    addr.sin_port = htons(port);
    addr.sin_family = AF_INET;
    if(bind(server_socket, (struct sockaddr*)&addr, sizeof(addr)) < 0) {
        cerr << "trackerd: error binding address with port "  << port << ": " << strerror(errno) << endl;
        exit(-1);
    }
    if( listen(server_socket, 5) < 0) {
        cerr << "trackerd: error listening on socket: " << strerror(errno) << endl;
    } 

    cout << "trackerd: ok. running...\n";

    char buff[8];
    memset(buff, 0, sizeof(buff));
    char output[128];
    memset(output, 0, sizeof(output));
    double mat[3][3];
    double y, p, r, px, py, pz;
    int n;
    set<int> client_sockets;
    fd_set rfds;
    fd_set wfds;
    int max;

    // do an initial read and save time
    if(ori) {
        try {
            ori->getOrientationMatrix(mat);
            ori->getEulerAngles(&y, &p, &r);
            last_ori_read = currentTime();
        } catch(exception& e) {
            cerr << "trackerd: Warning: error reading initial values from orientation device.\n";
        }
    } 
    if(pos) {
        try {
            pos->getPosition(&px, &py, &pz);
            last_pos_read = currentTime();
        } catch(exception& e) {
            cerr << "trackerd: Warning: error reading initial values from position device.\n";
        }
    }

    while(1) {

        /* Check for activity on sockets */
        FD_ZERO(&rfds);
        FD_ZERO(&wfds);
        FD_SET(server_socket, &rfds);
        max = server_socket;
        for(set<int>::const_iterator i = client_sockets.begin();
                                    i != client_sockets.end(); i++) {
            FD_SET(*i, &rfds);
            FD_SET(*i, &wfds);
            if(*i > max) max = *i;
        }
        if(select(max+1, &rfds, &wfds, NULL, NULL)) {

            /* Accept new connections? */
            if( FD_ISSET(server_socket, &rfds) ) {
                int s = accept(server_socket, NULL, 0);
                if(s == -1) {
                    cerr << "trackerd: warning: error accepting client: " << strerror(errno) << "\n"; 
                    continue;
                }
                client_sockets.insert(s);
            }

            /* Check clients for data */
            for(set<int>::const_iterator i = client_sockets.begin();
                                    i != client_sockets.end(); i++) {
                if( FD_ISSET(*i, &rfds) && FD_ISSET(*i, &wfds) ) {
                    int fd = *i;

                    memset(buff, 0, sizeof(buff));
                    n = read( fd, buff, sizeof(buff) - 1);
                    //XXX if there's more than 8 bytes on the socket, flush them?
                    if(n > 0) {
                        // remove last char if it's a newline
                        if( buff[n-1] == '\n' ) 
                            buff[n-1] = '\0';
                        // check input and construct output
                        if(!strcmp(buff, "om:0")) {
                            if(ori) {
                                try {
                                    double now =  currentTime();
                                    if(now - last_ori_read > ori_freq) {
                                        ori->getOrientationMatrix(mat);
                                        last_ori_read = now;
                                    }
                                    snprintf(output, sizeof(output), "om:0=%.8f,%.8f,%.8f;%.8f,%.8f,%.8f;%.8f,%.8f,%.8f\n",
                                          mat[0][0], mat[0][1], mat[0][2],
                                          mat[1][0], mat[1][1], mat[1][2],
                                          mat[2][0], mat[2][1], mat[2][2]);
                                } catch(exception& e) {
                                    snprintf(output, sizeof(output), "om:0=ERROR:Error reading device: %s\n", e.what());
                                }
                            } else {
                                snprintf(output, sizeof(output), "om:0=ERROR:No device\n");
                            }
                        } else if(!strcmp(buff, "oa:0")) {
                            if(ori) {
                                try {
                                    double now =  currentTime();
                                    if(now - last_ori_read > ori_freq) {
                                        ori->getEulerAngles(&y, &p, &r);
                                        last_ori_read = now;
                                    }
                                    snprintf(output, sizeof(output), "oa:0=%.8f,%.8f,%.8f\n", y, p, r);
                                } catch(exception& e) {
                                    snprintf(output, sizeof(output), "oa:0=ERROR:Error reading device: %s\n", e.what());
                                }
                            } else {
                                snprintf(output, sizeof(output), "oa:0=ERROR:No device\n");
                            }
                        } else if(!strcmp(buff, "p:0")) {
                            if(pos) {
                                try {
                                    double now = currentTime();
                                    if(now - last_pos_read > pos_freq) {
                                        pos->getPosition(&px, &py, &pz);
                                        last_pos_read = now;
                                    }
                                    snprintf(output, sizeof(output), "p:0=%.8f,%.8f,%.8f\n", px, py, pz);
                                } catch(exception& e) {
                                    snprintf(output, sizeof(output), "p:0=ERROR:Error reading device: %s\n", e.what());
                                }
                            } else {
                                snprintf(output, sizeof(output), "p:0=ERROR:No device\n");
                            }
                        } else {
                            snprintf(output, sizeof(output), "%s=ERROR:Unknown command\n", buff);
                        }

                        write(fd, output, strlen(output));

                    } else if(n == 0) {   // socket it closed?
            //            fprintf(stderr, "closing socket %d",  fd);
                        close(fd);
                        client_sockets.erase(fd);
                    } else {
                        fprintf(stderr, "trackerd: Error reading from fd %d.  closing socket.", fd);
                        close(fd);
                        client_sockets.erase(fd);
                    }// if read
                } // if FD_SET
            } // for each client_socket
        } //if select
    } // while(1)
} // main

---