vos/corelibs/tracking/trackerd_position.cc

Go to the documentation of this file.

/*
    This file is part of the Virtual Object System of
    the Interreality project (http://interreality.org).

    Copyright (C) 2001, 2002 Peter Amstutz, Reed Hedges

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

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 2 of the License, or (at your option) any later version.

    This library 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
    Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public
    License along with this library; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307  USA

    Peter Amstutz <tetron@interreality.org>
    Reed Hedges <reed@interreality.org>
*/


#include <sys/types.h>
#include <unistd.h>
#include <errno.h>
#include <string>
#include <stdio.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>
#include <errno.h>
#include <iostream>

#include "trackerd_position.hh"

using namespace std;

TrackerdPosition::TrackerdPosition(const char* host, unsigned int port, unsigned int tid) throw (DeviceError)
{
    id = tid;
    struct sockaddr_in address;
    if( (socket_fd = socket(PF_INET, SOCK_STREAM, 0)) == -1) {
        throw DeviceError(string("Error creating socket: ") + strerror(errno));
    }
    struct hostent* he;
    if(!(he = gethostbyname(host))) {
        throw DeviceError(string("Invalid address: ") + host);
    }
    memset(&address, 0, sizeof(address));
    address.sin_port=htons(port);
    address.sin_family=AF_INET;
    address.sin_addr=*((struct in_addr *)he->h_addr);
    if( connect(socket_fd, (struct sockaddr *)&address, sizeof(address)) == -1) {
        char port_s[16];
        snprintf(port_s, sizeof(port_s), "%d", port);
        throw DeviceError(string("Error connecting to server ") + host + ":" + port_s + ". " + strerror(errno));
    }
}

TrackerdPosition::~TrackerdPosition()
{
    shutdown(socket_fd, 2);
}

void TrackerdPosition::getPosition(double* x, double* y, double* z)  throw(DeviceError)
{
    char msg[256];
    unsigned int id = 0;
    if( write(socket_fd, "p:0", 4) != 4) {
        throw DeviceError( string("Error sending command to trackerd: ") + strerror(errno));
    }
    int nrecvd = recv(socket_fd, msg, 256, 0);
    //cerr << "Debug: got oa from trackerd: \"" << msg << "\"\n";
    if(sscanf(msg, "p:%u=%lf,%lf,%lf", &id, x, y, z) < 4) {
        throw DeviceError("Misparse of data from trackerd");
    }
    *x *= xScale;
    *y *= yScale;
    *z *= zScale;
    *x += xOffset;
    *y += yOffset;
    *z += zOffset;

}

---