Lidarlib

Thư viện C++17 thu nhận dữ liệu lidar 2D cho OLEI (UDP), SICK (TCP/UDP) và ESPE (TCP/UDP)

Mọi driver cùng implement một interface lidarlib::Lidar, khởi tạo qua một factory duy nhất lidarlib::make_lidar(), output thống nhất theo định dạng ROS sensor_msgs/LaserScan.

Tính năng

• Đa hãng, một API — OLEI (Family A/B/C), SICK (TiM 5xx/7xx, nanoScan3) và ESPE (LGA60) dùng chung interface: open() / recv_scan() / callback / close().
• Tự nhận diện giao thức — phân biệt họ giao thức OLEI theo frame ID từng gói; chế độ AUTO tự dò model từ dữ liệu (Family B/C).
• Chẩn đoán thiết bị — đọc trạng thái tự chẩn đoán nhúng trong stream: lỗi motor/điện áp/nhiệt độ (OLEI), kính bẩn/pollution (SICK TiM), contamination/manipulation (nanoScan3).
• Xử lý lỗi tường minh — ErrorCode phân loại từ errno thật; lifecycle an toàn với mọi thứ tự gọi open()/close().
• Đa luồng an toàn — mỗi instance độc lập hoàn toàn, chạy mỗi lidar một thread không cần khóa.
• Cấu hình JSON — khai báo danh sách lidar trong config.json, load/save bằng API kèm sẵn.

Cài đặt

Yêu cầu: Linux, CMake ≥ 3.10, trình dịch C++17. Không có dependency ngoài (chỉ pthread).

cmake -S . -B build -DCMAKE_BUILD_TYPE=Release
cmake --build build -j"$(nproc)"
cmake --install build --prefix "$HOME/.local"   # hoặc sudo với /usr/local

Tùy chọn CMake: -DLIDARLIB_BUILD_EXAMPLES=OFF (tắt binary demo), -DBUILD_SHARED_LIBS=OFF (build static).

Dùng từ project khác:

find_package(lidarlib REQUIRED)
target_link_libraries(my_app PRIVATE lidarlib::lidarlib)

Sử dụng

Đọc scan

#include "lidarlib/lidarlib.hpp"   // toàn bộ API trong một include

lidarlib::LidarConfig c{"front", "192.168.1.10", 2368, "AUTO", false, "OLEI"};
std::unique_ptr<lidarlib::Lidar> lidar = lidarlib::make_lidar(c);

if (lidar->open() != lidarlib::ErrorCode::Ok) {
    fprintf(stderr, "open: %s\n", lidarlib::to_string(lidar->last_error()));
    return 1;
}

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

Có thể khởi tạo driver trực tiếp không qua factory:

lidarlib::Driver         olei(lidarlib::MODEL_AUTO,          "192.168.100.100", 2368);
lidarlib::SickDriver     tim (lidarlib::MODEL_SICK_TIM571,   "192.168.0.1",     2111);
lidarlib::NanoScanDriver nano(lidarlib::MODEL_SICK_NANOSCAN3, "0.0.0.0",        6060);
lidarlib::EspeDriver     espe(lidarlib::MODEL_ESPE_LGA60,    "192.168.1.88",    8080);

Chế độ callback

Thay cho recv_scan() blocking:

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

Chẩn đoán thiết bị

Thiết bị nhúng thông tin tự chẩn đoán trong stream dữ liệu; driver decode sẵn qua get_diagnostics() (chi tiết layout từng giao thức: docs/diagnostics.md):

lidarlib::Diagnostics d = lidar->get_diagnostics();
if (!d.valid) {
    // chưa decode được vòng quét nào
} else if (d.has_fault()) {
    // OLEI Family A
    d.monitor_fault();       // motor/giám sát bất thường
    d.voltage_fault();       // điện áp ngoài dải
    d.temperature_fault();   // nhiệt độ bất thường
    // SICK
    d.sick_error();          // TiM: device error
    d.pollution_error();     // 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
    printf("fault: %s\n", lidarlib::to_string(d).c_str());
} else if (d.has_warning()) {
    // pollution_warning() / contamination_warning() — kính bẩn nhẹ, nên lau
}

d.healthy() = đã có dữ liệu và không có fault. Khuyến nghị giám sát: cảm biến khỏe khi và chỉ khi recv_scan() thành công đều đặn và get_diagnostics().has_fault() == false.

Xử lý lỗi & lifecycle

open() trả về ErrorCode; last_error() giữ kết quả của lần gọi 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	Port local đã bị chiếm
BindFailed / SocketError	Lỗi bind khác / không tạo được socket
ConnectionRefused / ConnectionFailed / Timeout	TCP connect bị từ chối / không tới được / quá thời hạn
HandshakeFailed	TCP nối được nhưng lệnh start-stream thất bại
DeviceDisconnected	Thiết bị đóng kết nối / lỗi recv giữa chừng

Lifecycle an toàn với mọi thứ tự gọi: close() là idempotent, open() lặp trả AlreadyOpen và không đụng kết nối đang chạy, sau close() có thể open() lại (state được reset).

Cấu hình JSON

lidar_app (binary demo) đọc config.json, mở từng lidar một thread:

{
  "lidars": [
    {"name":"front", "ip":"192.168.100.100", "port":2368, "brand":"OLEI", "model":"AUTO", "inverted":false},
    {"name":"rear",  "ip":"192.168.100.100", "port":2369, "brand":"OLEI", "model":"AUTO", "inverted":true},
    {"name":"sick1", "ip":"192.168.0.1",     "port":2111, "brand":"SICK", "model":"SICK-TIM571"},
    {"name":"nano1", "ip":"0.0.0.0",         "port":6060, "brand":"SICK", "model":"SICK-nanoScan3"},
    {"name":"espe1", "ip":"192.168.1.88",    "port":8080, "brand":"ESPE", "model":"ESPE-LGA60"}
  ]
}

Trường	Ý nghĩa
brand	"OLEI" (mặc định), "SICK" hoặc "ESPE"
model	Tên trong bảng model bên dưới; tên lạ → mặc định của hãng
inverted	true nếu lidar lắp úp ngược — driver tự đảo góc (mọi hãng)
use_udp	Chỉ ESPE: true để dùng transport UDP thay vì TCP
angle_min_deg / angle_max_deg	Tuỳ chọn: remap tuyến tính góc output sang cửa sổ này (không cắt điểm). Bỏ trống = tắt

Đọc/ghi bằng lidarlib::load_config(path) / lidarlib::save_config(path, cfg).

Model hỗ trợ

OLEI (UDP, port mặc định 2368)

Constant	FOV (°)	Range (m)	Giao thức
MODEL_VB	−135…135	0.05…30	Family A
MODEL_VF	−180…180	0.05…30	Family A
MODEL_LR1F	−180…180	0.05…50	Family A (0° thiết bị hướng đuôi, offset +180°)
MODEL_LR1FMI	−180…180	0.05…30	Family B, ~2400 điểm/vòng (0° hướng đuôi)
MODEL_LR1BS5	−180…180	0.05…30	Family B
MODEL_LR16F	−135…135	0.05…30	3D 16-line
MODEL_GS15	−180…180	0.05…30	Family C/V3 — chưa verify phần cứng
MODEL_AUTO	−180…180	0.05…30	Tự dò model (Family B/C)

Driver 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). Tên model đọc từ packet: detected_model() hoặc result.info.detected_model.

SICK

Constant	FOV (°)	Range (m)	Transport
MODEL_SICK_TIM5XX	−135…135	0.05…10	TCP/SOPAS (CoLa-A), port 2111
MODEL_SICK_TIM571	−135…135	0.05…25	TCP/SOPAS, port 2111
MODEL_SICK_TIM7XX	−135…135	0.05…25	TCP/SOPAS, port 2111 — verify trên TiM781S thật
MODEL_SICK_NANOSCAN3	−137.5…137.5	0.05…40	UDP safety-data, port 6060

• TiM (SickDriver) — open() tự gửi lệnh start-stream; góc output đã quy về 0° = phía trước.
• nanoScan3 (NanoScanDriver) — receiver UDP thụ động; đích UDP phải cấu hình sẵn trong SICK Safety Designer. Chưa verify phần cứng thật.

ESPE

Constant	FOV (°)	Range (m)	Transport
MODEL_ESPE_LGA60	−160…160	0.05…50	TCP (mặc định) hoặc UDP, port 8080

• LGA60 (EspeDriver) — laser scanner FOV 320°, thiết bị quét 20°→340° với 0° hướng đuôi (offset −180° để output 0° = phía trước). open() tự gửi lệnh start-capture RAuto; các tham số thiết bị (tốc độ quay, độ phân giải 0.025–0.5°, mức lọc nhiễu) lấy theo cấu hình đã nạp bằng phần mềm Windows của hãng — driver không tự đổi. Frame dữ liệu HISN (header big-endian, điểm đo little-endian: distance mm + intensity); frame vùng WSimu (nếu thiết bị gửi) được đọc lấy mã lỗi. Chuyển transport UDP qua tham số use_udp của constructor hoặc trường use_udp trong config JSON. Mặc định của hãng: IP 192.168.1.88, port 8080. Port từ driver ROS gốc của hãng — chưa verify trên phần cứng thật.

Kiểu dữ liệu

ScanResult

Kết quả một vòng quét: { LaserScan scan; ExtraInfo info; }.

LaserScan

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

Field	Kiểu	Ý nghĩa
angle_min / angle_max	float	Góc điểm đầu/cuối (rad), unwrap liên tục
angle_increment	float	Bước góc (rad); góc điểm i = angle_min + i·increment
ranges	vector<float>	Khoảng cách (m), theo thứ tự quét
intensities	vector<float>	Cường độ phản xạ 0–255
range_min / range_max	float	Dải đo hợp lệ (m), lấy từ ModelConfig
timestamp_ms	uint32_t	Đồng hồ thiết bị (ms); 0 nếu giao thức không có
time_increment / scan_time	float	Luôn 0 (thiết bị không cung cấp)

ExtraInfo

Metadata tuỳ giao thức; trường 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ị (xem Diagnostics)
distance_scale_mm	OLEI A/B	Hệ số mm/count của khoảng cách
rotation_raw	OLEI Family A	Tốc độ motor (raw)
scan_frequency_raw, input_status, output_status, field_status, status_flags	OLEI Family C, SICK TiM	Trạng thái I/O, field an toàn, cờ trạng thái (raw)
sick_device_status	SICK TiM	Cặp Device Status: 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 thiết bị trong frame vùng WSimu (chỉ có khi host poll area data)

Diagnostics

Trạng thái tự chẩn đoán đã decode (lidarlib/diagnostics.hpp), trả về từ get_diagnostics() hoặc decode_diagnostics(result.info):

API	Ý nghĩa
valid	Đã decode được ít nhất một vòng quét
monitor_fault() / voltage_fault() / temperature_fault()	OLEI Family A: motor / điện áp / nhiệt độ bất thường
sick_error() / pollution_warning() / pollution_error()	SICK TiM: lỗi thiết bị / kính bẩn nhẹ / kính bẩn nặng
contamination_warning() / contamination_error() / manipulation()	nanoScan3: kính bẩn / nghi bị can thiệp
espe_fault()	ESPE LGA60: từ lỗi thiết bị khác 0 (ý nghĩa bit chưa verify)
has_fault()	Gộp mọi nguồn lỗi
has_warning()	Gộp các cảnh báo kính bẩn (vẫn đo được)
healthy()	valid && !has_fault()
to_string(d)	Chuỗi log một dòng: no data / ok / WARN: … / FAULT: …

ModelConfig & LidarConfig

• ModelConfig — thông số một model: tên, FOV, dải đo, offset góc, cửa sổ remap. Các preset MODEL_* khai báo sẵn trong header.
• LidarConfig — một entry cấu hình runtime: {name, ip, port, model, inverted, brand}, dùng với make_lidar() và file JSON.

Cấu trúc source

File	Vai trò
include/lidarlib/lidarlib.hpp	Include tổng hợp toàn bộ API
include/lidarlib/lidar.hpp	Kiểu dữ liệu, interface Lidar, driver OLEI, preset MODEL_*
include/lidarlib/sick_lidar.hpp	SickDriver, NanoScanDriver, preset MODEL_SICK_*
include/lidarlib/espe_lidar.hpp	EspeDriver, preset MODEL_ESPE_LGA60
include/lidarlib/diagnostics.hpp	Diagnostics, bit lỗi, decode_diagnostics()
include/lidarlib/error.hpp	enum class ErrorCode + to_string()
include/lidarlib/config.hpp	LidarConfig, load/save JSON, make_lidar()
src/olei_lidar.cpp	Parse Family A/B/C, CRC, gom vòng quét
src/sick_lidar.cpp	Parse CoLa-A (TiM) + safety-data UDP (nanoScan3)
src/espe_lidar.cpp	Parse frame HISN/WSimu (LGA60), gom vòng quét
src/lidar_config.cpp	Bảng model/brand, config JSON, factory
docs/diagnostics.md	Nghiên cứu layout dữ liệu chẩn đoán từng giao thức
examples/	Demo: một lidar, hai lidar song song, SICK TiM, nanoScan3, app khung