ImuTestUtility.cc
Go to the documentation of this file.
00001 
00037 #ifdef WIN32
00038 #ifndef WIN32_LEAN_AND_MEAN
00039 #define WIN32_LEAN_AND_MEAN 1
00040 #endif
00041 
00042 #include <windows.h>
00043 #include <winsock2.h>
00044 #else
00045 #include <unistd.h>
00046 #endif
00047 
00048 #include <stdio.h>
00049 #include <stdlib.h>
00050 #include <signal.h>
00051 #include <string.h>
00052 #include <errno.h>
00053 #include <iostream>
00054 #include <iomanip>
00055 
00056 #include <LibMultiSense/details/utility/Portability.hh>
00057 #include <LibMultiSense/MultiSenseChannel.hh>
00058 
00059 #include <Utilities/portability/getopt/getopt.h>
00060 
00061 using namespace crl::multisense;
00062 
00063 namespace {  // anonymous
00064 
00065 volatile bool doneG         = false;
00066 FILE*         logFileP      = stdout;
00067 uint32_t      accel_samples = 0;
00068 uint32_t      gyro_samples  = 0;
00069 uint32_t      mag_samples   = 0;
00070 int64_t       sequence      = -1;
00071 uint32_t      messages      = 0;
00072 uint32_t      dropped       = 0;
00073 
00074 void usage(const char *programNameP) 
00075 {
00076     std::cerr << "USAGE: " << programNameP << " [<options>]" << std::endl;
00077     std::cerr << "Where <options> are:" << std::endl;
00078     std::cerr << "\t-a <ip_address>    : IPV4 address (default=10.66.171.21)" << std::endl;
00079     std::cerr << "\t-m <mtu>           : default=7200" << std::endl;
00080     std::cerr << "\t-f <log_file>      : FILE to log IMU data (stdout by default)" << std::endl;
00081     
00082     exit(-1);
00083 }
00084 
00085 #ifdef WIN32
00086 BOOL WINAPI signalHandler(DWORD dwCtrlType)
00087 {
00088     std::cerr << "Shutting down on signal: CTRL-C" << std::endl;
00089     doneG = true;
00090     return TRUE;
00091 }
00092 #else
00093 void signalHandler(int sig)
00094 {
00095     std::cerr << "Shutting down on signal: " << strsignal(sig) << std::endl;
00096     doneG = true;
00097 }
00098 #endif
00099 
00100 void imuCallback(const imu::Header& header,
00101                  void              *userDataP)
00102 {
00103     std::vector<imu::Sample>::const_iterator it = header.samples.begin();
00104 
00105     for(; it!=header.samples.end(); ++it) {
00106 
00107         const imu::Sample& s = *it;
00108 
00109         switch(s.type) {
00110         case imu::Sample::Type_Accelerometer: accel_samples ++; break;
00111         case imu::Sample::Type_Gyroscope:     gyro_samples ++;  break;
00112         case imu::Sample::Type_Magnetometer:  mag_samples ++;   break;
00113         }
00114 
00115         if (logFileP)
00116             fprintf(logFileP, "%d %.6f %.6f %.6f %.6f\n",
00117                     s.type, 
00118                     s.timeSeconds + 1e-6 * s.timeMicroSeconds,
00119                     s.x, s.y, s.z);
00120     }
00121 
00122     if (-1 == sequence)
00123         sequence = header.sequence;
00124     else if ((sequence + 1) != header.sequence) {
00125         const int32_t d = static_cast<int32_t> (header.sequence - (sequence + 1));
00126         dropped += d;
00127     }
00128 
00129     sequence = header.sequence;
00130     messages ++;
00131 }
00132 
00133 }; // anonymous
00134 
00135 int main(int    argc, 
00136          char **argvPP)
00137 {
00138     std::string currentAddress = "10.66.171.21";
00139     const char *logFileNameP   = NULL;
00140     uint32_t    mtu            = 7200;
00141 
00142 #if WIN32
00143     SetConsoleCtrlHandler (signalHandler, TRUE);
00144 #else
00145     signal(SIGINT, signalHandler);
00146 #endif
00147 
00148     //
00149     // Parse args
00150 
00151     int c;
00152 
00153     while(-1 != (c = getopt(argc, argvPP, "a:f:m:v")))
00154         switch(c) {
00155         case 'a': currentAddress = std::string(optarg);    break;
00156         case 'f': logFileNameP   = optarg;                 break;
00157         case 'm': mtu            = atoi(optarg);           break;
00158         default: usage(*argvPP);                           break;
00159         }
00160 
00161     //
00162     // Initialize communications.
00163 
00164     Channel *channelP = Channel::Create(currentAddress);
00165     if (NULL == channelP) {
00166         std::cerr << "Failed to establish communications with \"" << currentAddress << "\"" << std::endl;
00167         return -1;
00168     }
00169 
00170     //
00171     // Query firmware version
00172 
00173     system::VersionInfo v;
00174 
00175     Status status = channelP->getVersionInfo(v);
00176     if (Status_Ok != status) {
00177         std::cerr << "Failed to query sensor version: " << Channel::statusString(status) << std::endl; 
00178         goto clean_out;
00179     }
00180 
00181     //
00182     // Make sure firmware supports IMU
00183 
00184     if (v.sensorFirmwareVersion <= 0x0202) {
00185         std::cerr << "IMU support requires sensor firmware version v2.3 or greater, sensor is " <<
00186                 "running v" << (v.sensorFirmwareVersion >> 8) << "." << (v.sensorFirmwareVersion & 0xff) << std::endl;
00187         goto clean_out;
00188     }
00189 
00190     //
00191     // Turn off all streams by default
00192 
00193     status = channelP->stopStreams(Source_All);
00194     if (Status_Ok != status) {
00195         std::cerr << "Failed to stop streams: " << Channel::statusString(status) << std::endl; 
00196         goto clean_out;
00197     }
00198 
00199     //
00200     // Was logging requested ?
00201 
00202     if (NULL != logFileNameP) {
00203         
00204         //
00205         // Open the log file
00206 
00207         logFileP = fopen(logFileNameP, "w+");
00208         if (NULL == logFileP) {
00209             std::cerr << "Failed to open \"" << logFileNameP << "\" for writing: " << strerror(errno) << std::endl;
00210             goto clean_out;
00211         }
00212 
00213     }
00214 
00215     //
00216     // Change MTU
00217 
00218     status = channelP->setMtu(mtu);
00219     if (Status_Ok != status) {
00220         std::cerr << "Failed to set MTU to " << mtu << ": " << Channel::statusString(status) << std::endl;
00221         goto clean_out;
00222     }
00223 
00224     //
00225     // Add callbacks
00226 
00227     channelP->addIsolatedCallback(imuCallback);
00228 
00229     //
00230     // Start streaming
00231 
00232     status = channelP->startStreams(Source_Imu);
00233     if (Status_Ok != status) {
00234         std::cerr << "Failed to start streams: " << Channel::statusString(status) << std::endl; 
00235         goto clean_out;
00236     }
00237 
00238     while(!doneG)
00239         usleep(100000);
00240         
00241     //
00242     // Stop streaming
00243 
00244     status = channelP->stopStreams(Source_All);
00245     if (Status_Ok != status) {
00246         std::cerr << "Failed to stop streams: " << Channel::statusString(status) << std::endl;
00247     }
00248 
00249     //
00250     // Report simple stats
00251 
00252     {
00253         int64_t imu_total = accel_samples + gyro_samples + mag_samples;
00254         if (imu_total > 0) {
00255             std::cerr << "IMU samples : " <<
00256                          "total: " << imu_total << ", " <<
00257                          "accel: " << std::fixed << std::setprecision(1) << (100.0 * static_cast<double>(accel_samples) / static_cast<double>(imu_total)) << "%, " <<
00258                          "gyro: " << std::fixed << std::setprecision(1) << (100.0 * static_cast<double>(gyro_samples) / static_cast<double>(imu_total)) << "%, " <<
00259                          "mag: " << std::fixed << std::setprecision(1) << (100.0 * static_cast<double>(mag_samples) / static_cast<double>(imu_total)) << "%" << std::endl;
00260         }
00261         
00262         if (messages > 0) 
00263             std::cerr << "IMU messages: total: " << messages << ", " <<
00264                          "dropped: " << dropped << "(" << std::fixed << std::setprecision(6) << (100* static_cast<double>(dropped) / static_cast<double>(messages+dropped)) << "%)" << std::endl;
00265     }
00266 
00267 clean_out:
00268         
00269     if (logFileNameP)
00270         fclose(logFileP);
00271 
00272     Channel::Destroy(channelP);
00273     return 0;
00274 }


multisense_lib
Author(s):
autogenerated on Thu Aug 27 2015 14:01:11