Apollo 10.0
自动驾驶开放平台
apollo::drivers::radar::OculiiRadarUdpParser类 参考

#include <oculii_radar_udp_parser.h>

apollo::drivers::radar::OculiiRadarUdpParser 的协作图:

Public 成员函数

 OculiiRadarUdpParser ()
 constructor
 
 ~OculiiRadarUdpParser ()
 destructor
 
bool Init (uint16_t port)
 initialize radar parser instance
 
int Parse (apollo::drivers::OculiiPointCloud &oculii_output)
 compute coordinate values based on alpha, beta, doppler and range from point
 

详细描述

在文件 oculii_radar_udp_parser.h36 行定义.

构造及析构函数说明

◆ OculiiRadarUdpParser()

apollo::drivers::radar::OculiiRadarUdpParser::OculiiRadarUdpParser ( )

constructor

在文件 oculii_radar_udp_parser.cc44 行定义.

44{}

◆ ~OculiiRadarUdpParser()

apollo::drivers::radar::OculiiRadarUdpParser::~OculiiRadarUdpParser ( )

destructor

在文件 oculii_radar_udp_parser.cc83 行定义.

83 {
84 if (running_.load()) {
85 if (socket_ != 0)
86 close(socket_);
87 running_.exchange(false);
88 }
89}

成员函数说明

◆ Init()

bool apollo::drivers::radar::OculiiRadarUdpParser::Init ( uint16_t  port)

initialize radar parser instance

返回
bool initialization status

在文件 oculii_radar_udp_parser.cc46 行定义.

46 {
47 socket_ = -1;
48 frame_number_ = -1;
49 raw_buffer_ = std::shared_ptr<uint8_t>(
50 new uint8_t[OCULII_MAX_MESSAGE_LENGTH], std::default_delete<uint8_t[]>());
51 queue_ = std::shared_ptr<BoundedQueue<
52 std::pair<int, std::shared_ptr<uint8_t>>>>(
53 new BoundedQueue<std::pair<int, std::shared_ptr<uint8_t>>>());
54 queue_->Init(OCULII_QUEUE_SIZE);
55
56 socket_ = socket(AF_INET, SOCK_DGRAM, 0);
57 if (-1 == socket_) {
58 AERROR << "socket open error";
59 return false;
60 }
61
62 sockaddr_in address;
63 memset(&address, 0, sizeof(address));
64 address.sin_family = AF_INET;
65 address.sin_port = htons(port);
66 address.sin_addr.s_addr = INADDR_ANY;
67
68 if (bind(socket_, reinterpret_cast<sockaddr *>(&address),
69 sizeof(sockaddr)) == -1) {
70 AERROR << "socket bind error, port:" << port;
71 return false;
72 }
73
74 if (fcntl(socket_, F_SETFL, O_NONBLOCK | FASYNC) < 0) {
75 AERROR << "fcntl udp socket error";
76 return false;
77 }
78
79 async_result_ = cyber::Async(&OculiiRadarUdpParser::AsyncUdpInput, this);
80 return true;
81}
#define AERROR
Definition log.h:44
const int OCULII_MAX_MESSAGE_LENGTH
Definition const_val.h:26
const int OCULII_QUEUE_SIZE
Definition const_val.h:25

◆ Parse()

int apollo::drivers::radar::OculiiRadarUdpParser::Parse ( apollo::drivers::OculiiPointCloud oculii_output)

compute coordinate values based on alpha, beta, doppler and range from point

参数
oculii_outputsensor output
返回
int computation status

在文件 oculii_radar_udp_parser.cc145 行定义.

146 {
147 int16_t nbyte = -1;
148 bool multi_cast_mode;
149 OculiiHandshake handshake;
150 OculiiHeader header;
151 OculiiFooter footer;
152 auto acquired_packet_pair =
153 std::make_pair<int, std::shared_ptr<uint8_t>>(-1, nullptr); // NOLINT
154
155 while (nbyte != OCULII_HANDSHAKE_SIZE) {
156 if (!queue_->WaitDequeue(&acquired_packet_pair)) {
157 AERROR << "dequeue failed";
158 return -1;
159 }
160 nbyte = acquired_packet_pair.first;
161 memcpy(reinterpret_cast<void*>(&handshake),
162 reinterpret_cast<void*>(acquired_packet_pair.second.get()), nbyte);
163 }
164 uint32_t data_length = handshake.data_length;
165 uint32_t offset = 0;
166 auto buffer = std::shared_ptr<uint8_t>(
167 new uint8_t[data_length], std::default_delete<uint8_t[]>());
168
169 while (data_length > 0) {
170 auto packet_pair = std::make_pair<int, std::shared_ptr<uint8_t>>( // NOLINT
171 -1, nullptr);
172 if (!queue_->WaitDequeue(&packet_pair)) {
173 AERROR << "dequeue data packet failed";
174 return -1;
175 }
176 int packet_length = packet_pair.first;
177 if (packet_length > OCULII_MAX_MESSAGE_LENGTH) {
178 AERROR << "udp receive data is larger than OCULII_MAX_MESSAGE_LENGTH";
179 return -1;
180 }
181
182 if (offset > handshake.data_length) {
183 AERROR << "the length of data is not corresponding with the handshake";
184 return -1;
185 }
186 memcpy(reinterpret_cast<void*>(buffer.get()+offset),
187 reinterpret_cast<void*>(packet_pair.second.get()), packet_length);
188 offset += packet_length;
189 data_length -= packet_length;
190 }
191
192 if (data_length != 0) {
193 AERROR << "the length of data is not corresponding with the handshake";
194 return -1;
195 }
196
197 int parser_offset = 0;
198 parser_offset += ParseHeader(header,
199 reinterpret_cast<void*>(buffer.get()+parser_offset));
200 if (frame_number_ < 0) {
201 frame_number_ = static_cast<int64_t>(header.frame_number);
202 }
203 else if (frame_number_+1 != static_cast<int64_t>(header.frame_number)) { // NOLINT
204 AWARN << "packet id is not subsequent, last id: " << \
205 frame_number_ << " current id: " << header.frame_number;
206 AWARN << "may caused inference incorrectly";
207 frame_number_ = static_cast<int64_t>(header.frame_number);
208 }
209
210 std::vector<std::shared_ptr<OculiiDetection>> detections;
211 std::vector<std::shared_ptr<OculiiTrack>> tracks;
212
213 for (uint16_t i = 0; i < header.dections_number; i++) {
214 detections.push_back(std::shared_ptr<OculiiDetection>(new OculiiDetection));
215 parser_offset += ParseDection(*(detections[i].get()),
216 reinterpret_cast<void*>(buffer.get()+parser_offset));
217 }
218
219 for (uint16_t i = 0; i < header.tracks_number; i++) {
220 tracks.push_back(std::shared_ptr<OculiiTrack>(new OculiiTrack));
221 parser_offset += ParseTrack(*(tracks[i].get()),
222 reinterpret_cast<void*>(buffer.get()+parser_offset));
223 }
224
225 parser_offset += ParseFooter(footer,
226 reinterpret_cast<void*>(buffer.get()+parser_offset));
227
228 // parse raw data
229 oculii_output.set_packet_id(frame_number_);
230 oculii_output.set_ego_speed(static_cast<float>(header.host_speed) / 100);
231 oculii_output.set_ego_angle(static_cast<float>(header.host_angle) / 100);
232 oculii_output.set_detection_size(header.dections_number);
233 oculii_output.set_track_size(header.tracks_number);
234 oculii_output.set_width(header.dections_number);
235 oculii_output.set_height(1);
236
237 float header_range_idx =
238 static_cast<float>(header.range_accuracy_idx) / 10000;
239 float header_doppler_idx =
240 static_cast<float>(header.doppler_accuracy_idx) / 10000;
241 float header_azimuth_idx =
242 static_cast<float>(header.azimuth_accuracy_idx) / 10000;
243 float header_elevation_idx =
244 static_cast<float>(header.elevation_accuracy_idx) / 10000;
245
246 float footer_range_idx =
247 static_cast<float>(footer.range_accuracy_idx) / 10000;
248 float footer_doppler_idx =
249 static_cast<float>(footer.doppler_accuracy_idx) / 10000;
250 float footer_azimuth_idx =
251 static_cast<float>(footer.azimuth_accuracy_idx) / 10000;
252 float footer_elevation_idx =
253 static_cast<float>(footer.elevation_accuracy_idx) / 10000;
254
255 multi_cast_mode = IsMultiCastMode(
256 header_range_idx, header_doppler_idx, header_azimuth_idx,
257 header_elevation_idx, footer_range_idx, footer_doppler_idx,
258 footer_azimuth_idx, footer_elevation_idx);
259
260 for (uint16_t i = 0; i < header.dections_number; i++) {
261 auto detection = detections[i];
262 auto point = oculii_output.add_point();
263 auto raw_point = oculii_output.add_raw_pointclouds();
264
265 uint16_t dot_flag;
266 uint16_t power_value;
267
268 uint16_t range_index_raw, doppler_index_raw;
269 uint16_t azimuth_index_raw, beta_index_raw;
270 float ele_accu, azi_accu, dopp_accu, rang_accu;
271 float beta_decode, azimuth_decode, doppler_decode, range_decode;
272
273 dot_flag = (((*(reinterpret_cast<uint64_t*>(detection->raw))) >> 56) & 0x40); // NOLINT
274 power_value = (((*(reinterpret_cast<uint64_t*>(detection->raw))) >> 40) & 0xFFFF); // NOLINT
275 if (multi_cast_mode) {
276 if (dot_flag == DETECTION_FLAG::USING_IDX_HEADER) {
277 ele_accu = header_elevation_idx;
278 azi_accu = header_azimuth_idx;
279 dopp_accu = header_doppler_idx;
280 rang_accu = header_range_idx;
281 } else if (dot_flag == DETECTION_FLAG::USING_IDX_FOOTER) {
282 ele_accu = footer_elevation_idx;
283 azi_accu = footer_azimuth_idx;
284 dopp_accu = footer_doppler_idx;
285 rang_accu = footer_range_idx;
286 } else {
287 AERROR << "incorrect flag received";
288 return -1;
289 }
290 } else {
291 ele_accu = header_elevation_idx;
292 azi_accu = header_azimuth_idx;
293 dopp_accu = header_doppler_idx;
294 rang_accu = header_range_idx;
295 }
296
297 beta_index_raw = (((*(reinterpret_cast<uint64_t*>(detection->raw))) >> 30) & 0x03FF); // NOLINT
298 azimuth_index_raw = (((*(reinterpret_cast<uint64_t*>(detection->raw))) >> 20) & 0x03FF); // NOLINT
299 doppler_index_raw = (((*(reinterpret_cast<uint64_t*>(detection->raw))) >> 10) & 0x03FF); // NOLINT
300 range_index_raw = (((*(reinterpret_cast<uint64_t*>(detection->raw))) >> 0) & 0x03FF); // NOLINT
301
302 // label mask
303 if (0x0200 & beta_index_raw)
304 beta_decode = static_cast<int>(
305 (beta_index_raw & 0x01FF) - 0x0200) * ele_accu;
306 else
307 beta_decode = beta_index_raw * ele_accu;
308 if (0x0200 & azimuth_index_raw)
309 azimuth_decode = static_cast<int>(
310 (azimuth_index_raw & 0x01FF) - 0x0200) * azi_accu;
311 else
312 azimuth_decode = azimuth_index_raw * azi_accu;
313 if (0x0200 & doppler_index_raw)
314 doppler_decode = static_cast<int>(
315 (doppler_index_raw & 0x01FF) - 0x0200) * dopp_accu;
316 else
317 doppler_decode = doppler_index_raw * dopp_accu;
318
319 range_decode = range_index_raw * rang_accu;
320
321 raw_point->set_elevation(beta_decode);
322 raw_point->set_azimuth(azimuth_decode);
323 raw_point->set_doppler(doppler_decode);
324 raw_point->set_range(range_decode);
325 raw_point->set_power(static_cast<float>(power_value) / 100);
326
327 Eigen::Translation3d trans(Eigen::Vector3d(0, 0, 0));
328 Eigen::Quaterniond quater(-0.5, 0.5, -0.5, 0.5);
329 Eigen::Affine3d radar2lidar_pose =
330 trans * quater.toRotationMatrix();
331
332 float point_x = std::sin(raw_point->azimuth() * DEG2RAD) * \
333 std::cos(raw_point->elevation() * DEG2RAD) * raw_point->range();
334 float point_y = std::sin(raw_point->elevation() * DEG2RAD) * \
335 raw_point->range();
336 float point_z = std::cos(raw_point->azimuth() * DEG2RAD) * \
337 std::cos(raw_point->elevation() * DEG2RAD) * raw_point->range();
338
339 Eigen::Vector3d vec(point_x, point_y, point_z);
340 vec = radar2lidar_pose * vec;
341
342 point->set_x(vec.x());
343 point->set_y(vec.y());
344 point->set_z(vec.z());
345
346 point->set_intensity(raw_point->power());
347 point->set_velocity(raw_point->doppler());
348 }
349
350 for (uint16_t i = 0; i < header.tracks_number; i++) {
351 auto track = tracks[i];
352 auto track_res = oculii_output.add_tracks();
353 track_res->set_id(track->id);
354 track_res->set_x_pos(static_cast<float>(track->x_pos) / 100);
355 track_res->set_y_pos(static_cast<float>(track->y_pos) / 100);
356 track_res->set_z_pos(static_cast<float>(track->z_pos) / 100);
357 track_res->set_x_dot(static_cast<float>(track->x_dot) / 100);
358 track_res->set_y_dot(static_cast<float>(track->y_dot) / 100);
359 track_res->set_z_dot(static_cast<float>(track->z_dot) / 100);
360 track_res->set_confidence(track->confidence);
361 track_res->set_track_class(
362 static_cast<OculiiTrackTarget::ObstacleClass>(track->track_class));
363 }
364
365 return 0;
366}
#define AWARN
Definition log.h:43
const float DEG2RAD
Definition const_val.h:54
struct apollo::drivers::radar::OculiiFooter OculiiFooter
const int OCULII_HANDSHAKE_SIZE
Definition const_val.h:29

该类的文档由以下文件生成: