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_code/       # → driver_sick_code.so   (sick_tim_driver)
│   ├── driver_sick_safety/     # → driver_sick_safety.so (sick_nanoscan3_driver)
│   └── driver_espe/            # → driver_espe.so        (espe_lga60_driver)
├── 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). Plugin rplidar cần thêm source SDK của Slamtec.

```bash
cmake -S . -B build -DCMAKE_BUILD_TYPE=Release \
      -DXLIDAR_RPLIDAR_SDK_DIR=/path/to/rplidar_sdk   # dir chứa include/ + src/
cmake --build build -j"$(nproc)"
```

Kết quả: `build/src/liblidar_manager.so`, plugin trong `build/plugins/*.so`,
demo trong `build/examples/`. Không truyền `XLIDAR_RPLIDAR_SDK_DIR` thì các
vị trí quen thuộc được tự dò (`third_party/rplidar_sdk`,
`../rplidar_sdk/sdk`, `../xloc-monorepo/xlocd/deps/rplidar_sdk`); không thấy
SDK thì plugin rplidar bị bỏ qua, phần còn lại build bình thường.

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ị

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

xlidar::Diagnostics d = lidar->get_diagnostics();
if (d.has_fault()) {
    d.monitor_fault();       // OLEI: motor/giám sát
    d.voltage_fault();       // OLEI: điện áp
    d.temperature_fault();   // OLEI: nhiệt độ
    d.sick_error();          // SICK TiM: device error
    d.pollution_error();     // SICK 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
    d.espe_fault();          // ESPE: từ lỗi thiết bị
    d.rplidar_fault();       // RPLIDAR: health = Error (kèm rplidar_error_code)
    printf("fault: %s\n", xlidar::to_string(d).c_str());
} else if (d.has_warning()) {
    // pollution/contamination warning, rplidar health warning
}
```

`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. Layout
chẩn đoán từng giao thức: [docs/diagnostics.md](docs/diagnostics.md).

### 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",  "ip":"192.168.1.88",    "port":8080},
    {"name":"rp1",   "driver_id":"rplidar_c1_driver",   "model":"AUTO",        "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 |
| `ip` / `port` | Driver 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` | Driver serial (rplidar); mặc định `/dev/ttyUSB0` @ 460800 |
| `inverted` | `true` nếu lidar lắp úp ngược — driver tự đảo góc |
| `use_udp` | Chỉ driver `transport_selectable` (ESPE): chuyển sang UDP |
| `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ủa lidarlib (`{"brand": "OLEI", ...}`) được migrate tự động
khi load: `brand`+`model` → `driver_id`, cặp `angle_*_deg` cũ → remap.

## 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_code.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 |

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 (`detected_model`, byte lỗi OLEI,
status SICK, health RPLIDAR, ...) — field thiết bị không có giữ
`std::nullopt`. `Diagnostics` (từ `get_diagnostics()` hoặc
`decode_diagnostics(info)`) là bản decode tiện dùng: `has_fault()` /
`has_warning()` / `healthy()` / `to_string()` , kèm `model`, `firmware`
(rplidar), `device_timestamp_ms`.

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