timestamp_test.cpp
Go to the documentation of this file.
00001 #include <hokuyo/hokuyo.h>
00002 #include <time.h>
00003 #include <sys/time.h> 
00004 #include <algorithm>
00005 
00006 static uint64_t timeHelper()
00007 {
00008 #if POSIX_TIMERS > 0
00009   struct timespec curtime;
00010   clock_gettime(CLOCK_REALTIME, &curtime);
00011   return (uint64_t)(curtime.tv_sec) * 1000000000 + (uint64_t)(curtime.tv_nsec);
00012 #else
00013   struct timeval timeofday;
00014   gettimeofday(&timeofday,NULL);
00015   return (uint64_t)(timeofday.tv_sec) * 1000000000 + (uint64_t)(timeofday.tv_usec) * 1000;
00016 #endif
00017 }
00018 
00019 template <class C>
00020 C median(std::vector<C> &v)
00021 {
00022   std::vector<long long int>::iterator start  = v.begin();
00023   std::vector<long long int>::iterator end    = v.end();
00024   std::vector<long long int>::iterator median = start + (end - start) / 2;
00025   //std::vector<long long int>::iterator quarter1 = median - (end - start) / 4;
00026   //std::vector<long long int>::iterator quarter2 = median + (end - start) / 4;
00027   std::nth_element(start, median, end);
00028   //long long int medianval = *median;
00029   //std::nth_element(start, quarter1, end);
00030   //long long int quarter1val = *quarter1;
00031   //std::nth_element(start, quarter2, end);
00032   //long long int quarter2val = *quarter2;
00033   return *median;
00034 }
00035 
00036 long long int getClockOffset(hokuyo::Laser &laser, int reps)
00037 {
00038   std::vector<long long int> offset(reps);
00039   int timeout = 1000;
00040 
00041   laser.sendCmd("TM0",timeout);
00042   for (int i = 0; i < reps; i++)
00043   {
00044     long long int prestamp = timeHelper();
00045     laser.sendCmd("TM1",timeout);
00046     long long int hokuyostamp = laser.readTime();
00047     long long int poststamp = timeHelper();
00048     offset[i] = hokuyostamp - (prestamp + poststamp) / 2;
00049     printf("%lli %lli %lli - ", prestamp, hokuyostamp, poststamp);
00050   }
00051   laser.sendCmd("TM2",timeout);
00052 
00053   return median(offset);
00054 }
00055 
00056 long long int getScanOffset(hokuyo::Laser &laser, int reps)
00057 {
00058   std::vector<long long int> offset(reps);
00059   int timeout = 1000;
00060 
00061   if (reps > 99) 
00062     reps = 99;
00063   
00064   if (laser.requestScans(1, -2, 2, 0, 0, reps, timeout) != 0)
00065   {
00066     fprintf(stderr, "Error requesting scan.\n");
00067     return 1;
00068   }
00069 
00070   hokuyo::LaserScan scan;
00071   for (int i = 0; i < reps; i++)
00072   {
00073     laser.serviceScan(scan, timeout);
00074     offset[i] = scan.self_time_stamp - scan.system_time_stamp;
00075     printf("%lli %lli - ", scan.self_time_stamp, scan.system_time_stamp);
00076   }
00077 
00078   return median(offset);
00079 }
00080 
00081 long long int getDataOffset(hokuyo::Laser &laser, int cycles)
00082 {
00083   int ckreps = 1;
00084   int scanreps = 1;
00085   long long int pre = getClockOffset(laser, ckreps);
00086   std::vector<long long int> samples(cycles);
00087   for (int i = 0; i < cycles; i++)
00088   {                                                 
00089     long long int scan = getScanOffset(laser, ckreps);
00090     long long int post = getClockOffset(laser, scanreps);
00091     samples[i] = scan - (post+pre)/2;
00092 //    printf("%lli %lli %lli %lli\n", samples[i], post, pre, scan);
00093 //    fflush(stdout);
00094     pre = post;
00095   }
00096 
00097   printf("%lli\n", median(samples));
00098   return median(samples);
00099 }
00100 
00101 int main(int argc, char **argv)
00102 {
00103   if (argc != 2)
00104   {
00105     fprintf(stderr, "usage: timestamp_test <port>\n");
00106     return 1;
00107   }
00108 
00109   char *port = argv[1];
00110 
00111   hokuyo::Laser laser;
00112 
00113   //printf("# %s\n", laser.getFirmwareVersion().c_str());
00114   //fprintf(stderr, "%s\n", laser.getFirmwareVersion().c_str());
00115 
00116   /*
00117   int timeout = 1000;
00118   laser.sendCmd("TM0",timeout);
00119   long long int pc_start = timeHelper();
00120   laser.sendCmd("TM1",timeout);
00121   long long int hokuyo_start = laser.readTime();
00122   for (int i = 0; i < 10000; i++)
00123   {
00124     laser.sendCmd("TM1",timeout);
00125     long long int hokuyo_time = laser.readTime() - hokuyo_start;
00126     long long int pc_time = timeHelper() - pc_start;
00127     printf("%lli %lli\n", hokuyo_time, pc_time);
00128     fflush(stdout);
00129   }
00130   */
00131   /*int timeout = 1000;
00132   for (int i = 0; i < 10; i++)
00133   {
00134     fprintf(stderr, ".");
00135     laser.sendCmd("TM0",timeout);
00136     laser.sendCmd("TM1",timeout);
00137     long long int hokuyo_time = laser.readTime();
00138     long long int pc_time = timeHelper();
00139     laser.sendCmd("TM2",timeout);
00140    
00141     if (laser.requestScans(0, -1, 1, 0, 0, 1, timeout) != 0)
00142     {
00143       fprintf(stderr, "Error requesting scan.\n");
00144       return 1;
00145     }
00146 
00147     hokuyo::LaserScan scan;
00148     laser.serviceScan(scan, timeout);
00149     long long int hokuyo_time2 = scan.self_time_stamp;
00150     long long int pc_time2 = scan.system_time_stamp;
00151 
00152     printf("%lli %lli %lli %lli\n", hokuyo_time, pc_time, hokuyo_time2, pc_time2);
00153     fflush(stdout);
00154   }*/
00155 
00156   //long long int initial_offset = getClockOffset(laser, 20);
00157   //long long int end_offset = getClockOffset(laser, 20);
00158   
00159   /*while (true)
00160   {
00161     getDataOffset(laser, 1);
00162     fflush(stdout);
00163   } */
00164   
00165   hokuyo::LaserScan  scan_;
00166 
00167   laser.open(port);
00168   while (true)
00169   {
00170     try {
00171       //std::string status = laser.getStatus();
00172       //if (status == std::string("Sensor works well."))
00173       {
00174         laser.laserOn(); 
00175         laser.requestScans(true, -2.3562, 2.3562, 0, 0);
00176         //laser.serviceScan(scan_);
00177         laser.serviceScan(scan_);
00178       }
00179       //else
00180       //  fprintf(stderr, "%s\n", laser.getStatus().c_str());
00181     }
00182     catch (hokuyo::Exception e)
00183     {
00184       fprintf(stderr, "%s\n", e.what());
00185     }
00186     try {
00187         laser.stopScanning();
00188     }
00189     catch (hokuyo::Exception e)
00190     {
00191       fprintf(stderr, "%s\n", e.what());
00192     }
00193     //usleep(100000);
00194     try {
00195       //laser.close();
00196     }
00197     catch (hokuyo::Exception e)
00198     {
00199       fprintf(stderr, "%s\n", e.what());
00200     }
00201   }
00202 }


hokuyo_node
Author(s): Brian P. Gerkey, Jeremy Leibs, Blaise Gassend
autogenerated on Tue Apr 22 2014 18:56:48