Equalizer  1.2.1
osgScaleViewer/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 
00032 #include <iostream>
00033 #include <fcntl.h>
00034 #include <errno.h>
00035 
00036 #ifndef WIN32
00037 #  include <sys/select.h>
00038 #  include <sys/termios.h>
00039 #endif
00040 
00041 #define COMMAND_POS_ANG "Y"
00042 #define COMMAND_POINT "B"
00043 
00044 Tracker::Tracker()
00045         : _running( false ),
00046           _worldToEmitter( eq::Matrix4f::IDENTITY ),
00047           _sensorToObject( eq::Matrix4f::IDENTITY )
00048 {
00049 }
00050 
00051 bool Tracker::init( const std::string& port )
00052 {
00053 #ifdef WIN32
00054     return false;
00055 #else
00056    if( _running )
00057    {
00058       EQERROR << "Duplicate tracker initialisation" << std::endl;
00059       return false;
00060    }
00061  
00062    _fd = open( port.c_str(), O_RDWR | O_EXCL );
00063    if( _fd < 0 )
00064    {
00065        EQERROR << "Failed to open " << port << ": " << co::base::sysError
00066                << std::endl;
00067       return false;
00068    }
00069 
00070    // configure serial port
00071    struct termios termio;
00072    if( tcgetattr( _fd, &termio ) != 0 )
00073    {
00074       EQERROR << "tcgetattr failed: " << co::base::sysError << 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       EQERROR << "tcsetattr failed: " << co::base::sysError << 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       EQERROR << "Write error: " << co::base::sysError << 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       EQERROR << "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       EQERROR << "Write error: " << co::base::sysError << std::endl;
00147       return false;
00148    }
00149 
00150    unsigned char buffer[12];
00151    if( !_read( buffer, 12, 500000 ))
00152    {
00153        EQERROR << "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    EQINFO << "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    EQINFO << "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          EQERROR << "Error: no data from tracker" << std::endl;
00228          return false;
00229       }
00230       if( errCode == -1 )
00231       {
00232          EQERROR << "Select error: " << co::base::sysError << 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          EQERROR << "Read error: " << co::base::sysError << std::endl;
00241          return false;
00242       }
00243 
00244       EQASSERT( remaining >= (size_t)received );
00245       remaining -= received;
00246    }
00247    return true;
00248 #endif
00249 }
Generated on Fri Jun 8 2012 15:44:32 for Equalizer 1.2.1 by  doxygen 1.8.0