DriverLIdar/README.md

# Lidarlib

Thư viện C++17 cho lidar (OLEI + SICK), build bằng CMake ra **shared lib `.so`**
(`lidarlib::lidarlib`), hỗ trợ `find_package()` để link vào project khác. Tự nhận
diện họ giao thức (Family A/B/C) theo từng gói, hỗ trợ chạy nhiều lidar song
song, gộp cả OLEI (UDP) và SICK (TCP) sau **một hàm config duy nhất**
`lidarlib::make_lidar()`. **Không có Web UI** — người dùng tự viết giao diện trên
API C++ này (include header + link `.so`).

## Tóm tắt API (cho người viết giao diện)

```cpp
#include "lidarlib/lidarlib.hpp"            // gộp toàn bộ API trong 1 include

// 1) MỘT hàm config: từ LidarConfig -> handle chung (OLEI hoặc SICK)
lidarlib::LidarConfig c{"front", "192.168.1.10", 2368, "AUTO", false, "OLEI"};
std::unique_ptr<lidarlib::Lidar> lidar = lidarlib::make_lidar(c);
lidar->open();

// 2) HAI loại dữ liệu mỗi vòng quét
lidarlib::ScanResult r;
lidar->recv_scan(r, 1000);
//   r.scan : lidarlib::LaserScan  — chung cho mọi lidar, đúng format sensor_msgs/LaserScan của ROS
//   r.info : lidarlib::ExtraInfo  — thông tin thêm, tuỳ family/model thực tế

// (tuỳ chọn) lưu/đọc danh sách lidar ra file:
lidarlib::Config cfg = lidarlib::load_config("config.json");
lidarlib::save_config("config.json", cfg);
```

`lidarlib::Lidar` là interface chung; `lidarlib::Driver` (OLEI/UDP) và `lidarlib::SickDriver`
(SICK/TCP) đều kế thừa nó, nên giao diện chỉ cần thao tác qua `lidarlib::Lidar*` mà
không phải phân biệt hãng. Vẫn có thể `new` thẳng `Driver`/`SickDriver` nếu muốn.

## Kiến trúc

| File | Vai trò |
|------|---------|
| `include/lidarlib/lidarlib.hpp` | Header tổng hợp — include 1 dòng là có cả data model + 2 driver + config + factory |
| `include/lidarlib/lidar.hpp` | API public: `LaserScan`, `ExtraInfo`, `ScanResult`, `ModelConfig`, interface `Lidar`, class `Driver` |
| `src/olei_lidar.cpp` | Parse Family A (0xFAF0), Family B (0xFEF0), Family C/V3 (0xFEAC), CRC, gom scan |
| `include/lidarlib/config.hpp` + `src/olei_config.cpp` | `Config`/`LidarConfig` (gồm `brand`: `"OLEI"`/`"SICK"`), load/save `config.json`, tra cứu `ModelConfig` theo tên/theo hãng, và **hàm config `make_lidar()`** |
| `src/json_mini.hpp` | Parser/serializer JSON tối giản, chỉ dùng nội bộ cho `olei_config` (load/save `config.json`) |
| `include/lidarlib/sick_lidar.hpp` + `src/sick_lidar.cpp` | Driver riêng cho lidar **SICK TiM5xx/7xx** — giao thức SOPAS/CoLa-A qua TCP (port 2111), khác hoàn toàn UDP binary của OLEI. **Đã verify bằng TiM781S thật** (xem mục riêng bên dưới) |
| `examples/example.cpp` | Demo 1 lidar, `recv_scan()` blocking |
| `examples/test_dual.cpp` | Demo 2 lidar song song (2 thread) |
| `examples/sick_example.cpp` | Demo driver SICK TiM, `recv_scan()` blocking qua TCP |
| `examples/lidar_app.cpp` | Khung app headless: đọc `config.json` → `make_lidar()` từng con → in scan. Thay vòng `printf` bằng giao diện của bạn |
| `CMakeLists.txt` | Build `lidarlib` (OLEI + SICK + factory + config, chỉ phụ thuộc pthread) thành `.so`, cài `install()`/`find_package()` |

Ba họ giao thức được driver tự nhận diện theo Frame ID/magic trong từng gói:

- **Family A** (`0xFAF0`) — VB/VF/LR-1F. Header 20B + block 3B/điểm. Có CRC32.
- **Family B** (`0xFEF0`) — LR-1BS5/LR-1BS2. Header 40B (preamble `0x010F` +
  frame id ở offset [2-3], chuỗi tên model ASCII ở offset [7-17)) + block
  8B/điểm.
- **Family C / protocol V3** (`0xFEAC`) — GS1-5. Header 48B, block 2/4B/điểm
  tùy byte `Types`. **Port từ driver C# `OleiGS15Driver.cs`
  (RobotNet10.RobotApp), CHƯA verify bằng phần cứng GS1-5 thật** (không có
  thiết bị để sniff) — chỉ test bằng packet giả lập tự dựng theo đúng cấu trúc
  header.

## Output: 2 loại

`Driver::recv_scan()` (và callback `set_scan_callback`) trả về
`ScanResult { LaserScan scan; ExtraInfo info; }` mỗi khi gom đủ 1 vòng quay:

```cpp
lidarlib::Driver drv(lidarlib::MODEL_AUTO, "192.168.100.100", 2369);
drv.open();
lidarlib::ScanResult result;
drv.recv_scan(result, 2000);
printf("%zu diem, model=%s\n", result.scan.ranges.size(), result.info.detected_model.c_str());
```

**`LaserScan`** — cùng tên field/đơn vị với `sensor_msgs/LaserScan` của ROS
(radian, mét, giây):

| Field | Ý nghĩa |
|---|---|
| `timestamp_ms` | Đồng hồ thiết bị (ms từ lúc bật nguồn); = 0 nếu family không có (xem `ExtraInfo`) |
| `angle_min`/`angle_max`/`angle_increment` | rad — đã unwrap liên tục, KHÔNG bị giới hạn `[-π,π]` |
| `time_increment`/`scan_time` | Luôn = 0 — thiết bị không báo timing chi tiết đó |
| `range_min`/`range_max` | m — lấy từ `ModelConfig` (giá trị đặt sẵn, **không đo được mỗi scan**) |
| `ranges[]`/`intensities[]` | m / 0-255 (đọc lại thành float như ROS) |

**`ExtraInfo`** — thông tin thêm tuỳ family/model thực tế của packet, field
nào thiết bị không có thì giữ `std::nullopt`:

| Field | Family | Verify hardware? |
|---|---|---|
| `detected_model` | mọi family (qua `MODEL_AUTO`) | Family B verify bằng sniff sống |
| `error_status` | Family A | Verify |
| `distance_scale_mm` | Family A/B | Verify |
| `rotation_raw` | Family A | Raw, chưa decode ý nghĩa |
| `distance_ratio_raw`, `scan_frequency_raw`, `input_status`, `output_status`, `field_status`, `status_flags` | Family C/GS1-5 | Raw, **chưa verify hardware thật** |

Vì sao cần "unwrap": góc từng điểm được lọc theo FOV ở hệ **có dấu**
`[-180, 180]` (0 = phía trước, dương = bên trái) — nhưng hệ này gãy ở biên
±180° đối với lidar quét 360°. Trước khi đưa vào `LaserScan`, driver unwrap
lại thành một dải góc liên tục trong từng vòng quay (`Driver::push_point()`
trong `src/olei_lidar.cpp`), nên `angle_min`/`angle_max`/`ranges[]` luôn đơn
điệu — đúng kiểu mảng mà `sensor_msgs/LaserScan` kỳ vọng.

## Kết nối lidar

Mạng tham chiếu trên host này (`/home/robotics`):

```
eth0: 192.168.100.100/24
  ├─ front (scan_1): DeviceIp 192.168.100.11, DevicePort 2368
  └─ rear  (scan_2): DeviceIp 192.168.100.12, DevicePort 2369
```

(Khớp với `RobotApp/RobotNet10.RobotApp/appsettings.json`, các mục
`Olei-front`/`Olei-rear`.)

Kiểm tra kết nối trước khi test:

```bash
ip -4 addr show eth0          # phải thấy inet 192.168.100.100/24
ping -c1 192.168.100.11        # front
ping -c1 192.168.100.12        # rear
```

**Lưu ý quan trọng:** nếu `RobotNet10.RobotApp` đang chạy, nó bind sẵn port
2368/2369 (không bật `SO_REUSEPORT`) → driver standalone sẽ bind lỗi
(`Khong mo duoc socket... interface khong ton tai?`). Kiểm tra ai đang giữ port:

```bash
ss -lunp | grep -E '2368|2369'
```

## Build

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

Sinh ra `build/liblidarlib.so` (driver OLEI + SICK + factory + config, không
phụ thuộc gì ngoài pthread) và 4 binary demo (`example`, `test_dual`,
`sick_example`, `lidar_app`). Tắt build demo bằng `-DLIDARLIB_BUILD_EXAMPLES=OFF`.
Muốn ra static lib `.a` thay vì `.so` thì thêm `-DBUILD_SHARED_LIBS=OFF`.

Cài vào hệ thống để dùng `find_package(lidarlib)` từ project khác — `/usr/local`
chỉ root mới ghi được nên cần `sudo`, không thì `cmake --install` báo lỗi
permission denied:

```bash
sudo cmake --install build --prefix /usr/local
```

Muốn cài không cần `sudo` thì đổi prefix sang thư mục riêng trong `$HOME` (vd
`~/.local`), rồi thêm `-DCMAKE_PREFIX_PATH=~/.local` khi configure project nào
gọi `find_package(lidarlib)`:

```bash
cmake --install build --prefix "$HOME/.local"
```

```cmake
# trong CMakeLists.txt của project dùng thư viện này
find_package(lidarlib REQUIRED)
target_link_libraries(my_app PRIVATE lidarlib::lidarlib)
```

Sau khi link, giao diện chỉ cần `#include "lidarlib/lidarlib.hpp"` rồi gọi
`lidarlib::make_lidar()` — xem "Tóm tắt API" ở đầu README.

Vẫn có thể build từng file bằng g++ thuần nếu không muốn dùng CMake:

```bash
g++ -std=c++17 -O2 -pthread -Wall -Wextra -Iinclude -o test_dual examples/test_dual.cpp src/olei_lidar.cpp
g++ -std=c++17 -O2 -pthread -Wall -Wextra -Iinclude -o example   examples/example.cpp   src/olei_lidar.cpp
g++ -std=c++17 -O2 -pthread -Wall -Wextra -Iinclude -o lidar_app examples/lidar_app.cpp src/olei_lidar.cpp src/sick_lidar.cpp src/olei_config.cpp
g++ -std=c++17 -O2 -pthread -Wall -Wextra -Iinclude -o sick_example examples/sick_example.cpp src/sick_lidar.cpp
```

## Test thử

### 2 lidar song song

```bash
./build/test_dual
```

In ra 5 scan mỗi bên, kèm số điểm, timestamp, error status, và **model đã tự
dò được** (`model=...`).

### 1 lidar

Sửa model/IP/port trong `examples/example.cpp` rồi build lại, hoặc gọi trực tiếp:

```cpp
lidarlib::Driver drv(lidarlib::MODEL_AUTO, "192.168.100.100", 2369);
drv.open();
lidarlib::ScanResult result;
drv.recv_scan(result, 2000);
```

### Lidar lắp úp ngược

Constructor có tham số thứ 4 `inverted` (mặc định `false`). Đặt `true` nếu
thiết bị bị lắp lật 180° quanh trục hướng về phía trước — driver tự đảo dấu
góc từng điểm (`angle = -angle`, chuẩn hóa lại về `-180..180`) để output luôn
đúng theo hệ quy chiếu xe, không phụ thuộc hướng lắp vật lý:

```cpp
lidarlib::Driver drv(lidarlib::MODEL_AUTO, "192.168.100.100", 2369, /*inverted=*/true);
```

Đã verify bằng sniff sống: chạy `inverted=false` góc tăng dần theo thời gian,
chạy `inverted=true` góc giảm dần với cùng bước góc — đúng chữ ký của đảo dấu.

## Cấu hình & chạy (lidar_app + config.json)

Không còn Web UI. Cấu hình là một file JSON đơn giản — `examples/lidar_app.cpp`
đọc nó, mở từng lidar qua đúng **một hàm** `lidarlib::make_lidar()`, rồi đọc scan
trên mỗi thread. Đây là khung mẫu để bạn thay vòng `printf` bằng giao diện
riêng (Qt, ImGui, ROS node, v.v.).

```bash
./build/lidar_app                  # đọc/tạo config.json cạnh chỗ chạy
./build/lidar_app my_config.json   # đường dẫn config khác
```

`config.json` — danh sách lidar, không cố định số lượng. `brand` chọn loại
driver (`"OLEI"` = UDP, `"SICK"` = TCP/SOPAS); `model` tra trong bảng
`ModelConfig` (tên lạ → tự lùi về mặc định của hãng: `AUTO` cho OLEI,
`SICK-TIM571` cho SICK); `inverted` chỉ có tác dụng với OLEI. Bỏ trống `brand`
thì mặc định `"OLEI"` (tương thích file cũ).

```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", "inverted":false}
  ]
}
```

Trong code, đọc/ghi file bằng `lidarlib::load_config(path)` / `lidarlib::save_config(path, cfg)`
(file hỏng → trả về mặc định, không crash). Giao diện của bạn tự quyết khi nào
lưu — thư viện không tự bind port hay phục vụ HTTP gì cả.

### Sniff packet thô (debug khi nghi ngờ offset header)

```bash
python3 - <<'EOF'
import socket
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
s.bind(("192.168.100.100", 2369))   # đổi port theo lidar cần xem
data, addr = s.recvfrom(4096)
print("from", addr, "len", len(data))
print(data[:40].hex(' '))
EOF
```

Luôn ưu tiên capture thật hơn là tin comment trong code — comment mô tả ý
định lúc viết, không phải offset đã verify trên thiết bị thật.

## Chọn `ModelConfig`

| Constant | FOV (deg, có dấu) | range_min/max (m) | Khi dùng |
|---|---|---|---|
| `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 | 2D 360° 50m, Family A |
| `MODEL_LR1BS5` | -180…180 | 0.05…30 | 2D 360°, Family B |
| `MODEL_LR16F` | -135…135 | 0.05…30 | 3D 16-line |
| `MODEL_GS15` | -180…180 | 0.05…30 | 2D 360°, Family C/V3 — **chưa verify hardware thật** |
| `MODEL_AUTO` | -180…180 (mặc định, có thể tự thu hẹp) | 0.05…30 | Không biết trước model |

`range_min_m`/`range_max_m` chỉ là giá trị đặt sẵn để điền vào
`LaserScan::range_min/range_max` (không đọc từ packet) — chỉnh trong
`include/lidarlib/lidar.hpp` theo datasheet thật của từng model nếu cần chính xác.

`MODEL_AUTO`: chỉ có tác dụng tự-dò với **Family B** (đọc chuỗi tên model
trong header). Driver luôn lưu lại **tên thật** đọc từ packet (vd
`"OLELR-1BS5"`, `"OLELR-1BS2"`) — gọi `drv.detected_model()` hoặc
`result.info.detected_model` để xem (trả về `"AUTO"` nếu chưa nhận gói Family B
nào). FOV chỉ tự thu hẹp khi tên đó khớp một entry trong `kModelTable`
(`src/olei_lidar.cpp`); nếu không khớp, FOV giữ nguyên mặc định 360°
(`-180..180`, không mất điểm) — an toàn nhưng có thể giữ lại điểm ngoài FOV
thật của thiết bị nếu thiết bị đó không quét tròn.

Family A không mang chuỗi tên model trong packet, nên `MODEL_AUTO` trên thiết
bị Family A cũng giữ nguyên FOV rộng — phải chỉ định model cụ thể (VD
`MODEL_VB`) nếu muốn thu hẹp FOV cho thiết bị góc hẹp.

## Lidar SICK (TiM5xx/7xx) — driver riêng

`lidarlib::SickDriver` (`include/lidarlib/sick_lidar.hpp` + `src/sick_lidar.cpp`) là
driver **độc lập** với `lidarlib::Driver` ở trên — không phải thêm 1 family vào
driver OLEI, vì giao thức khác hẳn:

- Kết nối **TCP** (SOPAS, port mặc định 2111) tới lidar, không phải UDP
  broadcast như OLEI.
- Telegram là **ASCII** (CoLa-A), đóng khung bằng `STX`(0x02)/`ETX`(0x03),
  không có CRC32 như Family A.
- Thiết bị đứng im cho tới khi driver gửi lệnh `sEN LMDscandata 1` — `open()`
  tự làm việc này; `close()` gửi `sEN LMDscandata 0` trước khi đóng socket.

Output vẫn dùng chung `ScanResult`/`LaserScan`/`ExtraInfo` như driver OLEI nên
gọi giống hệt:

```cpp
lidarlib::SickDriver drv(lidarlib::MODEL_SICK_TIM571, "192.168.0.1", 2111);
drv.open();
lidarlib::ScanResult result;
drv.recv_scan(result, 2000);
```

| Constant | FOV (deg, có dấu) | range_min/max (m) | Khi dùng |
|---|---|---|---|
| `MODEL_SICK_TIM5XX` | -135…135 | 0.05…10 | TiM551/561, 270° |
| `MODEL_SICK_TIM571` | -135…135 | 0.05…25 | TiM571, 270° |
| `MODEL_SICK_TIM7XX` | -135…135 | 0.05…25 | TiM781, 270° |

**Đã verify bằng phần cứng thật**: chạy trực tiếp với 1 con **SICK TiM781S**
(FW `V5.11-14.10.24`, DeviceIdent đọc qua `sRN DeviceIdent` trên cổng 2111)
tại `192.168.100.22:2111` — `MODEL_SICK_TIM7XX`. Kết quả khớp đúng datasheet
TiM781S: 811 điểm/scan trải từ -45°…225° (270° FOV), `angle_increment` =
0.333° (1/3°, đúng độ phân giải góc của dòng 781), khoảng cách 0.3-1.5m ổn
định qua nhiều scan liên tiếp, kênh `RSSI1` có giá trị intensity hợp lý
(không phải toàn 0), `error_status` = 0x00. Điều này xác nhận layout
`LMDscandata` trong `parse_lmdscandata()` (`src/sick_lidar.cpp`) — kênh
`DIST1`/`RSSI1`, scaling factor IEEE-754, start angle/step width — đọc đúng
trên hardware thật, không chỉ đúng theo tài liệu nữa.

**Vẫn chưa verify**: chiều quy ước góc 0° (thẳng phía trước thiết bị hay
hướng khác — chưa đối chiếu với hướng lắp vật lý thật), thiết bị có encoder
(`NumEncoders > 0`, nhánh `next()`×2 chưa từng chạy qua vì test thực tế không
có encoder), và nhánh 8-bit channel (`Num8BitChannels`, thiết bị test chỉ
dùng kênh 16-bit). `MODEL_SICK_TIM5XX`/`MODEL_SICK_TIM571` (FOV/range theo
datasheet) cũng chưa test trên phần cứng — chỉ `MODEL_SICK_TIM7XX` đã chạy
thật.

`SickDriver` nối vào `config.json` qua field `LidarConfig::brand` (`"OLEI"`
hoặc `"SICK"`, mặc định `"OLEI"`): `lidarlib::make_lidar()` thấy `brand=="SICK"` thì
trả về `lidarlib::SickDriver` (TCP/SOPAS) thay cho `lidarlib::Driver` (UDP), cùng kiểu
trả về `std::unique_ptr<lidarlib::Lidar>` nên phía gọi không phải phân biệt. Xem
`examples/lidar_app.cpp` (chạy theo config) hoặc `examples/sick_example.cpp`
(dùng thẳng `SickDriver` qua API C++).