DriverLIdar/README.md

Lidarlib

Thư viện C++17 cho lidar (OLEI + SICK), build bằng CMake ra shared lib .so (lidarlib::lidarlib), hỗ trợ find_package() để link vào project khác. Tự nhận diện họ giao thức (Family A/B/C) theo từng gói, hỗ trợ chạy nhiều lidar song song, gộp cả OLEI (UDP) và SICK (TCP) sau một hàm config duy nhất lidarlib::make_lidar(). Không có Web UI — người dùng tự viết giao diện trên API C++ này (include header + link .so).

Tóm tắt API (cho người viết giao diện)

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

// 1) MỘT hàm config: từ LidarConfig -> handle chung (OLEI hoặc SICK)
lidarlib::LidarConfig c{"front", "192.168.1.10", 2368, "AUTO", false, "OLEI"};
std::unique_ptr<lidarlib::Lidar> lidar = lidarlib::make_lidar(c);
lidar->open();

// 2) HAI loại dữ liệu mỗi vòng quét
lidarlib::ScanResult r;
lidar->recv_scan(r, 1000);
//   r.scan : lidarlib::LaserScan  — chung cho mọi lidar, đúng format sensor_msgs/LaserScan của ROS
//   r.info : lidarlib::ExtraInfo  — thông tin thêm, tuỳ family/model thực tế

// (tuỳ chọn) lưu/đọc danh sách lidar ra file:
lidarlib::Config cfg = lidarlib::load_config("config.json");
lidarlib::save_config("config.json", cfg);

lidarlib::Lidar là interface chung; lidarlib::Driver (OLEI/UDP) và lidarlib::SickDriver (SICK/TCP) đều kế thừa nó, nên giao diện chỉ cần thao tác qua lidarlib::Lidar* mà không phải phân biệt hãng. Vẫn có thể new thẳng Driver/SickDriver nếu muốn.

Kiến trúc

File	Vai trò
include/lidarlib/lidarlib.hpp	Header tổng hợp — include 1 dòng là có cả data model + 2 driver + config + factory
include/lidarlib/lidar.hpp	API public: LaserScan, ExtraInfo, ScanResult, ModelConfig, interface Lidar, class Driver
src/olei_lidar.cpp	Parse Family A (0xFAF0), Family B (0xFEF0), Family C/V3 (0xFEAC), CRC, gom scan
include/lidarlib/config.hpp + src/olei_config.cpp	Config/LidarConfig (gồm brand: "OLEI"/"SICK"), load/save config.json, tra cứu ModelConfig theo tên/theo hãng, và hàm config make_lidar()
src/json_mini.hpp	Parser/serializer JSON tối giản, chỉ dùng nội bộ cho olei_config (load/save config.json)
include/lidarlib/sick_lidar.hpp + src/sick_lidar.cpp	Driver riêng cho lidar SICK TiM5xx/7xx — giao thức SOPAS/CoLa-A qua TCP (port 2111), khác hoàn toàn UDP binary của OLEI. Đã verify bằng TiM781S thật (xem mục riêng bên dưới)
examples/example.cpp	Demo 1 lidar, recv_scan() blocking
examples/test_dual.cpp	Demo 2 lidar song song (2 thread)
examples/sick_example.cpp	Demo driver SICK TiM, recv_scan() blocking qua TCP
examples/lidar_app.cpp	Khung app headless: đọc config.json → make_lidar() từng con → in scan. Thay vòng printf bằng giao diện của bạn
CMakeLists.txt	Build lidarlib (OLEI + SICK + factory + config, chỉ phụ thuộc pthread) thành .so, cài install()/find_package()

Ba họ giao thức được driver tự nhận diện theo Frame ID/magic trong từng gói:

• Family A (0xFAF0) — VB/VF/LR-1F. Header 20B + block 3B/điểm. Có CRC32.
• Family B (0xFEF0) — LR-1BS5/LR-1BS2. Header 40B (preamble 0x010F + frame id ở offset [2-3], chuỗi tên model ASCII ở offset [7-17)) + block 8B/điểm.
• Family C / protocol V3 (0xFEAC) — GS1-5. Header 48B, block 2/4B/điểm tùy byte Types. Port từ driver C# OleiGS15Driver.cs (RobotNet10.RobotApp), CHƯA verify bằng phần cứng GS1-5 thật (không có thiết bị để sniff) — chỉ test bằng packet giả lập tự dựng theo đúng cấu trúc header.

Output: 2 loại

Driver::recv_scan() (và callback set_scan_callback) trả về ScanResult { LaserScan scan; ExtraInfo info; } mỗi khi gom đủ 1 vòng quay:

lidarlib::Driver drv(lidarlib::MODEL_AUTO, "192.168.100.100", 2369);
drv.open();
lidarlib::ScanResult result;
drv.recv_scan(result, 2000);
printf("%zu diem, model=%s\n", result.scan.ranges.size(), result.info.detected_model.c_str());

LaserScan — cùng tên field/đơn vị với sensor_msgs/LaserScan của ROS (radian, mét, giây):

Field	Ý nghĩa
timestamp_ms	Đồng hồ thiết bị (ms từ lúc bật nguồn); = 0 nếu family không có (xem ExtraInfo)
angle_min/angle_max/angle_increment	rad — đã unwrap liên tục, KHÔNG bị giới hạn [-π,π]
time_increment/scan_time	Luôn = 0 — thiết bị không báo timing chi tiết đó
range_min/range_max	m — lấy từ ModelConfig (giá trị đặt sẵn, không đo được mỗi scan)
ranges[]/intensities[]	m / 0-255 (đọc lại thành float như ROS)

ExtraInfo — thông tin thêm tuỳ family/model thực tế của packet, field nào thiết bị không có thì giữ std::nullopt:

Field	Family	Verify hardware?
detected_model	mọi family (qua MODEL_AUTO)	Family B verify bằng sniff sống
error_status	Family A	Verify
distance_scale_mm	Family A/B	Verify
rotation_raw	Family A	Raw, chưa decode ý nghĩa
distance_ratio_raw, scan_frequency_raw, input_status, output_status, field_status, status_flags	Family C/GS1-5	Raw, chưa verify hardware thật

Vì sao cần "unwrap": góc từng điểm được lọc theo FOV ở hệ có dấu [-180, 180] (0 = phía trước, dương = bên trái) — nhưng hệ này gãy ở biên ±180° đối với lidar quét 360°. Trước khi đưa vào LaserScan, driver unwrap lại thành một dải góc liên tục trong từng vòng quay (Driver::push_point() trong src/olei_lidar.cpp), nên angle_min/angle_max/ranges[] luôn đơn điệu — đúng kiểu mảng mà sensor_msgs/LaserScan kỳ vọng.

Kết nối lidar

Mạng tham chiếu trên host này (/home/robotics):

eth0: 192.168.100.100/24
  ├─ front (scan_1): DeviceIp 192.168.100.11, DevicePort 2368
  └─ rear  (scan_2): DeviceIp 192.168.100.12, DevicePort 2369

(Khớp với RobotApp/RobotNet10.RobotApp/appsettings.json, các mục Olei-front/Olei-rear.)

Kiểm tra kết nối trước khi test:

ip -4 addr show eth0          # phải thấy inet 192.168.100.100/24
ping -c1 192.168.100.11        # front
ping -c1 192.168.100.12        # rear

Lưu ý quan trọng: nếu RobotNet10.RobotApp đang chạy, nó bind sẵn port 2368/2369 (không bật SO_REUSEPORT) → driver standalone sẽ bind lỗi (Khong mo duoc socket... interface khong ton tai?). Kiểm tra ai đang giữ port:

ss -lunp | grep -E '2368|2369'

Build

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

Sinh ra build/liblidarlib.so (driver OLEI + SICK + factory + config, không phụ thuộc gì ngoài pthread) và 4 binary demo (example, test_dual, sick_example, lidar_app). Tắt build demo bằng -DLIDARLIB_BUILD_EXAMPLES=OFF. Muốn ra static lib .a thay vì .so thì thêm -DBUILD_SHARED_LIBS=OFF.

Cài vào hệ thống để dùng find_package(lidarlib) từ project khác — /usr/local chỉ root mới ghi được nên cần sudo, không thì cmake --install báo lỗi permission denied:

sudo cmake --install build --prefix /usr/local

Muốn cài không cần sudo thì đổi prefix sang thư mục riêng trong $HOME (vd ~/.local), rồi thêm -DCMAKE_PREFIX_PATH=~/.local khi configure project nào gọi find_package(lidarlib):

cmake --install build --prefix "$HOME/.local"

# trong CMakeLists.txt của project dùng thư viện này
find_package(lidarlib REQUIRED)
target_link_libraries(my_app PRIVATE lidarlib::lidarlib)

Sau khi link, giao diện chỉ cần #include "lidarlib/lidarlib.hpp" rồi gọi lidarlib::make_lidar() — xem "Tóm tắt API" ở đầu README.

Vẫn có thể build từng file bằng g++ thuần nếu không muốn dùng CMake:

g++ -std=c++17 -O2 -pthread -Wall -Wextra -Iinclude -o test_dual examples/test_dual.cpp src/olei_lidar.cpp
g++ -std=c++17 -O2 -pthread -Wall -Wextra -Iinclude -o example   examples/example.cpp   src/olei_lidar.cpp
g++ -std=c++17 -O2 -pthread -Wall -Wextra -Iinclude -o lidar_app examples/lidar_app.cpp src/olei_lidar.cpp src/sick_lidar.cpp src/olei_config.cpp
g++ -std=c++17 -O2 -pthread -Wall -Wextra -Iinclude -o sick_example examples/sick_example.cpp src/sick_lidar.cpp

Test thử

2 lidar song song

./build/test_dual

In ra 5 scan mỗi bên, kèm số điểm, timestamp, error status, và model đã tự dò được (model=...).

1 lidar

Sửa model/IP/port trong examples/example.cpp rồi build lại, hoặc gọi trực tiếp:

lidarlib::Driver drv(lidarlib::MODEL_AUTO, "192.168.100.100", 2369);
drv.open();
lidarlib::ScanResult result;
drv.recv_scan(result, 2000);

Lidar lắp úp ngược

Constructor có tham số thứ 4 inverted (mặc định false). Đặt true nếu thiết bị bị lắp lật 180° quanh trục hướng về phía trước — driver tự đảo dấu góc từng điểm (angle = -angle, chuẩn hóa lại về -180..180) để output luôn đúng theo hệ quy chiếu xe, không phụ thuộc hướng lắp vật lý:

lidarlib::Driver drv(lidarlib::MODEL_AUTO, "192.168.100.100", 2369, /*inverted=*/true);

Đã verify bằng sniff sống: chạy inverted=false góc tăng dần theo thời gian, chạy inverted=true góc giảm dần với cùng bước góc — đúng chữ ký của đảo dấu.

Cấu hình & chạy (lidar_app + config.json)

Không còn Web UI. Cấu hình là một file JSON đơn giản — examples/lidar_app.cpp đọc nó, mở từng lidar qua đúng một hàm lidarlib::make_lidar(), rồi đọc scan trên mỗi thread. Đây là khung mẫu để bạn thay vòng printf bằng giao diện riêng (Qt, ImGui, ROS node, v.v.).

./build/lidar_app                  # đọc/tạo config.json cạnh chỗ chạy
./build/lidar_app my_config.json   # đường dẫn config khác

config.json — danh sách lidar, không cố định số lượng. brand chọn loại driver ("OLEI" = UDP, "SICK" = TCP/SOPAS); model tra trong bảng ModelConfig (tên lạ → tự lùi về mặc định của hãng: AUTO cho OLEI, SICK-TIM571 cho SICK); inverted chỉ có tác dụng với OLEI. Bỏ trống brand thì mặc định "OLEI" (tương thích file cũ).

{
  "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", "inverted":false}
  ]
}

Trong code, đọc/ghi file bằng lidarlib::load_config(path) / lidarlib::save_config(path, cfg) (file hỏng → trả về mặc định, không crash). Giao diện của bạn tự quyết khi nào lưu — thư viện không tự bind port hay phục vụ HTTP gì cả.

Sniff packet thô (debug khi nghi ngờ offset header)

python3 - <<'EOF'
import socket
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
s.bind(("192.168.100.100", 2369))   # đổi port theo lidar cần xem
data, addr = s.recvfrom(4096)
print("from", addr, "len", len(data))
print(data[:40].hex(' '))
EOF

Luôn ưu tiên capture thật hơn là tin comment trong code — comment mô tả ý định lúc viết, không phải offset đã verify trên thiết bị thật.

Chọn ModelConfig

Constant	FOV (deg, có dấu)	range_min/max (m)	Khi dùng
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	2D 360° 50m, Family A
MODEL_LR1BS5	-180…180	0.05…30	2D 360°, Family B
MODEL_LR16F	-135…135	0.05…30	3D 16-line
MODEL_GS15	-180…180	0.05…30	2D 360°, Family C/V3 — chưa verify hardware thật
MODEL_AUTO	-180…180 (mặc định, có thể tự thu hẹp)	0.05…30	Không biết trước model

range_min_m/range_max_m chỉ là giá trị đặt sẵn để điền vào LaserScan::range_min/range_max (không đọc từ packet) — chỉnh trong include/lidarlib/lidar.hpp theo datasheet thật của từng model nếu cần chính xác.

MODEL_AUTO: chỉ có tác dụng tự-dò với Family B (đọc chuỗi tên model trong header). Driver luôn lưu lại tên thật đọc từ packet (vd "OLELR-1BS5", "OLELR-1BS2") — gọi drv.detected_model() hoặc result.info.detected_model để xem (trả về "AUTO" nếu chưa nhận gói Family B nào). FOV chỉ tự thu hẹp khi tên đó khớp một entry trong kModelTable (src/olei_lidar.cpp); nếu không khớp, FOV giữ nguyên mặc định 360° (-180..180, không mất điểm) — an toàn nhưng có thể giữ lại điểm ngoài FOV thật của thiết bị nếu thiết bị đó không quét tròn.

Family A không mang chuỗi tên model trong packet, nên MODEL_AUTO trên thiết bị Family A cũng giữ nguyên FOV rộng — phải chỉ định model cụ thể (VD MODEL_VB) nếu muốn thu hẹp FOV cho thiết bị góc hẹp.

Lidar SICK (TiM5xx/7xx) — driver riêng

lidarlib::SickDriver (include/lidarlib/sick_lidar.hpp + src/sick_lidar.cpp) là driver độc lập với lidarlib::Driver ở trên — không phải thêm 1 family vào driver OLEI, vì giao thức khác hẳn:

• Kết nối TCP (SOPAS, port mặc định 2111) tới lidar, không phải UDP broadcast như OLEI.
• Telegram là ASCII (CoLa-A), đóng khung bằng STX(0x02)/ETX(0x03), không có CRC32 như Family A.
• Thiết bị đứng im cho tới khi driver gửi lệnh sEN LMDscandata 1 — open() tự làm việc này; close() gửi sEN LMDscandata 0 trước khi đóng socket.

Output vẫn dùng chung ScanResult/LaserScan/ExtraInfo như driver OLEI nên gọi giống hệt:

lidarlib::SickDriver drv(lidarlib::MODEL_SICK_TIM571, "192.168.0.1", 2111);
drv.open();
lidarlib::ScanResult result;
drv.recv_scan(result, 2000);

Constant	FOV (deg, có dấu)	range_min/max (m)	Khi dùng
MODEL_SICK_TIM5XX	-135…135	0.05…10	TiM551/561, 270°
MODEL_SICK_TIM571	-135…135	0.05…25	TiM571, 270°
MODEL_SICK_TIM7XX	-135…135	0.05…25	TiM781, 270°

Đã verify bằng phần cứng thật: chạy trực tiếp với 1 con SICK TiM781S (FW V5.11-14.10.24, DeviceIdent đọc qua sRN DeviceIdent trên cổng 2111) tại 192.168.100.22:2111 — MODEL_SICK_TIM7XX. Kết quả khớp đúng datasheet TiM781S: 811 điểm/scan trải từ -45°…225° (270° FOV), angle_increment = 0.333° (1/3°, đúng độ phân giải góc của dòng 781), khoảng cách 0.3-1.5m ổn định qua nhiều scan liên tiếp, kênh RSSI1 có giá trị intensity hợp lý (không phải toàn 0), error_status = 0x00. Điều này xác nhận layout LMDscandata trong parse_lmdscandata() (src/sick_lidar.cpp) — kênh DIST1/RSSI1, scaling factor IEEE-754, start angle/step width — đọc đúng trên hardware thật, không chỉ đúng theo tài liệu nữa.

Vẫn chưa verify: chiều quy ước góc 0° (thẳng phía trước thiết bị hay hướng khác — chưa đối chiếu với hướng lắp vật lý thật), thiết bị có encoder (NumEncoders > 0, nhánh next()×2 chưa từng chạy qua vì test thực tế không có encoder), và nhánh 8-bit channel (Num8BitChannels, thiết bị test chỉ dùng kênh 16-bit). MODEL_SICK_TIM5XX/MODEL_SICK_TIM571 (FOV/range theo datasheet) cũng chưa test trên phần cứng — chỉ MODEL_SICK_TIM7XX đã chạy thật.

SickDriver nối vào config.json qua field LidarConfig::brand ("OLEI" hoặc "SICK", mặc định "OLEI"): lidarlib::make_lidar() thấy brand=="SICK" thì trả về lidarlib::SickDriver (TCP/SOPAS) thay cho lidarlib::Driver (UDP), cùng kiểu trả về std::unique_ptr<lidarlib::Lidar> nên phía gọi không phải phân biệt. Xem examples/lidar_app.cpp (chạy theo config) hoặc examples/sick_example.cpp (dùng thẳng SickDriver qua API C++).