Equalizer  1.4.1
eqPly/tracker.cpp
00001 
00002 /* Copyright (c) 2006, Dustin Wueest <wueest@dustin.ch> 
00003  * Copyright (c) 2006-2010, Stefan Eilemann <eile@equalizergraphics.com>
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  * - Redistributions of source code must retain the above copyright notice, this
00009  *   list of conditions and the following disclaimer.
00010  * - Redistributions in binary form must reproduce the above copyright notice,
00011  *   this list of conditions and the following disclaimer in the documentation
00012  *   and/or other materials provided with the distribution.
00013  * - Neither the name of Eyescale Software GmbH nor the names of its
00014  *   contributors may be used to endorse or promote products derived from this
00015  *   software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "tracker.h"
00031 #include <iostream>
00032 #include <fcntl.h>
00033 #include <errno.h>
00034 
00035 #ifndef WIN32
00036 #  include <sys/select.h>
00037 #  include <sys/termios.h>
00038 #endif
00039 
00040 #define COMMAND_POS_ANG "Y"
00041 #define COMMAND_POINT "B"
00042 
00043 namespace eqPly
00044 {
00045 Tracker::Tracker()
00046         : _running( false ),
00047           _worldToEmitter( eq::Matrix4f::IDENTITY ),
00048           _sensorToObject( eq::Matrix4f::IDENTITY )
00049 {
00050 }
00051 
00052 bool Tracker::init( const std::string& port )
00053 {
00054 #ifdef WIN32
00055     return false;
00056 #else
00057    if( _running )
00058    {
00059       LBERROR << "Duplicate tracker initialisation" << std::endl;
00060       return false;
00061    }
00062  
00063    _fd = open( port.c_str(), O_RDWR | O_EXCL );
00064    if( _fd < 0 )
00065    {
00066       LBERROR << "Failed to open " << port << ": " << strerror( errno ) << std::endl;
00067       return false;
00068    }
00069 
00070    // configure serial port
00071    struct termios termio;
00072    if( tcgetattr( _fd, &termio ) != 0 )
00073    {
00074       LBERROR << "tcgetattr failed: " << strerror( errno ) << std::endl;
00075       close( _fd );
00076       return false;
00077    }
00078 
00079    termio.c_cflag &= ~(CSIZE|PARENB|CSTOPB|PARODD|HUPCL|CRTSCTS); 
00080    termio.c_cflag |= CS8|CREAD|CLOCAL;
00081    termio.c_iflag &= ~(IXON|IXANY|IMAXBEL|BRKINT|IGNPAR|PARMRK|
00082                        INPCK|ISTRIP|INLCR|IGNCR|ICRNL);
00083 #ifdef IUCLC
00084    termio.c_iflag &= ~IUCLC;
00085 #endif
00086 
00087    termio.c_iflag |= IXOFF|IGNBRK;
00088    termio.c_oflag &= ~(OPOST|OCRNL|ONLCR|ONOCR|ONLRET|OFILL|OFDEL);
00089 #ifdef OLCUC
00090    termio.c_oflag &= ~OLCUC;
00091 #endif
00092 
00093    termio.c_lflag &= ~(ISIG|ICANON|IEXTEN|ECHO|ECHOE|ECHOK|ECHONL|NOFLSH|
00094                        TOSTOP|ECHOPRT|ECHOCTL|ECHOKE);
00095 #ifdef XCASE
00096    termio.c_lflag &= ~XCASE;
00097 #endif
00098 
00099    cfsetspeed( &termio, B115200 );
00100    termio.c_cc[VMIN]  = 26;
00101    termio.c_cc[VTIME] = 1;
00102 
00103    if( tcsetattr( _fd, TCSANOW, &termio ) != 0)
00104    {
00105       LBERROR << "tcsetattr failed: " << strerror( errno ) << std::endl;
00106       close( _fd );
00107       return false;
00108    }
00109 
00110    // tell the tracker what kind of data to prepare
00111    int k = write( _fd, COMMAND_POS_ANG, 1 ); //take data
00112    if( k==-1 )
00113       LBERROR << "Write error: " << strerror( errno ) << std::endl;
00114 
00115    usleep( 10000 ); //give enough time for initializing
00116    
00117    if( _update( )) //try an update to see if it works
00118        _running = true;
00119 
00120    return _running;
00121 #endif
00122 }
00123 
00124 bool Tracker::update()
00125 {
00126    if( !_running )
00127    {
00128       LBERROR << "Update error, tracker not running" << std::endl;
00129       return false;
00130    }
00131    else
00132    {
00133       bool b = _update();
00134       return b;
00135    }
00136 }
00137 
00138 bool Tracker::_update()
00139 {
00140 #ifdef WIN32
00141     return false;
00142 #else
00143     const ssize_t wrote = write( _fd, COMMAND_POINT, 1 ); // send data
00144    if( wrote==-1 )
00145    {
00146       LBERROR << "Write error: " << strerror( errno ) << std::endl;
00147       return false;
00148    }
00149 
00150    unsigned char buffer[12];
00151    if( !_read( buffer, 12, 500000 ))
00152    {
00153        LBERROR << "Read error" << std::endl;
00154        return false;
00155    }
00156 
00157    const short xpos = (buffer[1]<<8 | buffer[0]);
00158    const short ypos = (buffer[3]<<8 | buffer[2]);
00159    const short zpos = (buffer[5]<<8 | buffer[4]);
00160 
00161    const short head = (buffer[7]<<8 | buffer[6]);
00162    const short pitch = (buffer[9]<<8 | buffer[8]);
00163    const short roll = (buffer[11]<<8 | buffer[10]);
00164 
00165    // 32640 is 360 degrees (2pi) -> scale is 1/5194.81734
00166    const eq::Vector3f hpr( head  / -5194.81734f + M_PI,
00167                              pitch / -5194.81734f + 2.0f * M_PI,
00168                              roll  / -5194.81734f + 2.0f * M_PI );
00169 
00170    eq::Vector3f pos;
00171 
00172    // highest value for y and z position of the tracker sensor is 32639,
00173    // after that it switches back to zero (and vice versa if descending values).
00174    pos.x() = ypos;
00175    if( pos.x() > 16320 )             //32640 / 2 = 16320
00176       pos.x() -= 32640;
00177    
00178    pos.y() = zpos;
00179    if( pos.y() > 16320 )
00180       pos.y() -= 32640;
00181 
00182    pos.z() = xpos;
00183 
00184    pos /= 18000.f; // scale to meter
00185 
00186    // position and rotation are stored in transformation matrix
00187    // and matrix is scaled to the application's units
00188    _matrix = eq::Matrix4f::IDENTITY;
00189    _matrix.rotate_x( hpr.x() );
00190    _matrix.rotate_y( hpr.y() );
00191    _matrix.rotate_z( hpr.z() );
00192    _matrix.set_translation( pos );
00193 
00194    LBINFO << "Tracker pos " << pos << " hpr " << hpr << " = " << _matrix;
00195 
00196     // M = M_world_emitter * M_emitter_sensor * M_sensor_object
00197    _matrix = _worldToEmitter * _matrix * _sensorToObject;
00198 
00199    LBINFO << "Tracker matrix " << _matrix;
00200 
00201    return true;
00202 #endif
00203 }
00204 
00205 bool Tracker::_read( unsigned char* buffer, const size_t size,
00206                      const unsigned long int timeout )
00207 {
00208 #ifdef WIN32
00209     return false;
00210 #else
00211     size_t remaining = size;
00212    struct timeval tv;
00213 
00214    tv.tv_sec = timeout / 1000000;
00215    tv.tv_usec = timeout % 1000000;
00216 
00217    while( remaining > 0 )
00218    {
00219       // wait for data
00220       fd_set readfds;
00221       FD_ZERO( &readfds );
00222       FD_SET( _fd, &readfds );
00223 
00224       const int errCode = select( _fd+1, &readfds, 0, 0, &tv );
00225       if( errCode == 0 )
00226       {
00227          LBERROR << "Error: no data from tracker" << std::endl;
00228          return false;
00229       }
00230       if( errCode == -1 )
00231       {
00232          LBERROR << "Select error: " << strerror( errno ) << std::endl;
00233          return false;
00234       }
00235 
00236       // try to read remaining bytes, returns # of read bytes
00237       const ssize_t received = read( _fd, &buffer[size-remaining], remaining );
00238       if( received == -1 )
00239       {
00240          LBERROR << "Read error: " << strerror( errno ) << std::endl;
00241          return false;
00242       }
00243 
00244       LBASSERT( remaining >= (size_t)received );
00245       remaining -= received;
00246    }
00247    return true;
00248 #endif
00249 }
00250 }
Generated on Mon Nov 26 2012 14:41:49 for Equalizer 1.4.1 by  doxygen 1.7.6.1