# 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

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

```bash
cmake --install build --prefix "$HOME/.local"   # hoặc sudo với /usr/local
```

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

## Quick start

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

```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);
```

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

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

```bash
./build/lidar_app [my_config.json]
```

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