viewer_sdl.c
Go to the documentation of this file.
1 
10 #include "urg_sensor.h"
11 #include "urg_utils.h"
12 #include "urg_connection.h"
13 #include "plotter_sdl.h"
14 #include <SDL.h>
15 #include <math.h>
16 
17 
18 #if defined(URG_WINDOWS_OS)
19 static const char *serial_device = "COM4";
20 #else
21 static const char *serial_device = "/dev/ttyACM0";
22 #endif
23 static const char *ethernet_address = "192.168.0.10";
24 //static const char *ethernet_address = "localhost";
25 
26 
27 typedef struct
28 {
30  const char *device;
35 } scan_mode_t;
36 
37 
38 static void help_exit(const char *program_name)
39 {
40  printf("URG simple data viewer\n"
41  "usage:\n"
42  " %s [options]\n"
43  "\n"
44  "options:\n"
45  " -h, --help display this help and exit\n"
46  " -i, intensity mode\n"
47  " -m, multiecho mode\n"
48  "\n",
49  program_name);
50 }
51 
52 
53 static void parse_args(scan_mode_t *mode, int argc, char *argv[])
54 {
55  int i;
56 
58  mode->device = serial_device;
59  mode->baudrate_or_port = 115200;
60  mode->is_multiecho = false;
61  mode->is_intensity = false;
62 
63  for (i = 1; i < argc; ++i) {
64  const char *token = argv[i];
65 
66  if (!strcmp(token, "-h") || !strcmp(token, "--help")) {
67  help_exit(argv[0]);
68 
69  } else if (!strcmp(token, "-e")) {
71  mode->device = ethernet_address;
72  mode->baudrate_or_port = 10940;
73 
74  } else if (!strcmp(token, "-m")) {
75  mode->is_multiecho = true;
76  } else if (!strcmp(token, "-i")) {
77  mode->is_intensity = true;
78  }
79  }
80 
81  if (mode->is_multiecho) {
82  mode->measurement_type =
84  } else {
85  mode->measurement_type =
87  }
88 }
89 
90 
91 static void plot_data_point(urg_t *urg, long data[], unsigned short intensity[],
92  int data_n, bool is_multiecho, int offset)
93 {
94  long min_distance;
95  long max_distance;
96  const double radian_offset = M_PI / 2.0;
97  int step = (is_multiecho) ? 3 : 1;
98  int i;
99 
100  urg_distance_min_max(urg, &min_distance, &max_distance);
101 
102  for (i = 0; i < data_n; ++i) {
103  int index = (step * i) + offset;
104  long l = (data) ? data[index] : intensity[index];
105  double rad;
106  float x;
107  float y;
108 
109  if ((l <= min_distance) || (l >= max_distance)) {
110  continue;
111  }
112 
113  rad = urg_index2rad(urg, i) + radian_offset;
114  x = l * cos(rad);
115  y = l * sin(rad);
116  plotter_plot(x, y);
117  }
118 }
119 
120 
121 static void plot_data(urg_t *urg,
122  long data[], unsigned short intensity[], int data_n,
123  bool is_multiecho)
124 {
125  plotter_clear();
126 
127  // 距離
128  plotter_set_color(0x00, 0xff, 0xff);
129  plot_data_point(urg, data, NULL, data_n, is_multiecho, 0);
130 
131  if (is_multiecho) {
132  plotter_set_color(0xff, 0x00, 0xff);
133  plot_data_point(urg, data, NULL, data_n, is_multiecho, 1);
134 
135  plotter_set_color(0x00, 0x00, 0xff);
136  plot_data_point(urg, data, NULL, data_n, is_multiecho, 2);
137  }
138 
139  if (intensity) {
140  // 強度
141  plotter_set_color(0xff, 0xff, 0x00);
142  plot_data_point(urg, NULL, intensity, data_n, is_multiecho, 0);
143 
144  if (is_multiecho) {
145  plotter_set_color(0xff, 0x00, 0x00);
146  plot_data_point(urg, NULL, intensity, data_n, is_multiecho, 1);
147 
148  plotter_set_color(0x00, 0xff, 0x00);
149  plot_data_point(urg, NULL, intensity, data_n, is_multiecho, 2);
150  }
151  }
152 
153  plotter_swap();
154 }
155 
156 
157 int main(int argc, char *argv[])
158 {
159  scan_mode_t mode;
160  urg_t urg;
161  long *data = NULL;
162  unsigned short *intensity = NULL;
163  //long previous_timestamp = 0;
164  long timestamp;
165  int data_size;
166 
167 
168  // 引数の解析
169  parse_args(&mode, argc, argv);
170 
171  // URG に接続
172  if (urg_open(&urg, mode.connection_type,
173  mode.device, mode.baudrate_or_port)) {
174  printf("urg_open: %s\n", urg_error(&urg));
175  return 1;
176  }
177 
178  // データ取得の準備
179  data_size = urg_max_data_size(&urg);
180  if (mode.is_multiecho) {
181  data_size *= 3;
182  }
183  data = malloc(data_size * sizeof(data[0]));
184  if (mode.is_intensity) {
185  intensity = malloc(data_size * sizeof(intensity[0]));
186  }
187 
188  // 画面の作成
189  if (!plotter_initialize(data_size * ((mode.is_intensity) ? 2 : 1))) {
190  return 1;
191  }
192 
193  // データの取得と描画
195  while (1) {
196  int n;
197  //urg_start_measurement(&urg, mode.measurement_type, 1, 0);
198  switch (mode.measurement_type) {
199  case URG_DISTANCE:
200  n = urg_get_distance(&urg, data, &timestamp);
201  break;
202 
204  n = urg_get_distance_intensity(&urg, data, intensity, &timestamp);
205  break;
206 
207  case URG_MULTIECHO:
208  n = urg_get_multiecho(&urg, data, &timestamp);
209  break;
210 
212  n = urg_get_multiecho_intensity(&urg, data, intensity, &timestamp);
213  break;
214 
215  default:
216  n = 0;
217  break;
218  }
219 
220  if (n <= 0) {
221  printf("urg_get_function: %s\n", urg_error(&urg));
222  break;
223  }
224 
225  //fprintf(stderr, "%ld, ", timestamp - previous_timestamp);
226  //previous_timestamp = timestamp;
227 
228  plot_data(&urg, data, intensity, n, mode.is_multiecho);
229  if (plotter_is_quit()) {
230  break;
231  }
232  }
233 
234  // リソースの解放
236  free(intensity);
237  free(data);
238  urg_close(&urg);
239 
240  return 0;
241 }
urg_open
int urg_open(urg_t *urg, urg_connection_type_t connection_type, const char *device_or_address, long baudrate_or_port)
Definition: urg_sensor.c:684
urg_t
URG sensor.
Definition: urg_sensor.h:72
urg_get_multiecho_intensity
int urg_get_multiecho_intensity(urg_t *urg, long data_multi[], unsigned short intensity_multi[], long *time_stamp, unsigned long long *system_time_stamp)
Definition: urg_sensor.c:965
plotter_plot
void plotter_plot(float x, float y)
Definition: plotter_sdl.c:224
urg_sensor.h
URG sensor.
serial_device
static const char * serial_device
Definition: viewer_sdl.c:21
urg_get_distance
int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp)
Definition: urg_sensor.c:934
ethernet_address
static const char * ethernet_address
Definition: viewer_sdl.c:23
urg_connection_type_t
urg_connection_type_t
ʐM^Cv
Definition: urg_connection.h:32
scan_mode_t
Definition: viewer_sdl.c:27
urg_distance_min_max
void urg_distance_min_max(const urg_t *urg, long *min_distance, long *max_distance)
Definition: urg_utils.c:73
plotter_terminate
void plotter_terminate(void)
Definition: plotter_sdl.c:193
scan_mode_t::baudrate_or_port
long baudrate_or_port
Definition: viewer_sdl.c:31
plotter_set_color
void plotter_set_color(unsigned char r, unsigned g, unsigned b)
Definition: plotter_sdl.c:215
urg_get_distance_intensity
int urg_get_distance_intensity(urg_t *urg, long data[], unsigned short intensity[], long *time_stamp, unsigned long long *system_time_stamp)
Definition: urg_sensor.c:943
urg_error
const char * urg_error(const urg_t *urg)
Definition: urg_utils.c:30
urg_index2rad
double urg_index2rad(const urg_t *urg, int index)
Definition: urg_utils.c:123
main
int main(int argc, char *argv[])
Definition: viewer_sdl.c:157
scan_mode_t::measurement_type
urg_measurement_type_t measurement_type
Definition: viewer_sdl.c:32
urg_close
void urg_close(urg_t *urg)
Definition: urg_sensor.c:745
plot_data
static void plot_data(urg_t *urg, long data[], unsigned short intensity[], int data_n, bool is_multiecho)
Definition: viewer_sdl.c:121
help_exit
static void help_exit(const char *program_name)
Definition: viewer_sdl.c:38
plotter_is_quit
bool plotter_is_quit(void)
Definition: plotter_sdl.c:236
urg_get_multiecho
int urg_get_multiecho(urg_t *urg, long data_multi[], long *time_stamp, unsigned long long *system_time_stamp)
Definition: urg_sensor.c:955
plotter_initialize
bool plotter_initialize(int data_size)
Definition: plotter_sdl.c:158
URG_DISTANCE
@ URG_DISTANCE
Definition: urg_sensor.h:36
urg_utils.h
URG sensor utility.
urg_measurement_type_t
urg_measurement_type_t
Definition: urg_sensor.h:35
plotter_sdl.h
Plotter (SDL)
urg_connection.h
ʐM̏
URG_MULTIECHO_INTENSITY
@ URG_MULTIECHO_INTENSITY
Definition: urg_sensor.h:39
URG_SERIAL
@ URG_SERIAL
VA, USB ڑ
Definition: urg_connection.h:33
scan_mode_t::connection_type
urg_connection_type_t connection_type
Definition: viewer_sdl.c:29
URG_SCAN_INFINITY
@ URG_SCAN_INFINITY
Definition: urg_sensor.h:55
plot_data_point
static void plot_data_point(urg_t *urg, long data[], unsigned short intensity[], int data_n, bool is_multiecho, int offset)
Definition: viewer_sdl.c:91
URG_ETHERNET
@ URG_ETHERNET
C[T[lbgڑ
Definition: urg_connection.h:34
URG_MULTIECHO
@ URG_MULTIECHO
Definition: urg_sensor.h:38
urg_max_data_size
int urg_max_data_size(const urg_t *urg)
Definition: urg_utils.c:114
scan_mode_t::is_intensity
bool is_intensity
Definition: viewer_sdl.c:33
scan_mode_t::device
const char * device
Definition: viewer_sdl.c:30
plotter_swap
void plotter_swap(void)
Definition: plotter_sdl.c:206
urg_start_measurement
int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan)
Definition: urg_sensor.c:877
URG_DISTANCE_INTENSITY
@ URG_DISTANCE_INTENSITY
Definition: urg_sensor.h:37
plotter_clear
void plotter_clear(void)
Definition: plotter_sdl.c:199
parse_args
static void parse_args(scan_mode_t *mode, int argc, char *argv[])
Definition: viewer_sdl.c:53
scan_mode_t::is_multiecho
bool is_multiecho
Definition: viewer_sdl.c:34


urg_c
Author(s): Satofumi Kamimura , Katsumi Kimoto, Adrian Boeing
autogenerated on Wed Mar 2 2022 01:08:11