DriverLIdar/README.md

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_tim/        # → driver_sick_tim.so    (sick_tim_driver)
│   ├── driver_sick_safety/     # → driver_sick_safety.so (sick_nanoscan3_driver)
│   └── driver_espe/            # → driver_espe.so        (espe_lga60_driver)
├── third_party/
│   └── rplidar_sdk/            # SDK Slamtec, build thẳng vào driver_rplidar.so
├── 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); SDK của hãng (Slamtec cho plugin rplidar) đã nằm sẵn trong third_party/ — clone là build được.

cmake -S . -B build -DCMAKE_BUILD_TYPE=Release
cmake --build build -j"$(nproc)"

Kết quả: build/src/liblidar_manager.so, plugin trong build/plugins/*.so, demo trong build/examples/.

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ị

API chẩn đoán chung cho mọi hãng: Diagnostics trả về danh sách issues (mỗi issue = severity fault/warning + code trung lập + detail người-đọc-được), không có hàm riêng từng hãng. Giá trị thô của hãng nằm trong map raw (VD "sick.device_status", "rplidar.error_code"); cần dạng chuỗi thì dùng to_json().

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

xlidar::Diagnostics d = lidar->get_diagnostics();
for (const xlidar::DiagnosticIssue& issue : d.issues) {
    // issue.severity : DiagSeverity::Fault | DiagSeverity::Warning
    // issue.code     : "motor" | "voltage" | "temperature" | "optics_dirty"
    //                  | "manipulation" | "device_error" | "device_warning"
    // issue.detail   : mô tả kèm tên hãng + giá trị thô, VD "ESPE fault word 0x0004"
    printf("[%s] %s — %s\n", xlidar::to_string(issue.severity),
           issue.code.c_str(), issue.detail.c_str());
}
if (d.has_fault())   { /* thiết bị báo hỏng — dừng tin dữ liệu */ }
if (d.has_warning()) { /* suy giảm (kính bẩn, ...) — lên lịch bảo trì */ }

printf("%s\n", xlidar::to_string(d).c_str()); // "ok" / "FAULT: voltage | WARN: optics_dirty"
printf("%s\n", xlidar::to_json(d).c_str());   // JSON đầy đủ cho REST/telemetry

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. Ý nghĩa từng code, key raw và layout chẩn đoán từng giao thức: docs/diagnostics.md + comment trong include/lidar_diagnostics.hpp.

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",  "transport":"udp",   "ip":"192.168.1.88", "port":8080},
    {"name":"rp1",   "driver_id":"rplidar_c1_driver",   "model":"AUTO",        "transport":"serial", "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
transport	"serial" / "udp" / "tcp". Bỏ trống = transport mặc định của driver. Driver cố định transport mà bị cấu hình sai → open() trả InvalidConfig; driver transport_selectable (ESPE) chuyển TCP↔UDP qua trường này
ip / port	Transport 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	Transport serial (rplidar); mặc định /dev/ttyUSB0 @ 460800
inverted	true nếu lidar lắp úp ngược — driver tự đảo góc
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 migrate tự động khi load: brand+model (lidarlib) → driver_id, cặp angle_*_deg cũ → remap, use_udp: true → transport: "udp".

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_tim.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

Thông số model

FOV/range dưới đây là mặc định theo ModelConfig của từng driver — ghi đè bằng range_min_m/range_max_m và cửa sổ angle_min_deg/angle_max_deg. Driver mạng dùng hệ góc có dấu, 0° = phía trước; rplidar giữ hệ góc thiết bị [0, 2π).

OLEI (UDP, port 2368) — nhận diện họ giao thức theo frame ID từng gói: Family A 0xFAF0 (header 20 B, 3 B/điểm, CRC32) · Family B 0xFEF0 (header 40 B kèm tên model ASCII, 8 B/điểm) · Family C/V3 0xFEAC (header 48 B, 2–4 B/điểm tuỳ kiểu dữ liệu). Tên model đọc từ packet (Family B/C): detected_model() hoặc result.info.detected_model.

Model	FOV (°)	Range (m)	Giao thức
AUTO	−180…180	0.05…30	Tự dò model từ dữ liệu (Family B/C)
VB	−135…135	0.05…30	Family A
VF	−180…180	0.05…30	Family A
LR-1F	−180…180	0.05…50	Family A — 0° thiết bị hướng đuôi (offset +180°)
LR-1FMI	−180…180	0.05…30	Family B — 0° thiết bị hướng đuôi
LR-1BS5	−180…180	0.05…30	Family B
LR-16F	−135…135	0.05…30	3D 16-line
GS1-5	−180…180	0.05…30	Family C/V3 — chưa verify phần cứng

SICK — TiM qua TCP/SOPAS (CoLa-A) port 2111: open() gửi sEN LMDscandata 1 để start stream; frame wire đặt 90° ở phía trước nên driver offset −90° để 0° = phía trước. nanoScan3 là receiver UDP thụ động port 6060 — đích UDP phải cấu hình sẵn bằng SICK Safety Designer, driver không handshake CoLa2/TCP.

Model	FOV (°)	Range (m)	Ghi chú
SICK-TIM5xx	−135…135	0.05…10	TiM551/561 — FOV/range theo datasheet, chưa verify
SICK-TIM571	−135…135	0.05…25	Chưa verify phần cứng
SICK-TIM7xx	−135…135	0.05…25	Verify trên TiM781S thật (FW V5.11)
SICK-nanoScan3	−137.5…137.5	0.05…40	Cả microScan3; layout port từ sick_safetyscanners, chưa verify

ESPE LGA60 (TCP mặc định / UDP, port 8080; IP mặc định của hãng 192.168.1.88) — FOV 320°: thiết bị quét 20°→340° với 0° hướng đuôi (offset −180°), preset −160…160°, range 0.05…50 m. Frame đo HISN (header 16 B big-endian; điểm = distance mm + intensity, distance 50 000 mm = không có phản hồi → ∞); frame vùng WSimu (nếu thiết bị gửi) được đọc lấy từ lỗi. Mỗi lượt quét được thiết bị chia thành NHIỀU packet HISN: các bộ đếm điểm trong header là của riêng packet đó, còn vòng quét là cửa sổ cố định 20°→340° = 320°/bước điểm — driver ghép các packet theo start_angle và chỉ phát scan khi packet đóng ở 340°. Điểm không packet nào gửi = NaN. Bước góc lấy từ packet mở vòng (span/số điểm) rồi giữ nguyên. Chu kỳ quay đo host-side giữa hai vòng: scan_time là thời gian QUÉT (period × 320/360, đúng như driver ROS của hãng dùng để đóng dấu), nên spin rate suy ra từ 1/scan_time cao hơn cơ khí 360/320; vòng đầu chưa đo được thì để 0. Trường time 16-bit trong header là bộ đếm thiết bị đơn vị chưa xác định (header của hãng ghi "chưa kích hoạt") → timestamp_ms = 0. Độ phân giải (0.025–0.5°), tốc độ quay, mức lọc nhiễu theo cấu hình đã nạp bằng tool Windows của hãng — driver không tự đổi. Chưa verify phần cứng.

RPLIDAR (serial, mặc định /dev/ttyUSB0 @ 460800) — preset C1: range 0.05…16 m (datasheet 12 m trên nền trắng), ~10 Hz, ~400–500 điểm/vòng (DenseBoost); A/S series dùng được với baud tương ứng. open() chạy health check (Error → DeviceError) và tự nhận model/firmware. Riêng driver này đo được time_increment/scan_time thực.

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 (chu kỳ grab thực) và ESPE (chu kỳ vòng quét, × 320/360) đo được; driver khác = 0

ExtraInfo: metadata thô tuỳ giao thức — field thiết bị không có giữ std::nullopt:

Field	Nguồn	Ý nghĩa
detected_model	mọi driver	Tên model thực đọc từ dữ liệu (hoặc tên cấu hình)
error_status	OLEI Family A	Byte lỗi thiết bị: BIT0 monitor, BIT1 điện áp, BIT2 nhiệt độ
distance_scale_mm	OLEI A/B	Hệ số mm/count của khoảng cách; 0 = không báo
rotation_raw	OLEI Family A	Tốc độ motor (raw)
distance_ratio_raw, scan_frequency_raw, input_status, output_status, field_status, status_flags	OLEI Family C/V3	Passthrough raw — ý nghĩa bit chưa verify
sick_device_status	SICK TiM	Cặp Device Status (word0<<8)|word1: 0 ok · 1 error · 2 pollution warning · 4 pollution error
nano_general_state	nanoScan3	Byte 0 block General System State (bit kNanoState*)
espe_error_status	ESPE LGA60	Từ lỗi từ frame vùng WSimu — chỉ có khi thiết bị gửi area data
rplidar_health_status, rplidar_error_code	RPLIDAR	Health từ SDK (0 ok · 1 warning · 2 error) + mã lỗi thiết bị đi kèm

Diagnostics (từ get_diagnostics() hoặc decode_diagnostics(info)) là bản decode trung lập hãng: issues (danh sách {severity, code, detail} với code chung: motor / voltage / temperature / optics_dirty / manipulation / device_error / device_warning), map raw giữ giá trị thô theo key ổn định ("olei.error_status", "sick.device_status", "nano.general_state", "espe.error_status", "rplidar.health_status", "rplidar.error_code", ...), cùng model, firmware (rplidar), device_timestamp_ms, và helper has_fault() / has_warning() / healthy() / to_string() / to_json().

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++).