DriverLIdar/README.md

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

```bash
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:

```cmake
find_package(lidarlib REQUIRED)
target_link_libraries(my_app PRIVATE lidarlib::lidarlib)
```

## Sử dụng

### Đọc scan

```cpp
#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:

```cpp
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:

```cpp
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](docs/diagnostics.md)):

```cpp
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:

```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"},
    {"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 |