174 {
175
176
177
182
184 LslidarPacket *packet = scan->add_firing_pkts();
185 int PKT_DATA_LENGTH = 1212;
186 void *data_ptr = malloc(PKT_DATA_LENGTH);
187 memcpy(data_ptr,
188 reinterpret_cast<uint8_t *>(
190 PKT_DATA_LENGTH);
191 packet->set_data(data_ptr, PKT_DATA_LENGTH);
193 AINFO <<
"scan->firing_pkts_size(): " << scan->firing_pkts_size();
194 }
195
197 int i = 1;
199
200 LslidarPacket *packet = scan->add_firing_pkts();
201 while (true) {
202
203 int rc =
input_->GetPacket(packet);
204
205 if (rc == 0) {
207 time_t t = time(NULL);
212 } else {
213 uint8_t *data = reinterpret_cast<uint8_t *>(
214 const_cast<char *>(packet->data().c_str()));
216 if (0xff == data[1200]) {
217
219 = (data[1201] * pow(2, 32)
220 + data[1202] * pow(2, 24)
221 + data[1203] * pow(2, 16)
222 + data[1204] * pow(2, 8)
223 + data[1205] * pow(2, 0));
225 = (static_cast<uint16_t>(data[1206])
226 + static_cast<uint16_t>(data[1207])
227 * pow(2, 8)
228 + static_cast<uint16_t>(data[1208])
229 * pow(2, 16)
230 + static_cast<uint16_t>(data[1209])
231 * pow(2, 24));
232 } else {
234 = static_cast<uint16_t>(data[1200])
235 + 2000 - 1900;
237 = static_cast<uint16_t>(data[1201]) - 1;
239 = static_cast<uint16_t>(data[1202]);
241 = static_cast<uint16_t>(data[1203]);
243 = static_cast<uint16_t>(data[1204]);
244
248 = static_cast<uint16_t>(data[1205])
249 + 1;
251 = (static_cast<uint16_t>(data[1206])
252 + static_cast<uint16_t>(
253 data[1207])
254 * pow(2, 8)
255 + static_cast<uint16_t>(
256 data[1208])
257 * pow(2, 16)
258 + static_cast<uint16_t>(
259 data[1209])
260 * pow(2, 24))
261 * 1e3;
262 } else if (
268 = static_cast<uint16_t>(data[1205]);
270 = (static_cast<uint16_t>(data[1206])
271 + static_cast<uint16_t>(
272 data[1207])
273 * pow(2, 8)
274 + static_cast<uint16_t>(
275 data[1208])
276 * pow(2, 16)
277 + static_cast<uint16_t>(
278 data[1209])
279 * pow(2, 24));
280 }
283 }
284 } else {
285 uint8_t *data = reinterpret_cast<uint8_t *>(
286 const_cast<char *>(packet->data().c_str()));
288 = (static_cast<uint16_t>(data[1200])
289 + static_cast<uint16_t>(data[1201])
290 * pow(2, 8)
291 + static_cast<uint16_t>(data[1202])
292 * pow(2, 16)
293 + static_cast<uint16_t>(data[1203])
294 * pow(2, 24))
295 * 1e3;
298 }
300 }
302 break;
303 } else if (rc < 0) {
304 return rc;
305 }
306 }
308
309 uint8_t *data = reinterpret_cast<uint8_t *>(
310 const_cast<char *>(packet->data().c_str()));
311 int azi1 = 256 * static_cast<uint16_t>(data[3])
312 + static_cast<uint16_t>(data[2]);
313 int azi2 = 256 * static_cast<uint16_t>(data[1103])
314 + static_cast<uint16_t>(data[1102]);
315
316 if (((azi1 > 35000 && azi2 < 1000)
320 break;
321 }
322 i++;
323 }
325
327 int i = 1;
328 bool is_found_frame_header = false;
330 && !is_found_frame_header) {
331 LslidarPacket *packet = scan->add_firing_pkts();
332 while (true) {
333
334 int rc =
input_->GetPacket(packet);
335 AINFO <<
"[debug ] line: " << __LINE__
336 << " file: " << __FILE__;
337 if (rc == 0) {
339 time_t t = time(NULL);
343 } else {
344 uint8_t *data = reinterpret_cast<uint8_t *>(
345 const_cast<char *>(packet->data().c_str()));
346 if (0xff
347 == static_cast<uint16_t>(data[1194])) {
349 = (static_cast<uint16_t>(data[1195]) * 0
350 + (static_cast<uint16_t>(data[1196])
351 << 24)
352 + (static_cast<uint16_t>(data[1197])
353 << 16)
354 + (static_cast<uint16_t>(data[1198])
355 << 8)
356 + static_cast<uint16_t>(data[1199])
357 * pow(2, 0));
359 = (static_cast<uint16_t>(data[1200]) << 24)
360 + (static_cast<uint16_t>(data[1201]) << 16)
361 + (static_cast<uint16_t>(data[1202]) << 8)
362 + (static_cast<uint16_t>(data[1203]));
364 } else {
365 struct tm cur_time {};
366 memset(&cur_time, 0, sizeof(cur_time));
367 cur_time.tm_sec = static_cast<uint16_t>(data[1199]);
368 cur_time.tm_min = static_cast<uint16_t>(data[1198]);
369 cur_time.tm_hour
370 = static_cast<uint16_t>(data[1197]);
371 cur_time.tm_mday
372 = static_cast<uint16_t>(data[1196]);
373 cur_time.tm_mon
374 = static_cast<uint16_t>(data[1195]) - 1;
375 cur_time.tm_year = static_cast<uint16_t>(data[1194])
376 + 2000 - 1900;
378 = static_cast<uint64_t>(timegm(&cur_time));
380 + (static_cast<uint16_t>(data[1202]) << 8)
381 + (static_cast<uint16_t>(data[1201]) << 16)
382 + (static_cast<uint16_t>(data[1200])
383 << 24);
385 }
386 }
387 break;
388 } else if (rc < 0) {
389 return rc;
390 }
391 }
393 uint8_t *data = reinterpret_cast<uint8_t *>(
394 const_cast<char *>(packet->data().c_str()));
395 int return_mode = static_cast<uint16_t>(data[1205]);
396
397 if (return_mode == 1) {
398 for (size_t point_idx = 0;
399 point_idx < LS_POINTS_PER_PACKET_SINGLE_ECHO;
400 point_idx += 8) {
401 if ((static_cast<uint16_t>(data[point_idx]) == 0xff)
402 && (static_cast<uint16_t>(data[point_idx + 1]) == 0xaa)
403 && (static_cast<uint16_t>(data[point_idx + 2]) == 0xbb)
404 && (static_cast<uint16_t>(data[point_idx + 3]) == 0xcc)
405 && (static_cast<uint16_t>(data[point_idx + 4])
406 == 0xdd)) {
408 is_found_frame_header = true;
409
410 break;
411 }
412 }
413 } else {
414 for (size_t point_idx = 0;
415 point_idx < LS_POINTS_PER_PACKET_DOUBLE_ECHO;
416 point_idx += 12) {
417 if ((static_cast<uint16_t>(data[point_idx]) == 0xff)
418 && (static_cast<uint16_t>(data[point_idx + 1]) == 0xaa)
419 && (static_cast<uint16_t>(data[point_idx + 2]) == 0xbb)
420 && (static_cast<uint16_t>(data[point_idx + 3]) == 0xcc)
421 && (static_cast<uint16_t>(data[point_idx + 4])
422 == 0xdd)) {
424 is_found_frame_header = true;
425 AERROR <<
"\none circle! scan->firing_pkts_size(): "
426 << scan->firing_pkts_size();
427 break;
428 }
429 }
430 }
431 i++;
432 }
433 } else {
434
436 int i = 1;
437 bool is_found_frame_header = false;
439 && !is_found_frame_header) {
440 LslidarPacket *packet = scan->add_firing_pkts();
441 while (true) {
442
443 int rc =
input_->GetPacket(packet);
444 AINFO <<
"[debug ] line: " << __LINE__
445 << " file: " << __FILE__;
446 if (rc == 0) {
452 time_t t = time(NULL);
458 } else {
459 uint8_t *data = reinterpret_cast<uint8_t *>(
460 const_cast<char *>(packet->data().c_str()));
462 = static_cast<uint16_t>(data[1197]);
464 = static_cast<uint16_t>(data[1198]);
466 = static_cast<uint16_t>(data[1199]);
471 = (static_cast<uint16_t>(data[1203])
472 + static_cast<uint16_t>(data[1202])
473 * pow(2, 8)
474 + static_cast<uint16_t>(data[1201])
475 * pow(2, 16)
476 + static_cast<uint16_t>(data[1200])
477 * pow(2, 24))
478 * 1e3;
481 = (static_cast<uint16_t>(data[1203])
482 + static_cast<uint16_t>(data[1202])
483 * pow(2, 8)
484 + static_cast<uint16_t>(data[1201])
485 * pow(2, 16)
486 + static_cast<uint16_t>(data[1200])
487 * pow(2, 24));
488 }
490 }
491 }
492 break;
493 } else if (rc < 0) {
494 return rc;
495 }
496 }
498 uint8_t *data = reinterpret_cast<uint8_t *>(
499 const_cast<char *>(packet->data().c_str()));
500
501 for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET;
502 point_idx += 7) {
503 if ((static_cast<uint16_t>(data[point_idx]) == 0xff)
504 && (static_cast<uint16_t>(data[point_idx + 1]) == 0xaa)
505 && (static_cast<uint16_t>(data[point_idx + 2]) == 0xbb)
506 && (static_cast<uint16_t>(data[point_idx + 3]) == 0xcc)) {
508 is_found_frame_header = true;
509 AERROR <<
"\none circle! scan->firing_pkts_size(): "
510 << scan->firing_pkts_size();
511 break;
512 }
513 }
514 i++;
515 }
516 }
517 return 0;
518}
Cyber has builtin time type Time.
uint64_t ToNanosecond() const
convert time to nanosecond.
static Time Now()
get the current time.