OpenTracker

An Open Architecture for Reconfigurable Tracking based on XML | Contact

ARTDataTrackerModule.cxx

Go to the documentation of this file.
00001 /* ========================================================================
00002  * Copyright (c) 2006,
00003  * Institute for Computer Graphics and Vision
00004  * Graz University of Technology
00005  * All rights reserved.
00006  *
00007  * Redistribution and use in source and binary forms, with or without
00008  * modification, are permitted provided that the following conditions are
00009  * met:
00010  *
00011  * Redistributions of source code must retain the above copyright notice,
00012  * this list of conditions and the following disclaimer.
00013  *
00014  * Redistributions in binary form must reproduce the above copyright
00015  * notice, this list of conditions and the following disclaimer in the
00016  * documentation and/or other materials provided with the distribution.
00017  *
00018  * Neither the name of the Graz University of Technology nor the names of
00019  * its contributors may be used to endorse or promote products derived from
00020  * this software without specific prior written permission.
00021  *
00022  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
00023  * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
00024  * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
00025  * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER
00026  * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
00027  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
00028  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
00029  * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00030  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
00031  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00032  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00033  * ========================================================================
00034  * PROJECT: OpenTracker
00035  * ======================================================================== */
00042 /* ======================================================================= */
00043 
00044 // this will remove the warning 4786
00045 #include "../tool/disable4786.h"
00046 
00047 #include <stdlib.h>
00048 #include <string>
00049 #include <ace/INET_Addr.h>
00050 #include <ace/SOCK_Dgram.h>
00051 #include <ace/Time_Value.h>
00052 #include <ace/Log_Msg.h>
00053 
00054 #include "ARTDataTrackerModule.h"
00055 #include "ARTDataTrackerSource.h"
00056 #include "ARTDataTrackerChomp.h"
00057 
00058 #include <math.h>
00059 #include <stdio.h>
00060 #include <iostream>
00061 
00062 
00063 #ifndef OT_NO_ARTDATATRACKER_SUPPORT
00064 
00065 
00066 namespace ot {
00067 
00068     static const float DEG_TO_RAD = (float)(3.14159/180.0);
00069 
00070     // -------------------------------------------------------------------------------------------------------
00071     // constructor initializing the thread manager
00072     ARTDataTrackerModule::ARTDataTrackerModule() : ThreadModule(), NodeFactory(), stop(0), DataTracker(NULL)
00073     {
00074 
00075     }
00076     // -------------------------------------------------------------------------------------------------------
00077 
00078     // Destructor method
00079 
00080     ARTDataTrackerModule::~ARTDataTrackerModule()
00081     {
00082     if (DataTracker)
00083         delete (DataTracker);
00084     sources.clear();
00085     }
00086 
00087 
00088     // This method is called to construct a new Node.
00089 
00090     Node * ARTDataTrackerModule::createNode( const std::string& name, StringTable& attributes)
00091     {
00092     if( name.compare("ARTDataTrackerSource") == 0 )
00093         {
00094         stop = 0;
00095         int number;
00096         int num = sscanf(attributes.get("number").c_str(), " %i", &number );
00097 
00098         if (attributes.get("type") == "3d" )
00099             {
00100             number += 20;
00101             }
00102         else if (attributes.get("type") == "fly" )
00103             {
00104             number += 40;
00105             }
00106         else if (attributes.get("type") == "pen" )
00107             {
00108             number += 60;
00109             }
00110 
00111         if( num == 0 )
00112             {
00113             ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:Error in converting ARTDataTrackerSource number !\n")));
00114             return NULL;
00115             }
00116 
00117         NodeVector::iterator it;
00118         for( it = sources.begin(); it != sources.end(); it++ ){
00119             ARTDataTrackerSource * source = (ARTDataTrackerSource*)(*it);
00120             if( source->number == number )
00121             {
00122                 break;
00123             }
00124         }
00125         if( it != sources.end())
00126             {
00127             ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:Source with number %d already exists\n"), number));
00128             return NULL;
00129             }
00130         ARTDataTrackerSource * source = new ARTDataTrackerSource( number);
00131         sources.push_back( source );
00132         ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:Built ARTDataTrackerSource node. Number: %d\n"), number));
00133         return source;
00134         }
00135     return NULL;
00136     }
00137 
00138     // -------------------------------------------------------------------------------------------------------
00139 
00140     void ARTDataTrackerModule::start()
00141     {
00142     if( isInitialized() && !sources.empty())
00143         ThreadModule::start();
00144     }
00145 
00146 
00147     // -------------------------------------------------------------------------------------------------------
00148 
00149     void ARTDataTrackerModule::close()
00150     {
00151     lock();
00152     stop = 1;
00153     unlock();
00154     }
00155 
00156     // -------------------------------------------------------------------------------------------------------
00157 
00158     void ARTDataTrackerModule::run()
00159     {
00160     ACE_Time_Value timeOut( 1, 0 );
00161 
00162     int retval;
00163 
00164     ACE_INET_Addr addr( port );
00165     ACE_Addr local( -1 , -1);
00166     socket = new ACE_SOCK_Dgram( addr );
00167 
00168     receiveBufferSize =  3 * sizeof(long) + 20 * sizeof(ARTDataTrackerChomp::BodyRecord) +
00169         20 * sizeof(ARTDataTrackerChomp::MarkerRecord);
00170     20 * sizeof(ARTDataTrackerChomp::FlystickRecord);
00171     20 * sizeof(ARTDataTrackerChomp::MeasuretargetRecord);
00172 
00173     receiveBuffer = new char[receiveBufferSize];
00174 
00175     std::string receiveString;
00176     // mainloop for reading data from the port
00177 
00178     while (1)
00179         {
00180         do
00181             {
00182             if( (retval = socket->recv( receiveBuffer, receiveBufferSize , addr, 0, &timeOut )) == -1 )
00183                 {
00184                 if(errno != ETIME && errno != 0)
00185                     {
00186                     ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:ERROR %d receiving data\n"), errno));
00187                     exit( -1 );
00188                     }
00189                 }
00190             } while( retval < 0 && stop == 0);
00191 
00192         if( stop != 0 )
00193             {
00194             break;
00195             }
00196 
00197         // from here the String is in the Buffer!
00198         // converts c-String into String
00199         receiveString = std::string(receiveBuffer, retval);
00200         // call .chomp Method from DataTrackerInstance to bring the received  String into a Record
00201         //endwin();
00202 
00203         DataTracker->chomp(receiveString);
00204         //DataTracker->displayRecords();
00205 
00206         // brings the Record from the ARTDataTrackerChomp class to the BodyRecordTemp Record
00207         std::map<int, ARTDataTrackerChomp::BodyRecord> & BodyRecordTemp = DataTracker->getBodyRecord();
00208         std::map<int, ARTDataTrackerChomp::MarkerRecord> & MarkerRecordTemp = DataTracker->getMarkerRecord();
00209         std::map<int, ARTDataTrackerChomp::FlystickRecord> & FlystickRecordTemp = DataTracker->getFlystickRecord();
00210         std::map<int, ARTDataTrackerChomp::MeasuretargetRecord> & MeasuretargetRecordTemp = DataTracker->getMeasuretargetRecord();
00211 
00212 
00213 
00214         NodeVector::iterator it;
00215 
00216         lock();
00217         for( it = sources.begin(); it != sources.end(); it++)
00218             {
00219             ARTDataTrackerSource * source = (ARTDataTrackerSource*)(*it);
00220             bodyID = source->number;
00221             if (bodyID < 20)
00222                 {
00223                 if( BodyRecordTemp[bodyID].valid == true )
00224                     {
00225                     // convert Data such as quaternion and euler-Angles
00226                     convert( BodyRecordTemp[bodyID] );
00227                     // Brings the locationdata from BodyRecordTemp to  source->event.position !
00228                     source->event.getPosition()[0] = BodyRecordTemp[bodyID].location[0];
00229                     source->event.getPosition()[1] = BodyRecordTemp[bodyID].location[1];
00230                     source->event.getPosition()[2] = BodyRecordTemp[bodyID].location[2];
00231                     // Brings the calculated Quaternion Data from BodyRecordTemp to source->event.orientation !
00232                     source->event.getOrientation()[0] = BodyRecordTemp[bodyID].orientation[0];
00233                     source->event.getOrientation()[1] = BodyRecordTemp[bodyID].orientation[1];
00234                     source->event.getOrientation()[2] = BodyRecordTemp[bodyID].orientation[2];
00235                     source->event.getOrientation()[3] = BodyRecordTemp[bodyID].orientation[3];
00236                     // Bring a timeStamp to source->event
00237                     source->event.timeStamp();
00238                     // Quality taken from the Datagramm (not used by DTrack in this Version of DTrack)
00239                     // fixed to 0.5 ??
00240                     source->event.getConfidence() = 0.5;
00241                     // Source was definitly changed !
00242                     source->changed = 1;
00243                     }
00244                 else
00245                     {
00246                     // only if marker was found in the last grab (event.getConfidence() > epsilon) set
00247                     // confidence to 0.0!
00248                     if (source->event.getConfidence() > 0.000001f)
00249                         {
00250                         source->changed = 1;
00251                         source->event.getConfidence() = 0.0f;
00252                         }
00253                     }
00254                 }
00255             else if (bodyID < 40)
00256                 {
00257                 if( MarkerRecordTemp[bodyID-20].valid == true )
00258                     {
00259                     // convert Data such as quaternion and euler-Angles
00260                     convert( BodyRecordTemp[bodyID-20] );
00261                     // Brings the locationdata from BodyRecordTemp to  source->event.position !
00262                     source->event.getPosition()[0] = MarkerRecordTemp[bodyID-20].location[0];
00263                     source->event.getPosition()[1] = MarkerRecordTemp[bodyID-20].location[1];
00264                     source->event.getPosition()[2] = MarkerRecordTemp[bodyID-20].location[2];
00265                     // Bring a timeStamp to source->event
00266                     source->event.timeStamp();
00267                     // Quality taken from the Datagramm (not used by DTrack in this Version of DTrack)
00268                     // fixed to 0.5 ??
00269                     source->event.getConfidence() = 0.5;
00270                     // Source was definitly changed !
00271                     source->changed = 1;
00272                     }
00273                 else
00274                     {
00275                     // only if marker was found in the last grab (event.getConfidence() > epsilon) set
00276                     // confidence to 0.0!
00277                     if (source->event.getConfidence() > 0.000001f)
00278                         {
00279                         source->changed = 1;
00280                         source->event.getConfidence() = 0.0f;
00281                         }
00282                     }
00283                 }
00284             else if (bodyID < 60)
00285                 {
00286                 if( FlystickRecordTemp[bodyID-40].valid == true )
00287                     {
00288                     // convert Data such as quaternion and euler-Angles
00289                     convert( FlystickRecordTemp[bodyID-40] );
00290                     // Brings the locationdata from BodyRecordTemp to  source->event.position !
00291                     source->event.getPosition()[0] = FlystickRecordTemp[bodyID-40].location[0];
00292                     source->event.getPosition()[1] = FlystickRecordTemp[bodyID-40].location[1];
00293                     source->event.getPosition()[2] = FlystickRecordTemp[bodyID-40].location[2];
00294                     // Brings the calculated Quaternion Data from BodyRecordTemp to source->event.orientation !
00295                     source->event.getOrientation()[0] = FlystickRecordTemp[bodyID-40].orientation[0];
00296                     source->event.getOrientation()[1] = FlystickRecordTemp[bodyID-40].orientation[1];
00297                     source->event.getOrientation()[2] = FlystickRecordTemp[bodyID-40].orientation[2];
00298                     source->event.getOrientation()[3] = FlystickRecordTemp[bodyID-40].orientation[3];
00299                     // buttons ...
00300                     source->event.getButton() = (unsigned short)FlystickRecordTemp[bodyID-40].buttons;
00301                     // Bring a timeStamp to source->event
00302                     source->event.timeStamp();
00303                     // Quality taken from the Datagramm (not used by DTrack in this Version of DTrack)
00304                     // fixed to 0.5 ??
00305                     source->event.getConfidence() = 0.5;
00306                     // Source was definitly changed !
00307                     source->changed = 1;
00308                     }
00309                 else
00310                     {
00311                     // only if marker was found in the last grab (event.getConfidence() > epsilon) set
00312                     // confidence to 0.0!
00313                     if (source->event.getConfidence() > 0.000001f)
00314                         {
00315                         source->changed = 1;
00316                         source->event.getConfidence() = 0.0f;
00317                         }
00318                     }
00319                 }
00320             else if (bodyID < 80)
00321                 {
00322                 if( MeasuretargetRecordTemp[bodyID-60].valid == true )
00323                     {
00324                     ACE_DEBUG((LM_ERROR, ACE_TEXT("---------\n"), errno));
00325                     // convert Data such as quaternion and euler-Angles
00326                     convert( MeasuretargetRecordTemp[bodyID-60] );
00327                     // Brings the locationdata from BodyRecordTemp to  source->event.position !
00328                     source->event.getPosition()[0] = MeasuretargetRecordTemp[bodyID-60].location[0];
00329                     source->event.getPosition()[1] = MeasuretargetRecordTemp[bodyID-60].location[1];
00330                     source->event.getPosition()[2] = MeasuretargetRecordTemp[bodyID-60].location[2];
00331                     // Brings the calculated Quaternion Data from BodyRecordTemp to source->event.orientation !
00332                     source->event.getOrientation()[0] = MeasuretargetRecordTemp[bodyID-60].orientation[0];
00333                     source->event.getOrientation()[1] = MeasuretargetRecordTemp[bodyID-60].orientation[1];
00334                     source->event.getOrientation()[2] = MeasuretargetRecordTemp[bodyID-60].orientation[2];
00335                     source->event.getOrientation()[3] = MeasuretargetRecordTemp[bodyID-60].orientation[3];
00336                     // buttons ...
00337                     source->event.getButton() = (unsigned short)MeasuretargetRecordTemp[bodyID-60].buttons;
00338                     // Bring a timeStamp to source->event
00339                     source->event.timeStamp();
00340                     // Quality taken from the Datagramm (not used by DTrack in this Version of DTrack)
00341                     // fixed to 0.5 ??
00342                     source->event.getConfidence() = 0.5;
00343                     // Source was definitly changed !
00344                     source->changed = 1;
00345                     }
00346                 else
00347                     {
00348                     // only if marker was found in the last grab (event.getConfidence() > epsilon) set
00349                     // confidence to 0.0!
00350                     if (source->event.getConfidence() > 0.000001f)
00351                         {
00352                         source->changed = 1;
00353                         source->event.getConfidence() = 0.0f;
00354                         }
00355                     }
00356                 }
00357             else
00358                 {
00359                 }
00360 
00361             } // for ...
00362 
00363         unlock();
00364 
00365         } // while(1)
00366 
00367     socket->close();
00368     }
00369 
00370 
00371     // -------------------------------------------------------------------------------------------------------
00372 
00373     void ARTDataTrackerModule::convert( ARTDataTrackerChomp::BodyRecord & BodyRecordTemp )
00374     {
00375     //  static const float DEG_TO_RAD = (3.14159/180.0);
00376     float m[3][3];
00377     //  set the matrix m with the values from our BodyRecordTemp.rotationMatrix
00378     //  as discribed in the DTrack Technical Appendix B the matrix is formed like this:
00379     //  in the ASCII String the rotation Data is like this: [b0 b1 b2 b3 b4 b5 b6 b7 b8]
00380     //  this formes the Rotation Matrix R:
00381     //      |b0 b3 b6|
00382     //  R = |b1 b4 b7|
00383     //      |b2 b5 b8|
00384     m[0][0] = BodyRecordTemp.rotationMatrix[0];
00385     m[0][1] = BodyRecordTemp.rotationMatrix[3];
00386     m[0][2] = BodyRecordTemp.rotationMatrix[6];
00387     m[1][0] = BodyRecordTemp.rotationMatrix[1];
00388     m[1][1] = BodyRecordTemp.rotationMatrix[4];
00389     m[1][2] = BodyRecordTemp.rotationMatrix[7];
00390     m[2][0] = BodyRecordTemp.rotationMatrix[2];
00391     m[2][1] = BodyRecordTemp.rotationMatrix[5];
00392     m[2][2] = BodyRecordTemp.rotationMatrix[8];
00393 
00394     // convert to Quaternion Format result is given back in .orientation Array
00395     MathUtils::matrixToQuaternion( m, BodyRecordTemp.orientation);
00396 
00397     }
00398 
00399     void ARTDataTrackerModule::convert( ARTDataTrackerChomp::FlystickRecord & FlystickRecordTemp )
00400     {
00401     //  static const float DEG_TO_RAD = (3.14159/180.0);
00402     float m[3][3];
00403     //  set the matrix m with the values from our BodyRecordTemp.rotationMatrix
00404     //  as discribed in the DTrack Technical Appendix B the matrix is formed like this:
00405     //  in the ASCII String the rotation Data is like this: [b0 b1 b2 b3 b4 b5 b6 b7 b8]
00406     //  this formes the Rotation Matrix R:
00407     //      |b0 b3 b6|
00408     //  R = |b1 b4 b7|
00409     //      |b2 b5 b8|
00410     m[0][0] = FlystickRecordTemp.rotationMatrix[0];
00411     m[0][1] = FlystickRecordTemp.rotationMatrix[3];
00412     m[0][2] = FlystickRecordTemp.rotationMatrix[6];
00413     m[1][0] = FlystickRecordTemp.rotationMatrix[1];
00414     m[1][1] = FlystickRecordTemp.rotationMatrix[4];
00415     m[1][2] = FlystickRecordTemp.rotationMatrix[7];
00416     m[2][0] = FlystickRecordTemp.rotationMatrix[2];
00417     m[2][1] = FlystickRecordTemp.rotationMatrix[5];
00418     m[2][2] = FlystickRecordTemp.rotationMatrix[8];
00419 
00420     // convert to Quaternion Format result is given back in .orientation Array
00421     MathUtils::matrixToQuaternion( m, FlystickRecordTemp.orientation);
00422 
00423     }
00424     void ARTDataTrackerModule::convert( ARTDataTrackerChomp::MeasuretargetRecord & MeasuretargetRecordTemp )
00425     {
00426     //  static const float DEG_TO_RAD = (3.14159/180.0);
00427     float m[3][3];
00428     //  set the matrix m with the values from our BodyRecordTemp.rotationMatrix
00429     //  as discribed in the DTrack Technical Appendix B the matrix is formed like this:
00430     //  in the ASCII String the rotation Data is like this: [b0 b1 b2 b3 b4 b5 b6 b7 b8]
00431     //  this formes the Rotation Matrix R:
00432     //      |b0 b3 b6|
00433     //  R = |b1 b4 b7|
00434     //      |b2 b5 b8|
00435     m[0][0] = MeasuretargetRecordTemp.rotationMatrix[0];
00436     m[0][1] = MeasuretargetRecordTemp.rotationMatrix[3];
00437     m[0][2] = MeasuretargetRecordTemp.rotationMatrix[6];
00438     m[1][0] = MeasuretargetRecordTemp.rotationMatrix[1];
00439     m[1][1] = MeasuretargetRecordTemp.rotationMatrix[4];
00440     m[1][2] = MeasuretargetRecordTemp.rotationMatrix[7];
00441     m[2][0] = MeasuretargetRecordTemp.rotationMatrix[2];
00442     m[2][1] = MeasuretargetRecordTemp.rotationMatrix[5];
00443     m[2][2] = MeasuretargetRecordTemp.rotationMatrix[8];
00444 
00445     // convert to Quaternion Format result is given back in .orientation Array
00446     MathUtils::matrixToQuaternion( m, MeasuretargetRecordTemp.orientation);
00447 
00448     }
00449 
00450 
00451 
00452     // -------------------------------------------------------------------------------------------------------
00453 
00454     // pushes events into the tracker tree
00455     void ARTDataTrackerModule::pushEvent()
00456     {
00457     if( isInitialized() )
00458         {
00459         for( NodeVector::iterator it = sources.begin(); it != sources.end(); it++ )
00460             {
00461             ARTDataTrackerSource *source = (ARTDataTrackerSource *) *it;
00462             lock();
00463             if( source->changed == 1 )
00464                 {
00465                 source->updateObservers( source->event );
00466                 source->changed = 0;
00467                 }
00468             unlock();
00469             }
00470         }
00471     }
00472 
00473     // -------------------------------------------------------------------------------------------------------
00474 
00475     // initializes the ARTDataTracker module
00476 
00477     void ARTDataTrackerModule::init(StringTable& attributes, ConfigNode * localTree)
00478     {
00479     ThreadModule::init( attributes, localTree );
00480     int num;
00481 
00482     // Scannig port number from XML-File
00483     num = sscanf(attributes.get("port").c_str(), " %i", &port );
00484     if( num == 0 )
00485         {
00486         port = 12345;
00487         }
00488 
00489     DataTracker = new ARTDataTrackerChomp();
00490     }
00491 
00492 } // namespace ot
00493 
00494 
00495 #else
00496 #pragma message(">>> OT_NO_ARTDATATRACKER_SUPPORT")
00497 #endif // OT_NO_ARTDATATRACKER_SUPPORT
00498 
00499 
00500 /* ===========================================================================
00501    End of ARTDataTrackerModule.cxx
00502    ===========================================================================
00503    Automatic Emacs configuration follows.
00504    Local Variables:
00505    mode:c++
00506    c-basic-offset: 4
00507    eval: (c-set-offset 'subeventment-open 0)
00508    eval: (c-set-offset 'case-label '+)
00509    eval: (c-set-offset 'statement 'c-lineup-runin-statements)
00510    eval: (setq indent-tabs-mode nil)
00511    End:
00512    =========================================================================== */

copyright (c) 2006 Graz University of Technology