loctv
f02d81c031
The driver targets the SICK TiM 5xx/7xx family over CoLa-A; "code" described neither the devices nor the protocol. driver_id stays sick_tim_driver. Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
266 lines
13 KiB
Markdown
266 lines
13 KiB
Markdown
# 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_tim/ # → driver_sick_tim.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_tim.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++).
|