vos/corelibs/tracking/trackerd_orientation.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_orientation.hh"

using namespace std;

TrackerdOrientation::TrackerdOrientation(const char* host, unsigned int port, unsigned int otid) throw (DeviceError)
{
    id = otid;
    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));
    }
}

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

void TrackerdOrientation::getOrientationMatrix(double mat[3][3]) throw(DeviceError)
{
    // TODO use write and read instead of send and recv?
    char msg[256];
    unsigned int id;
    if( write(socket_fd, "om:0", 4) != 4) {
        throw DeviceError( string("Error sending command to trackerd: ") + strerror(errno));
    }
    recv(socket_fd, msg, 256, 0);
    //cerr << "Debug: got om from trackerd: " << msg <<endl;
    if(sscanf(msg, "om:%u=%lf,%lf,%lf;%lf,%lf,%lf;%lf,%lf,%lf",
              &id,
              &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]) < 9)  
    {
        throw DeviceError("Misparse of data from trackerd");
    }
    // XXX TODO matrix offset
}

void TrackerdOrientation::getEulerAngles(double* yaw, double* pitch, double* roll)  throw(DeviceError)
{
    char msg[256];
    unsigned int id = 0;
    if( write(socket_fd, "oa:0", 4) != 4) {
        throw DeviceError( string("Error sending command to trackerd: ") + strerror(errno));
    }
    recv(socket_fd, msg, 256, 0);
    //cerr << "Debug: got oa from trackerd: \"" << msg << "\"\n";
    if(sscanf(msg, "oa:%u=%lf,%lf,%lf", &id, yaw, pitch, roll) < 4) {
        throw DeviceError("Misparse of data from trackerd");
    }
    *yaw += eulerYawOffset;
    *pitch += eulerPitchOffset;
    *roll += eulerRollOffset;

}

---