sync_time_stamp.c
Go to the documentation of this file.
1 
10 #include "urg_c/urg_sensor.h"
11 #include "urg_c/urg_utils.h"
12 #include "open_urg_sensor.h"
13 #include <stdio.h>
14 #if defined(URG_WINDOWS_OS)
15 #include <time.h>
16 #else
17 #include <sys/time.h>
18 #endif
19 
20 static int pc_msec_time(void)
21 {
22  static int is_initialized = 0;
23 #if defined(URG_WINDOWS_OS)
24  static clock_t first_clock;
25  clock_t current_clock;
26 #else
27  static struct timeval first_time;
28  struct timeval current_time;
29 #endif
30  long msec_time;
31 
32 #if defined(URG_WINDOWS_OS)
33  if (!is_initialized) {
34  first_clock = clock();
35  is_initialized = 1;
36  }
37  current_clock = clock();
38  msec_time = (current_clock - first_clock) * 1000 / CLOCKS_PER_SEC;
39 #else
40  if (!is_initialized) {
41  gettimeofday(&first_time, NULL);
42  is_initialized = 1;
43  }
44  gettimeofday(&current_time, NULL);
45 
46  msec_time =
47  ((current_time.tv_sec - first_time.tv_sec) * 1000) +
48  ((current_time.tv_usec - first_time.tv_usec) / 1000);
49 #endif
50  return msec_time;
51 }
52 
53 
58 static long print_time_stamp(urg_t *urg, long time_stamp_offset)
59 {
60  long sensor_time_stamp;
61  long pc_time_stamp;
62  long before_pc_time_stamp;
63  long after_pc_time_stamp;
64  long delay;
65 
67 
68  before_pc_time_stamp = pc_msec_time();
69  sensor_time_stamp = urg_time_stamp(urg);
70  after_pc_time_stamp = pc_msec_time();
71  delay = (after_pc_time_stamp - before_pc_time_stamp) / 2;
72 
73  if (sensor_time_stamp < 0) {
74  printf("urg_time_stamp: %s\n", urg_error(urg));
75  return -1;
76  }
77  sensor_time_stamp -= time_stamp_offset;
78 
79  pc_time_stamp = pc_msec_time();
81 
82  printf("%ld,\t%ld\n", pc_time_stamp, sensor_time_stamp);
83 
84  return sensor_time_stamp - (pc_time_stamp - delay);
85 }
86 
87 
88 int main(int argc, char *argv[])
89 {
90  enum {
91  TIME_STAMP_PRINT_TIMES = 5,
92  };
93 
94  urg_t urg;
95  long time_stamp_offset;
96  int i;
97 
98  if (open_urg_sensor(&urg, argc, argv) < 0) {
99  return 1;
100  }
101 
102  printf("# pc,\tsensor\n");
103 
104  // \~japanese URG のタイムスタンプと PC のタイムスタンプを表示
105  time_stamp_offset = print_time_stamp(&urg, 0);
106 
107  // \~japanese URG の補正後のタイムスタンプと PC タイムスタンプを表示
108  for (i = 0; i < TIME_STAMP_PRINT_TIMES; ++i) {
109  print_time_stamp(&urg, time_stamp_offset);
110  }
111 
112  urg_close(&urg);
113 
114 #if defined(URG_MSC)
115  getchar();
116 #endif
117  return 0;
118 }
URG sensor.
Definition: urg_sensor.h:72
int urg_start_time_stamp_mode(urg_t *urg)
Definition: urg_sensor.c:761
URG sensor.
long urg_time_stamp(urg_t *urg)
Definition: urg_sensor.c:780
const char * urg_error(const urg_t *urg)
Definition: urg_utils.c:30
void urg_close(urg_t *urg)
Definition: urg_sensor.c:745
int main(int argc, char *argv[])
static long print_time_stamp(urg_t *urg, long time_stamp_offset)
int open_urg_sensor(urg_t *urg, int argc, char *argv[])
static int pc_msec_time(void)
URG sensor utility.
int urg_stop_time_stamp_mode(urg_t *urg)
Definition: urg_sensor.c:813


urg_c
Author(s): Satofumi Kamimura , Katsumi Kimoto, Adrian Boeing
autogenerated on Wed Jun 10 2020 03:48:10