DriverLIdar/README.md

# xlidar-driver

SDK driver lidar 2D cho Linux, kiến trúc **plugin nạp động**: mỗi driver là
một file `.so` độc lập, cùng implement một interface chung
`xlidar::LidarDriverInterface`; host chỉ cần facade
**`xlidar::LidarManager`** (build ra `liblidar_manager.so`) để khám phá
plugin, đọc metadata và tạo instance thiết bị.

Driver đi kèm: **Slamtec RPLIDAR** (serial), **OLEI** (UDP), **SICK TiM**
(TCP/CoLa-A), **SICK nanoScan3** (UDP safety), **ESPE LGA60** (TCP/UDP).
Output thống nhất theo định dạng ROS `sensor_msgs/LaserScan`.

## Kiến trúc

```
xlidar_driver/
├── CMakeLists.txt
├── include/                    # API public — host chỉ include từ đây
│   ├── lidar_interface.hpp     #   LidarDriverInterface, DriverInfo, DeviceConfig,
│   │                           #   LaserScan/ScanResult, ErrorCode, plugin ABI
│   ├── lidar_diagnostics.hpp   #   Diagnostics + bit lỗi từng hãng
│   └── lidar_manager.hpp       #   LidarManager, PluginRegistry, config.json
├── src/
│   ├── CMakeLists.txt          # → liblidar_manager.so
│   ├── lidar_manager.cpp       #   dlopen/dlsym + load/save config.json
│   └── json_mini.hpp
├── plugins/                    # mỗi thư mục → một plugin .so
│   ├── CMakeLists.txt          #   helper xlidar_add_plugin()
│   ├── common/plugin_helpers.hpp
│   ├── driver_rplidar/         # → driver_rplidar.so     (rplidar_c1_driver)
│   ├── driver_olei/            # → driver_olei.so        (olei_lidar_driver)
│   ├── driver_sick_tim/        # → driver_sick_tim.so    (sick_tim_driver)
│   ├── driver_sick_safety/     # → driver_sick_safety.so (sick_nanoscan3_driver)
│   └── driver_espe/            # → driver_espe.so        (espe_lga60_driver)
├── third_party/
│   └── rplidar_sdk/            # SDK Slamtec, build thẳng vào driver_rplidar.so
├── examples/                   # list_drivers, example, lidar_app
└── docs/diagnostics.md         # nghiên cứu layout dữ liệu chẩn đoán
```

- **LidarManager** quét thư mục plugin, `dlopen` từng `.so`, resolve hai
  entry point C rồi đăng ký vào `available_drivers()` — map
  `<driver_id, PluginRegistry>` trong đó `PluginRegistry = {DriverInfo,
  file_path}`.
- **Plugin ABI** — mỗi plugin export đúng hai symbol C:

  ```cpp
  XLIDAR_PLUGIN_EXPORT void get_driver_info(xlidar::DriverInfo* out);
  XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface*
      create_driver_instance(const xlidar::DeviceConfig* cfg);
  ```

- **DriverInfo** — plugin tự đăng ký danh tính: `vendor`, `model` (dòng
  thiết bị driver phụ trách — một driver có thể cover cả series), `driver_id`
  (định danh duy nhất, ví dụ `rplidar_c1_driver`), `description` (mô tả ngắn:
  thiết bị hỗ trợ, transport, ghi chú), kèm metadata cho UI: `transport`
  (`serial`/`udp`/`tcp`), `transport_selectable`, `supported_models`.
- Plugin được giữ nguyên trong bộ nhớ tới khi manager bị hủy — **manager
  phải sống lâu hơn mọi instance nó tạo ra**.

## Cài đặt

Yêu cầu: Linux, CMake ≥ 3.16, C++17. Không có dependency ngoài (pthread,
dl); SDK của hãng (Slamtec cho plugin rplidar) đã nằm sẵn trong
`third_party/` — clone là build được.

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

Kết quả: `build/src/liblidar_manager.so`, plugin trong `build/plugins/*.so`,
demo trong `build/examples/`.

Tùy chọn: `-DXLIDAR_BUILD_EXAMPLES=OFF`. Hỗ trợ `cmake --install` +
`find_package(xlidar_driver)` (target `xlidar::lidar_manager`).

## Sử dụng

### Khám phá driver và đọc scan

```cpp
#include "lidar_manager.hpp"   // kéo theo lidar_interface.hpp

xlidar::LidarManager manager("plugins");
manager.load_all_plugins();

for (const auto& [id, plugin] : manager.available_drivers())
    printf("%s — %s\n", id.c_str(), plugin.info.description.c_str());

xlidar::DeviceConfig cfg;
cfg.driver_id = "sick_tim_driver";
cfg.model     = "SICK-TIM7xx";
cfg.ip        = "192.168.0.1";       // port 0 = port mặc định của driver

auto lidar = manager.create_lidar_device(cfg);   // nullptr nếu driver_id lạ
if (lidar->open() != xlidar::ErrorCode::Ok) {
    fprintf(stderr, "open: %s\n", xlidar::to_string(lidar->last_error()));
    return 1;
}

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

Driver serial (rplidar) dùng `cfg.serial_port` + `cfg.baudrate` thay cho
`ip`/`port`. Xem `examples/example.cpp`.

### Chế độ callback

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

Callback chỉ phát từ `spin_once()` — chọn một kiểu bơm dữ liệu:
`recv_scan()` (poll) hoặc callback + `spin_once()`.

### Sẵn sàng & chẩn đoán thiết bị

API chẩn đoán **chung cho mọi hãng**: `Diagnostics` trả về danh sách
`issues` (mỗi issue = `severity` fault/warning + `code` trung lập + `detail`
người-đọc-được), không có hàm riêng từng hãng. Giá trị thô của hãng nằm
trong map `raw` (VD `"sick.device_status"`, `"rplidar.error_code"`); cần
dạng chuỗi thì dùng `to_json()`.

```cpp
if (!lidar->wait_ready(5000)) { /* chưa có scan sạch nào trong 5s */ }

xlidar::Diagnostics d = lidar->get_diagnostics();
for (const xlidar::DiagnosticIssue& issue : d.issues) {
    // issue.severity : DiagSeverity::Fault | DiagSeverity::Warning
    // issue.code     : "motor" | "voltage" | "temperature" | "optics_dirty"
    //                  | "manipulation" | "device_error" | "device_warning"
    // issue.detail   : mô tả kèm tên hãng + giá trị thô, VD "ESPE fault word 0x0004"
    printf("[%s] %s — %s\n", xlidar::to_string(issue.severity),
           issue.code.c_str(), issue.detail.c_str());
}
if (d.has_fault())   { /* thiết bị báo hỏng — dừng tin dữ liệu */ }
if (d.has_warning()) { /* suy giảm (kính bẩn, ...) — lên lịch bảo trì */ }

printf("%s\n", xlidar::to_string(d).c_str()); // "ok" / "FAULT: voltage | WARN: optics_dirty"
printf("%s\n", xlidar::to_json(d).c_str());   // JSON đầy đủ cho REST/telemetry
```

`d.healthy()` = đã có dữ liệu và không fault. `is_ready(max_age_ms)` = đang
mở + healthy + scan mới nhất chưa quá hạn — diagnostics chỉ refresh qua
`recv_scan()`/`spin_once()`, nên gọi từ chính thread bơm dữ liệu. Ý nghĩa
từng `code`, key `raw` và layout chẩn đoán từng giao thức:
[docs/diagnostics.md](docs/diagnostics.md) + comment trong
`include/lidar_diagnostics.hpp`.

### Xử lý lỗi & lifecycle

`open()` trả về `ErrorCode`; `last_error()` giữ kết quả 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` / `BindFailed` / `SocketError` | Lỗi bind/socket (driver UDP) |
| `ConnectionRefused` / `ConnectionFailed` / `Timeout` | TCP connect bị từ chối / không tới được / quá hạn |
| `HandshakeFailed` | Nối được nhưng lệnh start-stream/start-scan thất bại |
| `SerialError` | Cổng serial không tồn tại / không mở được (rplidar) |
| `DeviceError` | Thiết bị tự báo fault (rplidar health check lúc `open()`) |
| `DeviceDisconnected` | Thiết bị đóng kết nối / lỗi recv giữa chừng |
| `InvalidConfig` | `DeviceConfig` không dùng được với driver |

Lifecycle an toàn với mọi thứ tự gọi: `close()` idempotent, `open()` lặp trả
`AlreadyOpen`, sau `close()` mở lại được. Mỗi instance độc lập hoàn toàn —
mỗi lidar một thread, không cần khóa.

### Cấu hình JSON

`lidar_app` (binary demo) đọc `config.json`, mở mỗi lidar một thread; API:
`xlidar::load_config(path)` / `save_config(path, cfg)` /
`manager.create_from_config_file(path)`.

```json
{
  "lidars": [
    {"name":"front", "driver_id":"olei_lidar_driver",   "model":"AUTO",        "ip":"192.168.100.100", "port":2368},
    {"name":"sick1", "driver_id":"sick_tim_driver",     "model":"SICK-TIM7xx", "ip":"192.168.0.1",     "port":2111},
    {"name":"nano1", "driver_id":"sick_nanoscan3_driver","model":"SICK-nanoScan3", "ip":"0.0.0.0",     "port":6060},
    {"name":"espe1", "driver_id":"espe_lga60_driver",   "model":"ESPE-LGA60",  "transport":"udp",   "ip":"192.168.1.88", "port":8080},
    {"name":"rp1",   "driver_id":"rplidar_c1_driver",   "model":"AUTO",        "transport":"serial", "serial_port":"/dev/ttyUSB0", "baudrate":460800}
  ]
}
```

Các trường `DeviceConfig` (mọi trường có default, chỉ khai báo cái cần):

| Trường | Ý nghĩa |
|---|---|
| `driver_id` | Plugin phụ trách thiết bị (bắt buộc) |
| `model` | Một trong `supported_models` của driver; tên lạ → mặc định của driver |
| `transport` | `"serial"` / `"udp"` / `"tcp"`. Bỏ trống = transport mặc định của driver. Driver cố định transport mà bị cấu hình sai → `open()` trả `InvalidConfig`; driver `transport_selectable` (ESPE) chuyển TCP↔UDP qua trường này |
| `ip` / `port` | Transport mạng: UDP = địa chỉ bind cục bộ, TCP = địa chỉ thiết bị; `port` 0 = mặc định của driver |
| `serial_port` / `baudrate` | Transport serial (rplidar); mặc định `/dev/ttyUSB0` @ 460800 |
| `inverted` | `true` nếu lidar lắp úp ngược — driver tự đảo góc |
| `angle_min_deg` / `angle_max_deg` | Cửa sổ FOV hợp lệ (hệ góc có dấu, 0 = phía trước): điểm ngoài cửa sổ thành NaN. Bỏ trống = tắt |
| `range_min_m` / `range_max_m` | Ghi đè dải đo; 0 = theo model |
| `remap_angle_min_deg` / `remap_angle_max_deg` | (legacy) remap tuyến tính góc output, không cắt điểm |
| `extra` | map chuỗi→chuỗi cho tuỳ chọn riêng của driver |

File format cũ được migrate tự động khi load: `brand`+`model` (lidarlib) →
`driver_id`, cặp `angle_*_deg` cũ → remap, `use_udp: true` → `transport:
"udp"`.

## Driver đi kèm

| driver_id | Plugin | Vendor / dòng máy | Transport | Model hỗ trợ | Ghi chú |
|---|---|---|---|---|---|
| `rplidar_c1_driver` | `driver_rplidar.so` | Slamtec RPLIDAR | serial | `AUTO`, `C1` | Build trên SDK vendor; default C1 @ 460800; A/S series dùng được với baud tương ứng. Health check lúc `open()`, model/firmware tự nhận |
| `olei_lidar_driver` | `driver_olei.so` | OLEI 2D | udp | `AUTO`, `VB`, `VF`, `LR-1F`, `LR-1FMI`, `LR-1BS5`, `LR-16F`, `GS1-5` | Tự nhận diện giao thức Family A/B/C theo frame ID từng gói; `AUTO` tự dò model (Family B/C). Port mặc định 2368 |
| `sick_tim_driver` | `driver_sick_tim.so` | SICK TiM 5xx/7xx | tcp | `SICK-TIM5xx`, `SICK-TIM571`, `SICK-TIM7xx` | SOPAS/CoLa-A; `open()` tự start stream. Port 2111. Verify trên TiM781S thật |
| `sick_nanoscan3_driver` | `driver_sick_safety.so` | SICK nanoScan3/microScan3 | udp | `SICK-nanoScan3` | Receiver thụ động UDP safety-data; đích UDP cấu hình sẵn bằng Safety Designer. Port 6060. Chưa verify phần cứng |
| `espe_lga60_driver` | `driver_espe.so` | ESPE LGA60 | tcp (+udp) | `ESPE-LGA60` | FOV 320°; `open()` gửi `RAuto`; tham số thiết bị theo tool Windows của hãng. Port 8080. Chưa verify phần cứng |

### Thông số model

FOV/range dưới đây là mặc định theo `ModelConfig` của từng driver — ghi đè
bằng `range_min_m`/`range_max_m` và cửa sổ `angle_min_deg`/`angle_max_deg`.
Driver mạng dùng hệ góc có dấu, 0° = phía trước; rplidar giữ hệ góc thiết bị
[0, 2π).

**OLEI** (UDP, port 2368) — 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 tuỳ kiểu dữ liệu). Tên model đọc từ packet
(Family B/C): `detected_model()` hoặc `result.info.detected_model`.

| Model | FOV (°) | Range (m) | Giao thức |
|---|---|---|---|
| `AUTO` | −180…180 | 0.05…30 | Tự dò model từ dữ liệu (Family B/C) |
| `VB` | −135…135 | 0.05…30 | Family A |
| `VF` | −180…180 | 0.05…30 | Family A |
| `LR-1F` | −180…180 | 0.05…50 | Family A — 0° thiết bị hướng đuôi (offset +180°) |
| `LR-1FMI` | −180…180 | 0.05…30 | Family B — 0° thiết bị hướng đuôi |
| `LR-1BS5` | −180…180 | 0.05…30 | Family B |
| `LR-16F` | −135…135 | 0.05…30 | 3D 16-line |
| `GS1-5` | −180…180 | 0.05…30 | Family C/V3 — chưa verify phần cứng |

**SICK** — TiM qua TCP/SOPAS (CoLa-A) port 2111: `open()` gửi
`sEN LMDscandata 1` để start stream; frame wire đặt 90° ở phía trước nên
driver offset −90° để 0° = phía trước. nanoScan3 là receiver UDP thụ động
port 6060 — đích UDP phải cấu hình sẵn bằng SICK Safety Designer, driver
không handshake CoLa2/TCP.

| Model | FOV (°) | Range (m) | Ghi chú |
|---|---|---|---|
| `SICK-TIM5xx` | −135…135 | 0.05…10 | TiM551/561 — FOV/range theo datasheet, chưa verify |
| `SICK-TIM571` | −135…135 | 0.05…25 | Chưa verify phần cứng |
| `SICK-TIM7xx` | −135…135 | 0.05…25 | Verify trên TiM781S thật (FW V5.11) |
| `SICK-nanoScan3` | −137.5…137.5 | 0.05…40 | Cả microScan3; layout port từ sick_safetyscanners, chưa verify |

**ESPE LGA60** (TCP mặc định / UDP, port 8080; IP mặc định của hãng
192.168.1.88) — FOV 320°: thiết bị quét 20°→340° với 0° hướng đuôi (offset
−180°), preset −160…160°, range 0.05…50 m. Frame đo `HISN` (header 16 B
big-endian; điểm = distance mm + intensity, distance 50 000 mm = không có
phản hồi → ∞); frame vùng `WSimu` (nếu thiết bị gửi) được đọc lấy từ lỗi.
Độ phân giải (0.025–0.5°), tốc độ quay, mức lọc nhiễu theo cấu hình đã nạp
bằng tool Windows của hãng — driver không tự đổi. Chưa verify phần cứng.

**RPLIDAR** (serial, mặc định `/dev/ttyUSB0` @ 460800) — preset C1: range
0.05…16 m (datasheet 12 m trên nền trắng), ~10 Hz, ~400–500 điểm/vòng
(DenseBoost); A/S series dùng được với baud tương ứng. `open()` chạy health
check (Error → `DeviceError`) và tự nhận model/firmware. Riêng driver này
đo được `time_increment`/`scan_time` thực.

Chi tiết giao thức từng hãng (frame layout, offset góc, đơn vị) nằm trong
comment đầu mỗi file plugin và [docs/diagnostics.md](docs/diagnostics.md).

## Kiểu dữ liệu

### `ScanResult` = `{ LaserScan scan; ExtraInfo info; }`

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

| Field | Ý nghĩa |
|---|---|
| `angle_min` / `angle_max` / `angle_increment` | Góc (rad); góc điểm *i* = `angle_min + i·increment`. Driver mạng: hệ góc có dấu, 0 = phía trước; rplidar: hệ góc thiết bị [0, 2π) |
| `ranges` | Khoảng cách (m); NaN = điểm không hợp lệ (rplidar/FOV filter), ∞ = không có phản hồi (nanoScan3/ESPE) |
| `intensities` | Cường độ 0–255 |
| `range_min` / `range_max` | Dải đo hợp lệ (m) |
| `timestamp_ms` | Đồng hồ thiết bị (ms); 0 nếu giao thức không có |
| `time_increment` / `scan_time` | Chỉ rplidar đo được (chu kỳ grab thực); driver khác = 0 |

`ExtraInfo`: metadata thô tuỳ giao thức — field 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ị: BIT0 monitor, BIT1 điện áp, BIT2 nhiệt độ |
| `distance_scale_mm` | OLEI A/B | Hệ số mm/count của khoảng cách; 0 = không báo |
| `rotation_raw` | OLEI Family A | Tốc độ motor (raw) |
| `distance_ratio_raw`, `scan_frequency_raw`, `input_status`, `output_status`, `field_status`, `status_flags` | OLEI Family C/V3 | Passthrough raw — ý nghĩa bit chưa verify |
| `sick_device_status` | SICK TiM | Cặp Device Status `(word0<<8)\|word1`: 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 từ frame vùng `WSimu` — chỉ có khi thiết bị gửi area data |
| `rplidar_health_status`, `rplidar_error_code` | RPLIDAR | Health từ SDK (0 ok · 1 warning · 2 error) + mã lỗi thiết bị đi kèm |

`Diagnostics` (từ `get_diagnostics()` hoặc `decode_diagnostics(info)`) là
bản decode **trung lập hãng**: `issues` (danh sách `{severity, code,
detail}` với code chung: `motor` / `voltage` / `temperature` /
`optics_dirty` / `manipulation` / `device_error` / `device_warning`), map
`raw` giữ giá trị thô theo key ổn định (`"olei.error_status"`,
`"sick.device_status"`, `"nano.general_state"`, `"espe.error_status"`,
`"rplidar.health_status"`, `"rplidar.error_code"`, ...), cùng `model`,
`firmware` (rplidar), `device_timestamp_ms`, và helper `has_fault()` /
`has_warning()` / `healthy()` / `to_string()` / `to_json()`.

## Viết một plugin mới

1. Tạo `plugins/driver_<tên>/` với `<tên>_driver.cpp` + `CMakeLists.txt`
   (`xlidar_add_plugin(driver_<tên> <tên>_driver.cpp)`), thêm
   `add_subdirectory` vào `plugins/CMakeLists.txt`.
2. Implement class kế thừa `xlidar::LidarDriverInterface` — đủ
   `open`/`close`/`recv_scan`/`spin_once`/`set_scan_callback`/
   `detected_model`/`is_open`/`get_driver_info`, và cập nhật
   `get_diagnostics()` + `mark_scan_decoded()` mỗi vòng quét.
3. Export hai entry point:

   ```cpp
   XLIDAR_PLUGIN_EXPORT void get_driver_info(xlidar::DriverInfo* out) { *out = kDriverInfo; }
   XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface*
   create_driver_instance(const xlidar::DeviceConfig* cfg) { return new MyDriver(...); }
   ```

4. Chọn `driver_id` duy nhất, mô tả `description` rõ driver phụ trách nhóm
   thiết bị nào; dùng `apply_device_config()` trong `plugins/common` để tôn
   trọng các cửa sổ góc/dải đo chung.

Plugin build với `-fvisibility=hidden` — chỉ hai entry point lộ ra ngoài.
Manager và plugin phải build cùng toolchain (chúng trao đổi kiểu C++).