OpenTracker

An Open Architecture for Reconfigurable Tracking based on XML | Contact

MulticastInputModule.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 // a trick to avoid warnings when ace includes the STL headers
00044 #ifdef WIN32
00045 #pragma warning(disable:4786)
00046 #endif
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 "MulticastInputModule.h"
00055 #include "MulticastInputSource.h"
00056 
00057 #include <math.h>
00058 #include <stdio.h>
00059 #include <iostream>
00060 
00061 
00062 #ifndef OT_NO_NETWORK_SUPPORT
00063 
00064 
00065 namespace ot {
00066 
00067 
00068     static const float DEG_TO_RAD = (float)(3.14159/180.0);
00069 
00070 
00071     // -------------------------------------------------------------------------------------------------------
00072     // constructor initializing the thread manager
00073     MulticastInputModule::MulticastInputModule() : ThreadModule(), NodeFactory(), stop(0)
00074     {
00075 
00076     }
00077     // -------------------------------------------------------------------------------------------------------
00078 
00079     // Destructor method
00080 
00081     MulticastInputModule::~MulticastInputModule()
00082     {
00083         sources.clear();
00084     }
00085 
00086 
00087     // This method is called to construct a new Node.
00088 
00089     Node * MulticastInputModule::createNode( const std::string& name, StringTable& attributes)
00090     {
00091         if( name.compare("MulticastInputSource") == 0 )
00092         {
00093             stop = 0;
00094             int number;
00095             int num = sscanf(attributes.get("number").c_str(), " %i", &number );
00096             if( num == 0 )
00097             {
00098                 ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:Error in converting MulticastInputSource number !\n")));
00099                 return NULL;
00100             }
00101             NodeVector::iterator it;
00102             for( it = sources.begin(); it != sources.end(); it++ ){
00103                 MulticastInputSource * source = (MulticastInputSource*)(*it);
00104                 if( source->number == number )
00105                 {
00106                     break;
00107                 }
00108             }
00109             if( it != sources.end())
00110             {
00111                 ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:Source with number %d exists allready\n"), number));
00112                 return NULL;
00113             }
00114             MulticastInputSource * source = new MulticastInputSource( number);
00115             sources.push_back( source );
00116             ACE_DEBUG((LM_INFO, ACE_TEXT("ot:Built MulticastInputSource node. Number: %d\n"), number));
00117             return source;
00118         }
00119         return NULL;
00120     }
00121 
00122     // -------------------------------------------------------------------------------------------------------
00123 
00124     void MulticastInputModule::start()
00125     {
00126         if( isInitialized() && !sources.empty())
00127             ThreadModule::start();
00128     }
00129 
00130 
00131     // -------------------------------------------------------------------------------------------------------
00132 
00133     void MulticastInputModule::close()
00134     {
00135         lock();
00136         stop = 1;
00137         unlock();
00138     }
00139 
00140     // -------------------------------------------------------------------------------------------------------
00141 
00142     void MulticastInputModule::run()
00143     {
00144         ACE_Time_Value timeOut( 1, 0 );
00145         int retval;
00146         ACE_INET_Addr addr( port );
00147         //  ACE_Addr local( -1 , -1);
00148         socket = new ACE_SOCK_Dgram( addr );
00149 
00150         receiveBufferSize =  3 * sizeof(short int) + 8 * sizeof(StationRecord);
00151         receiveBuffer = new char[receiveBufferSize];
00152 
00153 
00154 
00155         std::string receiveString;
00156         // mainloop for reading data from the port
00157         StationRecord *recordTemp = new StationRecord[8];
00158 
00159         while(1)
00160         {
00161             do
00162             {
00163                 if( (retval = socket->recv( receiveBuffer, receiveBufferSize , addr, 0, &timeOut )) == -1 )
00164                 {
00165                     if(errno != ETIME && errno != 0)
00166                     {
00167                         ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:Error %d receiving data !\n"), errno));
00168                         exit( -1 );
00169                     }
00170                 }
00171             } while( retval < 0 && stop == 0);
00172 
00173             if( stop != 0 )
00174             {
00175                 break;
00176             }
00177 
00178             // from here the String is in the Buffer!
00179             // converts c-String into String
00180             //  printf("recv size %d\n",retval);
00181             //  printf(".");
00182             parseString(std::string(receiveBuffer, retval), recordTemp);
00183             NodeVector::iterator it;
00184 
00185             lock();
00186             for( it = sources.begin(); it != sources.end(); it++)
00187             {
00188 
00189                 MulticastInputSource * source = (MulticastInputSource*)(*it);
00190                 int bodyID = source->number;
00191                 if( recordTemp[bodyID].valid == 1 )
00192                 {
00193                     // Brings the locationdata from BodyRecordTemp to  source->event.position !
00194                     source->event.getPosition()[0] = recordTemp[bodyID].position[0];
00195                     source->event.getPosition()[1] = recordTemp[bodyID].position[1];
00196                     source->event.getPosition()[2] = recordTemp[bodyID].position[2];
00197                     //printf("%f %f %f\n",source->event.getPosition()[0], source->event.getPosition()[1], source->event.getPosition()[2]);
00198 
00199                     // Brings the calculated Quaternion Data from BodyRecordTemp to source->event.orientation !
00200                     MathUtils::eulerToQuaternion( recordTemp[bodyID].orientation[0], recordTemp[bodyID].orientation[1], recordTemp[bodyID].orientation[2], source->event.getOrientation() );
00201                     // Bring a timeStamp to source->event
00202                     source->event.timeStamp();
00203                     // Quality taken from the Datagramm (not used by DTrack in this Version of DTrack)
00204                     // fixed to 0.5 ??
00205                     source->event.getConfidence() = recordTemp[bodyID].confidence;
00206                     // Source was definitly changed !
00207                     source->changed = 1;
00208                 }
00209                 else
00210                 {
00211                     // only if marker was found in the last grab (event.getConfidence() > epsilon) set
00212                     // confidence to 0.0!
00213                     if (source->event.getConfidence() > 0.000001f)
00214                     {
00215                         source->changed = 1;
00216                         source->event.getConfidence() = 0.0f;
00217                     }
00218                 }
00219 
00220             }
00221             unlock();
00222         }
00223         socket->close();
00224     }
00225 
00226     // data package
00227     // short int version
00228     // short int numberOfStations
00229     // short int flags
00230     // stations[8]:
00231     // short int valid
00232     // short int stationId
00233     // float[3] position
00234     // float[4] rotation (quaternion)
00235     // short int buttonbytes
00236     // float     confidence
00237 
00238     void MulticastInputModule::parseString(std::string input, StationRecord *recordTemp)
00239     {
00240         //  assert(recordTemp!=NULL);
00241 
00242         const char *stream = input.c_str();
00243 
00244         version = ntohs(*((short int*) stream));
00245         stream +=sizeof(u_short);
00246         u_short numberOfStations = ntohs(*((short int*) stream));
00247         stream +=sizeof(u_short);
00248         u_short flags = ntohs(*((short int*) stream));
00249         stream +=sizeof(u_short);
00250 
00251         if (input.size()< 6+numberOfStations*(3*sizeof(u_short)+7*sizeof(float)))
00252         {
00253             printf("version %d numberOfStations %d flags %d\n",version, numberOfStations, flags);
00254             printf("network packet size is %d instead of %d\nEXIT.", (int)input.size(),  (int)(6+numberOfStations*sizeof(StationRecord)));
00255             exit(1);
00256         }
00257 
00258         if (version == 1) {
00259             for (int i=0; i<numberOfStations; i++) {
00260                 // short int valid
00261                 // short int stationId
00262                 // float[3] position
00263                 // float[3] rotation (quaternion)
00264                 // short int buttonbytes
00265                 // float     confidence
00266                 recordTemp[i].valid = ntohs(*((u_short*) stream));
00267                 stream +=sizeof(u_short);
00268                 recordTemp[i].id = ntohs(*((u_short*) stream));
00269                 stream +=sizeof(u_short);
00270 
00271                 /*  for (int j=0; j < 4 ; j++)
00272                         {
00273                         printf("%d ",stream[j]);
00274                         }
00275                         printf("\n");
00276 
00277                         u_long c = 0;
00278                         char buffer[4];
00279                         memcpy((void*)&c, (void*) stream, sizeof(u_long));
00280 
00281 
00282                         for (j=0; j < 4 ; j++)
00283                         {
00284                         printf("%d ",buffer[j]);
00285                         }
00286 
00287 
00288                         printf("\n-- %f -----\n", t);
00289                 */
00290                 union longToFloat {
00291                     long l;
00292                     float f;
00293                 } con;
00294 
00295                 con.l = ntohl(*((long int*) stream));
00296                 recordTemp[i].position[0] = con.f;
00297                 stream +=sizeof(u_long);
00298 
00299 
00300                 con.l = ntohl(*((long int*) stream));
00301                 recordTemp[i].position[1] = con.f;
00302                 stream +=sizeof(u_long);
00303 
00304                 con.l = ntohl(*((long int*) stream));
00305                 recordTemp[i].position[2] = con.f;
00306                 stream +=sizeof(u_long);
00307 
00308                 con.l = ntohl(*((long int*) stream));
00309                 recordTemp[i].orientation[0] = con.f;
00310                 stream +=sizeof(u_long);
00311 
00312                 con.l = ntohl(*((long int*) stream));
00313                 recordTemp[i].orientation[1] = con.f;
00314                 stream +=sizeof(u_long);
00315 
00316                 con.l = ntohl(*((long int*) stream));
00317                 recordTemp[i].orientation[2] = con.f;
00318                 stream +=sizeof(u_long);
00319 
00320                 recordTemp[i].buttons = ntohs(*((u_short*) stream));
00321                 stream +=sizeof(u_short);
00322 
00323                 con.l = ntohl(*((long int*) stream));
00324                 recordTemp[i].confidence = con.f;
00325                 stream +=sizeof(u_long);
00326                 /*
00327                   printf("%d %d - %f/%f/%f - %f/%f/%f/%f - %d - %f\n",
00328                   recordTemp[i].valid,
00329                   recordTemp[i].id,
00330                   recordTemp[i].position[0],
00331                   recordTemp[i].position[1],
00332                   recordTemp[i].position[2],
00333                   recordTemp[i].orientation[0],
00334                   recordTemp[i].orientation[1],
00335                   recordTemp[i].orientation[2],
00336                   recordTemp[i].orientation[3],
00337                   recordTemp[i].buttons,
00338                   recordTemp[i].confidence);
00339                 */
00340             }
00341         } else {
00342             printf("received wrong version %d instead of 1. EXIT.\n",version);
00343             exit(1);
00344         }
00345     }
00346 
00347     // pushes events into the tracker tree
00348     void MulticastInputModule::pushEvent()
00349     {
00350         if( isInitialized() )
00351         {
00352             for( NodeVector::iterator it = sources.begin(); it != sources.end(); it++ )
00353             {
00354                 MulticastInputSource *source = (MulticastInputSource *) *it;
00355                 lock();
00356                 if( source->changed == 1 )
00357                 {
00358                     source->updateObservers( source->event );
00359                     source->changed = 0;
00360                 }
00361                 unlock();
00362             }
00363         }
00364     }
00365 
00366     // -------------------------------------------------------------------------------------------------------
00367 
00368     // initializes the ARTDataTracker module
00369 
00370     void MulticastInputModule::init(StringTable& attributes, ConfigNode * localTree)
00371     {
00372         ThreadModule::init( attributes, localTree );
00373         int num;
00374 
00375         // Scanning port number from XML-File
00376         num = sscanf(attributes.get("port").c_str(), " %i", &port );
00377         if( num == 0 )
00378         {
00379             port = 12348;
00380         }
00381 
00382     }
00383 
00384 
00385 }  // namespace ot
00386 
00387 
00388 #else
00389 #pragma message(">>> OT_NO_NETWORK_SUPPORT")
00390 #endif // OT_NO_NETWORK_SUPPORT
00391 
00392 /* 
00393  * ------------------------------------------------------------
00394  *   End of MulticastInputModule.cxx
00395  * ------------------------------------------------------------
00396  *   Automatic Emacs configuration follows.
00397  *   Local Variables:
00398  *   mode:c++
00399  *   c-basic-offset: 4
00400  *   eval: (c-set-offset 'substatement-open 0)
00401  *   eval: (c-set-offset 'case-label '+)
00402  *   eval: (c-set-offset 'statement 'c-lineup-runin-statements)
00403  *   eval: (setq indent-tabs-mode nil)
00404  *   End:
00405  * ------------------------------------------------------------ 
00406  */

copyright (c) 2006 Graz University of Technology