xlidar-driver

SDK driver lidar 2D cho Linux, kiến trúc plugin nạp động: mỗi driver là một file .so độc lập, cùng implement một interface chung xlidar::LidarDriverInterface; host chỉ cần facade xlidar::LidarManager (build ra liblidar_manager.so) để khám phá plugin, đọc metadata và tạo instance thiết bị.

Driver đi kèm: Slamtec RPLIDAR (serial), OLEI (UDP), SICK TiM (TCP/CoLa-A), SICK nanoScan3 (UDP safety), ESPE LGA60 (TCP/UDP). Output thống nhất theo định dạng ROS sensor_msgs/LaserScan.

Kiến trúc

xlidar_driver/
├── CMakeLists.txt
├── include/                    # API public — host chỉ include từ đây
│   ├── lidar_interface.hpp     #   LidarDriverInterface, DriverInfo, DeviceConfig,
│   │                           #   LaserScan/ScanResult, ErrorCode, plugin ABI
│   ├── lidar_diagnostics.hpp   #   Diagnostics + bit lỗi từng hãng
│   └── lidar_manager.hpp       #   LidarManager, PluginRegistry, config.json
├── src/
│   ├── CMakeLists.txt          # → liblidar_manager.so
│   ├── lidar_manager.cpp       #   dlopen/dlsym + load/save config.json
│   └── json_mini.hpp
├── plugins/                    # mỗi thư mục → một plugin .so
│   ├── CMakeLists.txt          #   helper xlidar_add_plugin()
│   ├── common/plugin_helpers.hpp
│   ├── driver_rplidar/         # → driver_rplidar.so     (rplidar_c1_driver)
│   ├── driver_olei/            # → driver_olei.so        (olei_lidar_driver)
│   ├── driver_sick_code/       # → driver_sick_code.so   (sick_tim_driver)
│   ├── driver_sick_safety/     # → driver_sick_safety.so (sick_nanoscan3_driver)
│   └── driver_espe/            # → driver_espe.so        (espe_lga60_driver)
├── examples/                   # list_drivers, example, lidar_app
└── docs/diagnostics.md         # nghiên cứu layout dữ liệu chẩn đoán

• LidarManager quét thư mục plugin, dlopen từng .so, resolve hai entry point C rồi đăng ký vào available_drivers() — map <driver_id, PluginRegistry> trong đó PluginRegistry = {DriverInfo, file_path}.

• Plugin ABI — mỗi plugin export đúng hai symbol C:

  XLIDAR_PLUGIN_EXPORT void get_driver_info(xlidar::DriverInfo* out);
  XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface*
      create_driver_instance(const xlidar::DeviceConfig* cfg);
  

• DriverInfo — plugin tự đăng ký danh tính: vendor, model (dòng thiết bị driver phụ trách — một driver có thể cover cả series), driver_id (định danh duy nhất, ví dụ rplidar_c1_driver), description (mô tả ngắn: thiết bị hỗ trợ, transport, ghi chú), kèm metadata cho UI: transport (serial/udp/tcp), transport_selectable, supported_models.

• Plugin được giữ nguyên trong bộ nhớ tới khi manager bị hủy — manager phải sống lâu hơn mọi instance nó tạo ra.

Cài đặt

Yêu cầu: Linux, CMake ≥ 3.16, C++17. Không có dependency ngoài (pthread, dl). Plugin rplidar cần thêm source SDK của Slamtec.

cmake -S . -B build -DCMAKE_BUILD_TYPE=Release \
      -DXLIDAR_RPLIDAR_SDK_DIR=/path/to/rplidar_sdk   # dir chứa include/ + src/
cmake --build build -j"$(nproc)"

Kết quả: build/src/liblidar_manager.so, plugin trong build/plugins/*.so, demo trong build/examples/. Không truyền XLIDAR_RPLIDAR_SDK_DIR thì các vị trí quen thuộc được tự dò (third_party/rplidar_sdk, ../rplidar_sdk/sdk, ../xloc-monorepo/xlocd/deps/rplidar_sdk); không thấy SDK thì plugin rplidar bị bỏ qua, phần còn lại build bình thường.

Tùy chọn: -DXLIDAR_BUILD_EXAMPLES=OFF. Hỗ trợ cmake --install + find_package(xlidar_driver) (target xlidar::lidar_manager).

Sử dụng

Khám phá driver và đọc scan

#include "lidar_manager.hpp"   // kéo theo lidar_interface.hpp

xlidar::LidarManager manager("plugins");
manager.load_all_plugins();

for (const auto& [id, plugin] : manager.available_drivers())
    printf("%s — %s\n", id.c_str(), plugin.info.description.c_str());

xlidar::DeviceConfig cfg;
cfg.driver_id = "sick_tim_driver";
cfg.model     = "SICK-TIM7xx";
cfg.ip        = "192.168.0.1";       // port 0 = port mặc định của driver

auto lidar = manager.create_lidar_device(cfg);   // nullptr nếu driver_id lạ
if (lidar->open() != xlidar::ErrorCode::Ok) {
    fprintf(stderr, "open: %s\n", xlidar::to_string(lidar->last_error()));
    return 1;
}

xlidar::ScanResult r;
if (lidar->recv_scan(r, 1000)) {
    // r.scan : LaserScan  — điểm đo, format ROS
    // r.info : ExtraInfo  — metadata tuỳ model
}

Driver serial (rplidar) dùng cfg.serial_port + cfg.baudrate thay cho ip/port. Xem examples/example.cpp.

Chế độ callback

lidar->set_scan_callback([](const xlidar::ScanResult& r) { /* mỗi vòng quét */ });
while (running) lidar->spin_once();

Callback chỉ phát từ spin_once() — chọn một kiểu bơm dữ liệu: recv_scan() (poll) hoặc callback + spin_once().

Sẵn sàng & chẩn đoán thiết bị

if (!lidar->wait_ready(5000)) { /* chưa có scan sạch nào trong 5s */ }

xlidar::Diagnostics d = lidar->get_diagnostics();
if (d.has_fault()) {
    d.monitor_fault();       // OLEI: motor/giám sát
    d.voltage_fault();       // OLEI: điện áp
    d.temperature_fault();   // OLEI: nhiệt độ
    d.sick_error();          // SICK TiM: device error
    d.pollution_error();     // SICK TiM: kính bẩn nặng
    d.contamination_error(); // nanoScan3: kính bẩn nặng
    d.manipulation();        // nanoScan3: nghi bị che/can thiệp
    d.espe_fault();          // ESPE: từ lỗi thiết bị
    d.rplidar_fault();       // RPLIDAR: health = Error (kèm rplidar_error_code)
    printf("fault: %s\n", xlidar::to_string(d).c_str());
} else if (d.has_warning()) {
    // pollution/contamination warning, rplidar health warning
}

d.healthy() = đã có dữ liệu và không fault. is_ready(max_age_ms) = đang mở + healthy + scan mới nhất chưa quá hạn — diagnostics chỉ refresh qua recv_scan()/spin_once(), nên gọi từ chính thread bơm dữ liệu. Layout chẩn đoán từng giao thức: docs/diagnostics.md.

Xử lý lỗi & lifecycle

open() trả về ErrorCode; last_error() giữ kết quả gần nhất.

ErrorCode	Ý nghĩa
Ok	Thành công
AlreadyOpen	open() khi đang mở — kết nối cũ giữ nguyên
NotOpen	recv_scan()/spin_once() khi chưa open()
InvalidAddress	Chuỗi IP không hợp lệ
PortInUse / BindFailed / SocketError	Lỗi bind/socket (driver UDP)
ConnectionRefused / ConnectionFailed / Timeout	TCP connect bị từ chối / không tới được / quá hạn
HandshakeFailed	Nối được nhưng lệnh start-stream/start-scan thất bại
SerialError	Cổng serial không tồn tại / không mở được (rplidar)
DeviceError	Thiết bị tự báo fault (rplidar health check lúc open())
DeviceDisconnected	Thiết bị đóng kết nối / lỗi recv giữa chừng
InvalidConfig	DeviceConfig không dùng được với driver

Lifecycle an toàn với mọi thứ tự gọi: close() idempotent, open() lặp trả AlreadyOpen, sau close() mở lại được. Mỗi instance độc lập hoàn toàn — mỗi lidar một thread, không cần khóa.

Cấu hình JSON

lidar_app (binary demo) đọc config.json, mở mỗi lidar một thread; API: xlidar::load_config(path) / save_config(path, cfg) / manager.create_from_config_file(path).

{
  "lidars": [
    {"name":"front", "driver_id":"olei_lidar_driver",   "model":"AUTO",        "ip":"192.168.100.100", "port":2368},
    {"name":"sick1", "driver_id":"sick_tim_driver",     "model":"SICK-TIM7xx", "ip":"192.168.0.1",     "port":2111},
    {"name":"nano1", "driver_id":"sick_nanoscan3_driver","model":"SICK-nanoScan3", "ip":"0.0.0.0",     "port":6060},
    {"name":"espe1", "driver_id":"espe_lga60_driver",   "model":"ESPE-LGA60",  "ip":"192.168.1.88",    "port":8080},
    {"name":"rp1",   "driver_id":"rplidar_c1_driver",   "model":"AUTO",        "serial_port":"/dev/ttyUSB0", "baudrate":460800}
  ]
}

Các trường DeviceConfig (mọi trường có default, chỉ khai báo cái cần):

Trường	Ý nghĩa
driver_id	Plugin phụ trách thiết bị (bắt buộc)
model	Một trong supported_models của driver; tên lạ → mặc định của driver
ip / port	Driver mạng: UDP = địa chỉ bind cục bộ, TCP = địa chỉ thiết bị; port 0 = mặc định của driver
serial_port / baudrate	Driver serial (rplidar); mặc định /dev/ttyUSB0 @ 460800
inverted	true nếu lidar lắp úp ngược — driver tự đảo góc
use_udp	Chỉ driver transport_selectable (ESPE): chuyển sang UDP
angle_min_deg / angle_max_deg	Cửa sổ FOV hợp lệ (hệ góc có dấu, 0 = phía trước): điểm ngoài cửa sổ thành NaN. Bỏ trống = tắt
range_min_m / range_max_m	Ghi đè dải đo; 0 = theo model
remap_angle_min_deg / remap_angle_max_deg	(legacy) remap tuyến tính góc output, không cắt điểm
extra	map chuỗi→chuỗi cho tuỳ chọn riêng của driver

File format cũ của lidarlib ({"brand": "OLEI", ...}) được migrate tự động khi load: brand+model → driver_id, cặp angle_*_deg cũ → remap.

Driver đi kèm

driver_id	Plugin	Vendor / dòng máy	Transport	Model hỗ trợ	Ghi chú
rplidar_c1_driver	driver_rplidar.so	Slamtec RPLIDAR	serial	AUTO, C1	Build trên SDK vendor; default C1 @ 460800; A/S series dùng được với baud tương ứng. Health check lúc open(), model/firmware tự nhận
olei_lidar_driver	driver_olei.so	OLEI 2D	udp	AUTO, VB, VF, LR-1F, LR-1FMI, LR-1BS5, LR-16F, GS1-5	Tự nhận diện giao thức Family A/B/C theo frame ID từng gói; AUTO tự dò model (Family B/C). Port mặc định 2368
sick_tim_driver	driver_sick_code.so	SICK TiM 5xx/7xx	tcp	SICK-TIM5xx, SICK-TIM571, SICK-TIM7xx	SOPAS/CoLa-A; open() tự start stream. Port 2111. Verify trên TiM781S thật
sick_nanoscan3_driver	driver_sick_safety.so	SICK nanoScan3/microScan3	udp	SICK-nanoScan3	Receiver thụ động UDP safety-data; đích UDP cấu hình sẵn bằng Safety Designer. Port 6060. Chưa verify phần cứng
espe_lga60_driver	driver_espe.so	ESPE LGA60	tcp (+udp)	ESPE-LGA60	FOV 320°; open() gửi RAuto; tham số thiết bị theo tool Windows của hãng. Port 8080. Chưa verify phần cứng

Chi tiết giao thức từng hãng (frame layout, offset góc, đơn vị) nằm trong comment đầu mỗi file plugin và docs/diagnostics.md.

Kiểu dữ liệu

ScanResult = { LaserScan scan; ExtraInfo info; }

LaserScan cùng field và đơn vị với ROS sensor_msgs/LaserScan:

Field	Ý nghĩa
angle_min / angle_max / angle_increment	Góc (rad); góc điểm i = angle_min + i·increment. Driver mạng: hệ góc có dấu, 0 = phía trước; rplidar: hệ góc thiết bị [0, 2π)
ranges	Khoảng cách (m); NaN = điểm không hợp lệ (rplidar/FOV filter), ∞ = không có phản hồi (nanoScan3/ESPE)
intensities	Cường độ 0–255
range_min / range_max	Dải đo hợp lệ (m)
timestamp_ms	Đồng hồ thiết bị (ms); 0 nếu giao thức không có
time_increment / scan_time	Chỉ rplidar đo được (chu kỳ grab thực); driver khác = 0

ExtraInfo: metadata thô tuỳ giao thức (detected_model, byte lỗi OLEI, status SICK, health RPLIDAR, ...) — field thiết bị không có giữ std::nullopt. Diagnostics (từ get_diagnostics() hoặc decode_diagnostics(info)) là bản decode tiện dùng: has_fault() / has_warning() / healthy() / to_string() , kèm model, firmware (rplidar), device_timestamp_ms.

Viết một plugin mới

1. Tạo plugins/driver_<tên>/ với <tên>_driver.cpp + CMakeLists.txt (xlidar_add_plugin(driver_<tên> <tên>_driver.cpp)), thêm add_subdirectory vào plugins/CMakeLists.txt.

2. Implement class kế thừa xlidar::LidarDriverInterface — đủ open/close/recv_scan/spin_once/set_scan_callback/ detected_model/is_open/get_driver_info, và cập nhật get_diagnostics() + mark_scan_decoded() mỗi vòng quét.

3. Export hai entry point:

   XLIDAR_PLUGIN_EXPORT void get_driver_info(xlidar::DriverInfo* out) { *out = kDriverInfo; }
   XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface*
   create_driver_instance(const xlidar::DeviceConfig* cfg) { return new MyDriver(...); }
   

4. Chọn driver_id duy nhất, mô tả description rõ driver phụ trách nhóm thiết bị nào; dùng apply_device_config() trong plugins/common để tôn trọng các cửa sổ góc/dải đo chung.

Plugin build với -fvisibility=hidden — chỉ hai entry point lộ ra ngoài. Manager và plugin phải build cùng toolchain (chúng trao đổi kiểu C++).