sync_time_stamp.c
Go to the documentation of this file.
00001 
00010 #include "urg_c/urg_sensor.h"
00011 #include "urg_c/urg_utils.h"
00012 #include "open_urg_sensor.h"
00013 #include <stdio.h>
00014 #if defined(URG_WINDOWS_OS)
00015 #include <time.h>
00016 #else
00017 #include <sys/time.h>
00018 #endif
00019 
00020 static int pc_msec_time(void)
00021 {
00022     static int is_initialized = 0;
00023 #if defined(URG_WINDOWS_OS)
00024     static clock_t first_clock;
00025     clock_t current_clock;
00026 #else
00027     static struct timeval first_time;
00028     struct timeval current_time;
00029 #endif
00030     long msec_time;
00031 
00032 #if defined(URG_WINDOWS_OS)
00033     if (!is_initialized) {
00034         first_clock = clock();
00035         is_initialized = 1;
00036     }
00037     current_clock = clock();
00038     msec_time = (current_clock - first_clock) * 1000 / CLOCKS_PER_SEC;
00039 #else
00040     if (!is_initialized) {
00041         gettimeofday(&first_time, NULL);
00042         is_initialized = 1;
00043     }
00044     gettimeofday(&current_time, NULL);
00045 
00046     msec_time =
00047         ((current_time.tv_sec - first_time.tv_sec) * 1000) +
00048         ((current_time.tv_usec - first_time.tv_usec) / 1000);
00049 #endif
00050     return msec_time;
00051 }
00052 
00053 
00058 static long print_time_stamp(urg_t *urg, long time_stamp_offset)
00059 {
00060     long sensor_time_stamp;
00061     long pc_time_stamp;
00062     long before_pc_time_stamp;
00063     long after_pc_time_stamp;
00064     long delay;
00065 
00066     urg_start_time_stamp_mode(urg);
00067 
00068     before_pc_time_stamp = pc_msec_time();
00069     sensor_time_stamp = urg_time_stamp(urg);
00070     after_pc_time_stamp = pc_msec_time();
00071     delay = (after_pc_time_stamp - before_pc_time_stamp) / 2;
00072 
00073     if (sensor_time_stamp < 0) {
00074         printf("urg_time_stamp: %s\n", urg_error(urg));
00075         return -1;
00076     }
00077     sensor_time_stamp -= time_stamp_offset;
00078 
00079     pc_time_stamp = pc_msec_time();
00080     urg_stop_time_stamp_mode(urg);
00081 
00082     printf("%ld,\t%ld\n", pc_time_stamp, sensor_time_stamp);
00083 
00084     return sensor_time_stamp - (pc_time_stamp - delay);
00085 }
00086 
00087 
00088 int main(int argc, char *argv[])
00089 {
00090     enum {
00091         TIME_STAMP_PRINT_TIMES = 5,
00092     };
00093 
00094     urg_t urg;
00095     long time_stamp_offset;
00096     int i;
00097 
00098     if (open_urg_sensor(&urg, argc, argv) < 0) {
00099         return 1;
00100     }
00101 
00102     printf("# pc,\tsensor\n");
00103 
00104     // \~japanese URG のタイムスタンプと PC のタイムスタンプを表示
00105     time_stamp_offset = print_time_stamp(&urg, 0);
00106 
00107     // \~japanese URG の補正後のタイムスタンプと PC タイムスタンプを表示
00108     for (i = 0; i < TIME_STAMP_PRINT_TIMES; ++i) {
00109         print_time_stamp(&urg, time_stamp_offset);
00110     }
00111 
00112     urg_close(&urg);
00113 
00114 #if defined(URG_MSC)
00115     getchar();
00116 #endif
00117     return 0;
00118 }


urg_c
Author(s): Satofumi Kamimura , Katsumi Kimoto, Adrian Boeing
autogenerated on Wed Aug 26 2015 16:38:27