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


vicon_bridge
Author(s):
autogenerated on Mon Sep 14 2015 03:57:50