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 #include <ViconDataStreamSDK_CPP/Client.h>
00012 #include <iostream>
00013 
00014 #ifdef WIN32
00015   #include <conio.h>   // For _kbhit()
00016   #include <cstdio>   // For getchar()
00017   #include <windows.h> // For Sleep()
00018 #endif // WIN32
00019 
00020 #include <time.h>
00021 
00022 using namespace ViconDataStreamSDK::CPP;
00023 
00024 namespace
00025 {
00026   std::string Adapt( const bool i_Value )
00027   {
00028     return i_Value ? "True" : "False";
00029   }
00030 
00031   std::string Adapt( const Direction::Enum i_Direction )
00032   {
00033     switch( i_Direction )
00034     {
00035       case Direction::Forward:
00036         return "Forward";
00037       case Direction::Backward:
00038         return "Backward";
00039       case Direction::Left:
00040         return "Left";
00041       case Direction::Right:
00042         return "Right";
00043       case Direction::Up:
00044         return "Up";
00045       case Direction::Down:
00046         return "Down";
00047       default:
00048         return "Unknown";
00049     }
00050   }
00051 
00052   std::string Adapt( const DeviceType::Enum i_DeviceType )
00053   {
00054     switch( i_DeviceType )
00055     {
00056       case DeviceType::ForcePlate:
00057         return "ForcePlate";
00058       case DeviceType::Unknown:
00059       default:
00060         return "Unknown";
00061     }
00062   }
00063 
00064   std::string Adapt( const Unit::Enum i_Unit )
00065   {
00066     switch( i_Unit )
00067     {
00068       case Unit::Meter:
00069         return "Meter";
00070       case Unit::Volt:
00071         return "Volt";
00072       case Unit::NewtonMeter:
00073         return "NewtonMeter";
00074       case Unit::Newton:
00075         return "Newton";
00076       case Unit::Unknown:
00077       default:
00078         return "Unknown";
00079     }
00080   }
00081 #ifdef WIN32
00082   bool Hit()
00083   {
00084     bool hit = false;
00085     while( _kbhit() )
00086     {
00087       getchar();
00088       hit = true;
00089     }
00090     return hit;
00091   }
00092 #endif
00093 }
00094 
00095 int main( int argc, char* argv[] )
00096 {
00097   // Program options
00098   bool TransmitMulticast = false;
00099   
00100   std::string HostName = "localhost:801";
00101   if( argc == 2 )
00102   {
00103     HostName = argv[1];
00104   }
00105 
00106   // Make a new client
00107   Client MyClient;
00108 
00109   for(int i=0; i != 3; ++i) // repeat to check disconnecting doesn't wreck next connect
00110   {
00111     // Connect to a server
00112     std::cout << "Connecting to " << HostName << " ..." << std::flush;
00113     while( !MyClient.IsConnected().Connected )
00114     {
00115       // Direct connection
00116       MyClient.Connect( HostName );
00117 
00118       // Multicast connection
00119       // MyClient.ConnectToMulticast( HostName, "224.0.0.0" );
00120 
00121       std::cout << ".";
00122   #ifdef WIN32
00123       Sleep( 200 );
00124   #else
00125       sleep(1);
00126   #endif
00127     }
00128     std::cout << std::endl;
00129 
00130     // Enable some different data types
00131     MyClient.EnableSegmentData();
00132     MyClient.EnableMarkerData();
00133     MyClient.EnableUnlabeledMarkerData();
00134     MyClient.EnableDeviceData();
00135 
00136     std::cout << "Segment Data Enabled: "          << Adapt( MyClient.IsSegmentDataEnabled().Enabled )         << std::endl;
00137     std::cout << "Marker Data Enabled: "           << Adapt( MyClient.IsMarkerDataEnabled().Enabled )          << std::endl;
00138     std::cout << "Unlabeled Marker Data Enabled: " << Adapt( MyClient.IsUnlabeledMarkerDataEnabled().Enabled ) << std::endl;
00139     std::cout << "Device Data Enabled: "           << Adapt( MyClient.IsDeviceDataEnabled().Enabled )          << std::endl;
00140 
00141     // Set the streaming mode
00142     MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ClientPull );
00143     // MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ClientPullPreFetch );
00144     // MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ServerPush );
00145 
00146     // Set the global up axis
00147     MyClient.SetAxisMapping( Direction::Forward, 
00148                              Direction::Left, 
00149                              Direction::Up ); // Z-up
00150     // MyClient.SetGlobalUpAxis( Direction::Forward, 
00151     //                           Direction::Up, 
00152     //                           Direction::Right ); // Y-up
00153 
00154     Output_GetAxisMapping _Output_GetAxisMapping = MyClient.GetAxisMapping();
00155     std::cout << "Axis Mapping: X-" << Adapt( _Output_GetAxisMapping.XAxis ) 
00156                            << " Y-" << Adapt( _Output_GetAxisMapping.YAxis ) 
00157                            << " Z-" << Adapt( _Output_GetAxisMapping.ZAxis ) << std::endl;
00158 
00159     // Discover the version number
00160     Output_GetVersion _Output_GetVersion = MyClient.GetVersion();
00161     std::cout << "Version: " << _Output_GetVersion.Major << "." 
00162                              << _Output_GetVersion.Minor << "." 
00163                              << _Output_GetVersion.Point << std::endl;
00164 
00165     if( TransmitMulticast )
00166     {
00167       MyClient.StartTransmittingMulticast( "localhost", "224.0.0.0" );
00168     }
00169 
00170     // Loop until a key is pressed
00171   #ifdef WIN32
00172     while( !Hit() )
00173   #else
00174     while( true)
00175   #endif
00176     {
00177       // Get a frame
00178       std::cout << "Waiting for new frame...";
00179       while( MyClient.GetFrame().Result != Result::Success )
00180       {
00181         // Sleep a little so that we don't lumber the CPU with a busy poll
00182         #ifdef WIN32
00183           Sleep( 200 );
00184         #else
00185           sleep(1);
00186         #endif
00187 
00188         std::cout << ".";
00189       }
00190       std::cout << std::endl;
00191 
00192       // Get the frame number
00193       Output_GetFrameNumber _Output_GetFrameNumber = MyClient.GetFrameNumber();
00194       std::cout << "Frame Number: " << _Output_GetFrameNumber.FrameNumber << std::endl;
00195 
00196       // Get the timecode
00197       Output_GetTimecode _Output_GetTimecode  = MyClient.GetTimecode();
00198 
00199       std::cout << "Timecode: "
00200                 << _Output_GetTimecode.Hours               << "h "
00201                 << _Output_GetTimecode.Minutes             << "m " 
00202                 << _Output_GetTimecode.Seconds             << "s "
00203                 << _Output_GetTimecode.Frames              << "f "
00204                 << _Output_GetTimecode.SubFrame            << "sf "
00205                 << Adapt( _Output_GetTimecode.FieldFlag ) << " " 
00206                 << _Output_GetTimecode.Standard            << " " 
00207                 << _Output_GetTimecode.SubFramesPerFrame   << " " 
00208                 << _Output_GetTimecode.UserBits            << std::endl << std::endl;
00209 
00210       // Get the latency
00211       std::cout << "Latency: " << MyClient.GetLatencyTotal().Total << "s" << std::endl;
00212       
00213       for( unsigned int LatencySampleIndex = 0 ; LatencySampleIndex < MyClient.GetLatencySampleCount().Count ; ++LatencySampleIndex )
00214       {
00215         std::string SampleName  = MyClient.GetLatencySampleName( LatencySampleIndex ).Name;
00216         double      SampleValue = MyClient.GetLatencySampleValue( SampleName ).Value;
00217 
00218         std::cout << "  " << SampleName << " " << SampleValue << "s" << std::endl;
00219       }
00220       std::cout << std::endl;
00221 
00222       // Count the number of subjects
00223       unsigned int SubjectCount = MyClient.GetSubjectCount().SubjectCount;
00224       std::cout << "Subjects (" << SubjectCount << "):" << std::endl;
00225       for( unsigned int SubjectIndex = 0 ; SubjectIndex < SubjectCount ; ++SubjectIndex )
00226       {
00227         std::cout << "  Subject #" << SubjectIndex << std::endl;
00228 
00229         // Get the subject name
00230         std::string SubjectName = MyClient.GetSubjectName( SubjectIndex ).SubjectName;
00231         std::cout << "            Name: " << SubjectName << std::endl;
00232 
00233         // Get the root segment
00234         std::string RootSegment = MyClient.GetSubjectRootSegmentName( SubjectName ).SegmentName;
00235         std::cout << "    Root Segment: " << RootSegment << std::endl;
00236 
00237         // Count the number of segments
00238         unsigned int SegmentCount = MyClient.GetSegmentCount( SubjectName ).SegmentCount;
00239         std::cout << "    Segments (" << SegmentCount << "):" << std::endl;
00240         for( unsigned int SegmentIndex = 0 ; SegmentIndex < SegmentCount ; ++SegmentIndex )
00241         {
00242           std::cout << "      Segment #" << SegmentIndex << std::endl;
00243 
00244           // Get the segment name
00245           std::string SegmentName = MyClient.GetSegmentName( SubjectName, SegmentIndex ).SegmentName;
00246           std::cout << "          Name: " << SegmentName << std::endl;
00247 
00248           // Get the segment parent
00249           std::string SegmentParentName = MyClient.GetSegmentParentName( SubjectName, SegmentName ).SegmentName;
00250           std::cout << "        Parent: " << SegmentParentName << std::endl;
00251 
00252           // Get the segment's children
00253           unsigned int ChildCount = MyClient.GetSegmentChildCount( SubjectName, SegmentName ).SegmentCount;
00254           std::cout << "     Children (" << ChildCount << "):" << std::endl;
00255           for( unsigned int ChildIndex = 0 ; ChildIndex < ChildCount ; ++ChildIndex )
00256           {
00257             std::string ChildName = MyClient.GetSegmentChildName( SubjectName, SegmentName, ChildIndex ).SegmentName;
00258             std::cout << "       " << ChildName << std::endl;
00259           }
00260 
00261           // Get the static segment translation
00262           Output_GetSegmentStaticTranslation _Output_GetSegmentStaticTranslation = 
00263             MyClient.GetSegmentStaticTranslation( SubjectName, SegmentName );
00264           std::cout << "        Static Translation: (" << _Output_GetSegmentStaticTranslation.Translation[ 0 ]  << ", " 
00265                                                        << _Output_GetSegmentStaticTranslation.Translation[ 1 ]  << ", " 
00266                                                        << _Output_GetSegmentStaticTranslation.Translation[ 2 ]  << ") " << std::endl;
00267 
00268           // Get the static segment rotation in helical co-ordinates
00269           Output_GetSegmentStaticRotationHelical _Output_GetSegmentStaticRotationHelical = 
00270             MyClient.GetSegmentStaticRotationHelical( SubjectName, SegmentName );
00271           std::cout << "        Static Rotation Helical: (" << _Output_GetSegmentStaticRotationHelical.Rotation[ 0 ]     << ", " 
00272                                                             << _Output_GetSegmentStaticRotationHelical.Rotation[ 1 ]     << ", " 
00273                                                             << _Output_GetSegmentStaticRotationHelical.Rotation[ 2 ]     << ") " << std::endl;
00274 
00275           // Get the static segment rotation as a matrix
00276           Output_GetSegmentStaticRotationMatrix _Output_GetSegmentStaticRotationMatrix = 
00277             MyClient.GetSegmentStaticRotationMatrix( SubjectName, SegmentName );
00278           std::cout << "        Static Rotation Matrix: (" << _Output_GetSegmentStaticRotationMatrix.Rotation[ 0 ]     << ", " 
00279                                                            << _Output_GetSegmentStaticRotationMatrix.Rotation[ 1 ]     << ", " 
00280                                                            << _Output_GetSegmentStaticRotationMatrix.Rotation[ 2 ]     << ", " 
00281                                                            << _Output_GetSegmentStaticRotationMatrix.Rotation[ 3 ]     << ", " 
00282                                                            << _Output_GetSegmentStaticRotationMatrix.Rotation[ 4 ]     << ", " 
00283                                                            << _Output_GetSegmentStaticRotationMatrix.Rotation[ 5 ]     << ", " 
00284                                                            << _Output_GetSegmentStaticRotationMatrix.Rotation[ 6 ]     << ", " 
00285                                                            << _Output_GetSegmentStaticRotationMatrix.Rotation[ 7 ]     << ", " 
00286                                                            << _Output_GetSegmentStaticRotationMatrix.Rotation[ 8 ]     << ") " << std::endl;
00287 
00288           // Get the static segment rotation in quaternion co-ordinates
00289           Output_GetSegmentStaticRotationQuaternion _Output_GetSegmentStaticRotationQuaternion = 
00290             MyClient.GetSegmentStaticRotationQuaternion( SubjectName, SegmentName );
00291           std::cout << "        Static Rotation Quaternion: (" << _Output_GetSegmentStaticRotationQuaternion.Rotation[ 0 ]     << ", " 
00292                                                                << _Output_GetSegmentStaticRotationQuaternion.Rotation[ 1 ]     << ", " 
00293                                                                << _Output_GetSegmentStaticRotationQuaternion.Rotation[ 2 ]     << ", " 
00294                                                                << _Output_GetSegmentStaticRotationQuaternion.Rotation[ 3 ]     << ") " << std::endl;
00295 
00296           // Get the static segment rotation in EulerXYZ co-ordinates
00297           Output_GetSegmentStaticRotationEulerXYZ _Output_GetSegmentStaticRotationEulerXYZ = 
00298             MyClient.GetSegmentStaticRotationEulerXYZ( SubjectName, SegmentName );
00299           std::cout << "        Static Rotation EulerXYZ: (" << _Output_GetSegmentStaticRotationEulerXYZ.Rotation[ 0 ]     << ", " 
00300                                                              << _Output_GetSegmentStaticRotationEulerXYZ.Rotation[ 1 ]     << ", " 
00301                                                              << _Output_GetSegmentStaticRotationEulerXYZ.Rotation[ 2 ]     << ") " << std::endl;
00302 
00303           // Get the global segment translation
00304           Output_GetSegmentGlobalTranslation _Output_GetSegmentGlobalTranslation = 
00305             MyClient.GetSegmentGlobalTranslation( SubjectName, SegmentName );
00306           std::cout << "        Global Translation: (" << _Output_GetSegmentGlobalTranslation.Translation[ 0 ]  << ", " 
00307                                                        << _Output_GetSegmentGlobalTranslation.Translation[ 1 ]  << ", " 
00308                                                        << _Output_GetSegmentGlobalTranslation.Translation[ 2 ]  << ") " 
00309                                                        << Adapt( _Output_GetSegmentGlobalTranslation.Occluded ) << std::endl;
00310 
00311           // Get the global segment rotation in helical co-ordinates
00312           Output_GetSegmentGlobalRotationHelical _Output_GetSegmentGlobalRotationHelical = 
00313             MyClient.GetSegmentGlobalRotationHelical( SubjectName, SegmentName );
00314           std::cout << "        Global Rotation Helical: (" << _Output_GetSegmentGlobalRotationHelical.Rotation[ 0 ]     << ", " 
00315                                                             << _Output_GetSegmentGlobalRotationHelical.Rotation[ 1 ]     << ", " 
00316                                                             << _Output_GetSegmentGlobalRotationHelical.Rotation[ 2 ]     << ") " 
00317                                                             << Adapt( _Output_GetSegmentGlobalRotationHelical.Occluded ) << std::endl;
00318 
00319           // Get the global segment rotation as a matrix
00320           Output_GetSegmentGlobalRotationMatrix _Output_GetSegmentGlobalRotationMatrix = 
00321             MyClient.GetSegmentGlobalRotationMatrix( SubjectName, SegmentName );
00322           std::cout << "        Global Rotation Matrix: (" << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 0 ]     << ", " 
00323                                                            << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 1 ]     << ", " 
00324                                                            << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 2 ]     << ", " 
00325                                                            << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 3 ]     << ", " 
00326                                                            << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 4 ]     << ", " 
00327                                                            << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 5 ]     << ", " 
00328                                                            << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 6 ]     << ", " 
00329                                                            << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 7 ]     << ", " 
00330                                                            << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 8 ]     << ") " 
00331                                                            << Adapt( _Output_GetSegmentGlobalRotationMatrix.Occluded ) << std::endl;
00332 
00333           // Get the global segment rotation in quaternion co-ordinates
00334           Output_GetSegmentGlobalRotationQuaternion _Output_GetSegmentGlobalRotationQuaternion = 
00335             MyClient.GetSegmentGlobalRotationQuaternion( SubjectName, SegmentName );
00336           std::cout << "        Global Rotation Quaternion: (" << _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 0 ]     << ", " 
00337                                                                << _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 1 ]     << ", " 
00338                                                                << _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 2 ]     << ", " 
00339                                                                << _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 3 ]     << ") " 
00340                                                                << Adapt( _Output_GetSegmentGlobalRotationQuaternion.Occluded ) << std::endl;
00341 
00342           // Get the global segment rotation in EulerXYZ co-ordinates
00343           Output_GetSegmentGlobalRotationEulerXYZ _Output_GetSegmentGlobalRotationEulerXYZ = 
00344             MyClient.GetSegmentGlobalRotationEulerXYZ( SubjectName, SegmentName );
00345           std::cout << "        Global Rotation EulerXYZ: (" << _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 0 ]     << ", " 
00346                                                              << _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 1 ]     << ", " 
00347                                                              << _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 2 ]     << ") " 
00348                                                              << Adapt( _Output_GetSegmentGlobalRotationEulerXYZ.Occluded ) << std::endl;
00349 
00350           // Get the local segment translation
00351           Output_GetSegmentLocalTranslation _Output_GetSegmentLocalTranslation = 
00352             MyClient.GetSegmentLocalTranslation( SubjectName, SegmentName );
00353           std::cout << "        Local Translation: (" << _Output_GetSegmentLocalTranslation.Translation[ 0 ]  << ", " 
00354                                                       << _Output_GetSegmentLocalTranslation.Translation[ 1 ]  << ", " 
00355                                                       << _Output_GetSegmentLocalTranslation.Translation[ 2 ]  << ") " 
00356                                                       << Adapt( _Output_GetSegmentLocalTranslation.Occluded ) << std::endl;
00357 
00358           // Get the local segment rotation in helical co-ordinates
00359           Output_GetSegmentLocalRotationHelical _Output_GetSegmentLocalRotationHelical = 
00360             MyClient.GetSegmentLocalRotationHelical( SubjectName, SegmentName );
00361           std::cout << "        Local Rotation Helical: (" << _Output_GetSegmentLocalRotationHelical.Rotation[ 0 ]     << ", " 
00362                                                            << _Output_GetSegmentLocalRotationHelical.Rotation[ 1 ]     << ", " 
00363                                                            << _Output_GetSegmentLocalRotationHelical.Rotation[ 2 ]     << ") " 
00364                                                            << Adapt( _Output_GetSegmentLocalRotationHelical.Occluded ) << std::endl;
00365 
00366           // Get the local segment rotation as a matrix
00367           Output_GetSegmentLocalRotationMatrix _Output_GetSegmentLocalRotationMatrix = 
00368             MyClient.GetSegmentLocalRotationMatrix( SubjectName, SegmentName );
00369           std::cout << "        Local Rotation Matrix: (" << _Output_GetSegmentLocalRotationMatrix.Rotation[ 0 ]     << ", " 
00370                                                           << _Output_GetSegmentLocalRotationMatrix.Rotation[ 1 ]     << ", " 
00371                                                           << _Output_GetSegmentLocalRotationMatrix.Rotation[ 2 ]     << ", " 
00372                                                           << _Output_GetSegmentLocalRotationMatrix.Rotation[ 3 ]     << ", " 
00373                                                           << _Output_GetSegmentLocalRotationMatrix.Rotation[ 4 ]     << ", " 
00374                                                           << _Output_GetSegmentLocalRotationMatrix.Rotation[ 5 ]     << ", " 
00375                                                           << _Output_GetSegmentLocalRotationMatrix.Rotation[ 6 ]     << ", " 
00376                                                           << _Output_GetSegmentLocalRotationMatrix.Rotation[ 7 ]     << ", " 
00377                                                           << _Output_GetSegmentLocalRotationMatrix.Rotation[ 8 ]     << ") " 
00378                                                           << Adapt( _Output_GetSegmentLocalRotationMatrix.Occluded ) << std::endl;
00379 
00380           // Get the local segment rotation in quaternion co-ordinates
00381           Output_GetSegmentLocalRotationQuaternion _Output_GetSegmentLocalRotationQuaternion = 
00382             MyClient.GetSegmentLocalRotationQuaternion( SubjectName, SegmentName );
00383           std::cout << "        Local Rotation Quaternion: (" << _Output_GetSegmentLocalRotationQuaternion.Rotation[ 0 ]     << ", " 
00384                                                               << _Output_GetSegmentLocalRotationQuaternion.Rotation[ 1 ]     << ", " 
00385                                                               << _Output_GetSegmentLocalRotationQuaternion.Rotation[ 2 ]     << ", " 
00386                                                               << _Output_GetSegmentLocalRotationQuaternion.Rotation[ 3 ]     << ") " 
00387                                                               << Adapt( _Output_GetSegmentLocalRotationQuaternion.Occluded ) << std::endl;
00388 
00389           // Get the local segment rotation in EulerXYZ co-ordinates
00390           Output_GetSegmentLocalRotationEulerXYZ _Output_GetSegmentLocalRotationEulerXYZ = 
00391             MyClient.GetSegmentLocalRotationEulerXYZ( SubjectName, SegmentName );
00392           std::cout << "        Local Rotation EulerXYZ: (" << _Output_GetSegmentLocalRotationEulerXYZ.Rotation[ 0 ]     << ", " 
00393                                                             << _Output_GetSegmentLocalRotationEulerXYZ.Rotation[ 1 ]     << ", " 
00394                                                             << _Output_GetSegmentLocalRotationEulerXYZ.Rotation[ 2 ]     << ") " 
00395                                                             << Adapt( _Output_GetSegmentLocalRotationEulerXYZ.Occluded ) << std::endl;
00396         }
00397 
00398         // Count the number of markers
00399         unsigned int MarkerCount = MyClient.GetMarkerCount( SubjectName ).MarkerCount;
00400         std::cout << "    Markers (" << MarkerCount << "):" << std::endl;
00401         for( unsigned int MarkerIndex = 0 ; MarkerIndex < MarkerCount ; ++MarkerIndex )
00402         {
00403           // Get the marker name
00404           std::string MarkerName = MyClient.GetMarkerName( SubjectName, MarkerIndex ).MarkerName;
00405 
00406           // Get the marker parent
00407           std::string MarkerParentName = MyClient.GetMarkerParentName( SubjectName, MarkerName ).SegmentName;
00408 
00409           // Get the global marker translation
00410           Output_GetMarkerGlobalTranslation _Output_GetMarkerGlobalTranslation =
00411             MyClient.GetMarkerGlobalTranslation( SubjectName, MarkerName );
00412 
00413           std::cout << "      Marker #" << MarkerIndex            << ": "
00414                                         << MarkerName             << " ("
00415                                         << _Output_GetMarkerGlobalTranslation.Translation[ 0 ]  << ", "
00416                                         << _Output_GetMarkerGlobalTranslation.Translation[ 1 ]  << ", "
00417                                         << _Output_GetMarkerGlobalTranslation.Translation[ 2 ]  << ") "
00418                                         << Adapt( _Output_GetMarkerGlobalTranslation.Occluded ) << std::endl;
00419         }
00420       }
00421 
00422       // Get the unlabeled markers
00423       unsigned int UnlabeledMarkerCount = MyClient.GetUnlabeledMarkerCount().MarkerCount;
00424       std::cout << "  Unlabeled Markers (" << UnlabeledMarkerCount << "):" << std::endl;
00425       for( unsigned int UnlabeledMarkerIndex = 0 ; UnlabeledMarkerIndex < UnlabeledMarkerCount ; ++UnlabeledMarkerIndex )
00426       { 
00427         // Get the global marker translation
00428         Output_GetUnlabeledMarkerGlobalTranslation _Output_GetUnlabeledMarkerGlobalTranslation =
00429           MyClient.GetUnlabeledMarkerGlobalTranslation( UnlabeledMarkerIndex );
00430 
00431         std::cout << "      Marker #" << UnlabeledMarkerIndex   << ": ("
00432                                       << _Output_GetUnlabeledMarkerGlobalTranslation.Translation[ 0 ] << ", "
00433                                       << _Output_GetUnlabeledMarkerGlobalTranslation.Translation[ 1 ] << ", "
00434                                       << _Output_GetUnlabeledMarkerGlobalTranslation.Translation[ 2 ] << ") " << std::endl;
00435       }
00436 
00437       // Count the number of devices
00438       unsigned int DeviceCount = MyClient.GetDeviceCount().DeviceCount;
00439       std::cout << "  Devices (" << DeviceCount << "):" << std::endl;
00440       for( unsigned int DeviceIndex = 0 ; DeviceIndex < DeviceCount ; ++DeviceIndex )
00441       {
00442         std::cout << "    Device #" << DeviceIndex << ":" << std::endl;
00443 
00444         // Get the device name and type
00445         Output_GetDeviceName _Output_GetDeviceName = MyClient.GetDeviceName( DeviceIndex );
00446         std::cout << "      Name: " << _Output_GetDeviceName.DeviceName << std::endl;
00447         std::cout << "      Type: " << Adapt( _Output_GetDeviceName.DeviceType ) << std::endl;
00448 
00449         // Count the number of device outputs
00450         unsigned int DeviceOutputCount = MyClient.GetDeviceOutputCount( _Output_GetDeviceName.DeviceName ).DeviceOutputCount;
00451         std::cout << "      Device Outputs (" << DeviceOutputCount << "):" << std::endl;
00452         for( unsigned int DeviceOutputIndex = 0 ; DeviceOutputIndex < DeviceOutputCount ; ++DeviceOutputIndex )
00453         {
00454           // Get the device output name and unit
00455           Output_GetDeviceOutputName _Output_GetDeviceOutputName = 
00456             MyClient.GetDeviceOutputName( _Output_GetDeviceName.DeviceName, DeviceOutputIndex );
00457 
00458           // Get the device output value
00459           Output_GetDeviceOutputValue _Output_GetDeviceOutputValue = 
00460             MyClient.GetDeviceOutputValue( _Output_GetDeviceName.DeviceName, _Output_GetDeviceOutputName.DeviceOutputName );
00461 
00462           std::cout << "        Device Output #" << DeviceOutputIndex                                     << ": "
00463                                                  << _Output_GetDeviceOutputName.DeviceOutputName          << " "
00464                                                  << _Output_GetDeviceOutputValue.Value                    << " " 
00465                                                  << Adapt( _Output_GetDeviceOutputName.DeviceOutputUnit ) << " " 
00466                                                  << Adapt( _Output_GetDeviceOutputValue.Occluded )        << std::endl;
00467         }
00468       }
00469 
00470       // Output the force plate information.
00471       unsigned int ForcePlateCount = MyClient.GetForcePlateCount().ForcePlateCount;
00472       std::cout << "Force Plates: " << ForcePlateCount << std::endl;
00473 
00474       for( unsigned int ForcePlateIndex = 0 ; ForcePlateIndex < ForcePlateCount ; ++ForcePlateIndex )
00475       {
00476         std::cout << "    Force Plate #" << ForcePlateIndex << ":" << std::endl;
00477 
00478         Output_GetGlobalForceVector _Output_GetForceVector = MyClient.GetGlobalForceVector( ForcePlateIndex );
00479         std::cout << "  Force:" << std::endl;
00480         std::cout << "  X: " << _Output_GetForceVector.ForceVector[ 0 ] << std::endl;
00481         std::cout << "  Y: " << _Output_GetForceVector.ForceVector[ 1 ] << std::endl;
00482         std::cout << "  Z: " << _Output_GetForceVector.ForceVector[ 2 ] << std::endl;
00483 
00484         Output_GetGlobalMomentVector _Output_GetMomentVector = MyClient.GetGlobalMomentVector( ForcePlateIndex );
00485         std::cout << "  Moment:" << std::endl;
00486         std::cout << "  X: " << _Output_GetMomentVector.MomentVector[ 0 ] << std::endl;
00487         std::cout << "  Y: " << _Output_GetMomentVector.MomentVector[ 1 ] << std::endl;
00488         std::cout << "  Z: " << _Output_GetMomentVector.MomentVector[ 2 ] << std::endl;
00489 
00490         Output_GetGlobalCentreOfPressure _Output_GetCentreOfPressure = MyClient.GetGlobalCentreOfPressure( ForcePlateIndex );
00491         std::cout << "  Centre Of Pressure:" << std::endl;
00492         std::cout << "  X: " << _Output_GetCentreOfPressure.CentreOfPressure[ 0 ] << std::endl;
00493         std::cout << "  Y: " << _Output_GetCentreOfPressure.CentreOfPressure[ 1 ] << std::endl;
00494         std::cout << "  Z: " << _Output_GetCentreOfPressure.CentreOfPressure[ 2 ] << std::endl;
00495       }
00496     }
00497 
00498     if( TransmitMulticast )
00499     {
00500       MyClient.StopTransmittingMulticast();
00501     }
00502     MyClient.DisableSegmentData();
00503     MyClient.DisableMarkerData();
00504     MyClient.DisableUnlabeledMarkerData();
00505     MyClient.DisableDeviceData();
00506 
00507     // Disconnect and dispose
00508     int t = clock();
00509     std::cout << " Disconnecting..." << std::endl;
00510     MyClient.Disconnect();
00511     int dt = clock() - t;
00512     double secs = (double) (dt)/(double)CLOCKS_PER_SEC;
00513     std::cout << " Disconnect time = " << secs << " secs" << std::endl;
00514 
00515   }
00516 }


vicon_bridge
Author(s): Markus Achtelik
autogenerated on Sun Oct 5 2014 23:47:39