QUYVN
59880871b0
parse_family_b() dùng sai hệ số góc 0.25°/LSB; theo spec Olei chính hãng (Olei.LidarSensor/LidarDataBlock.GetAngleDegrees) AngleRaw là 0.01°/LSB. Sai 25× khiến điểm bị gán nhầm góc → một phòng bị bôi thành vòng tròn trên RViz. Đã verify với thiết bị thật OLELR-1FMI: sau khi sửa ra 2400 điểm/vòng, 0–359.9°, đúng hình học môi trường. - Đổi hệ số góc 0.25° → 0.01° trong parse_family_b(). - Bỏ qua block invalid (AngleRaw >= 0xFF00) theo spec. - Dò ranh giới vòng quay PER-POINT thay vì per-packet (một gói có thể chứa >1 vòng), tránh gộp nhiều vòng vào một scan. - Thêm model LR-1FMI (360°, 0.01°/LSB, ~2400 pts/rev) vào bảng model + kModelTable, đặt "1FMI" trước "1F" để khớp đúng chuỗi tên. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
49 lines
2.1 KiB
C++
49 lines
2.1 KiB
C++
// example.cpp — quick try-out of the OLEI LiDAR driver
|
|
#include "lidarlib/lidar.hpp"
|
|
#include <cstdio>
|
|
|
|
int main() {
|
|
// ── pick a model ────────────────────────────────────────────────────────
|
|
// lidarlib::Driver drv(lidarlib::MODEL_VF); // 2D 360°
|
|
// lidarlib::Driver drv(lidarlib::MODEL_LR1F); // 2D 360°, 50m
|
|
lidarlib::Driver drv(lidarlib::MODEL_VB); // 2D 270°
|
|
|
|
if (!drv.open()) {
|
|
fprintf(stderr, "Không mở được socket\n");
|
|
return 1;
|
|
}
|
|
|
|
// ── option 1: blocking recv ─────────────────────────────────────────────
|
|
for (int i = 0; i < 10; ++i) {
|
|
lidarlib::ScanResult result;
|
|
if (!drv.recv_scan(result, 2000)) {
|
|
fprintf(stderr, "Timeout hoặc lỗi nhận packet\n");
|
|
break;
|
|
}
|
|
const lidarlib::LaserScan& scan = result.scan;
|
|
const lidarlib::ExtraInfo& info = result.info;
|
|
printf("Scan #%d: %zu điểm, ts=%u ms, err=0x%02X, model=%s\n",
|
|
i, scan.ranges.size(), scan.timestamp_ms, info.error_status,
|
|
info.detected_model.c_str());
|
|
|
|
// Print the first few points
|
|
for (size_t j = 0; j < 20 && j < scan.ranges.size(); ++j) {
|
|
float angle_deg = (scan.angle_min + j * scan.angle_increment) * 180.f / 3.14159265f;
|
|
printf(" [%zu] angle=%.2f° dist=%.3fm intensity=%.0f\n",
|
|
j, angle_deg, scan.ranges[j], scan.intensities[j]);
|
|
}
|
|
}
|
|
|
|
// ── option 2: callback (your own loop) ──────────────────────────────────
|
|
// drv.set_scan_callback([](const lidarlib::ScanResult& result) {
|
|
// printf("Got scan: %zu pts\n", result.scan.ranges.size());
|
|
// });
|
|
// while (true) drv.spin_once();
|
|
|
|
drv.close();
|
|
return 0;
|
|
}
|
|
|
|
// Build:
|
|
// g++ -std=c++17 -O2 -Iinclude -o example examples/example.cpp src/olei_lidar.cpp
|