get_distance.c
Go to the documentation of this file.
1 
11 #include "urg_c/urg_sensor.h"
12 #include "urg_c/urg_utils.h"
13 #include "open_urg_sensor.h"
14 #include <stdlib.h>
15 #include <stdio.h>
16 
17 
18 static void print_data(urg_t *urg, long data[], int data_n, long time_stamp)
19 {
20 #if 1
21  int front_index;
22 
23  (void)data_n;
24 
25  // \~japanese 前方のデータのみを表示
26  front_index = urg_step2index(urg, 0);
27  printf("%ld [mm], (%ld [msec])\n", data[front_index], time_stamp);
28 
29 #else
30  (void)time_stamp;
31 
32  int i;
33  long min_distance;
34  long max_distance;
35 
36  // \~japanese 全てのデータの X-Y の位置を表示
37  urg_distance_min_max(urg, &min_distance, &max_distance);
38  for (i = 0; i < data_n; ++i) {
39  long l = data[i];
40  double radian;
41  long x;
42  long y;
43 
44  if ((l <= min_distance) || (l >= max_distance)) {
45  continue;
46  }
47  radian = urg_index2rad(urg, i);
48  x = (long)(l * cos(radian));
49  y = (long)(l * sin(radian));
50  printf("(%ld, %ld), ", x, y);
51  }
52  printf("\n");
53 #endif
54 }
55 
56 
57 int main(int argc, char *argv[])
58 {
59  enum {
60  CAPTURE_TIMES = 10,
61  };
62  urg_t urg;
63  long *data = NULL;
64  long time_stamp;
65  unsigned long long system_time_stamp;
66  int n;
67  int i;
68 
69  if (open_urg_sensor(&urg, argc, argv) < 0) {
70  return 1;
71  }
72 
73  data = (long *)malloc(urg_max_data_size(&urg) * sizeof(data[0]));
74  if (!data) {
75  perror("urg_max_index()");
76  return 1;
77  }
78 
79  // \~japanese データ取得
80 #if 0
81  // \~japanese データの取得範囲を変更する場合
83  urg_deg2step(&urg, -90),
84  urg_deg2step(&urg, +90), 0);
85 #endif
86 
87  urg_start_measurement(&urg, URG_DISTANCE, CAPTURE_TIMES, 0);
88  for (i = 0; i < CAPTURE_TIMES; ++i) {
89  n = urg_get_distance(&urg, data, &time_stamp, &system_time_stamp);
90  if (n <= 0) {
91  printf("urg_get_distance: %s\n", urg_error(&urg));
92  free(data);
93  urg_close(&urg);
94  return 1;
95  }
96  print_data(&urg, data, n, time_stamp);
97  }
98 
99  // \~japanese 切断
100  free(data);
101  urg_close(&urg);
102 
103 #if defined(URG_MSC)
104  getchar();
105 #endif
106  return 0;
107 }
int urg_deg2step(const urg_t *urg, double degree)
Definition: urg_utils.c:176
int urg_start_measurement(urg_t *urg, urg_measurement_type_t type, int scan_times, int skip_scan)
Definition: urg_sensor.c:877
URG sensor.
Definition: urg_sensor.h:72
double urg_index2rad(const urg_t *urg, int index)
Definition: urg_utils.c:123
void urg_distance_min_max(const urg_t *urg, long *min_distance, long *max_distance)
Definition: urg_utils.c:73
URG sensor.
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 urg_set_scanning_parameter(urg_t *urg, int first_step, int last_step, int skip_step)
Definition: urg_sensor.c:1009
int urg_step2index(const urg_t *urg, int step)
Definition: urg_utils.c:198
int open_urg_sensor(urg_t *urg, int argc, char *argv[])
int urg_get_distance(urg_t *urg, long data[], long *time_stamp, unsigned long long *system_time_stamp)
Definition: urg_sensor.c:934
static void print_data(urg_t *urg, long data[], int data_n, long time_stamp)
Definition: get_distance.c:18
URG sensor utility.
int main(int argc, char *argv[])
Definition: get_distance.c:57
int urg_max_data_size(const urg_t *urg)
Definition: urg_utils.c:114


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