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 {
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 };
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
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
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
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
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
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
00201
00202 if (NULL != logFileNameP) {
00203
00204
00205
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
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
00226
00227 channelP->addIsolatedCallback(imuCallback);
00228
00229
00230
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
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
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 }