QUYVN/DriverLIdar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
1c347a4918e76b9c99462f8caed4171846031d0d
DriverLIdar/README.md
QUYVN 1c347a4918 update Diagnostics
2026-07-07 08:59:36 +07:00

15 KiB
Raw Blame History

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

Chẩn đoán thiết bị (diagnostics)

Lidar OLEI nhúng thông tin tự chẩn đoán trong header gói dữ liệu (không có kênh query riêng). API đọc ra qua lidarlib/diagnostics.hpp — nghiên cứu chi tiết layout từng family xem docs/diagnostics.md.

lidarlib::Diagnostics d = lidar->get_diagnostics();
if (!d.valid) {
    // chưa decode được scan 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("lidar 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
}
  • get_diagnostics() — snapshot từ vòng quét gần nhất; gọi cùng thread với recv_scan()/spin_once(). d.healthy() = có data và không có fault.
  • decode_diagnostics(result.info) — decode gắn với một scan cụ thể.
  • Nguồn dữ liệu: OLEI Family A byte lỗi + rotation_raw + timestamp; Family B không có gì trên wire; Family C input/output/field_status, status_flags (raw, chưa verify); SICK TiM cặp device status trong LMDscandata; nanoScan3 block General System State (cần bật trong Safety Designer).

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/diagnostics.hpp Diagnostics, bit lỗi Family A, decode_diagnostics()
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-07 — Diagnostics API cho SICK

  • SickDriver (TiM): decode cặp Device Status của LMDscandata (0 ok / 1 error / 2 pollution warning / 4 pollution error) vào Diagnostics — sick_error(), pollution_warning(), pollution_error(). info.sick_device_status là trường mới; info.error_status không còn nhận status0 (trường đó thuộc Family A OLEI).
  • NanoScanDriver: parse block General System State (offset header [32]/[34], byte 0) → contamination_warning(), contamination_error(), manipulation(). Layout theo sick_safetyscanners, chưa verify phần cứng; block phải được bật trong Safety Designer.
  • Diagnostics thêm has_warning() (kính bẩn mức cảnh báo) và to_string() in thêm WARN: pollution / FAULT: ... contamination.
  • Test loopback: TiM server TCP giả (status 0 4 → pollution error) và gói nanoScan3 tổng hợp (state 0x09 → contamination error, gói sạch → healthy()); tài liệu tại docs/diagnostics.md §4-5.

2026-07-07 — Diagnostics API

  • Header mới lidarlib/diagnostics.hpp: struct Diagnostics (bit lỗi Family A decode sẵn: monitor/voltage/temperature_fault(), has_fault(), healthy(); trường raw Family C), hằng kFaultMonitor/Voltage/Temperature, to_string() để log.
  • Lidar::get_diagnostics(): snapshot chẩn đoán từ vòng quét decode gần nhất; valid = false khi chưa có scan. Driver OLEI implement đầy đủ; SICK tạm trả mặc định.
  • decode_diagnostics(const ExtraInfo&): decode gắn với một ScanResult cụ thể.
  • Tài liệu nghiên cứu docs/diagnostics.md: layout byte chẩn đoán từng family (A: byte lỗi [5] + rotation + timestamp; B: không có; C: input/output/field/status raw), chẩn đoán tầng transport, hướng mở rộng SICK.
  • Đã test bằng gói Family A tổng hợp qua loopback (fault set → decode đúng từng bit, vòng sạch → healthy()); bit map lấy theo tài liệu OLEI, chưa tái tạo fault trên phần cứng thật.

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).
Reference in New Issue View Git Blame Copy Permalink