ViconDataStreamSDK_CPPTest.cpp
Go to the documentation of this file.
00001 
00002 //
00003 // Copyright (C) OMG Plc 2009.
00004 // All rights reserved.  This software is protected by copyright
00005 // law and international treaties.  No part of this software / document
00006 // may be reproduced or distributed in any form or by any means,
00007 // whether transiently or incidentally to some other use of this software,
00008 // without the written permission of the copyright owner.
00009 //
00011 
00012 #include "Client.h"
00013 
00014 #include <iostream>
00015 #include <fstream>
00016 #include <cassert>
00017 #include <ctime>
00018 
00019 #ifdef WIN32
00020   #include <conio.h>   // For _kbhit()
00021   #include <cstdio>   // For getchar()
00022   #include <windows.h> // For Sleep()
00023 #endif // WIN32
00024 
00025 #include <time.h>
00026 
00027 using namespace ViconDataStreamSDK::CPP;
00028 
00029 #define output_stream if(!LogFile.empty()) ; else std::cout 
00030 
00031 namespace
00032 {
00033   std::string Adapt( const bool i_Value )
00034   {
00035     return i_Value ? "True" : "False";
00036   }
00037 
00038   std::string Adapt( const Direction::Enum i_Direction )
00039   {
00040     switch( i_Direction )
00041     {
00042       case Direction::Forward:
00043         return "Forward";
00044       case Direction::Backward:
00045         return "Backward";
00046       case Direction::Left:
00047         return "Left";
00048       case Direction::Right:
00049         return "Right";
00050       case Direction::Up:
00051         return "Up";
00052       case Direction::Down:
00053         return "Down";
00054       default:
00055         return "Unknown";
00056     }
00057   }
00058 
00059   std::string Adapt( const DeviceType::Enum i_DeviceType )
00060   {
00061     switch( i_DeviceType )
00062     {
00063       case DeviceType::ForcePlate:
00064         return "ForcePlate";
00065       case DeviceType::Unknown:
00066       default:
00067         return "Unknown";
00068     }
00069   }
00070 
00071   std::string Adapt( const Unit::Enum i_Unit )
00072   {
00073     switch( i_Unit )
00074     {
00075       case Unit::Meter:
00076         return "Meter";
00077       case Unit::Volt:
00078         return "Volt";
00079       case Unit::NewtonMeter:
00080         return "NewtonMeter";
00081       case Unit::Newton:
00082         return "Newton";
00083       case Unit::Kilogram:
00084         return "Kilogram";
00085       case Unit::Second:
00086         return "Second";
00087       case Unit::Ampere:
00088         return "Ampere";
00089       case Unit::Kelvin:
00090         return "Kelvin";
00091       case Unit::Mole:
00092         return "Mole";
00093       case Unit::Candela:
00094         return "Candela";
00095       case Unit::Radian:
00096         return "Radian";
00097       case Unit::Steradian:
00098         return "Steradian";
00099       case Unit::MeterSquared:
00100         return "MeterSquared";
00101       case Unit::MeterCubed:
00102         return "MeterCubed";
00103       case Unit::MeterPerSecond:
00104         return "MeterPerSecond";
00105       case Unit::MeterPerSecondSquared:
00106         return "MeterPerSecondSquared";
00107       case Unit::RadianPerSecond:
00108         return "RadianPerSecond";
00109       case Unit::RadianPerSecondSquared:
00110         return "RadianPerSecondSquared";
00111       case Unit::Hertz:
00112         return "Hertz";
00113       case Unit::Joule:
00114         return "Joule";
00115       case Unit::Watt:
00116         return "Watt";
00117       case Unit::Pascal:
00118         return "Pascal";
00119       case Unit::Lumen:
00120         return "Lumen";
00121       case Unit::Lux:
00122         return "Lux";
00123       case Unit::Coulomb:
00124         return "Coulomb";
00125       case Unit::Ohm:
00126         return "Ohm";
00127       case Unit::Farad:
00128         return "Farad";
00129       case Unit::Weber:
00130         return "Weber";
00131       case Unit::Tesla:
00132         return "Tesla";
00133       case Unit::Henry:
00134         return "Henry";
00135       case Unit::Siemens:
00136         return "Siemens";
00137       case Unit::Becquerel:
00138         return "Becquerel";
00139       case Unit::Gray:
00140         return "Gray";
00141       case Unit::Sievert:
00142         return "Sievert";
00143       case Unit::Katal:
00144         return "Katal";
00145 
00146       case Unit::Unknown:
00147       default:
00148         return "Unknown";
00149     }
00150   }
00151 #ifdef WIN32
00152   bool Hit()
00153   {
00154     bool hit = false;
00155     while( _kbhit() )
00156     {
00157       getchar();
00158       hit = true;
00159     }
00160     return hit;
00161   }
00162 #endif
00163 }
00164 
00165 int main( int argc, char* argv[] )
00166 {
00167   // Program options
00168   
00169   std::string HostName = "localhost:801";
00170   if( argc > 1 )
00171   {
00172     HostName = argv[1];
00173   }
00174   // log contains:
00175   // version number
00176   // log of framerate over time
00177   // --multicast
00178   // kill off internal app
00179   std::string LogFile = "";
00180   std::string MulticastAddress = "244.0.0.0:44801";
00181   bool ConnectToMultiCast = false;
00182   bool EnableMultiCast = false;
00183   for(int a=2; a < argc; ++a)
00184   {
00185     std::string arg = argv[a];
00186     if(arg == "--help")
00187     {
00188       std::cout << argv[0] << " <HostName>: allowed options include:\n  --log_file <LogFile> --enable_multicast <MulticastAddress:Port> --connect_to_multicast <MulticastAddress:Port> --help" << std::endl;
00189       return 0;
00190     }
00191     else if (arg=="--log_file")
00192     {
00193       if(a < argc)
00194       {
00195         LogFile = argv[a+1];
00196         std::cout << "Using log file <"<< LogFile << "> ..." << std::endl;
00197         ++a;
00198       }
00199     }
00200     else if (arg=="--enable_multicast")
00201     {
00202       EnableMultiCast = true;
00203       if(a < argc)
00204       {
00205         MulticastAddress = argv[a+1];
00206         std::cout << "Enabling multicast address <"<< MulticastAddress << "> ..." << std::endl;
00207         ++a;
00208       }
00209     }
00210     else if (arg=="--connect_to_multicast")
00211     {
00212       ConnectToMultiCast = true;
00213       if(a < argc)
00214       {
00215         MulticastAddress = argv[a+1];
00216         std::cout << "connecting to multicast address <"<< MulticastAddress << "> ..." << std::endl;
00217         ++a;
00218       }
00219     }
00220     else
00221     {
00222       std::cout << "Failed to understand argument <" << argv[a] << ">...exiting" << std::endl;
00223       return 1;
00224     }
00225   }
00226 
00227   std::ofstream ofs;
00228   if(!LogFile.empty())
00229   {
00230     ofs.open(LogFile.c_str());
00231     if(!ofs.is_open())
00232     {
00233       std::cout << "Could not open log file <" << LogFile << ">...exiting" << std::endl;
00234       return 1;
00235     }
00236   }
00237   // Make a new client
00238   Client MyClient;
00239 
00240   for(int i=0; i != 3; ++i) // repeat to check disconnecting doesn't wreck next connect
00241   {
00242     // Connect to a server
00243     std::cout << "Connecting to " << HostName << " ..." << std::flush;
00244     while( !MyClient.IsConnected().Connected )
00245     {
00246       // Direct connection
00247 
00248       bool ok = false;
00249       if(ConnectToMultiCast)
00250       {
00251         // Multicast connection
00252         ok = ( MyClient.ConnectToMulticast( HostName, MulticastAddress ).Result == Result::Success );
00253 
00254       }
00255       else
00256       {
00257         ok =( MyClient.Connect( HostName ).Result == Result::Success );
00258       }
00259       if(!ok)
00260       {
00261         std::cout << "Warning - connect failed..." << std::endl;
00262       }
00263 
00264 
00265       std::cout << ".";
00266   #ifdef WIN32
00267       Sleep( 200 );
00268   #else
00269       sleep(1);
00270   #endif
00271     }
00272     std::cout << std::endl;
00273 
00274     // Enable some different data types
00275     MyClient.EnableSegmentData();
00276     MyClient.EnableMarkerData();
00277     MyClient.EnableUnlabeledMarkerData();
00278     MyClient.EnableDeviceData();
00279 
00280     std::cout << "Segment Data Enabled: "          << Adapt( MyClient.IsSegmentDataEnabled().Enabled )         << std::endl;
00281     std::cout << "Marker Data Enabled: "           << Adapt( MyClient.IsMarkerDataEnabled().Enabled )          << std::endl;
00282     std::cout << "Unlabeled Marker Data Enabled: " << Adapt( MyClient.IsUnlabeledMarkerDataEnabled().Enabled ) << std::endl;
00283     std::cout << "Device Data Enabled: "           << Adapt( MyClient.IsDeviceDataEnabled().Enabled )          << std::endl;
00284 
00285     // Set the streaming mode
00286     //MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ClientPull );
00287     // MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ClientPullPreFetch );
00288     MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ServerPush );
00289 
00290     // Set the global up axis
00291     MyClient.SetAxisMapping( Direction::Forward, 
00292                              Direction::Left, 
00293                              Direction::Up ); // Z-up
00294     // MyClient.SetGlobalUpAxis( Direction::Forward, 
00295     //                           Direction::Up, 
00296     //                           Direction::Right ); // Y-up
00297 
00298     Output_GetAxisMapping _Output_GetAxisMapping = MyClient.GetAxisMapping();
00299     std::cout << "Axis Mapping: X-" << Adapt( _Output_GetAxisMapping.XAxis ) 
00300                            << " Y-" << Adapt( _Output_GetAxisMapping.YAxis ) 
00301                            << " Z-" << Adapt( _Output_GetAxisMapping.ZAxis ) << std::endl;
00302 
00303     // Discover the version number
00304     Output_GetVersion _Output_GetVersion = MyClient.GetVersion();
00305     std::cout << "Version: " << _Output_GetVersion.Major << "." 
00306                              << _Output_GetVersion.Minor << "." 
00307                              << _Output_GetVersion.Point << std::endl;
00308 
00309     if( EnableMultiCast )
00310     {
00311       assert( MyClient.IsConnected().Connected );
00312       MyClient.StartTransmittingMulticast( HostName, MulticastAddress );
00313     }
00314 
00315     size_t FrameRateWindow = 1000; // frames
00316     size_t Counter = 0;
00317     clock_t LastTime = clock();
00318     // Loop until a key is pressed
00319   #ifdef WIN32
00320     while( !Hit() )
00321   #else
00322     while( true)
00323   #endif
00324     {
00325       // Get a frame
00326       output_stream << "Waiting for new frame...";
00327       while( MyClient.GetFrame().Result != Result::Success )
00328       {
00329         // Sleep a little so that we don't lumber the CPU with a busy poll
00330         #ifdef WIN32
00331           Sleep( 200 );
00332         #else
00333           sleep(1);
00334         #endif
00335 
00336         output_stream << ".";
00337       }
00338       output_stream << std::endl;
00339       if(++Counter == FrameRateWindow)
00340       {
00341         clock_t Now = clock();
00342         double FrameRate = (double)(FrameRateWindow * CLOCKS_PER_SEC) / (double)(Now - LastTime);
00343         if(!LogFile.empty())
00344         {
00345           time_t rawtime;
00346           struct tm * timeinfo;
00347           time ( &rawtime );
00348           timeinfo = localtime ( &rawtime );
00349 
00350           ofs << "Frame rate = " << FrameRate << " at " <<  asctime (timeinfo)<< std::endl;
00351         }
00352 
00353         LastTime = Now;
00354         Counter = 0;
00355       }
00356 
00357       // Get the frame number
00358       Output_GetFrameNumber _Output_GetFrameNumber = MyClient.GetFrameNumber();
00359       output_stream << "Frame Number: " << _Output_GetFrameNumber.FrameNumber << std::endl;
00360 
00361       Output_GetFrameRate Rate = MyClient.GetFrameRate();
00362       std::cout << "Frame rate: "           << Rate.FrameRateHz          << std::endl;
00363       // Get the timecode
00364       Output_GetTimecode _Output_GetTimecode  = MyClient.GetTimecode();
00365 
00366       output_stream << "Timecode: "
00367                 << _Output_GetTimecode.Hours               << "h "
00368                 << _Output_GetTimecode.Minutes             << "m " 
00369                 << _Output_GetTimecode.Seconds             << "s "
00370                 << _Output_GetTimecode.Frames              << "f "
00371                 << _Output_GetTimecode.SubFrame            << "sf "
00372                 << Adapt( _Output_GetTimecode.FieldFlag ) << " " 
00373                 << _Output_GetTimecode.Standard            << " " 
00374                 << _Output_GetTimecode.SubFramesPerFrame   << " " 
00375                 << _Output_GetTimecode.UserBits            << std::endl << std::endl;
00376 
00377       // Get the latency
00378       output_stream << "Latency: " << MyClient.GetLatencyTotal().Total << "s" << std::endl;
00379       
00380       for( unsigned int LatencySampleIndex = 0 ; LatencySampleIndex < MyClient.GetLatencySampleCount().Count ; ++LatencySampleIndex )
00381       {
00382         std::string SampleName  = MyClient.GetLatencySampleName( LatencySampleIndex ).Name;
00383         double      SampleValue = MyClient.GetLatencySampleValue( SampleName ).Value;
00384 
00385         output_stream << "  " << SampleName << " " << SampleValue << "s" << std::endl;
00386       }
00387       output_stream << std::endl;
00388 
00389       // Count the number of subjects
00390       unsigned int SubjectCount = MyClient.GetSubjectCount().SubjectCount;
00391       output_stream << "Subjects (" << SubjectCount << "):" << std::endl;
00392       for( unsigned int SubjectIndex = 0 ; SubjectIndex < SubjectCount ; ++SubjectIndex )
00393       {
00394         output_stream << "  Subject #" << SubjectIndex << std::endl;
00395 
00396         // Get the subject name
00397         std::string SubjectName = MyClient.GetSubjectName( SubjectIndex ).SubjectName;
00398         output_stream << "    Name: " << SubjectName << std::endl;
00399 
00400         // Get the root segment
00401         std::string RootSegment = MyClient.GetSubjectRootSegmentName( SubjectName ).SegmentName;
00402         output_stream << "    Root Segment: " << RootSegment << std::endl;
00403 
00404         // Count the number of segments
00405         unsigned int SegmentCount = MyClient.GetSegmentCount( SubjectName ).SegmentCount;
00406         output_stream << "    Segments (" << SegmentCount << "):" << std::endl;
00407         for( unsigned int SegmentIndex = 0 ; SegmentIndex < SegmentCount ; ++SegmentIndex )
00408         {
00409           output_stream << "      Segment #" << SegmentIndex << std::endl;
00410 
00411           // Get the segment name
00412           std::string SegmentName = MyClient.GetSegmentName( SubjectName, SegmentIndex ).SegmentName;
00413           output_stream << "        Name: " << SegmentName << std::endl;
00414 
00415           // Get the segment parent
00416           std::string SegmentParentName = MyClient.GetSegmentParentName( SubjectName, SegmentName ).SegmentName;
00417           output_stream << "        Parent: " << SegmentParentName << std::endl;
00418 
00419           // Get the segment's children
00420           unsigned int ChildCount = MyClient.GetSegmentChildCount( SubjectName, SegmentName ).SegmentCount;
00421           output_stream << "     Children (" << ChildCount << "):" << std::endl;
00422           for( unsigned int ChildIndex = 0 ; ChildIndex < ChildCount ; ++ChildIndex )
00423           {
00424             std::string ChildName = MyClient.GetSegmentChildName( SubjectName, SegmentName, ChildIndex ).SegmentName;
00425             output_stream << "       " << ChildName << std::endl;
00426           }
00427 
00428           // Get the static segment translation
00429           Output_GetSegmentStaticTranslation _Output_GetSegmentStaticTranslation = 
00430             MyClient.GetSegmentStaticTranslation( SubjectName, SegmentName );
00431           output_stream << "        Static Translation: (" << _Output_GetSegmentStaticTranslation.Translation[ 0 ]  << ", " 
00432                                                        << _Output_GetSegmentStaticTranslation.Translation[ 1 ]  << ", " 
00433                                                        << _Output_GetSegmentStaticTranslation.Translation[ 2 ]  << ")" << std::endl;
00434 
00435           // Get the static segment rotation in helical co-ordinates
00436           Output_GetSegmentStaticRotationHelical _Output_GetSegmentStaticRotationHelical = 
00437             MyClient.GetSegmentStaticRotationHelical( SubjectName, SegmentName );
00438           output_stream << "        Static Rotation Helical: (" << _Output_GetSegmentStaticRotationHelical.Rotation[ 0 ]     << ", " 
00439                                                             << _Output_GetSegmentStaticRotationHelical.Rotation[ 1 ]     << ", " 
00440                                                             << _Output_GetSegmentStaticRotationHelical.Rotation[ 2 ]     << ")" << std::endl;
00441 
00442           // Get the static segment rotation as a matrix
00443           Output_GetSegmentStaticRotationMatrix _Output_GetSegmentStaticRotationMatrix = 
00444             MyClient.GetSegmentStaticRotationMatrix( SubjectName, SegmentName );
00445           output_stream << "        Static Rotation Matrix: (" << _Output_GetSegmentStaticRotationMatrix.Rotation[ 0 ]     << ", " 
00446                                                            << _Output_GetSegmentStaticRotationMatrix.Rotation[ 1 ]     << ", " 
00447                                                            << _Output_GetSegmentStaticRotationMatrix.Rotation[ 2 ]     << ", " 
00448                                                            << _Output_GetSegmentStaticRotationMatrix.Rotation[ 3 ]     << ", " 
00449                                                            << _Output_GetSegmentStaticRotationMatrix.Rotation[ 4 ]     << ", " 
00450                                                            << _Output_GetSegmentStaticRotationMatrix.Rotation[ 5 ]     << ", " 
00451                                                            << _Output_GetSegmentStaticRotationMatrix.Rotation[ 6 ]     << ", " 
00452                                                            << _Output_GetSegmentStaticRotationMatrix.Rotation[ 7 ]     << ", " 
00453                                                            << _Output_GetSegmentStaticRotationMatrix.Rotation[ 8 ]     << ")" << std::endl;
00454 
00455           // Get the static segment rotation in quaternion co-ordinates
00456           Output_GetSegmentStaticRotationQuaternion _Output_GetSegmentStaticRotationQuaternion = 
00457             MyClient.GetSegmentStaticRotationQuaternion( SubjectName, SegmentName );
00458           output_stream << "        Static Rotation Quaternion: (" << _Output_GetSegmentStaticRotationQuaternion.Rotation[ 0 ]     << ", " 
00459                                                                << _Output_GetSegmentStaticRotationQuaternion.Rotation[ 1 ]     << ", " 
00460                                                                << _Output_GetSegmentStaticRotationQuaternion.Rotation[ 2 ]     << ", " 
00461                                                                << _Output_GetSegmentStaticRotationQuaternion.Rotation[ 3 ]     << ")" << std::endl;
00462 
00463           // Get the static segment rotation in EulerXYZ co-ordinates
00464           Output_GetSegmentStaticRotationEulerXYZ _Output_GetSegmentStaticRotationEulerXYZ = 
00465             MyClient.GetSegmentStaticRotationEulerXYZ( SubjectName, SegmentName );
00466           output_stream << "        Static Rotation EulerXYZ: (" << _Output_GetSegmentStaticRotationEulerXYZ.Rotation[ 0 ]     << ", " 
00467                                                              << _Output_GetSegmentStaticRotationEulerXYZ.Rotation[ 1 ]     << ", " 
00468                                                              << _Output_GetSegmentStaticRotationEulerXYZ.Rotation[ 2 ]     << ")" << std::endl;
00469 
00470           // Get the global segment translation
00471           Output_GetSegmentGlobalTranslation _Output_GetSegmentGlobalTranslation = 
00472             MyClient.GetSegmentGlobalTranslation( SubjectName, SegmentName );
00473           output_stream << "        Global Translation: (" << _Output_GetSegmentGlobalTranslation.Translation[ 0 ]  << ", " 
00474                                                        << _Output_GetSegmentGlobalTranslation.Translation[ 1 ]  << ", " 
00475                                                        << _Output_GetSegmentGlobalTranslation.Translation[ 2 ]  << ") " 
00476                                                        << Adapt( _Output_GetSegmentGlobalTranslation.Occluded ) << std::endl;
00477 
00478           // Get the global segment rotation in helical co-ordinates
00479           Output_GetSegmentGlobalRotationHelical _Output_GetSegmentGlobalRotationHelical = 
00480             MyClient.GetSegmentGlobalRotationHelical( SubjectName, SegmentName );
00481           output_stream << "        Global Rotation Helical: (" << _Output_GetSegmentGlobalRotationHelical.Rotation[ 0 ]     << ", " 
00482                                                             << _Output_GetSegmentGlobalRotationHelical.Rotation[ 1 ]     << ", " 
00483                                                             << _Output_GetSegmentGlobalRotationHelical.Rotation[ 2 ]     << ") " 
00484                                                             << Adapt( _Output_GetSegmentGlobalRotationHelical.Occluded ) << std::endl;
00485 
00486           // Get the global segment rotation as a matrix
00487           Output_GetSegmentGlobalRotationMatrix _Output_GetSegmentGlobalRotationMatrix = 
00488             MyClient.GetSegmentGlobalRotationMatrix( SubjectName, SegmentName );
00489           output_stream << "        Global Rotation Matrix: (" << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 0 ]     << ", " 
00490                                                            << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 1 ]     << ", " 
00491                                                            << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 2 ]     << ", " 
00492                                                            << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 3 ]     << ", " 
00493                                                            << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 4 ]     << ", " 
00494                                                            << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 5 ]     << ", " 
00495                                                            << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 6 ]     << ", " 
00496                                                            << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 7 ]     << ", " 
00497                                                            << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 8 ]     << ") " 
00498                                                            << Adapt( _Output_GetSegmentGlobalRotationMatrix.Occluded ) << std::endl;
00499 
00500           // Get the global segment rotation in quaternion co-ordinates
00501           Output_GetSegmentGlobalRotationQuaternion _Output_GetSegmentGlobalRotationQuaternion = 
00502             MyClient.GetSegmentGlobalRotationQuaternion( SubjectName, SegmentName );
00503           output_stream << "        Global Rotation Quaternion: (" << _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 0 ]     << ", " 
00504                                                                << _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 1 ]     << ", " 
00505                                                                << _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 2 ]     << ", " 
00506                                                                << _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 3 ]     << ") " 
00507                                                                << Adapt( _Output_GetSegmentGlobalRotationQuaternion.Occluded ) << std::endl;
00508 
00509           // Get the global segment rotation in EulerXYZ co-ordinates
00510           Output_GetSegmentGlobalRotationEulerXYZ _Output_GetSegmentGlobalRotationEulerXYZ = 
00511             MyClient.GetSegmentGlobalRotationEulerXYZ( SubjectName, SegmentName );
00512           output_stream << "        Global Rotation EulerXYZ: (" << _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 0 ]     << ", " 
00513                                                              << _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 1 ]     << ", " 
00514                                                              << _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 2 ]     << ") " 
00515                                                              << Adapt( _Output_GetSegmentGlobalRotationEulerXYZ.Occluded ) << std::endl;
00516 
00517           // Get the local segment translation
00518           Output_GetSegmentLocalTranslation _Output_GetSegmentLocalTranslation = 
00519             MyClient.GetSegmentLocalTranslation( SubjectName, SegmentName );
00520           output_stream << "        Local Translation: (" << _Output_GetSegmentLocalTranslation.Translation[ 0 ]  << ", " 
00521                                                       << _Output_GetSegmentLocalTranslation.Translation[ 1 ]  << ", " 
00522                                                       << _Output_GetSegmentLocalTranslation.Translation[ 2 ]  << ") " 
00523                                                       << Adapt( _Output_GetSegmentLocalTranslation.Occluded ) << std::endl;
00524 
00525           // Get the local segment rotation in helical co-ordinates
00526           Output_GetSegmentLocalRotationHelical _Output_GetSegmentLocalRotationHelical = 
00527             MyClient.GetSegmentLocalRotationHelical( SubjectName, SegmentName );
00528           output_stream << "        Local Rotation Helical: (" << _Output_GetSegmentLocalRotationHelical.Rotation[ 0 ]     << ", " 
00529                                                            << _Output_GetSegmentLocalRotationHelical.Rotation[ 1 ]     << ", " 
00530                                                            << _Output_GetSegmentLocalRotationHelical.Rotation[ 2 ]     << ") " 
00531                                                            << Adapt( _Output_GetSegmentLocalRotationHelical.Occluded ) << std::endl;
00532 
00533           // Get the local segment rotation as a matrix
00534           Output_GetSegmentLocalRotationMatrix _Output_GetSegmentLocalRotationMatrix = 
00535             MyClient.GetSegmentLocalRotationMatrix( SubjectName, SegmentName );
00536           output_stream << "        Local Rotation Matrix: (" << _Output_GetSegmentLocalRotationMatrix.Rotation[ 0 ]     << ", " 
00537                                                           << _Output_GetSegmentLocalRotationMatrix.Rotation[ 1 ]     << ", " 
00538                                                           << _Output_GetSegmentLocalRotationMatrix.Rotation[ 2 ]     << ", " 
00539                                                           << _Output_GetSegmentLocalRotationMatrix.Rotation[ 3 ]     << ", " 
00540                                                           << _Output_GetSegmentLocalRotationMatrix.Rotation[ 4 ]     << ", " 
00541                                                           << _Output_GetSegmentLocalRotationMatrix.Rotation[ 5 ]     << ", " 
00542                                                           << _Output_GetSegmentLocalRotationMatrix.Rotation[ 6 ]     << ", " 
00543                                                           << _Output_GetSegmentLocalRotationMatrix.Rotation[ 7 ]     << ", " 
00544                                                           << _Output_GetSegmentLocalRotationMatrix.Rotation[ 8 ]     << ") " 
00545                                                           << Adapt( _Output_GetSegmentLocalRotationMatrix.Occluded ) << std::endl;
00546 
00547           // Get the local segment rotation in quaternion co-ordinates
00548           Output_GetSegmentLocalRotationQuaternion _Output_GetSegmentLocalRotationQuaternion = 
00549             MyClient.GetSegmentLocalRotationQuaternion( SubjectName, SegmentName );
00550           output_stream << "        Local Rotation Quaternion: (" << _Output_GetSegmentLocalRotationQuaternion.Rotation[ 0 ]     << ", " 
00551                                                               << _Output_GetSegmentLocalRotationQuaternion.Rotation[ 1 ]     << ", " 
00552                                                               << _Output_GetSegmentLocalRotationQuaternion.Rotation[ 2 ]     << ", " 
00553                                                               << _Output_GetSegmentLocalRotationQuaternion.Rotation[ 3 ]     << ") " 
00554                                                               << Adapt( _Output_GetSegmentLocalRotationQuaternion.Occluded ) << std::endl;
00555 
00556           // Get the local segment rotation in EulerXYZ co-ordinates
00557           Output_GetSegmentLocalRotationEulerXYZ _Output_GetSegmentLocalRotationEulerXYZ = 
00558             MyClient.GetSegmentLocalRotationEulerXYZ( SubjectName, SegmentName );
00559           output_stream << "        Local Rotation EulerXYZ: (" << _Output_GetSegmentLocalRotationEulerXYZ.Rotation[ 0 ]     << ", " 
00560                                                             << _Output_GetSegmentLocalRotationEulerXYZ.Rotation[ 1 ]     << ", " 
00561                                                             << _Output_GetSegmentLocalRotationEulerXYZ.Rotation[ 2 ]     << ") " 
00562                                                             << Adapt( _Output_GetSegmentLocalRotationEulerXYZ.Occluded ) << std::endl;
00563         }
00564 
00565         // Count the number of markers
00566         unsigned int MarkerCount = MyClient.GetMarkerCount( SubjectName ).MarkerCount;
00567         output_stream << "    Markers (" << MarkerCount << "):" << std::endl;
00568         for( unsigned int MarkerIndex = 0 ; MarkerIndex < MarkerCount ; ++MarkerIndex )
00569         {
00570           // Get the marker name
00571           std::string MarkerName = MyClient.GetMarkerName( SubjectName, MarkerIndex ).MarkerName;
00572 
00573           // Get the marker parent
00574           std::string MarkerParentName = MyClient.GetMarkerParentName( SubjectName, MarkerName ).SegmentName;
00575 
00576           // Get the global marker translation
00577           Output_GetMarkerGlobalTranslation _Output_GetMarkerGlobalTranslation =
00578             MyClient.GetMarkerGlobalTranslation( SubjectName, MarkerName );
00579 
00580           output_stream << "      Marker #" << MarkerIndex            << ": "
00581                                         << MarkerName             << " ("
00582                                         << _Output_GetMarkerGlobalTranslation.Translation[ 0 ]  << ", "
00583                                         << _Output_GetMarkerGlobalTranslation.Translation[ 1 ]  << ", "
00584                                         << _Output_GetMarkerGlobalTranslation.Translation[ 2 ]  << ") "
00585                                         << Adapt( _Output_GetMarkerGlobalTranslation.Occluded ) << std::endl;
00586         }
00587       }
00588 
00589       // Get the unlabeled markers
00590       unsigned int UnlabeledMarkerCount = MyClient.GetUnlabeledMarkerCount().MarkerCount;
00591       output_stream << "    Unlabeled Markers (" << UnlabeledMarkerCount << "):" << std::endl;
00592       for( unsigned int UnlabeledMarkerIndex = 0 ; UnlabeledMarkerIndex < UnlabeledMarkerCount ; ++UnlabeledMarkerIndex )
00593       { 
00594         // Get the global marker translation
00595         Output_GetUnlabeledMarkerGlobalTranslation _Output_GetUnlabeledMarkerGlobalTranslation =
00596           MyClient.GetUnlabeledMarkerGlobalTranslation( UnlabeledMarkerIndex );
00597 
00598         output_stream << "      Marker #" << UnlabeledMarkerIndex   << ": ("
00599                                       << _Output_GetUnlabeledMarkerGlobalTranslation.Translation[ 0 ] << ", "
00600                                       << _Output_GetUnlabeledMarkerGlobalTranslation.Translation[ 1 ] << ", "
00601                                       << _Output_GetUnlabeledMarkerGlobalTranslation.Translation[ 2 ] << ")" << std::endl;
00602       }
00603 
00604       // Count the number of devices
00605       unsigned int DeviceCount = MyClient.GetDeviceCount().DeviceCount;
00606       output_stream << "  Devices (" << DeviceCount << "):" << std::endl;
00607       for( unsigned int DeviceIndex = 0 ; DeviceIndex < DeviceCount ; ++DeviceIndex )
00608       {
00609         output_stream << "    Device #" << DeviceIndex << ":" << std::endl;
00610 
00611         // Get the device name and type
00612         Output_GetDeviceName _Output_GetDeviceName = MyClient.GetDeviceName( DeviceIndex );
00613         output_stream << "      Name: " << _Output_GetDeviceName.DeviceName << std::endl;
00614         output_stream << "      Type: " << Adapt( _Output_GetDeviceName.DeviceType ) << std::endl;
00615 
00616         // Count the number of device outputs
00617         unsigned int DeviceOutputCount = MyClient.GetDeviceOutputCount( _Output_GetDeviceName.DeviceName ).DeviceOutputCount;
00618         output_stream << "      Device Outputs (" << DeviceOutputCount << "):" << std::endl;
00619         for( unsigned int DeviceOutputIndex = 0 ; DeviceOutputIndex < DeviceOutputCount ; ++DeviceOutputIndex )
00620         {
00621           // Get the device output name and unit
00622           Output_GetDeviceOutputName _Output_GetDeviceOutputName = 
00623             MyClient.GetDeviceOutputName( _Output_GetDeviceName.DeviceName, DeviceOutputIndex );
00624 
00625           unsigned int DeviceOutputSubsamples = 
00626                          MyClient.GetDeviceOutputSubsamples( _Output_GetDeviceName.DeviceName, 
00627                                                              _Output_GetDeviceOutputName.DeviceOutputName ).DeviceOutputSubsamples;
00628 
00629           output_stream << "      Device Output #" << DeviceOutputIndex << ":" << std::endl;
00630           output_stream << "      Samples (" << DeviceOutputSubsamples << "):" << std::endl;
00631 
00632           for( unsigned int DeviceOutputSubsample = 0; DeviceOutputSubsample < DeviceOutputSubsamples; ++DeviceOutputSubsample )
00633           {
00634             output_stream << "        Sample #" << DeviceOutputSubsample << ":" << std::endl;
00635 
00636             // Get the device output value
00637             Output_GetDeviceOutputValue _Output_GetDeviceOutputValue = 
00638               MyClient.GetDeviceOutputValue( _Output_GetDeviceName.DeviceName, 
00639                                              _Output_GetDeviceOutputName.DeviceOutputName, 
00640                                              DeviceOutputSubsample );
00641 
00642             output_stream << "          '" << _Output_GetDeviceOutputName.DeviceOutputName          << "' "
00643                                           << _Output_GetDeviceOutputValue.Value                    << " " 
00644                                           << Adapt( _Output_GetDeviceOutputName.DeviceOutputUnit ) << " " 
00645                                           << Adapt( _Output_GetDeviceOutputValue.Occluded )        << std::endl;
00646           }
00647         }
00648       }
00649 
00650       // Output the force plate information.
00651       unsigned int ForcePlateCount = MyClient.GetForcePlateCount().ForcePlateCount;
00652       output_stream << "  Force Plates: (" << ForcePlateCount << ")" << std::endl;
00653 
00654       for( unsigned int ForcePlateIndex = 0 ; ForcePlateIndex < ForcePlateCount ; ++ForcePlateIndex )
00655       {
00656         output_stream << "    Force Plate #" << ForcePlateIndex << ":" << std::endl;
00657 
00658         unsigned int ForcePlateSubsamples = MyClient.GetForcePlateSubsamples( ForcePlateIndex ).ForcePlateSubsamples;
00659 
00660                 output_stream << "    Samples (" << ForcePlateSubsamples << "):" << std::endl;
00661 
00662         for( unsigned int ForcePlateSubsample = 0; ForcePlateSubsample < ForcePlateSubsamples; ++ForcePlateSubsample )
00663         {
00664           output_stream << "      Sample #" << ForcePlateSubsample << ":" << std::endl;
00665 
00666           Output_GetGlobalForceVector _Output_GetForceVector = MyClient.GetGlobalForceVector( ForcePlateIndex, ForcePlateSubsample );
00667           output_stream << "        Force (" << _Output_GetForceVector.ForceVector[ 0 ] << ", ";
00668           output_stream << _Output_GetForceVector.ForceVector[ 1 ] << ", ";
00669           output_stream << _Output_GetForceVector.ForceVector[ 2 ] << ")" << std::endl;
00670 
00671           Output_GetGlobalMomentVector _Output_GetMomentVector = 
00672                                          MyClient.GetGlobalMomentVector( ForcePlateIndex, ForcePlateSubsample );
00673           output_stream << "        Moment (" << _Output_GetMomentVector.MomentVector[ 0 ] << ", ";
00674           output_stream << _Output_GetMomentVector.MomentVector[ 1 ] << ", ";
00675           output_stream << _Output_GetMomentVector.MomentVector[ 2 ] << ")" << std::endl;
00676 
00677           Output_GetGlobalCentreOfPressure _Output_GetCentreOfPressure = 
00678                                              MyClient.GetGlobalCentreOfPressure( ForcePlateIndex, ForcePlateSubsample );
00679           output_stream << "        CoP (" << _Output_GetCentreOfPressure.CentreOfPressure[ 0 ] << ", ";
00680           output_stream << _Output_GetCentreOfPressure.CentreOfPressure[ 1 ] << ", ";
00681           output_stream << _Output_GetCentreOfPressure.CentreOfPressure[ 2 ] << ")" << std::endl;
00682         }
00683       }
00684 
00685       // Output eye tracker information.
00686       unsigned int EyeTrackerCount = MyClient.GetEyeTrackerCount().EyeTrackerCount;
00687       output_stream << "  Eye Trackers: (" << EyeTrackerCount << ")" << std::endl;
00688 
00689       for( unsigned int EyeTrackerIndex = 0 ; EyeTrackerIndex < EyeTrackerCount ; ++EyeTrackerIndex )
00690       {
00691         output_stream << "    Eye Tracker #" << EyeTrackerIndex << ":" << std::endl;
00692 
00693         Output_GetEyeTrackerGlobalPosition _Output_GetEyeTrackerGlobalPosition = MyClient.GetEyeTrackerGlobalPosition( EyeTrackerIndex );
00694 
00695         output_stream << "      Position (" << _Output_GetEyeTrackerGlobalPosition.Position[ 0 ] << ", ";
00696         output_stream << _Output_GetEyeTrackerGlobalPosition.Position[ 1 ] << ", ";
00697         output_stream << _Output_GetEyeTrackerGlobalPosition.Position[ 2 ] << ") ";
00698         output_stream << Adapt( _Output_GetEyeTrackerGlobalPosition.Occluded ) << std::endl;
00699 
00700         Output_GetEyeTrackerGlobalGazeVector _Output_GetEyeTrackerGlobalGazeVector = MyClient.GetEyeTrackerGlobalGazeVector( EyeTrackerIndex );
00701 
00702         output_stream << "      Gaze (" << _Output_GetEyeTrackerGlobalGazeVector.GazeVector[ 0 ] << ", ";
00703         output_stream << _Output_GetEyeTrackerGlobalGazeVector.GazeVector[ 1 ] << ", ";
00704         output_stream << _Output_GetEyeTrackerGlobalGazeVector.GazeVector[ 2 ] << ") ";
00705         output_stream << Adapt( _Output_GetEyeTrackerGlobalGazeVector.Occluded ) << std::endl;
00706       }
00707     }
00708 
00709     if( EnableMultiCast )
00710     {
00711       MyClient.StopTransmittingMulticast();
00712     }
00713     MyClient.DisableSegmentData();
00714     MyClient.DisableMarkerData();
00715     MyClient.DisableUnlabeledMarkerData();
00716     MyClient.DisableDeviceData();
00717 
00718     // Disconnect and dispose
00719     int t = clock();
00720     std::cout << " Disconnecting..." << std::endl;
00721     MyClient.Disconnect();
00722     int dt = clock() - t;
00723     double secs = (double) (dt)/(double)CLOCKS_PER_SEC;
00724     std::cout << " Disconnect time = " << secs << " secs" << std::endl;
00725 
00726   }
00727 }


vicon_bridge
Author(s):
autogenerated on Sat Jun 8 2019 20:48:39