DirectedStreamsUtility.cc
Go to the documentation of this file.
00001 
00038 #ifdef WIN32
00039 #ifndef WIN32_LEAN_AND_MEAN
00040 #define WIN32_LEAN_AND_MEAN 1
00041 #endif
00042 
00043 #include <windows.h>
00044 #include <winsock2.h>
00045 #else
00046 #include <unistd.h>
00047 #include <arpa/inet.h> // htons
00048 #endif
00049 
00050 #include <stdio.h>
00051 #include <stdlib.h>
00052 #include <signal.h>
00053 #include <string.h>
00054 #include <string>
00055 #include <fstream>
00056 #include <iostream>
00057 #include <iomanip>
00058 
00059 #include <Utilities/portability/getopt/getopt.h>
00060 
00061 #include <LibMultiSense/details/utility/Portability.hh>
00062 #include <LibMultiSense/MultiSenseChannel.hh>
00063 
00064 using namespace crl::multisense;
00065 
00066 namespace {  // anonymous
00067 
00068 volatile bool doneG = false;
00069 
00070 void usage(const char *programNameP)
00071 {
00072         std::cerr << "USAGE: " << programNameP << " [<options>]" << std::endl;
00073     std::cerr << "Where <options> are:" << std::endl;
00074         std::cerr << "\t-a <current_address>    : CURRENT IPV4 address (default=10.66.171.21)" << std::endl;
00075         std::cerr << "\t-d <decimation>         : Decimation to apply for the directed streams (default=1)" << std::endl;
00076 
00077     exit(-1);
00078 }
00079 
00080 #ifdef WIN32
00081 BOOL WINAPI signalHandler(DWORD dwCtrlType)
00082 {
00083         std::cerr << "Shutting down on signal: CTRL-C" << std::endl;
00084     doneG = true;
00085     return TRUE;
00086 }
00087 #else
00088 void signalHandler(int sig)
00089 {
00090         std::cerr << "Shutting down on signal: " << strsignal(sig) << std::endl;
00091     doneG = true;
00092 }
00093 #endif
00094 
00095 bool savePgm(const std::string& fileName,
00096              uint32_t           width,
00097              uint32_t           height,
00098              uint32_t           bitsPerPixel,
00099              const void        *dataP)
00100 {
00101     std::ofstream outputStream(fileName.c_str(), std::ios::binary | std::ios::out);
00102 
00103     if (false == outputStream.good()) {
00104                 std::cerr << "Failed to open \"" << fileName << "\"" << std::endl;
00105         return false;
00106     }
00107 
00108     const uint32_t imageSize = height * width;
00109 
00110     switch(bitsPerPixel) {
00111     case 8:
00112     {
00113 
00114         outputStream << "P5\n"
00115                      << width << " " << height << "\n"
00116                      << 0xFF << "\n";
00117 
00118         outputStream.write(reinterpret_cast<const char*>(dataP), imageSize);
00119 
00120         break;
00121     }
00122     case 16:
00123     {
00124         outputStream << "P5\n"
00125                      << width << " " << height << "\n"
00126                      << 0xFFFF << "\n";
00127 
00128         const uint16_t *imageP = reinterpret_cast<const uint16_t*>(dataP);
00129 
00130         for (uint32_t i=0; i<imageSize; ++i) {
00131             uint16_t o = htons(imageP[i]);
00132             outputStream.write(reinterpret_cast<const char*>(&o), sizeof(uint16_t));
00133         }
00134 
00135         break;
00136     }
00137     }
00138 
00139     outputStream.close();
00140     return true;
00141 }
00142 
00143 void ppsCallback(const pps::Header& header,
00144                  void              *userDataP)
00145 {
00146         std::cerr << "PPS: " << header.sensorTime << " ns" << std::endl;
00147 }
00148 
00149 void laserCallback(const lidar::Header& header,
00150                    void                *userDataP)
00151 {
00152     double timeStamp = header.timeStartSeconds + 1e-6 * header.timeStartMicroSeconds;
00153     static double lastTimeStamp = timeStamp;
00154 
00155     if (header.scanId % 100 == 0)
00156         std::cout << "Laser Frequency "   << (1 / (timeStamp - lastTimeStamp)) << std::endl;
00157 
00158     lastTimeStamp = timeStamp;
00159 }
00160 
00161 void imageCallback(const image::Header& header,
00162                    void                *userDataP)
00163 {
00164     Channel *channelP = reinterpret_cast<Channel*>(userDataP);
00165 
00166     double timeStamp = header.timeSeconds + 1e-6 * header.timeMicroSeconds;
00167     static double lastTimeStamp = timeStamp;
00168 
00169     if (header.frameId % 100 == 0)
00170         std::cout << "Left Image Frequency " << (1 / (timeStamp - lastTimeStamp)) << " "
00171                   << "Nominal Frequency " << header.framesPerSecond << std::endl;
00172 
00173     lastTimeStamp = timeStamp;
00174 
00175     static int64_t lastFrameId = -1;
00176 
00177     if (-1 == lastFrameId)
00178         savePgm("left_rect.pgm",
00179                 header.width,
00180                 header.height,
00181                 header.bitsPerPixel,
00182                 header.imageDataP);
00183 
00184     lastFrameId = header.frameId;
00185 
00186     image::Histogram histogram;
00187 
00188         if (Status_Ok != channelP->getImageHistogram(header.frameId, histogram))
00189                 std::cerr << "failed to get histogram for frame " << header.frameId << std::endl;
00190 }
00191 
00192 void disparityCallback(const image::Header& header,
00193                    void                *userDataP)
00194 {
00195     Channel *channelP = reinterpret_cast<Channel*>(userDataP);
00196 
00197     double timeStamp = header.timeSeconds + 1e-6 * header.timeMicroSeconds;
00198     static double lastTimeStamp = timeStamp;
00199 
00200     if (header.frameId % 100 == 0)
00201         std::cout << "Disparity Frequency " << (1 / (timeStamp - lastTimeStamp)) << " "
00202                   << "Nominal Frequency " << header.framesPerSecond << std::endl;
00203 
00204     lastTimeStamp = timeStamp;
00205 
00206     static int64_t lastFrameId = -1;
00207 
00208     if (-1 == lastFrameId)
00209         savePgm("disparity.pgm",
00210                 header.width,
00211                 header.height,
00212                 header.bitsPerPixel,
00213                 header.imageDataP);
00214 
00215     lastFrameId = header.frameId;
00216 
00217     image::Histogram histogram;
00218 
00219         if (Status_Ok != channelP->getImageHistogram(header.frameId, histogram))
00220                 std::cerr << "failed to get histogram for frame " << header.frameId << std::endl;
00221 }
00222 
00223 }; // anonymous
00224 
00225 int main(int    argc, 
00226          char **argvPP)
00227 {
00228     std::string currentAddress = "10.66.171.21";
00229     int32_t mtu = 7200;
00230     uint32_t decimation = 1;
00231 
00232 #if WIN32
00233     SetConsoleCtrlHandler (signalHandler, TRUE);
00234 #else
00235     signal(SIGINT, signalHandler);
00236 #endif
00237 
00238     //
00239     // A directed stream to start
00240     DirectedStream stream;
00241 
00242     //
00243     // A vector to store the directed streams query
00244     std::vector<DirectedStream> queryStreams;
00245 
00246     uint32_t streamIndex;
00247 
00248     //
00249     // The port to stream data to
00250     uint16_t port;
00251 
00252     //
00253     // Parse args
00254 
00255     int c;
00256 
00257     while(-1 != (c = getopt(argc, argvPP, "a:m:d:")))
00258         switch(c) {
00259         case 'a': currentAddress = std::string(optarg);    break;
00260         case 'm': mtu            = atoi(optarg);           break;
00261         case 'd': decimation     = atoi(optarg);           break;
00262         default: usage(*argvPP);                           break;
00263         }
00264 
00265     //
00266     // Initialize communications.
00267 
00268     Channel *channelP = Channel::Create(currentAddress);
00269     if (NULL == channelP) {
00270                 std::cerr << "Failed to establish communications with \"" << currentAddress << "\"" << std::endl;
00271         return -1;
00272     }
00273 
00274     //
00275     // Query version
00276 
00277     Status status;
00278     system::VersionInfo v;
00279 
00280     status = channelP->getVersionInfo(v);
00281     if (Status_Ok != status) {
00282                 std::cerr << "Failed to query sensor version: " << Channel::statusString(status) << std::endl;
00283         goto clean_out;
00284     }
00285 
00286         std::cout << "API build date      :  " << v.apiBuildDate << "\n";
00287     std::cout << "API version         :  0x" << std::hex << std::setw(4) << std::setfill('0') << v.apiVersion << "\n";
00288         std::cout << "Firmware build date :  " << v.sensorFirmwareBuildDate << "\n";
00289         std::cout << "Firmware version    :  0x" << std::hex << std::setw(4) << std::setfill('0') << v.sensorFirmwareVersion << "\n";
00290         std::cout << "Hardware version    :  0x" << std::hex << v.sensorHardwareVersion << "\n";
00291         std::cout << "Hardware magic      :  0x" << std::hex << v.sensorHardwareMagic << "\n";
00292         std::cout << "FPGA DNA            :  0x" << std::hex << v.sensorFpgaDna << "\n";
00293         std::cout << std::dec;
00294 
00295     //
00296     // Change framerate
00297 
00298     {
00299         image::Config cfg;
00300 
00301         status = channelP->getImageConfig(cfg);
00302         if (Status_Ok != status) {
00303                         std::cerr << "Failed to get image config: " << Channel::statusString(status) << std::endl;
00304             goto clean_out;
00305         } else {
00306 
00307             cfg.setResolution(1024, 544);
00308             cfg.setFps(30.0);
00309         
00310             status = channelP->setImageConfig(cfg);
00311             if (Status_Ok != status) {
00312                                 std::cerr << "Failed to configure sensor: " << Channel::statusString(status) << std::endl;
00313                 goto clean_out;
00314             }
00315         }
00316     }
00317 
00318     //
00319     // Change MTU
00320 
00321     status = channelP->setMtu(mtu);
00322     if (Status_Ok != status) {
00323                 std::cerr << "Failed to set MTU to " << mtu << ": " << Channel::statusString(status) << std::endl;
00324         goto clean_out;
00325     }
00326 
00327     //
00328     // Change trigger source
00329 
00330     status = channelP->setTriggerSource(Trigger_Internal);
00331     if (Status_Ok != status) {
00332                 std::cerr << "Failed to set trigger source: " << Channel::statusString(status) << std::endl;
00333         goto clean_out;
00334     }
00335 
00336     //
00337     // Add callbacks
00338 
00339     channelP->addIsolatedCallback(imageCallback, Source_Luma_Rectified_Left, channelP);
00340     channelP->addIsolatedCallback(disparityCallback, Source_Disparity, channelP);
00341     channelP->addIsolatedCallback(laserCallback, channelP);
00342     channelP->addIsolatedCallback(ppsCallback, channelP);
00343 
00344     //
00345     // Get the local UDP port
00346 
00347     status = channelP->getLocalUdpPort(port);
00348 
00349     //
00350     // Start streaming
00351 
00352     stream = DirectedStream(Source_Luma_Rectified_Left | Source_Lidar_Scan | Source_Disparity, "", port, decimation);
00353 
00354     status = channelP->startDirectedStream(stream);
00355     if (Status_Ok != status) {
00356         std::cerr << "Failed to start streams: " << Channel::statusString(status) << std::endl;
00357         goto clean_out;
00358     }
00359 
00360 
00361     //
00362     // Query all the active directed streams
00363 
00364     status = channelP->getDirectedStreams(queryStreams);
00365 
00366     for (streamIndex = 0 ; streamIndex < queryStreams.size() ; ++streamIndex) {
00367         std::cout << "Directed Stream " << streamIndex << std::endl;
00368         std::cout << "Address: " << queryStreams[streamIndex].address << std::endl;
00369         std::cout << "Port: " << std::dec <<  queryStreams[streamIndex].udpPort << std::endl;
00370         std::cout << "Mask: 0x" << std::hex << queryStreams[streamIndex].mask << std::endl;
00371         std::cout << "Decimation: " << std::dec << queryStreams[streamIndex].fpsDecimation << std::endl;
00372         std::cout << std::endl;
00373     }
00374 
00375     while(!doneG)
00376         usleep(100000);
00377 
00378     //
00379     // Stop the directed streams
00380 
00381     status = channelP->stopDirectedStream(stream);
00382     if (Status_Ok != status) {
00383                 std::cerr << "Failed to stop streams: " << Channel::statusString(status) << std::endl;
00384     }
00385 
00386 clean_out:
00387 
00388     Channel::Destroy(channelP);
00389     return 0;
00390 }


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