302 lines
15 KiB
Markdown
302 lines
15 KiB
Markdown
# 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).
|
|
|
|
### Chẩn đoán thiết bị (diagnostics)
|
|
|
|
Lidar OLEI nhúng thông tin tự chẩn đoán trong header gói dữ liệu (không có
|
|
kênh query riêng). API đọc ra qua `lidarlib/diagnostics.hpp` — nghiên cứu
|
|
chi tiết layout từng family xem [docs/diagnostics.md](docs/diagnostics.md).
|
|
|
|
```cpp
|
|
lidarlib::Diagnostics d = lidar->get_diagnostics();
|
|
if (!d.valid) {
|
|
// chưa decode được scan 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("lidar 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
|
|
}
|
|
```
|
|
|
|
- `get_diagnostics()` — snapshot từ vòng quét gần nhất; gọi cùng thread với
|
|
`recv_scan()`/`spin_once()`. `d.healthy()` = có data và không có fault.
|
|
- `decode_diagnostics(result.info)` — decode gắn với một scan cụ thể.
|
|
- Nguồn dữ liệu: OLEI Family A byte lỗi + `rotation_raw` + timestamp;
|
|
Family B không có gì trên wire; Family C `input/output/field_status`,
|
|
`status_flags` (raw, chưa verify); SICK TiM cặp device status trong
|
|
`LMDscandata`; nanoScan3 block General System State (cần bật trong
|
|
Safety Designer).
|
|
|
|
## 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/diagnostics.hpp` | `Diagnostics`, bit lỗi Family A, `decode_diagnostics()` |
|
|
| `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-07 — Diagnostics API cho SICK
|
|
|
|
- **`SickDriver` (TiM)**: decode cặp Device Status của `LMDscandata`
|
|
(0 ok / 1 error / 2 pollution warning / 4 pollution error) vào
|
|
`Diagnostics` — `sick_error()`, `pollution_warning()`, `pollution_error()`.
|
|
`info.sick_device_status` là trường mới; `info.error_status` **không còn**
|
|
nhận `status0` (trường đó thuộc Family A OLEI).
|
|
- **`NanoScanDriver`**: parse block General System State (offset header
|
|
`[32]/[34]`, byte 0) → `contamination_warning()`, `contamination_error()`,
|
|
`manipulation()`. Layout theo `sick_safetyscanners`, chưa verify phần cứng;
|
|
block phải được bật trong Safety Designer.
|
|
- **`Diagnostics` thêm `has_warning()`** (kính bẩn mức cảnh báo) và
|
|
`to_string()` in thêm `WARN: pollution` / `FAULT: ... contamination`.
|
|
- Test loopback: TiM server TCP giả (status `0 4` → pollution error) và gói
|
|
nanoScan3 tổng hợp (state `0x09` → contamination error, gói sạch →
|
|
`healthy()`); tài liệu tại [docs/diagnostics.md](docs/diagnostics.md) §4-5.
|
|
|
|
### 2026-07-07 — Diagnostics API
|
|
|
|
- **Header mới `lidarlib/diagnostics.hpp`**: struct `Diagnostics` (bit lỗi
|
|
Family A decode sẵn: `monitor/voltage/temperature_fault()`, `has_fault()`,
|
|
`healthy()`; trường raw Family C), hằng `kFaultMonitor/Voltage/Temperature`,
|
|
`to_string()` để log.
|
|
- **`Lidar::get_diagnostics()`**: snapshot chẩn đoán từ vòng quét decode gần
|
|
nhất; `valid = false` khi chưa có scan. Driver OLEI implement đầy đủ;
|
|
SICK tạm trả mặc định.
|
|
- **`decode_diagnostics(const ExtraInfo&)`**: decode gắn với một
|
|
`ScanResult` cụ thể.
|
|
- **Tài liệu nghiên cứu [docs/diagnostics.md](docs/diagnostics.md)**: layout
|
|
byte chẩn đoán từng family (A: byte lỗi `[5]` + rotation + timestamp;
|
|
B: không có; C: input/output/field/status raw), chẩn đoán tầng transport,
|
|
hướng mở rộng SICK.
|
|
- Đã test bằng gói Family A tổng hợp qua loopback (fault set → decode đúng
|
|
từng bit, vòng sạch → `healthy()`); bit map lấy theo tài liệu OLEI, chưa
|
|
tái tạo fault trên phần cứng thật.
|
|
|
|
### 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)`.
|