Fix Family B angle decode + add LR-1FMI model
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>
This commit is contained in:
83
examples/lidar_app.cpp
Normal file
83
examples/lidar_app.cpp
Normal file
@@ -0,0 +1,83 @@
|
||||
// lidar_app.cpp — headless skeleton app and integration template.
|
||||
//
|
||||
// Loads the lidar list from config.json, opens each one through the SINGLE
|
||||
// config function lidarlib::make_lidar() (no per-brand branching), then reads scans
|
||||
// on one thread per lidar and prints a one-line summary. There is no web UI:
|
||||
// edit config.json directly, or build your own GUI on top of this same API.
|
||||
//
|
||||
// What a GUI author keeps: load_config() + make_lidar() + the recv_scan() loop.
|
||||
// What a GUI author replaces: the printf() with their own rendering/persistence,
|
||||
// and save_config() to write edits back.
|
||||
//
|
||||
// ./lidar_app [config.json]
|
||||
#include "lidarlib/lidarlib.hpp"
|
||||
#include <atomic>
|
||||
#include <csignal>
|
||||
#include <cstdio>
|
||||
#include <memory>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
namespace {
|
||||
|
||||
std::atomic<bool> g_running{true};
|
||||
void on_signal(int) { g_running = false; }
|
||||
|
||||
// One reader thread per lidar. Owns the handle for its whole lifetime so the
|
||||
// per-instance receive buffers never race another thread.
|
||||
void run_lidar(lidarlib::LidarConfig cfg) {
|
||||
std::unique_ptr<lidarlib::Lidar> lidar = lidarlib::make_lidar(cfg); // the one config call
|
||||
if (!lidar->open()) {
|
||||
fprintf(stderr, "[%s] khong mo duoc %s %s:%u\n",
|
||||
cfg.name.c_str(), cfg.brand.c_str(), cfg.ip.c_str(), cfg.port);
|
||||
return;
|
||||
}
|
||||
printf("[%s] da mo %s %s:%u (model=%s, inverted=%d)\n",
|
||||
cfg.name.c_str(), cfg.brand.c_str(), cfg.ip.c_str(), cfg.port,
|
||||
cfg.model.c_str(), cfg.inverted);
|
||||
|
||||
while (g_running) {
|
||||
lidarlib::ScanResult result;
|
||||
if (!lidar->recv_scan(result, 1000)) continue; // timeout -> retry
|
||||
|
||||
// Output #1: ROS-shaped LaserScan (same for every lidar)
|
||||
const lidarlib::LaserScan& scan = result.scan;
|
||||
// Output #2: ExtraInfo (fields vary by model/family)
|
||||
const lidarlib::ExtraInfo& info = result.info;
|
||||
|
||||
printf("[%s] %zu diem | ts=%u ms | model=%s | err=0x%02X\n",
|
||||
cfg.name.c_str(), scan.ranges.size(), scan.timestamp_ms,
|
||||
info.detected_model.c_str(), info.error_status);
|
||||
}
|
||||
|
||||
lidar->close();
|
||||
printf("[%s] da dong\n", cfg.name.c_str());
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
setvbuf(stdout, nullptr, _IOLBF, 0); // line-buffer so logs show promptly
|
||||
|
||||
const std::string config_path = (argc > 1) ? argv[1] : "config.json";
|
||||
|
||||
lidarlib::Config cfg = lidarlib::load_config(config_path);
|
||||
lidarlib::save_config(config_path, cfg); // ensure the file exists & is editable
|
||||
|
||||
if (cfg.lidars.empty()) {
|
||||
fprintf(stderr, "Khong co lidar nao trong %s\n", config_path.c_str());
|
||||
return 1;
|
||||
}
|
||||
|
||||
std::signal(SIGINT, on_signal);
|
||||
std::signal(SIGTERM, on_signal);
|
||||
|
||||
std::vector<std::thread> threads;
|
||||
threads.reserve(cfg.lidars.size());
|
||||
for (const auto& lc : cfg.lidars) threads.emplace_back(run_lidar, lc);
|
||||
|
||||
printf("Dang chay %zu lidar tu %s. Ctrl-C de dung.\n",
|
||||
cfg.lidars.size(), config_path.c_str());
|
||||
for (auto& t : threads) t.join();
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user