refactor: restructure lidarlib into xlidar-driver plugin SDK
- LidarManager facade (liblidar_manager.so): dlopen plugin discovery, available_drivers map<driver_id, PluginRegistry>, create_lidar_device, config.json load/save with legacy lidarlib migration - Common LidarDriverInterface + DriverInfo/DeviceConfig plugin ABI (extern C get_driver_info / create_driver_instance) - Plugins: driver_rplidar (ported from xlocd, Slamtec SDK), driver_olei, driver_sick_code (TiM CoLa-A), driver_sick_safety (nanoScan3), driver_espe - Diagnostics extended with rplidar health + firmware; FOV filter window, range override and legacy remap window unified in DeviceConfig - Rewritten README, diagnostics doc and examples (list_drivers, example, lidar_app) Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
This commit is contained in:
389
README.md
389
README.md
@@ -1,125 +1,155 @@
|
||||
# Lidarlib
|
||||
# xlidar-driver
|
||||
|
||||
Thư viện C++17 thu nhận dữ liệu lidar 2D cho **OLEI** (UDP), **SICK**
|
||||
(TCP/UDP) và **ESPE** (TCP/UDP)
|
||||
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ị.
|
||||
|
||||
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`.
|
||||
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`.
|
||||
|
||||
## Tính năng
|
||||
## Kiến trúc
|
||||
|
||||
- **Đ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.
|
||||
```
|
||||
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_code/ # → driver_sick_code.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.10, trình dịch C++17. Không có dependency ngoài
|
||||
(chỉ pthread).
|
||||
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
|
||||
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)"
|
||||
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).
|
||||
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.
|
||||
|
||||
Dùng từ project khác:
|
||||
|
||||
```cmake
|
||||
find_package(lidarlib REQUIRED)
|
||||
target_link_libraries(my_app PRIVATE lidarlib::lidarlib)
|
||||
```
|
||||
Tùy chọn: `-DXLIDAR_BUILD_EXAMPLES=OFF`. Hỗ trợ `cmake --install` +
|
||||
`find_package(xlidar_driver)` (target `xlidar::lidar_manager`).
|
||||
|
||||
## Sử dụng
|
||||
|
||||
### Đọc scan
|
||||
### Khám phá driver và đọc scan
|
||||
|
||||
```cpp
|
||||
#include "lidarlib/lidarlib.hpp" // toàn bộ API trong một include
|
||||
#include "lidar_manager.hpp" // kéo theo lidar_interface.hpp
|
||||
|
||||
lidarlib::LidarConfig c{"front", "192.168.1.10", 2368, "AUTO", false, "OLEI"};
|
||||
std::unique_ptr<lidarlib::Lidar> lidar = lidarlib::make_lidar(c);
|
||||
xlidar::LidarManager manager("plugins");
|
||||
manager.load_all_plugins();
|
||||
|
||||
if (lidar->open() != lidarlib::ErrorCode::Ok) {
|
||||
fprintf(stderr, "open: %s\n", lidarlib::to_string(lidar->last_error()));
|
||||
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;
|
||||
}
|
||||
|
||||
lidarlib::ScanResult r;
|
||||
xlidar::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);
|
||||
```
|
||||
Driver serial (rplidar) dùng `cfg.serial_port` + `cfg.baudrate` thay cho
|
||||
`ip`/`port`. Xem `examples/example.cpp`.
|
||||
|
||||
### Chế độ callback
|
||||
|
||||
Thay cho `recv_scan()` blocking:
|
||||
|
||||
```cpp
|
||||
lidar->set_scan_callback([](const lidarlib::ScanResult& r) { /* mỗi vòng quét */ });
|
||||
lidar->set_scan_callback([](const xlidar::ScanResult& r) { /* mỗi vòng quét */ });
|
||||
while (running) lidar->spin_once();
|
||||
```
|
||||
|
||||
### Chẩn đoán thiết bị
|
||||
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()`.
|
||||
|
||||
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)):
|
||||
### Sẵn sàng & chẩn đoán thiết bị
|
||||
|
||||
```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
|
||||
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
|
||||
printf("fault: %s\n", lidarlib::to_string(d).c_str());
|
||||
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_warning() / contamination_warning() — kính bẩn nhẹ, nên lau
|
||||
// pollution/contamination warning, rplidar health warning
|
||||
}
|
||||
```
|
||||
|
||||
`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`.
|
||||
`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ả của lần gọi gần nhất.
|
||||
`open()` trả về `ErrorCode`; `last_error()` giữ kết quả gần nhất.
|
||||
|
||||
| ErrorCode | Ý nghĩa |
|
||||
|---|---|
|
||||
@@ -127,166 +157,109 @@ biến khỏe khi và chỉ khi `recv_scan()` thành công đều đặn **và**
|
||||
| `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 |
|
||||
| `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()` 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).
|
||||
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ở từng lidar một thread:
|
||||
`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", "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"}
|
||||
{"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 |
|
||||
|---|---|
|
||||
| `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 |
|
||||
| `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 |
|
||||
|
||||
Đọc/ghi bằng `lidarlib::load_config(path)` / `lidarlib::save_config(path, cfg)`.
|
||||
File format cũ của lidarlib (`{"brand": "OLEI", ...}`) được migrate tự động
|
||||
khi load: `brand`+`model` → `driver_id`, cặp `angle_*_deg` cũ → remap.
|
||||
|
||||
## Model hỗ trợ
|
||||
## Driver đi kèm
|
||||
|
||||
### OLEI (UDP, port mặc định 2368)
|
||||
| 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_code.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 |
|
||||
|
||||
| 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.
|
||||
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`
|
||||
### `ScanResult` = `{ LaserScan scan; ExtraInfo info; }`
|
||||
|
||||
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`:
|
||||
|
||||
### `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 |
|
||||
| Field | Ý 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: …` |
|
||||
| `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 |
|
||||
|
||||
### `ModelConfig` & `LidarConfig`
|
||||
`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`.
|
||||
|
||||
- `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.
|
||||
## Viết một plugin mới
|
||||
|
||||
## Cấu trúc source
|
||||
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:
|
||||
|
||||
| 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 |
|
||||
```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++).
|
||||
|
||||
Reference in New Issue
Block a user