Files

2026-07-07 10:38:42 +07:00

14 KiB

Raw Blame History

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

14 KiB Raw Blame History Unescape Escape