146 {
147 int16_t nbyte = -1;
148 bool multi_cast_mode;
149 OculiiHandshake handshake;
150 OculiiHeader header;
152 auto acquired_packet_pair =
153 std::make_pair<int, std::shared_ptr<uint8_t>>(-1, nullptr);
154
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>>(
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;
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)) {
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
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);
274 power_value = (((*(reinterpret_cast<uint64_t*>(detection->raw))) >> 40) & 0xFFFF);
275 if (multi_cast_mode) {
277 ele_accu = header_elevation_idx;
278 azi_accu = header_azimuth_idx;
279 dopp_accu = header_doppler_idx;
280 rang_accu = header_range_idx;
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);
298 azimuth_index_raw = (((*(reinterpret_cast<uint64_t*>(detection->raw))) >> 20) & 0x03FF);
299 doppler_index_raw = (((*(reinterpret_cast<uint64_t*>(detection->raw))) >> 10) & 0x03FF);
300 range_index_raw = (((*(reinterpret_cast<uint64_t*>(detection->raw))) >> 0) & 0x03FF);
301
302
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(
363 }
364
365 return 0;
366}
struct apollo::drivers::radar::OculiiFooter OculiiFooter
const int OCULII_HANDSHAKE_SIZE