urg_utils.c
Go to the documentation of this file.
1 
9 #include "urg_c/urg_utils.h"
10 #include "urg_c/urg_errno.h"
11 #define _USE_MATH_DEFINES
12 #include <math.h>
13 
14 #undef max
15 #undef min
16 
17 
18 static int max(int a, int b)
19 {
20  return (a > b) ? a : b;
21 }
22 
23 
24 static int min(int a, int b)
25 {
26  return (a < b) ? a : b;
27 }
28 
29 
30 const char *urg_error(const urg_t *urg)
31 {
32  typedef struct
33  {
34  int no;
35  const char* message;
36  } error_messages_t;
37 
38 
39  error_messages_t errors[] = {
40  { URG_NO_ERROR, "no error." },
41  { URG_UNKNOWN_ERROR, "unknown error." },
42  { URG_NOT_CONNECTED, "not connected." },
43  { URG_NOT_IMPLEMENTED, "not implemented." },
44  { URG_INVALID_RESPONSE, "invalid response." },
45  { URG_NO_RESPONSE, "no response." },
46 
47  { URG_SEND_ERROR, "send error." },
48  { URG_RECEIVE_ERROR, "receive error." },
49  { URG_CHECKSUM_ERROR, "checksum error." },
50  { URG_INVALID_PARAMETER, "invalid parameter." },
51  { URG_MEASUREMENT_TYPE_MISMATCH, "measurement type mismatch." },
52 
53  { URG_SERIAL_OPEN_ERROR, "could not open serial device." },
54  { URG_NOT_DETECT_BAUDRATE_ERROR, "could not detect serial baudrate." },
55  { URG_ETHERNET_OPEN_ERROR, "could not open ethernet port." },
56  { URG_SCANNING_PARAMETER_ERROR, "scanning parameter error." },
57  { URG_DATA_SIZE_PARAMETER_ERROR, "data size parameter error." },
58  };
59 
60  int n = sizeof(errors) / sizeof(errors[0]);
61  int i;
62 
63  for (i = 0; i < n; ++i) {
64  if (errors[i].no == urg->last_errno) {
65  return errors[i].message;
66  }
67  }
68 
69  return "Unknown error.";
70 }
71 
72 
73 void urg_distance_min_max(const urg_t *urg,
74  long *min_distance, long *max_distance)
75 {
76  if (!urg->is_active) {
77  *min_distance = 1;
78  *max_distance = 0;
79  return;
80  }
81 
82  *min_distance = urg->min_distance;
83 
84  // urg_set_communication_data_size() を反映した距離を返す
85  *max_distance =
87  max(urg->max_distance, 4095) : urg->max_distance;
88 }
89 
90 
91 void urg_step_min_max(const urg_t *urg, int *min_index, int *max_index)
92 {
93  if (!urg->is_active) {
94  *min_index = 1;
95  *max_index = 0;
96  return;
97  }
98 
99  *min_index = urg->first_data_index - urg->front_data_index;
100  *max_index = urg->last_data_index - urg->front_data_index;
101 }
102 
103 
104 long urg_scan_usec(const urg_t *urg)
105 {
106  if (!urg->is_active) {
107  return URG_NOT_CONNECTED;
108  }
109 
110  return urg->scan_usec;
111 }
112 
113 
114 int urg_max_data_size(const urg_t *urg)
115 {
116  if (!urg->is_active) {
117  return URG_NOT_CONNECTED;
118  }
119  return urg->last_data_index + 1;
120 }
121 
122 
123 double urg_index2rad(const urg_t *urg, int index)
124 {
125  int actual_index;
126  int step;
127 
128  if (!urg->is_active) {
129  return URG_NOT_CONNECTED;
130  }
131 
132  actual_index = min(max(0, index), urg->last_data_index);
133  step = actual_index - urg->front_data_index + urg->received_first_index;
134  return urg_step2rad(urg, step);
135 }
136 
137 
138 double urg_index2deg(const urg_t *urg, int index)
139 {
140  return urg_index2rad(urg, index) * 180.0 / M_PI;
141 }
142 
143 
144 int urg_rad2index(const urg_t *urg, double radian)
145 {
146  int index;
147 
148  if (!urg->is_active) {
149  return URG_NOT_CONNECTED;
150  }
151 
152  index =
153  (int)(floor((urg->area_resolution * radian / (2.0 * M_PI) + 0.5)))
154  + urg->front_data_index;
155 
156  return min(max(0, index), urg->last_data_index);
157 }
158 
159 
160 int urg_deg2index(const urg_t *urg, double degree)
161 {
162  return urg_rad2index(urg, degree * M_PI / 180.0);
163 }
164 
165 
166 int urg_rad2step(const urg_t *urg, double radian)
167 {
168  if (!urg->is_active) {
169  return URG_NOT_CONNECTED;
170  }
171 
172  return urg_rad2index(urg, radian) - urg->front_data_index;
173 }
174 
175 
176 int urg_deg2step(const urg_t *urg, double degree)
177 {
178  return urg_rad2step(urg, degree * M_PI / 180.0);
179 }
180 
181 
182 double urg_step2rad(const urg_t *urg, int step)
183 {
184  if (!urg->is_active) {
185  return URG_NOT_CONNECTED;
186  }
187 
188  return (2.0 * M_PI) * step / urg->area_resolution;
189 }
190 
191 
192 double urg_step2deg(const urg_t *urg, int step)
193 {
194  return urg_step2rad(urg, step) * 180.0 / M_PI;
195 }
196 
197 
198 int urg_step2index(const urg_t *urg, int step)
199 {
200  int measure_step;
201 
202  if (!urg->is_active) {
203  return URG_NOT_CONNECTED;
204  }
205 
206  measure_step = step - urg->received_first_index;
207  return min(max(0, measure_step + urg->front_data_index),
208  urg->last_data_index);
209 }
urg_errno.h
URG ライブラリのエラー定義
urg_t
URG sensor.
Definition: urg_sensor.h:72
urg_max_data_size
int urg_max_data_size(const urg_t *urg)
Definition: urg_utils.c:114
URG_SCANNING_PARAMETER_ERROR
@ URG_SCANNING_PARAMETER_ERROR
Definition: urg_errno.h:31
urg_error
const char * urg_error(const urg_t *urg)
Definition: urg_utils.c:30
urg_t::front_data_index
int front_data_index
Definition: urg_sensor.h:80
urg_t::received_first_index
int received_first_index
Definition: urg_sensor.h:96
URG_MEASUREMENT_TYPE_MISMATCH
@ URG_MEASUREMENT_TYPE_MISMATCH
Definition: urg_errno.h:25
urg_t::max_distance
int max_distance
Definition: urg_sensor.h:84
URG_SEND_ERROR
@ URG_SEND_ERROR
Definition: urg_errno.h:21
urg_t::area_resolution
int area_resolution
Definition: urg_sensor.h:81
urg_t::scan_usec
long scan_usec
Definition: urg_sensor.h:82
URG_INVALID_RESPONSE
@ URG_INVALID_RESPONSE
Definition: urg_errno.h:18
urg_index2deg
double urg_index2deg(const urg_t *urg, int index)
Definition: urg_utils.c:138
URG_ETHERNET_OPEN_ERROR
@ URG_ETHERNET_OPEN_ERROR
Definition: urg_errno.h:30
urg_t::is_active
int is_active
Definition: urg_sensor.h:74
URG_COMMUNICATION_2_BYTE
@ URG_COMMUNICATION_2_BYTE
Definition: urg_sensor.h:50
URG_INVALID_PARAMETER
@ URG_INVALID_PARAMETER
Definition: urg_errno.h:24
URG_NO_RESPONSE
@ URG_NO_RESPONSE
Definition: urg_errno.h:19
URG_NOT_CONNECTED
@ URG_NOT_CONNECTED
Definition: urg_errno.h:16
URG_NOT_DETECT_BAUDRATE_ERROR
@ URG_NOT_DETECT_BAUDRATE_ERROR
Definition: urg_errno.h:29
urg_t::range_data_byte
urg_range_data_byte_t range_data_byte
Definition: urg_sensor.h:89
urg_scan_usec
long urg_scan_usec(const urg_t *urg)
Definition: urg_utils.c:104
urg_step2deg
double urg_step2deg(const urg_t *urg, int step)
Definition: urg_utils.c:192
urg_step_min_max
void urg_step_min_max(const urg_t *urg, int *min_index, int *max_index)
Definition: urg_utils.c:91
urg_utils.h
URG sensor utility.
max
static int max(int a, int b)
Definition: urg_utils.c:18
urg_t::last_data_index
int last_data_index
Definition: urg_sensor.h:79
URG_CHECKSUM_ERROR
@ URG_CHECKSUM_ERROR
Definition: urg_errno.h:23
urg_distance_min_max
void urg_distance_min_max(const urg_t *urg, long *min_distance, long *max_distance)
Definition: urg_utils.c:73
urg_deg2step
int urg_deg2step(const urg_t *urg, double degree)
Definition: urg_utils.c:176
urg_step2rad
double urg_step2rad(const urg_t *urg, int step)
Definition: urg_utils.c:182
URG_SERIAL_OPEN_ERROR
@ URG_SERIAL_OPEN_ERROR
Definition: urg_errno.h:28
min
static int min(int a, int b)
Definition: urg_utils.c:24
URG_NO_ERROR
@ URG_NO_ERROR
Definition: urg_errno.h:14
urg_index2rad
double urg_index2rad(const urg_t *urg, int index)
Definition: urg_utils.c:123
URG_NOT_IMPLEMENTED
@ URG_NOT_IMPLEMENTED
Definition: urg_errno.h:17
URG_DATA_SIZE_PARAMETER_ERROR
@ URG_DATA_SIZE_PARAMETER_ERROR
Definition: urg_errno.h:32
urg_rad2step
int urg_rad2step(const urg_t *urg, double radian)
Definition: urg_utils.c:166
urg_t::min_distance
int min_distance
Definition: urg_sensor.h:83
URG_RECEIVE_ERROR
@ URG_RECEIVE_ERROR
Definition: urg_errno.h:22
URG_UNKNOWN_ERROR
@ URG_UNKNOWN_ERROR
Definition: urg_errno.h:15
urg_rad2index
int urg_rad2index(const urg_t *urg, double radian)
Definition: urg_utils.c:144
urg_t::last_errno
int last_errno
Definition: urg_sensor.h:75
urg_t::first_data_index
int first_data_index
Definition: urg_sensor.h:78
urg_step2index
int urg_step2index(const urg_t *urg, int step)
Definition: urg_utils.c:198
urg_deg2index
int urg_deg2index(const urg_t *urg, double degree)
Definition: urg_utils.c:160


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