OpenTracker

An Open Architecture for Reconfigurable Tracking based on XML | Contact

MagicYModule.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 "../tool/FixWinCE.h"
00049 #include <ace/Time_Value.h>
00050 
00051 #include "MagicYModule.h"
00052 
00053 #include <iostream>
00054 #include <string>
00055 
00056 #include <ace/Log_Msg.h>
00057 
00058 
00059 #ifndef OT_NO_MAGICY_SUPPORT
00060 
00061 
00062 namespace ot {
00063 
00064     // constructor initializing the thread manager
00065     MagicYModule::MagicYModule() : ThreadModule(), NodeFactory(), stop(0)
00066     {
00067 
00068     }
00069 
00070     // destructor cleans up any allocated memory
00071     MagicYModule::~MagicYModule()
00072     {
00073         unsigned int i;
00074         for (i=0; i<magicYs.size(); i++) delete magicYs[i];
00075         for (i=0; i<screens.size(); i++) delete screens[i];
00076     }
00077 
00078     // open all sockets
00079     int MagicYModule::connect()
00080     {
00081         int retval;
00082         char buffer[32];
00083         ACE_Time_Value timeOut(1,0);
00084 
00085         for(unsigned int i=0; i<screens.size(); i++)
00086         {
00087             retval = connector.connect(screens[i]->socket, screens[i]->address, &timeOut);
00088             if(retval == -1 && errno != ETIME && errno != 0 )
00089             {
00090                 ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:Error %d connection failed for socket nr.: %d\n"), errno, i));
00091                 return -1;
00092             }
00093             else
00094             {
00095                 ACE_DEBUG((LM_INFO, ACE_TEXT("ot:connected to socket nr.: %d - sending GO command\n"), i));
00096                 sprintf(buffer, "GO\n\r");
00097                 retval = screens[i]->socket.send_n(buffer, sizeof(buffer), &timeOut);
00098                 if(retval == -1 && errno != ETIME && errno != 0 )
00099                 {
00100                     ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:Error %d sending command for socket nr.: %d\n"), errno, i));
00101                     return -1;
00102                 }
00103             }
00104             screens[i]->connected = true;
00105         }
00106         return 0;
00107     }
00108 
00109     // set FD_SET for ACE::select function
00110     void MagicYModule::setSelect()
00111     {
00112         readHandles.reset();
00113         for(unsigned int i=0; i<screens.size(); i++)
00114             if(screens[i]->connected)
00115                 readHandles.set_bit(screens[i]->socket.get_handle());
00116     }
00117 
00118     // receive data from all active sockets
00119     int MagicYModule::receive()
00120     {
00121         ACE_Time_Value timeOut(0,5000);
00122         std::string message(""), accumulated("");
00123         char buffer[32], t='X';
00124         int retval, x, y, pos;
00125         size_t trans_bytes = 0;
00126         bool complete, trigger;
00127 
00128         points.clear();
00129         for(unsigned int i=0; i<screens.size(); i++)
00130         {
00131             complete = false;
00132             message.erase(message.begin(), message.end());
00133 
00134             if(readHandles.is_set(screens[i]->socket.get_handle()))
00135             {
00136                 do
00137                 {
00138                     retval = screens[i]->socket.recv_n(buffer, sizeof(buffer), &timeOut, (size_t*)&trans_bytes);
00139                     if(retval == -1 && errno != ETIME && errno != 0)
00140                     {
00141                         ACE_DEBUG((LM_ERROR, ACE_TEXT("ot:Error %d receiving command for socket nr.: %d\n"), errno, i));
00142                         return -1;
00143                     }
00144                     else
00145                     {
00146                         accumulated.append(buffer, trans_bytes);
00147                         pos = accumulated.find("\n\r", 0);
00148                         if (pos < 0)
00149                             pos = accumulated.find("\r\n", 0);
00150 
00151                         if (pos >= 0)
00152                         {
00153                             message.assign(accumulated, 0, pos);
00154                             accumulated.erase(0, pos+2);
00155                             complete = true;
00156                         }
00157                     }
00158                 } while(!complete && stop == 0);
00159 
00160                 if(message.compare("READY") && message.compare("0"))
00161                 {
00162                     pos = message.find(',', 0);
00163                     message.erase(0, pos+1);
00164                     // extract event and points
00165                     while (1)
00166                     {
00167                         if (sscanf(message.c_str(), "%c,%d,%d", &t, &x, &y) == 3)
00168                         {
00169                             if(t == 'Y')
00170                                 trigger = true;
00171                             else
00172                                 trigger = false;
00173 
00174                             x += screens[i]->x_offset;
00175                             y += screens[i]->y_offset;
00176 
00177                             points.push_back(MagicPoint(x,y,trigger));
00178 
00179                             pos = message.find(',', 0);
00180                             pos = message.find(',', pos+1);
00181                             pos = message.find(',', pos+1);
00182                             message.erase(0, pos+1);
00183 
00184                             if (pos < 0)
00185                                 break;
00186                         }
00187                         else
00188                             break;
00189                     }
00190                 }
00191             }
00192         }
00193         return 0;
00194     }
00195 
00196     // checks if all sockets are still connected by sending a PING
00197     int MagicYModule::stillConnected()
00198     {
00199         ACE_Time_Value timeOut(1,0);
00200         char buffer[16];
00201         int retval;
00202 
00203         sprintf(buffer, "PING\n\r");
00204         for(unsigned int i=0; i<screens.size(); i++)
00205         {
00206             retval = screens[i]->socket.send_n(buffer, sizeof(buffer), &timeOut);
00207             if(retval == -1 && errno != ETIME && errno != 0 )
00208             {
00209                 ACE_DEBUG((LM_ERROR, ACE_TEXT("Error %d connection broken for socket nr.: %d\n"), errno, i));
00210                 return 0;
00211             }
00212         }
00213         return 1;
00214     }
00215 
00216     // close all sockets
00217     void MagicYModule::disconnect()
00218     {
00219         for(unsigned int i=0; i<screens.size(); i++)
00220         {
00221             screens[i]->socket.close();
00222             screens[i]->connected = false;
00223         }
00224         OSUtils::sleep(3000);
00225     }
00226 
00227     // reads from the MagicY server and parses MagicY packages
00228     void MagicYModule::run()
00229     {
00230     ACE_DEBUG((LM_INFO, ACE_TEXT("starting MagicY module thread\n")));
00231 
00232         ACE_Time_Value timeOut(1,0);
00233         int average_x=0, average_y=0, socks_active;
00234         bool connected = false;
00235 
00236         while(stop == 0)
00237         {
00238             // connecting
00239             ACE_DEBUG((LM_INFO, ACE_TEXT("Trying to connect ... \n")));
00240             if(connect() == 0)
00241             {
00242                 connected = true;
00243                 // do while no error occurs
00244                 while(stop == 0)
00245                 {
00246                     average_x=0;
00247                     average_y=0;
00248 
00249                     if(! stillConnected())
00250                         break;
00251 
00252                     // build FD_SET
00253                     setSelect();
00254                     // wait for sockets to be ready for reading
00255                     socks_active = ACE::select(ACE_Handle_Set::MAXSIZE, readHandles, &timeOut);
00256                     if(! socks_active)
00257                     {
00258                         ACE_DEBUG((LM_INFO, ACE_TEXT("Error: Socket select time out\n")));
00259                         break;
00260                     }
00261                     if(receive())
00262                         break;
00263 
00264                     lock();
00265                     // calculate average
00266                     for (unsigned int i=0; i < points.size(); i++)
00267                     {
00268                         average_x += points[i].x;
00269                         average_y += points[i].y;
00270                     }
00271                     if(points.size())
00272                     {
00273                         average_x /= points.size();
00274                         average_y /= points.size();
00275                     }
00276 
00277                     // try to find source 1 to n for the n extracted points and fill their event
00278                     MagicYVector::iterator mY_it;
00279                     for( mY_it = magicYs.begin(); mY_it != magicYs.end(); mY_it++ )
00280                     {
00281                         // critical section start, fill event
00282                         Event & event = (*mY_it)->event;
00283                         if ((*mY_it)->average)
00284                         {
00285                             if(points.size())
00286                             {
00287                                 event.getPosition()[0] = float(average_x);
00288                                 event.getPosition()[1] = float(average_y);
00289                                 event.getConfidence() = 1.0f;
00290                             }
00291                             else
00292                             {
00293                                 event.getConfidence() = 0.0f;
00294                             }
00295                         }
00296                         else
00297                         {
00298                             if((*mY_it)->number >= 0 && (unsigned int)(*mY_it)->number < points.size())
00299                             {
00300                                 event.getPosition()[0] = float(points[(*mY_it)->number].x);
00301                                 event.getPosition()[1] = float(points[(*mY_it)->number].y);
00302                                 event.getButton() = points[(*mY_it)->number].trigger;
00303                                 event.getConfidence() = 1.0f;
00304                             }
00305                             else
00306                             {
00307                                 event.getConfidence() = 0.0f;
00308                             }
00309                         }
00310                         event.getPosition()[2] = z_value;
00311 
00312                         correctData(event.getPosition(), positionMapping, invertPosition);
00313 
00314                         event.getOrientation()[0] = orientation[0];
00315                         event.getOrientation()[1] = orientation[1];
00316                         event.getOrientation()[2] = orientation[2];
00317                         event.getOrientation()[3] = orientation[3];
00318 
00319                         (*mY_it)->modified = 1;
00320                         event.timeStamp();
00321                         // end of critical section
00322                     }// for all MagicY sources
00323                     unlock();
00324                 }// while no error
00325             }// if connected
00326             disconnect();
00327         } // forever
00328     ACE_DEBUG((LM_INFO, ACE_TEXT("Stopping thread\n")));
00329     }
00330 
00331 
00332     //  constructs a new Node
00333     Node * MagicYModule::createNode( const std::string& name,  StringTable& attributes)
00334     {
00335         if( name.compare("MagicYSource") == 0 )
00336         {
00337             int number = atoi(attributes.get("number").c_str());
00338 
00339             bool average = false;
00340             std::string avrg = attributes.get("average");
00341             if(!avrg.empty() && !(avrg.compare("true") && avrg.compare("t") && avrg.compare("1")))
00342                 average = true;
00343 
00344             MagicYVector::iterator it;
00345             for( it = magicYs.begin(); it != magicYs.end(); it++ )
00346             {
00347                 MagicY *mY = (MagicY*)(*it);
00348                 if( mY->number == number )
00349                 {
00350                     break;
00351                 }
00352             }
00353             if( it != magicYs.end())
00354             {
00355                 ACE_DEBUG((LM_ERROR, ACE_TEXT("Source with number %d already exists\n"), number));
00356                 return NULL;
00357             }
00358 
00359             MagicYSource *source = new MagicYSource;
00360             MagicY *magicY = new MagicY(number, average, source);
00361             magicYs.push_back( magicY );
00362             ACE_DEBUG((LM_INFO, ACE_TEXT("Built MagicYSource node.\n")));
00363 
00364             return source;
00365         }
00366         return NULL;
00367     }
00368 
00369 
00370     // opens the sockets needed for communication and starts the receive thread
00371     void MagicYModule::start()
00372     {
00373         if (isInitialized() && !magicYs.empty())
00374             ThreadModule::start();
00375     }
00376 
00377     // closes the module and closes any communication sockets and stops thread
00378     void MagicYModule::close()
00379     {
00380         // stop thread
00381         lock();
00382         stop = 1;
00383         unlock();
00384     }
00385 
00386     // pushes event information into the tree
00387     void MagicYModule::pushEvent()
00388     {
00389         if (magicYs.empty())
00390             return;
00391 
00392         for (MagicYVector::iterator it = magicYs.begin(); it != magicYs.end(); it++ )
00393         {
00394             // critical section start
00395             lock();
00396             if((*it)->modified == 1 )
00397             {
00398                 (*it)->source->event = (*it)->event;
00399                 (*it)->modified = 0;
00400                 unlock();
00401                 (*it)->source->updateObservers( (*it)->source->event );
00402             }
00403             else
00404                 unlock();
00405             // end of critical section
00406         }
00407     }
00408 
00409 
00410     int MagicYModule::parseVector(const std::string & line, int * val )
00411     {
00412         int help[3];
00413         int num;
00414         num = sscanf( line.c_str()," %d %d %d", &help[0], &help[1], &help[2]);
00415         if( num != 3 )
00416         {
00417             return 1;
00418         }
00419         val[0] = help[0];
00420         val[1] = help[1];
00421         val[2] = help[2];
00422 
00423         return 0;
00424     }
00425 
00426 
00427     int MagicYModule::parseVector(const std::string & line, float * val )
00428     {
00429         float help[4];
00430         int num;
00431         num = sscanf( line.c_str()," %f %f %f %f", &help[0], &help[1], &help[2], &help[3]);
00432         if( num != 4 )
00433         {
00434             return 1;
00435         }
00436 
00437         val[0] = help[0];
00438         val[1] = help[1];
00439         val[2] = help[2];
00440         val[3] = help[3];
00441 
00442         return 0;
00443     }
00444 
00445     // parse data for extra screens, format: "port x_offset y_offset"
00446     // currently streams for extra screens must come from one and the same server
00447 
00448     int MagicYModule::parseScreens(const std::string & line)
00449     {
00450         int port, x_off, y_off, pos=0;
00451         std::string temp = line;
00452 
00453         do {
00454             if(sscanf(temp.c_str(), "%d %d %d", &port, &x_off, &y_off) < 3)
00455                 return -1;
00456 
00457             Screen *scr = new Screen(port, hostname, x_off, y_off);
00458             screens.push_back(scr);
00459 
00460             ACE_DEBUG((LM_INFO, ACE_TEXT("Extra screen %d : %d : %d\n"), port, x_off, y_off));
00461 
00462             pos = temp.find(' ', 0);
00463             pos = temp.find(' ', pos+1);
00464             pos = temp.find(' ', pos+1);
00465             temp.erase(0, pos+1);
00466         } while(pos >= 0);
00467 
00468         return 0;
00469     }
00470 
00471     void MagicYModule::correctData(std::vector<float> &d, int *mapping, int *inversion)
00472     {
00473         float h[3];
00474         int i;
00475         for(i=0; i<3; i++) h[i] = d[mapping[i]]*inversion[i];
00476         for(i=0; i<3; i++) d[i] = h[i];
00477     }
00478 
00479 
00480     void MagicYModule::initMappping(int *mapping)
00481     {
00482         for (int i=0; i<3; i++)
00483             mapping[i] = i;
00484     }
00485 
00486     void MagicYModule::initInversion(int *inversion)
00487     {
00488         for (int i=0; i<3; i++)
00489             inversion[i] = 1;
00490     }
00491 
00492     void MagicYModule::initOrientation(float *orientation)
00493     {
00494         orientation[0] = 0.0f;
00495         orientation[1] = 0.0f;
00496         orientation[2] = 0.0f;
00497         orientation[3] = 1.0f;
00498     }
00499 
00500     void MagicYModule::calcMapping(int *mapping)
00501     {
00502         for (int i=0; i<3; i++)
00503             if (mapping[i] > 2)
00504                 mapping[i] = 2;
00505             else if (mapping[i] < 0)
00506                 mapping[i] = 0;
00507     }
00508 
00509     void MagicYModule::calcInversion(int *inversion)
00510     {
00511         for (int i=0; i<3; i++)
00512             inversion[i] = inversion[i] ? -1 : 1;
00513     }
00514 
00515 
00516     void MagicYModule::init(StringTable& attributes, ConfigNode * localTree)
00517     {
00518         ThreadModule::init(attributes, localTree);
00519 
00520         // Reading hostname and port number from XML-File
00521         hostname = std::string(attributes.get("hostname"));
00522 
00523         if( parseVector(attributes.get("positionMapping"), positionMapping ) != 0 )
00524         {
00525             ACE_DEBUG((LM_INFO, ACE_TEXT("Error parsing positionMapping !")));
00526             initMappping(positionMapping);
00527         }
00528         calcMapping(positionMapping);
00529         if( parseVector(attributes.get("invertPosition"), invertPosition ) != 0 )
00530         {
00531             ACE_DEBUG((LM_INFO, ACE_TEXT("Error parsing invertPosition!")));
00532             initInversion(invertPosition);
00533         }
00534         calcInversion(invertPosition);
00535         if( parseVector(attributes.get("orientation"), orientation ) != 0 )
00536         {
00537             ACE_DEBUG((LM_INFO, ACE_TEXT("Error parsing orientation!")));
00538             initOrientation(orientation);
00539         }
00540         if( parseScreens(attributes.get("screens")) != 0 )
00541         {
00542             ACE_DEBUG((LM_INFO, ACE_TEXT("Error parsing extra screens!")));
00543         }
00544         if( attributes.get("z_value", &z_value) != 1 )
00545             z_value = 0;
00546     }
00547 
00548 } // namespace ot
00549 
00550 
00551 #else
00552 #pragma message(">>> OT_NO_MAGICY_SUPPORT")
00553 #endif // OT_NO_MAGICY_SUPPORT
00554 
00555 /* 
00556  * ------------------------------------------------------------
00557  *   End of MagicYModule.cxx
00558  * ------------------------------------------------------------
00559  *   Automatic Emacs configuration follows.
00560  *   Local Variables:
00561  *   mode:c++
00562  *   c-basic-offset: 4
00563  *   eval: (c-set-offset 'substatement-open 0)
00564  *   eval: (c-set-offset 'case-label '+)
00565  *   eval: (c-set-offset 'statement 'c-lineup-runin-statements)
00566  *   eval: (setq indent-tabs-mode nil)
00567  *   End:
00568  * ------------------------------------------------------------ 
00569  */

copyright (c) 2006 Graz University of Technology