DriverLIdar/README.md

Lidarlib

Thư viện C++17 cho lidar OLEI (UDP) và SICK (TCP/UDP). Build bằng CMake ra shared lib, hỗ trợ find_package(lidarlib). Mọi driver dùng chung một interface lidarlib::Lidar và một hàm khởi tạo duy nhất lidarlib::make_lidar().

• Tự nhận diện họ giao thức OLEI (Family A/B/C) theo từng gói tin
• Tự dò model (MODEL_AUTO) với Family B/C
• Chạy nhiều lidar song song (mỗi instance độc lập, an toàn đa luồng)
• Output chuẩn ROS sensor_msgs/LaserScan (radian, mét)
• Không có UI — tự viết giao diện trên API này

Build

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

Sinh ra build/liblidarlib.so (chỉ phụ thuộc pthread) và các binary demo (example, test_dual, sick_example, nanoscan_example, lidar_app).

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

Cài đặt và dùng từ project khác:

cmake --install build --prefix "$HOME/.local"   # hoặc sudo với /usr/local

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

Quick start

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

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

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

lidarlib::ScanResult r;
if (!lidar->recv_scan(r, 1000)) {
    // Timeout / DeviceDisconnected / NotOpen — xem lidar->last_error()
}
// r.scan : LaserScan — format sensor_msgs/LaserScan của ROS, chung mọi lidar
// r.info : ExtraInfo — thông tin thêm tuỳ family/model
printf("%zu diem, model=%s\n", r.scan.ranges.size(), r.info.detected_model.c_str());

Có thể khởi tạo driver trực tiếp thay vì qua make_lidar():

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);

Ngoài recv_scan() blocking còn có callback: set_scan_callback() + spin_once() trong vòng lặp riêng.

Error handling & lifecycle

open() trả về lidarlib::ErrorCode (header lidarlib/error.hpp, to_string() để log). Các code chính:

ErrorCode	Khi nào
Ok	Thành công
AlreadyOpen	Gọi open() khi đang mở — kết nối cũ giữ nguyên
NotOpen	Gọi recv_scan()/spin_once() khi chưa open()
InvalidAddress	Chuỗi IP không hợp lệ
PortInUse	Port local đã bị chiếm (bind EADDRINUSE/EACCES)
BindFailed / SocketError	Lỗi bind khác / không tạo được socket
ConnectionRefused / ConnectionFailed / Timeout	TCP connect (SICK TiM) bị từ chối / không tới được / quá 2s
HandshakeFailed	TCP nối được nhưng gửi sEN LMDscandata 1 thất bại
DeviceDisconnected	Thiết bị đóng kết nối / lỗi recv giữa chừng

Trạng thái instance:

• is_open() — socket đang mở hay không.
• last_error() — kết quả của lần open()/recv_scan()/spin_once() gần nhất (recv_scan() trả false thì gọi hàm này để biết Timeout hay DeviceDisconnected).
• Lifecycle chịu lỗi mọi thứ tự gọi: close() trước open() hoặc close() hai lần là no-op; open() hai lần trả AlreadyOpen và không đụng kết nối đang chạy; sau close() có thể open() lại (state scan dở được reset).

Cấu hình (config.json)

lidar_app là app mẫu headless: đọc config.json, mở từng lidar bằng make_lidar(), một thread mỗi con.

./build/lidar_app [my_config.json]

{
  "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"}
  ]
}

Trường	Ý nghĩa
brand	"OLEI" (mặc định) hoặc "SICK"
model	Tên trong bảng model bên dưới; tên lạ → mặc định của hãng (AUTO / SICK-TIM571). Với SICK, "SICK-nanoScan3" → driver UDP, còn lại → driver TCP
inverted	Chỉ OLEI: true nếu lidar lắp úp ngược, driver tự đảo góc về hệ quy chiếu xe
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 nào, chỉ ghi lại angle_min/max/increment. Bỏ trống (±360) = tắt

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

Model

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

Constant	FOV (°)	Range (m)	Ghi chú
MODEL_VB	-135…135	0.05…30	2D 270°, Family A
MODEL_VF	-180…180	0.05…30	2D 360°, Family A
MODEL_LR1F	-180…180	0.05…50	Family A; 0° thô của máy chỉ về đuôi (offset +180°)
MODEL_LR1FMI	-180…180	0.05…30	Family B, ~2400 điểm/vòng; 0° thô chỉ về đuôi (offset +180°)
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	Không biết trước model; tự dò với Family B (chuỗi tên) và C (magic). Family A không mang tên model nên giữ FOV rộng

Driver nhận diện họ giao thức theo Frame ID mỗi gói: Family A 0xFAF0 (header 20B, 3B/điểm, CRC32) · Family B 0xFEF0 (header 40B kèm tên model ASCII, 8B/điểm) · Family C/V3 0xFEAC (header 48B, 2/4B/điểm — port từ driver C#, chưa verify).

Tên model thật đọc từ packet xem qua detected_model() hoặc result.info.detected_model.

SICK (brand = "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
MODEL_SICK_NANOSCAN3	-137.5…137.5	0.05…40	UDP safety-data, port 6060

TiM (SickDriver) — open() tự gửi sEN LMDscandata 1 để bắt đầu stream. Hệ góc trên dây đặt 90° = trước mặt nên preset có angle_offset_deg = -90, output ra -135…135° với 0° = phía trước. Đã verify trên TiM781S thật (811 điểm/scan, increment 0.333°, DIST1/RSSI1 đúng layout). Chưa verify: encoder, kênh 8-bit, thông số TiM5xx/571.

nanoScan3 (NanoScanDriver) — UDP receiver thụ động: chỉ bind cổng và parse datagram; đích UDP phải cấu hình sẵn trong SICK Safety Designer (driver không bắt tay CoLa2). Layout port từ sick_safetyscanners (Apache-2.0). Chưa verify phần cứng thật — mới test bằng gói tổng hợp qua loopback.

Output

ScanResult { LaserScan scan; ExtraInfo info; } mỗi vòng quét:

• LaserScan — cùng field/đơn vị với ROS: angle_min/max/increment (rad, đã unwrap liên tục, không giới hạn ±π), ranges[] (m), intensities[] (0-255), timestamp_ms (đồng hồ thiết bị, 0 nếu family không có). range_min/max lấy từ ModelConfig (đặt sẵn, không đo mỗi scan); time_increment/scan_time luôn 0.
• ExtraInfo — field tuỳ family: detected_model, error_status (Family A), distance_scale_mm (A/B), và các trường raw của Family C (chưa verify). Field thiết bị không có giữ std::nullopt.

Cấu trúc source

File	Vai trò
include/lidarlib/error.hpp	enum class ErrorCode + to_string()
include/lidarlib/lidar.hpp	Data model, interface Lidar, driver OLEI, các MODEL_* OLEI
include/lidarlib/sick_lidar.hpp	SickDriver, NanoScanDriver, các MODEL_SICK_*
include/lidarlib/config.hpp	LidarConfig, load/save JSON, make_lidar()
src/olei_lidar.cpp	Parse Family A/B/C, CRC, gom scan
src/sick_lidar.cpp	Parse CoLa-A (TiM) + safety-data UDP (nanoScan3)
src/lidar_config.cpp	Bảng model/brand, config JSON, factory
examples/	Demo: 1 lidar, 2 lidar song song, SICK TiM, nanoScan3, app khung

Ghi chú

• Nếu port UDP đã bị app khác giữ (không bật SO_REUSEPORT), open() trả ErrorCode::PortInUse. Kiểm tra: ss -lunp | grep 2368.
• inverted đã verify bằng sniff sống: false góc tăng dần, true góc giảm dần cùng bước.

Changelog

2026-07-06 — Error handling & lifecycle API

• Header mới lidarlib/error.hpp: enum class ErrorCode — Ok, AlreadyOpen, NotOpen, SocketError, InvalidAddress, PortInUse, BindFailed, ConnectionRefused, ConnectionFailed, HandshakeFailed, Timeout, DeviceDisconnected — kèm to_string() để log.
• open() đổi chữ ký bool → ErrorCode trên cả 3 driver (Driver, SickDriver, NanoScanDriver). Lỗi phân loại từ errno thật: bind EADDRINUSE → PortInUse, connect TCP bị từ chối → ConnectionRefused, quá 2s → Timeout, gửi lệnh start-stream fail → HandshakeFailed, IP sai format → InvalidAddress.
• API trạng thái mới trên interface Lidar:
  • is_open() — socket đang mở hay không (cả 3 driver implement);
  • last_error() — kết quả lần open()/recv_scan()/spin_once() gần nhất; recv_scan() trả false thì gọi hàm này để biết Timeout hay DeviceDisconnected.
• Lifecycle chịu lỗi mọi thứ tự gọi: close() trước open() hoặc gọi hai lần là no-op; open() khi đang mở trả AlreadyOpen và không đụng kết nối đang chạy; recv_scan()/spin_once() khi chưa mở trả false + NotOpen; sau close() có thể open() lại (state scan dở được reset).
• NanoScanDriver bỏ SO_REUSEADDR: với UDP không có tác dụng (không có TIME_WAIT) mà còn che mất lỗi trùng port — giờ PortInUse báo được thật.
• ⚠️ Breaking change: code cũ viết if (!lidar->open()) bị đảo ngược logic vì ErrorCode::Ok == 0 — phải đổi thành if (lidar->open() != lidarlib::ErrorCode::Ok).