SaveImageUtility.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 #include <arpa/inet.h> // htons
00047 #endif
00048 
00049 #include <stdio.h>
00050 #include <stdlib.h>
00051 #include <signal.h>
00052 #include <string.h>
00053 #include <string>
00054 #include <fstream>
00055 #include <iostream>
00056 #include <iomanip>
00057 
00058 #include <Utilities/portability/getopt/getopt.h>
00059 
00060 #include <LibMultiSense/details/utility/Portability.hh>
00061 #include <LibMultiSense/MultiSenseChannel.hh>
00062 
00063 using namespace crl::multisense;
00064 
00065 namespace {  // anonymous
00066 
00067 volatile bool doneG = false;
00068 
00069 void usage(const char *programNameP) 
00070 {
00071         std::cerr << "USAGE: " << programNameP << " [<options>]" << std::endl;
00072     std::cerr << "Where <options> are:" << std::endl;
00073         std::cerr << "\t-a <current_address>    : CURRENT IPV4 address (default=10.66.171.21)" << std::endl;
00074     
00075     exit(-1);
00076 }
00077 
00078 #ifdef WIN32
00079 BOOL WINAPI signalHandler(DWORD dwCtrlType)
00080 {
00081         std::cerr << "Shutting down on signal: CTRL-C" << std::endl;
00082     doneG = true;
00083     return TRUE;
00084 }
00085 #else
00086 void signalHandler(int sig)
00087 {
00088         std::cerr << "Shutting down on signal: " << strsignal(sig) << std::endl;
00089     doneG = true;
00090 }
00091 #endif
00092 
00093 bool savePgm(const std::string& fileName,
00094              uint32_t           width,
00095              uint32_t           height,
00096              uint32_t           bitsPerPixel,
00097              const void        *dataP)
00098 {
00099     std::ofstream outputStream(fileName.c_str(), std::ios::binary | std::ios::out);
00100     
00101     if (false == outputStream.good()) {
00102                 std::cerr << "Failed to open \"" << fileName << "\"" << std::endl;
00103         return false;
00104     }
00105 
00106     const uint32_t imageSize = height * width;
00107 
00108     switch(bitsPerPixel) {
00109     case 8: 
00110     {
00111 
00112         outputStream << "P5\n"
00113                      << width << " " << height << "\n"
00114                      << 0xFF << "\n";
00115         
00116         outputStream.write(reinterpret_cast<const char*>(dataP), imageSize);
00117 
00118         break;
00119     }
00120     case 16:
00121     {
00122         outputStream << "P5\n"
00123                      << width << " " << height << "\n"
00124                      << 0xFFFF << "\n";
00125 
00126         const uint16_t *imageP = reinterpret_cast<const uint16_t*>(dataP);
00127         
00128         for (uint32_t i=0; i<imageSize; ++i) {
00129             uint16_t o = htons(imageP[i]);
00130             outputStream.write(reinterpret_cast<const char*>(&o), sizeof(uint16_t));
00131         }
00132 
00133         break;
00134     }
00135     }
00136         
00137     outputStream.close();
00138     return true;
00139 }
00140 
00141 void ppsCallback(const pps::Header& header,
00142                  void              *userDataP)
00143 {
00144         std::cerr << "PPS: " << header.sensorTime << " ns" << std::endl;
00145 }                
00146 
00147 void laserCallback(const lidar::Header& header,
00148                    void                *userDataP)
00149 {
00150 //      std::cerr << "lidar: " << header.pointCount << std::endl;
00151 }
00152 
00153 void imageCallback(const image::Header& header,
00154                    void                *userDataP)
00155 {
00156     Channel *channelP = reinterpret_cast<Channel*>(userDataP);
00157     
00158 //    double timeStamp = header.timeSeconds + 1e-6 * header.timeMicroSeconds;
00159     
00160     static int64_t lastFrameId = -1;
00161 
00162     if (-1 == lastFrameId)
00163     {
00164         savePgm("test.pgm",
00165                 header.width,
00166                 header.height,
00167                 header.bitsPerPixel,
00168                 header.imageDataP);
00169     }
00170 
00171     lastFrameId = header.frameId;
00172 
00173     image::Histogram histogram;
00174 
00175         if (Status_Ok != channelP->getImageHistogram(header.frameId, histogram))
00176                 std::cerr << "failed to get histogram for frame " << header.frameId << std::endl;
00177 }
00178 
00179 }; // anonymous
00180 
00181 int main(int    argc, 
00182          char **argvPP)
00183 {
00184     std::string currentAddress = "10.66.171.21";
00185     int32_t mtu = 7200;
00186 
00187 #if WIN32
00188     SetConsoleCtrlHandler (signalHandler, TRUE);
00189 #else
00190     signal(SIGINT, signalHandler);
00191 #endif
00192 
00193     //
00194     // Parse args
00195 
00196     int c;
00197 
00198     while(-1 != (c = getopt(argc, argvPP, "a:m:")))
00199         switch(c) {
00200         case 'a': currentAddress = std::string(optarg);    break;
00201         case 'm': mtu            = atoi(optarg);           break;
00202         default: usage(*argvPP);                           break;
00203         }
00204 
00205     //
00206     // Initialize communications.
00207 
00208     Channel *channelP = Channel::Create(currentAddress);
00209     if (NULL == channelP) {
00210                 std::cerr << "Failed to establish communications with \"" << currentAddress << "\"" << std::endl;
00211         return -1;
00212     }
00213 
00214     //
00215     // Query version
00216 
00217     Status status;
00218     system::VersionInfo v;
00219     VersionType version;
00220     status = channelP->getSensorVersion(version);
00221     status = channelP->getVersionInfo(v);
00222     if (Status_Ok != status) {
00223                 std::cerr << "Failed to query sensor version: " << Channel::statusString(status) << std::endl;
00224         goto clean_out;
00225     }
00226 
00227         std::cout << "API build date      :  " << v.apiBuildDate << "\n";
00228     std::cout << "API version         :  0x" << std::hex << std::setw(4) << std::setfill('0') << v.apiVersion << "\n";
00229         std::cout << "Firmware build date :  " << v.sensorFirmwareBuildDate << "\n";
00230         std::cout << "Firmware version    :  0x" << std::hex << std::setw(4) << std::setfill('0') << v.sensorFirmwareVersion << "\n";
00231         std::cout << "Hardware version    :  0x" << std::hex << v.sensorHardwareVersion << "\n";
00232         std::cout << "Hardware magic      :  0x" << std::hex << v.sensorHardwareMagic << "\n";
00233         std::cout << "FPGA DNA            :  0x" << std::hex << v.sensorFpgaDna << "\n";
00234         std::cout << std::dec;
00235 
00236     //
00237     // Change framerate
00238 
00239     {
00240         image::Config cfg;
00241 
00242         status = channelP->getImageConfig(cfg);
00243         if (Status_Ok != status) {
00244                         std::cerr << "Failed to get image config: " << Channel::statusString(status) << std::endl;
00245             goto clean_out;
00246         } else {
00247 
00248             cfg.setResolution(1024, 544);
00249             cfg.setFps(30.0);
00250         
00251             status = channelP->setImageConfig(cfg);
00252             if (Status_Ok != status) {
00253                                 std::cerr << "Failed to configure sensor: " << Channel::statusString(status) << std::endl;
00254                 goto clean_out;
00255             }
00256         }
00257     }
00258 
00259     //
00260     // Change MTU
00261 
00262     status = channelP->setMtu(mtu);
00263     if (Status_Ok != status) {
00264                 std::cerr << "Failed to set MTU to " << mtu << ": " << Channel::statusString(status) << std::endl;
00265         goto clean_out;
00266     }
00267 
00268     //
00269     // Change trigger source
00270 
00271     status = channelP->setTriggerSource(Trigger_Internal);
00272     if (Status_Ok != status) {
00273                 std::cerr << "Failed to set trigger source: " << Channel::statusString(status) << std::endl;
00274         goto clean_out;
00275     }
00276 
00277     //
00278     // Add callbacks
00279 
00280     channelP->addIsolatedCallback(imageCallback, Source_All, channelP);
00281     channelP->addIsolatedCallback(laserCallback, channelP);
00282     channelP->addIsolatedCallback(ppsCallback, channelP);
00283 
00284     //
00285     // Start streaming
00286 
00287     status = channelP->startStreams(Source_Luma_Rectified_Left | Source_Lidar_Scan);
00288     if (Status_Ok != status) {
00289                 std::cerr << "Failed to start streams: " << Channel::statusString(status) << std::endl;
00290         goto clean_out;
00291     }
00292 
00293     while(!doneG)
00294     {
00295         system::StatusMessage statusMessage;
00296         status = channelP->getDeviceStatus(statusMessage);
00297 
00298         if (Status_Ok == status) {
00299             std::cout << "Uptime: " << statusMessage.uptime << ", " <<
00300             "SystemOk: " << statusMessage.systemOk << ", " <<
00301             "FPGA Temp: " << statusMessage.fpgaTemperature << ", " <<
00302             "Left Imager Temp: " << statusMessage.leftImagerTemperature << ", " <<
00303             "Right Imager Temp: " << statusMessage.rightImagerTemperature << ", " <<
00304             "Input Voltage: " << statusMessage.inputVoltage << ", " <<
00305             "Input Current: " << statusMessage.inputCurrent << ", " <<
00306             "FPGA Power: " << statusMessage.fpgaPower << ", " <<
00307             "Logic Power: " << statusMessage.logicPower << ", " <<
00308             "Imager Power: " << statusMessage.imagerPower << std::endl;
00309         }
00310 
00311 
00312         usleep(1e6);
00313     }
00314 
00315     status = channelP->stopStreams(Source_All);
00316     if (Status_Ok != status) {
00317                 std::cerr << "Failed to stop streams: " << Channel::statusString(status) << std::endl;
00318     }
00319 
00320 clean_out:
00321 
00322     Channel::Destroy(channelP);
00323     return 0;
00324 }


multisense_lib
Author(s):
autogenerated on Fri Apr 5 2019 02:28:24