feat(config,diagnostics): explicit transport in DeviceConfig, vendor-neutral diagnostics

DeviceConfig now carries an optional transport (serial/udp/tcp) instead of
the ESPE-only use_udp bool. Plugins validate it in create_driver_instance:
a fixed-transport driver configured with the wrong transport fails open()
with InvalidConfig (via InvalidConfigDriver — the plugin ABI forbids
returning nullptr) rather than silently ignoring the setting. Selectable
drivers (ESPE) switch TCP/UDP through the same field. config.json
load/save round-trips "transport" for every transport, including serial,
and migrates legacy use_udp:true entries.

Diagnostics drops the per-vendor accessors (espe_fault, rplidar_fault,
monitor_fault, sick_error, pollution_*, contamination_*, manipulation) for
one common shape: a list of DiagnosticIssue{severity, code, detail} with
cross-vendor codes, plus a raw map of vendor passthrough values and
to_json() for hosts that prefer a string. Vendor bit decoding now lives in
one place (decode_diagnostics); has_fault/has_warning/healthy keep their
meaning, so is_ready()/wait_ready() are unchanged.

Also: README regains the model/protocol and ExtraInfo tables lost in the
lidarlib->xlidar refactor (verified against current code), and the empty
xlocd/ tree left by a stray sync run is gone.

Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>

CMakeLists.txt

@@ -1,5 +1,5 @@
-cmake_minimum_required(VERSION 3.10)
+cmake_minimum_required(VERSION 3.16)
-project(lidarlib VERSION 1.0.0 LANGUAGES CXX)
+project(xlidar_driver VERSION 2.0.0 LANGUAGES CXX)
 
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
@@ -9,77 +9,48 @@ endif()
 
 find_package(Threads REQUIRED)
 
-option(BUILD_SHARED_LIBS "Build shared (.so) libraries instead of static" ON)
+option(XLIDAR_BUILD_EXAMPLES "Build example/demo binaries" ON)
 
-set(LIDARLIB_SOURCES
+# Where the rplidar plugin finds the Slamtec SDK sources (include/ + src/).
-    src/olei_lidar.cpp
+# Empty + not auto-detected -> the rplidar plugin is skipped with a warning.
-    src/sick_lidar.cpp
+set(XLIDAR_RPLIDAR_SDK_DIR "" CACHE PATH "Path to the Slamtec RPLIDAR SDK (dir containing include/ and src/)")
-    src/espe_lidar.cpp
-    src/lidar_config.cpp
-)
 
-add_library(lidarlib ${LIDARLIB_SOURCES})
+add_subdirectory(src)
-set_target_properties(lidarlib PROPERTIES
+add_subdirectory(plugins)
-    VERSION ${PROJECT_VERSION}
-    SOVERSION ${PROJECT_VERSION_MAJOR}
-    POSITION_INDEPENDENT_CODE ON
-)
-target_include_directories(lidarlib PUBLIC
-    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-    $<INSTALL_INTERFACE:include>
-)
-target_link_libraries(lidarlib PUBLIC Threads::Threads)
 
-option(LIDARLIB_BUILD_EXAMPLES "Build example/demo binaries" ON)
+if(XLIDAR_BUILD_EXAMPLES)
-if(LIDARLIB_BUILD_EXAMPLES)
+  add_subdirectory(examples)
-  add_executable(example examples/example.cpp)
-  target_link_libraries(example PRIVATE lidarlib)
-
-  add_executable(test_dual examples/test_dual.cpp)
-  target_link_libraries(test_dual PRIVATE lidarlib)
-
-  add_executable(sick_example examples/sick_example.cpp)
-  target_link_libraries(sick_example PRIVATE lidarlib)
-
-  add_executable(nanoscan_example examples/nanoscan_example.cpp)
-  target_link_libraries(nanoscan_example PRIVATE lidarlib)
-
-  add_executable(espe_example examples/espe_example.cpp)
-  target_link_libraries(espe_example PRIVATE lidarlib)
-
-  add_executable(lidar_app examples/lidar_app.cpp)
-  target_link_libraries(lidar_app PRIVATE lidarlib)
 endif()
 
-# install + find_package() support
+# ── install + find_package() support ─────────────────────────────────────────
 include(GNUInstallDirs)
 include(CMakePackageConfigHelpers)
 
-install(TARGETS lidarlib
+install(TARGETS lidar_manager
-    EXPORT lidarlibTargets
+    EXPORT xlidar_driverTargets
     ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
     LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
 )
-install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR})
+install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/xlidar)
 
-install(EXPORT lidarlibTargets
+install(EXPORT xlidar_driverTargets
-    FILE lidarlibTargets.cmake
+    FILE xlidar_driverTargets.cmake
-    NAMESPACE lidarlib::
+    NAMESPACE xlidar::
-    DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/lidarlib
+    DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/xlidar_driver
 )
 
 configure_package_config_file(
-    cmake/lidarlibConfig.cmake.in
+    cmake/xlidar_driverConfig.cmake.in
-    ${CMAKE_CURRENT_BINARY_DIR}/lidarlibConfig.cmake
+    ${CMAKE_CURRENT_BINARY_DIR}/xlidar_driverConfig.cmake
-    INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/lidarlib
+    INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/xlidar_driver
 )
 write_basic_package_version_file(
-    ${CMAKE_CURRENT_BINARY_DIR}/lidarlibConfigVersion.cmake
+    ${CMAKE_CURRENT_BINARY_DIR}/xlidar_driverConfigVersion.cmake
     VERSION ${PROJECT_VERSION}
     COMPATIBILITY SameMajorVersion
 )
 install(FILES
-    ${CMAKE_CURRENT_BINARY_DIR}/lidarlibConfig.cmake
+    ${CMAKE_CURRENT_BINARY_DIR}/xlidar_driverConfig.cmake
-    ${CMAKE_CURRENT_BINARY_DIR}/lidarlibConfigVersion.cmake
+    ${CMAKE_CURRENT_BINARY_DIR}/xlidar_driverConfigVersion.cmake
-    DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/lidarlib
+    DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/xlidar_driver
 )

README.md

@@ -1,125 +1,162 @@
-# Lidarlib
+# xlidar-driver
 
-Thư viện C++17 thu nhận dữ liệu lidar 2D cho **OLEI** (UDP), **SICK**
+SDK driver lidar 2D cho Linux, kiến trúc **plugin nạp động**: mỗi driver là
-(TCP/UDP) và **ESPE** (TCP/UDP)
+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
+Driver đi kèm: **Slamtec RPLIDAR** (serial), **OLEI** (UDP), **SICK TiM**
-factory duy nhất `lidarlib::make_lidar()`, output thống nhất theo định dạng
+(TCP/CoLa-A), **SICK nanoScan3** (UDP safety), **ESPE LGA60** (TCP/UDP).
-ROS `sensor_msgs/LaserScan`.
+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 /
+xlidar_driver/
-  `close()`.
+├── CMakeLists.txt
-- **Tự nhận diện giao thức** — phân biệt họ giao thức OLEI theo frame ID từng
+├── include/                    # API public — host chỉ include từ đây
-  gói; chế độ `AUTO` tự dò model từ dữ liệu (Family B/C).
+│   ├── lidar_interface.hpp     #   LidarDriverInterface, DriverInfo, DeviceConfig,
-- **Chẩn đoán thiết bị** — đọc trạng thái tự chẩn đoán nhúng trong stream:
+│   │                           #   LaserScan/ScanResult, ErrorCode, plugin ABI
-  lỗi motor/điện áp/nhiệt độ (OLEI), kính bẩn/pollution (SICK TiM),
+│   ├── lidar_diagnostics.hpp   #   Diagnostics + bit lỗi từng hãng
-  contamination/manipulation (nanoScan3).
+│   └── lidar_manager.hpp       #   LidarManager, PluginRegistry, config.json
-- **Xử lý lỗi tường minh** — `ErrorCode` phân loại từ `errno` thật;
+├── src/
-  lifecycle an toàn với mọi thứ tự gọi `open()`/`close()`.
+│   ├── CMakeLists.txt          # → liblidar_manager.so
-- **Đa luồng an toàn** — mỗi instance độc lập hoàn toàn, chạy mỗi lidar một
+│   ├── lidar_manager.cpp       #   dlopen/dlsym + load/save config.json
-  thread không cần khóa.
+│   └── json_mini.hpp
-- **Cấu hình JSON** — khai báo danh sách lidar trong `config.json`,
+├── plugins/                    # mỗi thư mục → một plugin .so
-  load/save bằng API kèm sẵn.
+│   ├── 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.10, trình dịch C++17. Không có dependency ngoài
+Yêu cầu: Linux, CMake ≥ 3.16, C++17. Không có dependency ngoài (pthread,
-(chỉ 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),
+Kết quả: `build/src/liblidar_manager.so`, plugin trong `build/plugins/*.so`,
-`-DBUILD_SHARED_LIBS=OFF` (build static).
+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:
+Tùy chọn: `-DXLIDAR_BUILD_EXAMPLES=OFF`. Hỗ trợ `cmake --install` +
-
+`find_package(xlidar_driver)` (target `xlidar::lidar_manager`).
-```cmake
-find_package(lidarlib REQUIRED)
-target_link_libraries(my_app PRIVATE lidarlib::lidarlib)
-```
 
 ## 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"};
+xlidar::LidarManager manager("plugins");
-std::unique_ptr<lidarlib::Lidar> lidar = lidarlib::make_lidar(c);
+manager.load_all_plugins();
 
-if (lidar->open() != lidarlib::ErrorCode::Ok) {
+for (const auto& [id, plugin] : manager.available_drivers())
-    fprintf(stderr, "open: %s\n", lidarlib::to_string(lidar->last_error()));
+    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:
+Driver serial (rplidar) dùng `cfg.serial_port` + `cfg.baudrate` thay cho
-
+`ip`/`port`. Xem `examples/example.cpp`.
-```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);
-```
 
 ### 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 sàng & chẩn đoán thiết bị
-sẵn qua `get_diagnostics()` (chi tiết layout từng giao thức:
+
-[docs/diagnostics.md](docs/diagnostics.md)):
+API chẩn đoán **chung cho mọi hãng**: `Diagnostics` trả về danh sách
+`issues` (mỗi issue = `severity` fault/warning + `code` trung lập + `detail`
+người-đọc-được), không có hàm riêng từng hãng. Giá trị thô của hãng nằm
+trong map `raw` (VD `"sick.device_status"`, `"rplidar.error_code"`); cần
+dạng chuỗi thì dùng `to_json()`.
 
 ```cpp
-lidarlib::Diagnostics d = lidar->get_diagnostics();
+if (!lidar->wait_ready(5000)) { /* chưa có scan sạch nào trong 5s */ }
-if (!d.valid) {
+
-    // chưa decode được vòng quét nào
+xlidar::Diagnostics d = lidar->get_diagnostics();
-} else if (d.has_fault()) {
+for (const xlidar::DiagnosticIssue& issue : d.issues) {
-    // OLEI Family A
+    // issue.severity : DiagSeverity::Fault | DiagSeverity::Warning
-    d.monitor_fault();       // motor/giám sát bất thường
+    // issue.code     : "motor" | "voltage" | "temperature" | "optics_dirty"
-    d.voltage_fault();       // điện áp ngoài dải
+    //                  | "manipulation" | "device_error" | "device_warning"
-    d.temperature_fault();   // nhiệt độ bất thường
+    // issue.detail   : mô tả kèm tên hãng + giá trị thô, VD "ESPE fault word 0x0004"
-    // SICK
+    printf("[%s] %s — %s\n", xlidar::to_string(issue.severity),
-    d.sick_error();          // TiM: device error
+           issue.code.c_str(), issue.detail.c_str());
-    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("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
 }
+if (d.has_fault())   { /* thiết bị báo hỏng — dừng tin dữ liệu */ }
+if (d.has_warning()) { /* suy giảm (kính bẩn, ...) — lên lịch bảo trì */ }
+
+printf("%s\n", xlidar::to_string(d).c_str()); // "ok" / "FAULT: voltage | WARN: optics_dirty"
+printf("%s\n", xlidar::to_json(d).c_str());   // JSON đầy đủ cho REST/telemetry
 ```
 
-`d.healthy()` = đã có dữ liệu và không có fault. Khuyến nghị giám sát: cảm
+`d.healthy()` = đã có dữ liệu và không fault. `is_ready(max_age_ms)` = đang
-biến khỏe khi và chỉ khi `recv_scan()` thành công đều đặn **và**
+mở + healthy + scan mới nhất chưa quá hạn — diagnostics chỉ refresh qua
-`get_diagnostics().has_fault() == false`.
+`recv_scan()`/`spin_once()`, nên gọi từ chính thread bơm dữ liệu. Ý nghĩa
+từng `code`, key `raw` và layout chẩn đoán từng giao thức:
+[docs/diagnostics.md](docs/diagnostics.md) + comment trong
+`include/lidar_diagnostics.hpp`.
 
 ### 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 +164,179 @@ 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 |
+| `PortInUse` / `BindFailed` / `SocketError` | Lỗi bind/socket (driver UDP) |
-| `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á hạn |
-| `ConnectionRefused` / `ConnectionFailed` / `Timeout` | TCP connect bị từ chối / không tới được / quá thời hạn |
+| `HandshakeFailed` | Nối được nhưng lệnh start-stream/start-scan thất bại |
-| `HandshakeFailed` | TCP nối được nhưng lệnh start-stream 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
+Lifecycle an toàn với mọi thứ tự gọi: `close()` idempotent, `open()` lặp trả
-trả `AlreadyOpen` và không đụng kết nối đang chạy, sau `close()` có thể
+`AlreadyOpen`, sau `close()` mở lại được. Mỗi instance độc lập hoàn toàn —
-`open()` lại (state được reset).
+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":"front", "driver_id":"olei_lidar_driver",   "model":"AUTO",        "ip":"192.168.100.100", "port":2368},
-    {"name":"rear",  "ip":"192.168.100.100", "port":2369, "brand":"OLEI", "model":"AUTO", "inverted":true},
+    {"name":"sick1", "driver_id":"sick_tim_driver",     "model":"SICK-TIM7xx", "ip":"192.168.0.1",     "port":2111},
-    {"name":"sick1", "ip":"192.168.0.1",     "port":2111, "brand":"SICK", "model":"SICK-TIM571"},
+    {"name":"nano1", "driver_id":"sick_nanoscan3_driver","model":"SICK-nanoScan3", "ip":"0.0.0.0",     "port":6060},
-    {"name":"nano1", "ip":"0.0.0.0",         "port":6060, "brand":"SICK", "model":"SICK-nanoScan3"},
+    {"name":"espe1", "driver_id":"espe_lga60_driver",   "model":"ESPE-LGA60",  "transport":"udp",   "ip":"192.168.1.88", "port":8080},
-    {"name":"espe1", "ip":"192.168.1.88",    "port":8080, "brand":"ESPE", "model":"ESPE-LGA60"}
+    {"name":"rp1",   "driver_id":"rplidar_c1_driver",   "model":"AUTO",        "transport":"serial", "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"` |
+| `driver_id` | Plugin phụ trách thiết bị (bắt buộc) |
-| `model` | Tên trong bảng model bên dưới; tên lạ → mặc định của hãng |
+| `model` | Một trong `supported_models` của driver; tên lạ → mặc định của driver |
-| `inverted` | `true` nếu lidar lắp úp ngược — driver tự đảo góc (mọi hãng) |
+| `transport` | `"serial"` / `"udp"` / `"tcp"`. Bỏ trống = transport mặc định của driver. Driver cố định transport mà bị cấu hình sai → `open()` trả `InvalidConfig`; driver `transport_selectable` (ESPE) chuyển TCP↔UDP qua trường này |
-| `use_udp` | Chỉ ESPE: `true` để dùng transport UDP thay vì TCP |
+| `ip` / `port` | Transport mạng: UDP = địa chỉ bind cục bộ, TCP = địa chỉ thiết bị; `port` 0 = mặc định của driver |
-| `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 |
+| `serial_port` / `baudrate` | Transport serial (rplidar); mặc định `/dev/ttyUSB0` @ 460800 |
+| `inverted` | `true` nếu lidar lắp úp ngược — driver tự đảo góc |
+| `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 migrate tự động khi load: `brand`+`model` (lidarlib) →
+`driver_id`, cặp `angle_*_deg` cũ → remap, `use_udp: true` → `transport:
+"udp"`.
 
-## 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_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 |
 
-| Constant | FOV (°) | Range (m) | Giao thức |
+### Thông số model
+
+FOV/range dưới đây là mặc định theo `ModelConfig` của từng driver — ghi đè
+bằng `range_min_m`/`range_max_m` và cửa sổ `angle_min_deg`/`angle_max_deg`.
+Driver mạng dùng hệ góc có dấu, 0° = phía trước; rplidar giữ hệ góc thiết bị
+[0, 2π).
+
+**OLEI** (UDP, port 2368) — 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 tuỳ kiểu dữ liệu). Tên model đọc từ packet
+(Family B/C): `detected_model()` hoặc `result.info.detected_model`.
+
+| Model | FOV (°) | Range (m) | Giao thức |
 |---|---|---|---|
-| `MODEL_VB` | −135…135 | 0.05…30 | Family A |
+| `AUTO` | −180…180 | 0.05…30 | Tự dò model từ dữ liệu (Family B/C) |
-| `MODEL_VF` | −180…180 | 0.05…30 | Family A |
+| `VB` | −135…135 | 0.05…30 | Family A |
-| `MODEL_LR1F` | −180…180 | 0.05…50 | Family A (0° thiết bị hướng đuôi, offset +180°) |
+| `VF` | −180…180 | 0.05…30 | Family A |
-| `MODEL_LR1FMI` | −180…180 | 0.05…30 | Family B, ~2400 điểm/vòng (0° hướng đuôi) |
+| `LR-1F` | −180…180 | 0.05…50 | Family A — 0° thiết bị hướng đuôi (offset +180°) |
-| `MODEL_LR1BS5` | −180…180 | 0.05…30 | Family B |
+| `LR-1FMI` | −180…180 | 0.05…30 | Family B — 0° thiết bị hướng đuôi |
-| `MODEL_LR16F` | −135…135 | 0.05…30 | 3D 16-line |
+| `LR-1BS5` | −180…180 | 0.05…30 | Family B |
-| `MODEL_GS15` | −180…180 | 0.05…30 | Family C/V3 — chưa verify phần cứng |
+| `LR-16F` | −135…135 | 0.05…30 | 3D 16-line |
-| `MODEL_AUTO` | −180…180 | 0.05…30 | Tự dò model (Family B/C) |
+| `GS1-5` | −180…180 | 0.05…30 | Family C/V3 — chưa verify phần cứng |
 
-Driver nhận diện họ giao thức theo frame ID từng gói: **Family A** `0xFAF0`
+**SICK** — TiM qua TCP/SOPAS (CoLa-A) port 2111: `open()` gửi
-(header 20 B, 3 B/điểm, CRC32) · **Family B** `0xFEF0` (header 40 B kèm tên
+`sEN LMDscandata 1` để start stream; frame wire đặt 90° ở phía trước nên
-model ASCII, 8 B/điểm) · **Family C/V3** `0xFEAC` (header 48 B, 2–4 B/điểm).
+driver offset −90° để 0° = phía trước. nanoScan3 là receiver UDP thụ động
-Tên model đọc từ packet: `detected_model()` hoặc `result.info.detected_model`.
+port 6060 — đích UDP phải cấu hình sẵn bằng SICK Safety Designer, driver
+không handshake CoLa2/TCP.
 
-### SICK
+| Model | FOV (°) | Range (m) | Ghi chú |
-
-| Constant | FOV (°) | Range (m) | Transport |
 |---|---|---|---|
-| `MODEL_SICK_TIM5XX` | −135…135 | 0.05…10 | TCP/SOPAS (CoLa-A), port 2111 |
+| `SICK-TIM5xx` | −135…135 | 0.05…10 | TiM551/561 — FOV/range theo datasheet, chưa verify |
-| `MODEL_SICK_TIM571` | −135…135 | 0.05…25 | TCP/SOPAS, port 2111 |
+| `SICK-TIM571` | −135…135 | 0.05…25 | Chưa verify phần cứng |
-| `MODEL_SICK_TIM7XX` | −135…135 | 0.05…25 | TCP/SOPAS, port 2111 — verify trên TiM781S thật |
+| `SICK-TIM7xx` | −135…135 | 0.05…25 | Verify trên TiM781S thật (FW V5.11) |
-| `MODEL_SICK_NANOSCAN3` | −137.5…137.5 | 0.05…40 | UDP safety-data, port 6060 |
+| `SICK-nanoScan3` | −137.5…137.5 | 0.05…40 | Cả microScan3; layout port từ sick_safetyscanners, chưa verify |
 
-- **TiM (`SickDriver`)** — `open()` tự gửi lệnh start-stream; góc output đã
+**ESPE LGA60** (TCP mặc định / UDP, port 8080; IP mặc định của hãng
-  quy về 0° = phía trước.
+192.168.1.88) — FOV 320°: thiết bị quét 20°→340° với 0° hướng đuôi (offset
-- **nanoScan3 (`NanoScanDriver`)** — receiver UDP thụ động; đích UDP phải
+−180°), preset −160…160°, range 0.05…50 m. Frame đo `HISN` (header 16 B
-  cấu hình sẵn trong SICK Safety Designer. Chưa verify phần cứng thật.
+big-endian; điểm = distance mm + intensity, distance 50 000 mm = không có
+phản hồi → ∞); frame vùng `WSimu` (nếu thiết bị gửi) được đọc lấy từ lỗi.
+Độ phân giải (0.025–0.5°), tốc độ quay, mức lọc nhiễu theo cấu hình đã nạp
+bằng tool Windows của hãng — driver không tự đổi. Chưa verify phần cứng.
 
-### ESPE
+**RPLIDAR** (serial, mặc định `/dev/ttyUSB0` @ 460800) — preset C1: range
+0.05…16 m (datasheet 12 m trên nền trắng), ~10 Hz, ~400–500 điểm/vòng
+(DenseBoost); A/S series dùng được với baud tương ứng. `open()` chạy health
+check (Error → `DeviceError`) và tự nhận model/firmware. Riêng driver này
+đo được `time_increment`/`scan_time` thực.
 
-| Constant | FOV (°) | Range (m) | Transport |
+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).
-| `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.
 
 ## 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`
+| 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 |
 
-Cùng field và đơn vị với ROS `sensor_msgs/LaserScan`:
+`ExtraInfo`: metadata thô tuỳ giao thức — field thiết bị không có giữ
-
+`std::nullopt`:
-| 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) |
+| `error_status` | OLEI Family A | Byte lỗi thiết bị: BIT0 monitor, BIT1 điện áp, BIT2 nhiệt độ |
-| `distance_scale_mm` | OLEI A/B | Hệ số mm/count của khoảng cách |
+| `distance_scale_mm` | OLEI A/B | Hệ số mm/count của khoảng cách; 0 = không báo |
 | `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) |
+| `distance_ratio_raw`, `scan_frequency_raw`, `input_status`, `output_status`, `field_status`, `status_flags` | OLEI Family C/V3 | Passthrough raw — ý nghĩa bit chưa verify |
-| `sick_device_status` | SICK TiM | Cặp Device Status: 0 ok · 1 error · 2 pollution warning · 4 pollution error |
+| `sick_device_status` | SICK TiM | Cặp Device Status `(word0<<8)\|word1`: 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) |
+| `espe_error_status` | ESPE LGA60 | Từ lỗi từ frame vùng `WSimu` — chỉ có khi thiết bị gửi area data |
+| `rplidar_health_status`, `rplidar_error_code` | RPLIDAR | Health từ SDK (0 ok · 1 warning · 2 error) + mã lỗi thiết bị đi kèm |
 
-### `Diagnostics`
+`Diagnostics` (từ `get_diagnostics()` hoặc `decode_diagnostics(info)`) là
+bản decode **trung lập hãng**: `issues` (danh sách `{severity, code,
+detail}` với code chung: `motor` / `voltage` / `temperature` /
+`optics_dirty` / `manipulation` / `device_error` / `device_warning`), map
+`raw` giữ giá trị thô theo key ổn định (`"olei.error_status"`,
+`"sick.device_status"`, `"nano.general_state"`, `"espe.error_status"`,
+`"rplidar.health_status"`, `"rplidar.error_code"`, ...), cùng `model`,
+`firmware` (rplidar), `device_timestamp_ms`, và helper `has_fault()` /
+`has_warning()` / `healthy()` / `to_string()` / `to_json()`.
 
-Trạng thái tự chẩn đoán đã decode (`lidarlib/diagnostics.hpp`), trả về từ
+## Viết một plugin mới
-`get_diagnostics()` hoặc `decode_diagnostics(result.info)`:
 
-| API | Ý nghĩa |
+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
-| `valid` | Đã decode được ít nhất một vòng quét |
+   `add_subdirectory` vào `plugins/CMakeLists.txt`.
-| `monitor_fault()` / `voltage_fault()` / `temperature_fault()` | OLEI Family A: motor / điện áp / nhiệt độ bất thường |
+2. Implement class kế thừa `xlidar::LidarDriverInterface` — đủ
-| `sick_error()` / `pollution_warning()` / `pollution_error()` | SICK TiM: lỗi thiết bị / kính bẩn nhẹ / kính bẩn nặng |
+   `open`/`close`/`recv_scan`/`spin_once`/`set_scan_callback`/
-| `contamination_warning()` / `contamination_error()` / `manipulation()` | nanoScan3: kính bẩn / nghi bị can thiệp |
+   `detected_model`/`is_open`/`get_driver_info`, và cập nhật
-| `espe_fault()` | ESPE LGA60: từ lỗi thiết bị khác 0 (ý nghĩa bit chưa verify) |
+   `get_diagnostics()` + `mark_scan_decoded()` mỗi vòng quét.
-| `has_fault()` | Gộp mọi nguồn lỗi |
+3. Export hai entry point:
-| `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: …` |
 
-### `ModelConfig` & `LidarConfig`
+   ```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(...); }
+   ```
 
-- `ModelConfig` — thông số một model: tên, FOV, dải đo, offset góc, cửa sổ
+4. Chọn `driver_id` duy nhất, mô tả `description` rõ driver phụ trách nhóm
-  remap. Các preset `MODEL_*` khai báo sẵn trong header.
+   thiết bị nào; dùng `apply_device_config()` trong `plugins/common` để tôn
-- `LidarConfig` — một entry cấu hình runtime: `{name, ip, port, model,
+   trọng các cửa sổ góc/dải đo chung.
-  inverted, brand}`, dùng với `make_lidar()` và file JSON.
 
-## Cấu trúc source
+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++).
-| 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 |

cmake/lidarlibConfig.cmake.in

@@ -1,6 +0,0 @@
-@PACKAGE_INIT@
-
-include(CMakeFindDependencyMacro)
-find_dependency(Threads)
-
-include("${CMAKE_CURRENT_LIST_DIR}/lidarlibTargets.cmake")

cmake/xlidar_driverConfig.cmake.in

@@ -0,0 +1,8 @@
+@PACKAGE_INIT@
+
+include(CMakeFindDependencyMacro)
+find_dependency(Threads)
+
+include("${CMAKE_CURRENT_LIST_DIR}/xlidar_driverTargets.cmake")
+
+check_required_components(xlidar_driver)

config.json

@@ -1 +1 @@
-{"lidars":[{"name":"sonle","ip":"192.168.100.100","port":2368,"brand":"OLEI","model":"AUTO","inverted":false},{"name":"sonpham","ip":"192.168.100.100","port":2371,"brand":"OLEI","model":"AUTO","inverted":true},{"name":"minhtt","ip":"192.168.100.22","port":2111,"brand":"SICK","model":"SICK-TIM7xx","inverted":false}]}
+{"lidars":[{"name":"front","driver_id":"olei_lidar_driver","model":"AUTO","transport":"udp","ip":"192.168.100.100","port":2368,"inverted":false},{"name":"rear","driver_id":"olei_lidar_driver","model":"AUTO","ip":"192.168.100.100","port":2371,"inverted":true},{"name":"sick1","driver_id":"sick_tim_driver","model":"SICK-TIM7xx","transport":"tcp","ip":"192.168.100.22","port":2111,"inverted":false},{"name":"rp1","driver_id":"rplidar_c1_driver","model":"AUTO","transport":"serial","serial_port":"/dev/ttyUSB0","baudrate":460800,"inverted":false}]}

docs/diagnostics.md

@@ -1,9 +1,9 @@
-# Nghiên cứu: Dữ liệu chẩn đoán (diagnosis) của lidar OLEI & SICK
+# Nghiên cứu: Dữ liệu chẩn đoán (diagnosis) của các driver xlidar
 
 Tài liệu này tổng hợp những gì các gói dữ liệu OLEI mang theo về **tình trạng
 thiết bị** (self-diagnostics), ngoài dữ liệu điểm quét. Kết quả nghiên cứu này
-là cơ sở cho API `lidarlib::Diagnostics` / `Lidar::get_diagnostics()`
+là cơ sở cho API `xlidar::Diagnostics` / `LidarDriverInterface::get_diagnostics()`
-(header `include/lidarlib/diagnostics.hpp`).
+(header `include/lidar_diagnostics.hpp`).
 
 Điểm quan trọng: **lidar OLEI không có kênh/query chẩn đoán riêng** — driver
 chỉ nhận UDP thụ động, thiết bị không nhận lệnh hỏi trạng thái. Toàn bộ thông
@@ -74,9 +74,11 @@ an toàn kiểu safety-scanner). Các trường chẩn đoán (port từ driver
 | `[44-47]` | u32 LE | `status_flags` | Cờ trạng thái tổng — bit map chưa có tài liệu |
 
 Vì bit map chưa xác minh, driver **truyền nguyên giá trị raw** qua
-`Diagnostics` (các trường `std::optional`) thay vì decode sai. Khi có tài
+`Diagnostics::raw` (key `"olei.scan_frequency"`, `"olei.input_status"`,
-liệu V3 chính thức hoặc thiết bị GS1-5 để thử, bổ sung decode tại
+`"olei.output_status"`, `"olei.field_status"`, `"olei.status_flags"`,
-`decode_diagnostics()` trong `src/olei_lidar.cpp`.
+`"olei.distance_ratio"`) thay vì decode sai thành issue. Khi có tài liệu V3
+chính thức hoặc thiết bị GS1-5 để thử, bổ sung decode tại
+`decode_diagnostics()` trong `include/lidar_interface.hpp` (và bảng field trong `plugins/driver_olei/olei_driver.cpp`).
 
 ## 4. SICK TiM (TCP/CoLa-A — telegram `LMDscandata`)
 
@@ -93,8 +95,11 @@ Khác OLEI, TiM có **hai đường** lấy chẩn đoán:
    | `0 2` | Pollution warning — kính bắt đầu bẩn, vẫn đo được |
    | `0 4` | Pollution error — kính bẩn nặng, phải lau |
 
-   Driver ghép cặp này vào `info.sick_device_status` (`(word0<<8)|word1`) và
+   Driver ghép cặp này vào `info.sick_device_status` (`(word0<<8)|word1`);
-   decode qua `sick_error()` / `pollution_warning()` / `pollution_error()`.
+   `decode_diagnostics()` biến nó thành issue chung: bit error →
+   `{fault, device_error}`, pollution warning → `{warning, optics_dirty}`,
+   pollution error → `{fault, optics_dirty}` (raw giữ ở
+   `raw["sick.device_status"]`).
    Ngoài ra telegram còn mang input/output số (`input_status`/`output_status`)
    và tần số quét (`scan_frequency_raw`, đơn vị 1/100 Hz).
    **Chưa verify trên TiM781S thật với kính bẩn** — cần che/bôi bẩn kính để
@@ -121,12 +126,30 @@ offset/size. Block **General System State** (offset tại header `[32]`, size
 | 4 | `0x10` | Reference contour status |
 | 5 | `0x20` | Manipulation — nghi bị che/can thiệp cố ý |
 
-Driver đọc byte này vào `info.nano_general_state`, decode qua
+Driver đọc byte này vào `info.nano_general_state`; `decode_diagnostics()`
-`contamination_warning()` / `contamination_error()` / `manipulation()`.
+biến nó thành issue chung: contamination warning → `{warning, optics_dirty}`,
+contamination error → `{fault, optics_dirty}`, manipulation →
+`{fault, manipulation}` (raw giữ ở `raw["nano.general_state"]`).
 Lưu ý: block này **chỉ có mặt nếu được tick chọn** trong cấu hình data output
 của Safety Designer — thiếu block thì trường giữ `nullopt`.
 
-## 6. Chẩn đoán tầng transport (mọi driver)
+## 6. Slamtec RPLIDAR (serial — health qua SDK)
+
+RPLIDAR không nhúng chẩn đoán trong stream điểm quét; thay vào đó SDK có lệnh
+`getHealth()` trả về `status` (0 = OK, 1 = Warning, 2 = Error) kèm
+`error_code` 16-bit. Driver (`plugins/driver_rplidar`) gọi health check
+**một lần lúc `open()`** — status Error thì `open()` trả `DeviceError` và
+không dùng thiết bị; Warning vẫn chạy nhưng để lại dấu.
+
+- Snapshot health nằm ở `raw["rplidar.health_status"]` /
+  `raw["rplidar.error_code"]`; `decode_diagnostics()` sinh issue chung:
+  health Error → `{fault, device_error}`, Warning → `{warning, device_warning}`.
+- `Diagnostics::model` (`slamtec-0xNN` từ device info) và
+  `Diagnostics::firmware` (`fw M.mm hw H`) được tự nhận lúc `open()`.
+- Chẩn đoán runtime chủ yếu là gián tiếp: `recv_scan()` timeout / mất kết
+  nối serial (`Timeout` / `DeviceDisconnected`), giống mục transport dưới đây.
+
+## 7. Chẩn đoán tầng transport (mọi driver)
 
 Ngoài dữ liệu trên wire, bản thân driver cung cấp lớp chẩn đoán kết nối:
 
@@ -140,9 +163,9 @@ Ngoài dữ liệu trên wire, bản thân driver cung cấp lớp chẩn đoán
 Chiến lược giám sát khuyến nghị cho app: coi cảm biến **healthy** khi và chỉ
 khi `recv_scan()` thành công đều đặn **và** `get_diagnostics().has_fault() == false`.
 
-## 7. Kiểm tra sẵn sàng: `is_ready()` / `wait_ready()`
+## 8. Kiểm tra sẵn sàng: `is_ready()` / `wait_ready()`
 
-Chiến lược trên được gói sẵn trong hai hàm của `Lidar`:
+Chiến lược trên được gói sẵn trong hai hàm của `LidarDriverInterface`:
 
 ```cpp
 lidar->open();
@@ -166,48 +189,58 @@ if (!lidar->is_ready()) { /* mất dữ liệu hoặc thiết bị báo fault */
   vẫn ready); app muốn chặt hơn thì tự kiểm tra thêm
   `!get_diagnostics().has_warning()`.
 
-## 8. API
+## 9. API
+
+API chung cho mọi hãng — không có hàm decode riêng từng vendor. Mỗi vấn đề
+thiết bị là một `DiagnosticIssue{severity, code, detail}`; bảng map từ wire
+sang code chung:
+
+| Nguồn wire | Issue (severity, code) |
+|---|---|
+| OLEI Family A bit monitor / voltage / temp | fault `motor` / `voltage` / `temperature` |
+| OLEI Family A bit 3-7 (reserved) ≠ 0 | fault `device_error` |
+| SICK TiM device error / pollution warning / pollution error | fault `device_error` / warning `optics_dirty` / fault `optics_dirty` |
+| nanoScan3 contamination warning / error / manipulation | warning `optics_dirty` / fault `optics_dirty` / fault `manipulation` |
+| ESPE fault word ≠ 0 | fault `device_error` (detail kèm giá trị hex) |
+| RPLIDAR health Warning / Error | warning `device_warning` / fault `device_error` |
 
 ```cpp
-#include "lidarlib/lidarlib.hpp"
+#include "lidar_manager.hpp"
 
-lidarlib::ScanResult r;
+xlidar::ScanResult r;
 if (lidar->recv_scan(r, 1000)) {
-    lidarlib::Diagnostics d = lidar->get_diagnostics();
+    xlidar::Diagnostics d = lidar->get_diagnostics();
 
     if (!d.valid) {
         // chưa có scan nào được decode
-    } else if (d.has_fault()) {
-        // Family A: đọc từng bit
-        if (d.voltage_fault())     /* điện áp bất thường */;
-        if (d.temperature_fault()) /* nhiệt độ bất thường */;
-        if (d.monitor_fault())     /* motor/giám sát bất thường */;
-        printf("lidar fault: %s\n", lidarlib::to_string(d).c_str());
     }
-
+    for (const xlidar::DiagnosticIssue& issue : d.issues) {
-    // SICK: cảnh báo kính bẩn — chưa phải fault nhưng nên lên lịch lau
+        printf("[%s] %s — %s\n", xlidar::to_string(issue.severity),
-    if (d.has_warning()) {
+               issue.code.c_str(), issue.detail.c_str());
-        d.pollution_warning();      // TiM
-        d.contamination_warning();  // nanoScan3
     }
-    if (d.manipulation()) /* nanoScan3: nghi bị che/can thiệp */;
+    if (d.has_fault())   { /* dừng tin dữ liệu */ }
+    if (d.has_warning()) { /* lên lịch bảo trì */ }
 
-    // Family C raw (nullopt nếu không phải GS1-5)
+    // Giá trị thô của hãng (chỉ có khi wire mang nó), VD Family C raw:
-    if (d.status_flags) printf("status_flags=0x%08X\n", *d.status_flags);
+    if (auto it = d.raw.find("olei.status_flags"); it != d.raw.end())
+        printf("status_flags=0x%08X\n", it->second);
+
+    printf("%s\n", xlidar::to_json(d).c_str());  // JSON cho REST/telemetry
 }
 ```
 
-- `Lidar::get_diagnostics()` — snapshot từ vòng quét decode gần nhất; gọi từ
+- `LidarDriverInterface::get_diagnostics()` — snapshot từ vòng quét decode gần nhất; gọi từ
   cùng thread đang bơm `recv_scan()`/`spin_once()` (driver không khóa nội bộ).
 - `decode_diagnostics(const ExtraInfo&)` — hàm free, decode trực tiếp từ
   `ScanResult::info` nếu app muốn gắn chẩn đoán với đúng scan cụ thể.
 - `to_string(Diagnostics)` — chuỗi log 1 dòng: `no data` / `ok` /
-  `WARN: pollution` / `FAULT: voltage temperature`.
+  `WARN: optics_dirty` / `FAULT: voltage temperature | WARN: optics_dirty`.
-- `has_fault()` gộp mọi nguồn lỗi (OLEI byte lỗi, TiM device/pollution error,
+- `to_json(Diagnostics)` — chuỗi JSON đầy đủ (`valid`, `model`, `firmware`,
-  nano contamination error/manipulation); `has_warning()` gộp các mức cảnh
+  `healthy`, `issues[]`, `raw{}`) cho host nào muốn nhận string thay struct.
-  báo kính bẩn.
+- `has_fault()` / `has_warning()` quét `issues` theo severity;
+  `healthy()` = `valid && !has_fault()`.
 
-## 9. Hướng mở rộng
+## 10. Hướng mở rộng
 
 - **SICK SOPAS query chủ động**: `sRN SCdevicestate` (0=busy, 1=ready,
   2=error), `sRN LCMstate` (mức nhiễm bẩn chi tiết) — cần cơ chế

examples/CMakeLists.txt

@@ -0,0 +1,6 @@
+# Demo binaries. Run from the build dir so the default plugins path
+# ("plugins") resolves to build/plugins.
+foreach(demo list_drivers example lidar_app)
+    add_executable(${demo} ${demo}.cpp)
+    target_link_libraries(${demo} PRIVATE lidar_manager)
+endforeach()

examples/espe_example.cpp

@@ -1,39 +0,0 @@
-// ESPE LGA60 driver example (TCP, port 8080 theo cấu hình mặc định của hãng).
-#include "lidarlib/espe_lidar.hpp"
-#include <cstdio>
-
-int main() {
-    lidarlib::EspeDriver drv(lidarlib::MODEL_ESPE_LGA60, "192.168.1.88", 8080);
-
-    lidarlib::ErrorCode err = drv.open();
-    if (err != lidarlib::ErrorCode::Ok) {
-        fprintf(stderr, "Không kết nối được tới lidar ESPE: %s\n",
-                lidarlib::to_string(err));
-        return 1;
-    }
-
-    for (int i = 0; i < 10; ++i) {
-        lidarlib::ScanResult result;
-        if (!drv.recv_scan(result, 1000)) {
-            fprintf(stderr, "Timeout hoặc lỗi nhận dữ liệu: %s\n",
-                    lidarlib::to_string(drv.last_error()));
-            continue;
-        }
-        const lidarlib::LaserScan& scan = result.scan;
-        printf("Scan #%d: %zu điểm, model=%s, diag=%s\n",
-               i, scan.ranges.size(), result.info.detected_model.c_str(),
-               lidarlib::to_string(drv.get_diagnostics()).c_str());
-
-        for (size_t j = 0; j < 20 && j < scan.ranges.size(); ++j) {
-            float angle_deg = (scan.angle_min + j * scan.angle_increment) * 180.f / 3.14159265f;
-            printf("  [%zu] angle=%.2f° dist=%.3fm intensity=%.0f\n",
-                   j, angle_deg, scan.ranges[j], scan.intensities[j]);
-        }
-    }
-
-    drv.close();
-    return 0;
-}
-
-// Build:
-//   g++ -std=c++17 -O2 -Iinclude -o espe_example examples/espe_example.cpp src/espe_lidar.cpp src/olei_lidar.cpp

examples/example.cpp

@@ -1,57 +1,71 @@
-// OLEI driver example.
+// Open one lidar through the manager and read 10 scans.
-#include "lidarlib/lidar.hpp"
+//   ./example <driver_id> [plugins_dir]
+//   ./example olei_lidar_driver
+//   ./example rplidar_c1_driver
+#include "lidar_manager.hpp"
 #include <cstdio>
 
-int main() {
+int main(int argc, char** argv) {
-    lidarlib::Driver drv(lidarlib::MODEL_VB);
+    if (argc < 2) {
+        fprintf(stderr, "usage: %s <driver_id> [plugins_dir]\n", argv[0]);
+        return 1;
+    }
+    const std::string plugins_dir = (argc > 2) ? argv[2] : "plugins";
 
-    lidarlib::ErrorCode err = drv.open();
+    xlidar::LidarManager manager(plugins_dir);
-    if (err != lidarlib::ErrorCode::Ok) {
+    manager.load_all_plugins();
-        fprintf(stderr, "Không mở được socket: %s\n", lidarlib::to_string(err));
+
+    xlidar::DeviceConfig cfg;
+    cfg.name      = "demo";
+    cfg.driver_id = argv[1];
+    // Network drivers use cfg.ip / cfg.port (0 = driver default port);
+    // the rplidar driver uses cfg.serial_port / cfg.baudrate instead.
+
+    auto lidar = manager.create_lidar_device(cfg);
+    if (!lidar) return 1;
+
+    xlidar::DriverInfo info = lidar->get_driver_info();
+    printf("driver: %s (%s %s)\n", info.driver_id.c_str(), info.vendor.c_str(),
+           info.model.c_str());
+
+    xlidar::ErrorCode err = lidar->open();
+    if (err != xlidar::ErrorCode::Ok) {
+        fprintf(stderr, "open failed: %s\n", xlidar::to_string(err));
         return 1;
     }
 
-    // Chờ cảm biến sẵn sàng: đã nhận được ít nhất một scan hoàn chỉnh
+    // Wait until the sensor is usable: at least one complete scan decoded
-    // và thiết bị không báo lỗi (motor/điện áp/nhiệt độ...)
+    // and the device reports no fault (motor/voltage/pollution/...).
-    if (!drv.wait_ready(5000)) {
+    if (!lidar->wait_ready(5000)) {
-        fprintf(stderr, "Cảm biến chưa sẵn sàng: %s (diag: %s)\n",
+        fprintf(stderr, "sensor not ready: %s (diag: %s)\n",
-                lidarlib::to_string(drv.last_error()),
+                xlidar::to_string(lidar->last_error()),
-                lidarlib::to_string(drv.get_diagnostics()).c_str());
+                xlidar::to_string(lidar->get_diagnostics()).c_str());
-        drv.close();
+        lidar->close();
         return 1;
     }
 
     for (int i = 0; i < 10; ++i) {
-        lidarlib::ScanResult result;
+        xlidar::ScanResult result;
-        if (!drv.recv_scan(result, 2000)) {
+        if (!lidar->recv_scan(result, 2000)) {
-            fprintf(stderr, "Timeout hoặc lỗi nhận packet\n");
+            fprintf(stderr, "recv_scan failed: %s\n", xlidar::to_string(lidar->last_error()));
             break;
         }
-        const lidarlib::LaserScan& scan = result.scan;
+        const xlidar::LaserScan& scan = result.scan;
-        const lidarlib::ExtraInfo& info = result.info;
+        printf("scan #%d: %zu points, ts=%u ms, model=%s\n",
-        printf("Scan #%d: %zu điểm, ts=%u ms, err=0x%02X, model=%s\n",
+               i, scan.ranges.size(), scan.timestamp_ms,
-               i, scan.ranges.size(), scan.timestamp_ms, info.error_status,
+               result.info.detected_model.c_str());
-               info.detected_model.c_str());
 
-        // Chẩn đoán thiết bị từ scan mới nhất
+        // Device self-diagnostics from the newest scan.
-        lidarlib::Diagnostics diag = drv.get_diagnostics();
+        xlidar::Diagnostics diag = lidar->get_diagnostics();
-        printf("  diag: %s\n", lidarlib::to_string(diag).c_str());
+        printf("  diag: %s\n", xlidar::to_string(diag).c_str());
-        if (diag.has_fault()) {
-            if (diag.monitor_fault())     printf("    !! lỗi monitor/motor\n");
-            if (diag.voltage_fault())     printf("    !! điện áp bất thường\n");
-            if (diag.temperature_fault()) printf("    !! nhiệt độ bất thường\n");
-        }
 
-        for (size_t j = 0; j < 20 && j < scan.ranges.size(); ++j) {
+        for (size_t j = 0; j < 5 && j < scan.ranges.size(); ++j) {
             float angle_deg = (scan.angle_min + j * scan.angle_increment) * 180.f / 3.14159265f;
             printf("  [%zu] angle=%.2f° dist=%.3fm intensity=%.0f\n",
                    j, angle_deg, scan.ranges[j], scan.intensities[j]);
         }
     }
 
-    drv.close();
+    lidar->close();
     return 0;
 }
-
-// Build:
-//   g++ -std=c++17 -O2 -Iinclude -o example examples/example.cpp src/olei_lidar.cpp

examples/lidar_app.cpp

@@ -1,6 +1,6 @@
 // Headless skeleton app: loads config.json, one reader thread per lidar.
-//   ./lidar_app [config.json]
+//   ./lidar_app [config.json] [plugins_dir]
-#include "lidarlib/lidarlib.hpp"
+#include "lidar_manager.hpp"
 #include <atomic>
 #include <csignal>
 #include <cstdio>
@@ -13,33 +13,31 @@ namespace {
 std::atomic<bool> g_running{true};
 void on_signal(int) { g_running = false; }
 
-void run_lidar(lidarlib::LidarConfig cfg) {
+void run_lidar(xlidar::LidarManager& manager, xlidar::DeviceConfig cfg) {
-    std::unique_ptr<lidarlib::Lidar> lidar = lidarlib::make_lidar(cfg);
+    std::unique_ptr<xlidar::LidarDriverInterface> lidar = manager.create_lidar_device(cfg);
-    lidarlib::ErrorCode err = lidar->open();
+    if (!lidar) return;
-    if (err != lidarlib::ErrorCode::Ok) {
+
-        fprintf(stderr, "[%s] khong mo duoc %s %s:%u (%s)\n",
+    xlidar::ErrorCode err = lidar->open();
-                cfg.name.c_str(), cfg.brand.c_str(), cfg.ip.c_str(), cfg.port,
+    if (err != xlidar::ErrorCode::Ok) {
-                lidarlib::to_string(err));
+        fprintf(stderr, "[%s] open failed %s (%s)\n",
+                cfg.name.c_str(), cfg.driver_id.c_str(), xlidar::to_string(err));
         return;
     }
-    printf("[%s] da mo %s %s:%u (model=%s, inverted=%d)\n",
+    printf("[%s] opened %s (model=%s, inverted=%d)\n",
-           cfg.name.c_str(), cfg.brand.c_str(), cfg.ip.c_str(), cfg.port,
+           cfg.name.c_str(), cfg.driver_id.c_str(), cfg.model.c_str(), cfg.inverted);
-           cfg.model.c_str(), cfg.inverted);
 
     while (g_running) {
-        lidarlib::ScanResult result;
+        xlidar::ScanResult result;
         if (!lidar->recv_scan(result, 1000)) continue;
 
-        const lidarlib::LaserScan& scan = result.scan;
+        printf("[%s] %zu points | ts=%u ms | model=%s | diag=%s\n",
-        const lidarlib::ExtraInfo& info = result.info;
+               cfg.name.c_str(), result.scan.ranges.size(), result.scan.timestamp_ms,
-
+               result.info.detected_model.c_str(),
-        printf("[%s] %zu diem | ts=%u ms | model=%s | err=0x%02X\n",
+               xlidar::to_string(lidar->get_diagnostics()).c_str());
-               cfg.name.c_str(), scan.ranges.size(), scan.timestamp_ms,
-               info.detected_model.c_str(), info.error_status);
     }
 
     lidar->close();
-    printf("[%s] da dong\n", cfg.name.c_str());
+    printf("[%s] closed\n", cfg.name.c_str());
 }
 
 } // namespace
@@ -48,23 +46,30 @@ int main(int argc, char** argv) {
     setvbuf(stdout, nullptr, _IOLBF, 0);
 
     const std::string config_path = (argc > 1) ? argv[1] : "config.json";
+    const std::string plugins_dir = (argc > 2) ? argv[2] : "plugins";
 
-    lidarlib::Config cfg = lidarlib::load_config(config_path);
+    xlidar::LidarManager manager(plugins_dir);
-    lidarlib::save_config(config_path, cfg);  // ensure the file exists
+    printf("%zu driver(s) available\n", manager.load_all_plugins());
+
+    xlidar::ManagerConfig cfg = xlidar::load_config(config_path);
+    xlidar::save_config(config_path, cfg);  // ensure the file exists (and migrate legacy keys)
 
     if (cfg.lidars.empty()) {
-        fprintf(stderr, "Khong co lidar nao trong %s\n", config_path.c_str());
+        fprintf(stderr, "no lidars in %s\n", config_path.c_str());
         return 1;
     }
 
     std::signal(SIGINT, on_signal);
     std::signal(SIGTERM, on_signal);
 
+    // One thread per device — instances are fully independent. The manager
+    // outlives every thread (join below), as the plugin contract requires.
     std::vector<std::thread> threads;
     threads.reserve(cfg.lidars.size());
-    for (const auto& lc : cfg.lidars) threads.emplace_back(run_lidar, lc);
+    for (const auto& lc : cfg.lidars)
+        threads.emplace_back(run_lidar, std::ref(manager), lc);
 
-    printf("Dang chay %zu lidar tu %s. Ctrl-C de dung.\n",
+    printf("running %zu lidar(s) from %s. Ctrl-C to stop.\n",
            cfg.lidars.size(), config_path.c_str());
     for (auto& t : threads) t.join();
     return 0;

examples/list_drivers.cpp

@@ -0,0 +1,28 @@
+// Discover plugins and print every registered driver.
+//   ./list_drivers [plugins_dir]        (default: ./plugins)
+#include "lidar_manager.hpp"
+#include <cstdio>
+
+int main(int argc, char** argv) {
+    const std::string plugins_dir = (argc > 1) ? argv[1] : "plugins";
+
+    xlidar::LidarManager manager(plugins_dir);
+    size_t count = manager.load_all_plugins();
+    printf("%zu driver(s) in %s\n\n", count, plugins_dir.c_str());
+
+    for (const auto& [driver_id, plugin] : manager.available_drivers()) {
+        const xlidar::DriverInfo& info = plugin.info;
+        printf("%s\n", driver_id.c_str());
+        printf("  vendor:      %s\n", info.vendor.c_str());
+        printf("  model:       %s\n", info.model.c_str());
+        printf("  transport:   %s%s\n", xlidar::to_string(info.transport),
+               info.transport_selectable ? " (selectable)" : "");
+        printf("  models:      ");
+        for (size_t i = 0; i < info.supported_models.size(); ++i)
+            printf("%s%s", i ? ", " : "", info.supported_models[i].c_str());
+        printf("\n");
+        printf("  description: %s\n", info.description.c_str());
+        printf("  file:        %s\n\n", plugin.file_path.c_str());
+    }
+    return 0;
+}

examples/nanoscan_example.cpp

@@ -1,39 +0,0 @@
-// SICK nanoScan3 example. The sensor's UDP output target must be configured
-// in SICK Safety Designer; this driver only binds a local UDP port.
-#include "lidarlib/sick_lidar.hpp"
-#include <cstdio>
-
-int main() {
-    lidarlib::NanoScanDriver drv(lidarlib::MODEL_SICK_NANOSCAN3, "0.0.0.0", 6060);
-
-    lidarlib::ErrorCode err = drv.open();
-    if (err != lidarlib::ErrorCode::Ok) {
-        fprintf(stderr, "Không mở được UDP socket cho nanoScan3: %s\n",
-                lidarlib::to_string(err));
-        return 1;
-    }
-
-    for (int i = 0; i < 10; ++i) {
-        lidarlib::ScanResult result;
-        if (!drv.recv_scan(result, 2000)) {
-            fprintf(stderr, "Timeout hoặc lỗi nhận UDP datagram\n");
-            break;
-        }
-        const lidarlib::LaserScan& scan = result.scan;
-        const lidarlib::ExtraInfo& info = result.info;
-        printf("Scan #%d: %zu điểm, ts=%u, model=%s\n",
-               i, scan.ranges.size(), scan.timestamp_ms, info.detected_model.c_str());
-
-        for (size_t j = 0; j < 20 && j < scan.ranges.size(); ++j) {
-            float angle_deg = (scan.angle_min + j * scan.angle_increment) * 180.f / 3.14159265f;
-            printf("  [%zu] angle=%.2f° dist=%.3fm intensity=%.0f\n",
-                   j, angle_deg, scan.ranges[j], scan.intensities[j]);
-        }
-    }
-
-    drv.close();
-    return 0;
-}
-
-// Build:
-//   g++ -std=c++17 -O2 -Iinclude -o nanoscan_example examples/nanoscan_example.cpp src/sick_lidar.cpp

examples/sick_example.cpp

@@ -1,39 +0,0 @@
-// SICK TiM driver example (SOPAS/CoLa-A, TCP).
-#include "lidarlib/sick_lidar.hpp"
-#include <cstdio>
-
-int main() {
-    lidarlib::SickDriver drv(lidarlib::MODEL_SICK_TIM571, "192.168.0.1", 2111);
-
-    lidarlib::ErrorCode err = drv.open();
-    if (err != lidarlib::ErrorCode::Ok) {
-        fprintf(stderr, "Không kết nối được TCP tới lidar SICK: %s\n",
-                lidarlib::to_string(err));
-        return 1;
-    }
-
-    for (int i = 0; i < 10; ++i) {
-        lidarlib::ScanResult result;
-        if (!drv.recv_scan(result, 2000)) {
-            fprintf(stderr, "Timeout hoặc lỗi nhận telegram\n");
-            break;
-        }
-        const lidarlib::LaserScan& scan = result.scan;
-        const lidarlib::ExtraInfo& info = result.info;
-        printf("Scan #%d: %zu điểm, ts=%u ms, err=0x%02X, model=%s\n",
-               i, scan.ranges.size(), scan.timestamp_ms, info.error_status,
-               info.detected_model.c_str());
-
-        for (size_t j = 0; j < 20 && j < scan.ranges.size(); ++j) {
-            float angle_deg = (scan.angle_min + j * scan.angle_increment) * 180.f / 3.14159265f;
-            printf("  [%zu] angle=%.2f° dist=%.3fm intensity=%.0f\n",
-                   j, angle_deg, scan.ranges[j], scan.intensities[j]);
-        }
-    }
-
-    drv.close();
-    return 0;
-}
-
-// Build:
-//   g++ -std=c++17 -O2 -Iinclude -o sick_example examples/sick_example.cpp src/sick_lidar.cpp

examples/test_dual.cpp

@@ -1,47 +0,0 @@
-// Test 2 Olei lidars (front + rear) concurrently.
-#include "lidarlib/lidar.hpp"
-#include <cstdio>
-#include <thread>
-
-static void run_lidar(const char* tag, const lidarlib::ModelConfig& cfg,
-                       const std::string& local_ip, uint16_t port, bool inverted, int n_scans) {
-    lidarlib::Driver drv(cfg, local_ip, port, inverted);
-    lidarlib::ErrorCode err = drv.open();
-    if (err != lidarlib::ErrorCode::Ok) {
-        fprintf(stderr, "[%s] Khong mo duoc socket tren %s:%u (%s)\n",
-                tag, local_ip.c_str(), port, lidarlib::to_string(err));
-        return;
-    }
-    printf("[%s] Da bind %s:%u, dang doi scan...\n", tag, local_ip.c_str(), port);
-
-    for (int i = 0; i < n_scans; ++i) {
-        lidarlib::ScanResult result;
-        if (!drv.recv_scan(result, 2000)) {
-            fprintf(stderr, "[%s] Timeout/loi nhan packet (scan #%d)\n", tag, i);
-            continue;
-        }
-        const lidarlib::LaserScan& scan = result.scan;
-        const lidarlib::ExtraInfo& info = result.info;
-        printf("[%s] Scan #%d: %zu diem, ts=%u ms, err=0x%02X, model=%s\n",
-               tag, i, scan.ranges.size(), scan.timestamp_ms, info.error_status,
-               info.detected_model.c_str());
-        for (size_t j = 0; j < 20 && j < scan.ranges.size(); ++j) {
-            float angle_deg = (scan.angle_min + j * scan.angle_increment) * 180.f / 3.14159265f;
-            printf("       [%zu] angle=%.2f dist=%.3fm intensity=%.0f\n",
-                   j, angle_deg, scan.ranges[j], scan.intensities[j]);
-        }
-    }
-    drv.close();
-}
-
-int main() {
-    // Real headers (UDP sniff): front = "OLELR-1BS2", rear = "OLELR-1BS5".
-    std::thread t_front(run_lidar, "front/scan_1", lidarlib::MODEL_AUTO,
-                         "192.168.100.100", 2368, false, 5);
-    std::thread t_rear(run_lidar, "rear/scan_2", lidarlib::MODEL_AUTO,
-                        "192.168.100.100", 2369, true, 5);
-
-    t_front.join();
-    t_rear.join();
-    return 0;
-}

include/lidar_diagnostics.hpp

@@ -0,0 +1,181 @@
+#pragma once
+// xlidar-driver — device self-diagnostics decoded from the data stream.
+//
+// The public surface is vendor-neutral: every driver reports through the same
+// Diagnostics struct — a list of DiagnosticIssue with stable cross-vendor
+// codes, plus a raw field map for vendor-specific passthrough. Hosts never
+// need per-vendor accessors; serialize with to_json() when a string API is
+// more convenient.
+#include <cstdint>
+#include <cstdio>
+#include <map>
+#include <string>
+#include <vector>
+
+namespace xlidar {
+
+struct ExtraInfo;  // lidar_interface.hpp
+
+// ── Raw wire constants (document the values in Diagnostics::raw) ────────────
+
+// OLEI Family A (0xFAF0) error_status bits, header byte [5]. Bits 3-7 are
+// reserved on the wire; a nonzero reserved bit is still reported as a fault.
+inline constexpr uint8_t kFaultMonitor     = 1u << 0;  // monitor / motor abnormal
+inline constexpr uint8_t kFaultVoltage     = 1u << 1;  // supply voltage out of range
+inline constexpr uint8_t kFaultTemperature = 1u << 2;  // internal temperature abnormal
+
+// SICK TiM LMDscandata device status (low word; Telegram Listing).
+inline constexpr uint16_t kSickStatusError            = 1u << 0;
+inline constexpr uint16_t kSickStatusPollutionWarning = 1u << 1;
+inline constexpr uint16_t kSickStatusPollutionError   = 1u << 2;
+
+// SICK nanoScan3 General System State byte 0 (layout from sick_safetyscanners;
+// NOT verified on real hardware).
+inline constexpr uint8_t kNanoStateRunMode              = 1u << 0;
+inline constexpr uint8_t kNanoStateStandby              = 1u << 1;
+inline constexpr uint8_t kNanoStateContaminationWarning = 1u << 2;
+inline constexpr uint8_t kNanoStateContaminationError   = 1u << 3;
+inline constexpr uint8_t kNanoStateReferenceContour     = 1u << 4;
+inline constexpr uint8_t kNanoStateManipulation         = 1u << 5;
+
+// RPLIDAR SDK health status values (sl_lidar_response_device_health_t.status).
+inline constexpr uint8_t kRplidarHealthOk      = 0;
+inline constexpr uint8_t kRplidarHealthWarning = 1;
+inline constexpr uint8_t kRplidarHealthError   = 2;
+
+// ── Common diagnostics structure ────────────────────────────────────────────
+
+// Fault = device says something is wrong now, stop trusting the data;
+// Warning = degraded but still measuring (dirty optics, weak motor) —
+// schedule cleaning/service.
+enum class DiagSeverity { Warning, Fault };
+
+inline const char* to_string(DiagSeverity s) {
+    return s == DiagSeverity::Fault ? "fault" : "warning";
+}
+
+// One decoded device issue. `code` is a stable, machine-readable identifier
+// shared across vendors:
+//   "motor"          — motor/monitor subsystem abnormal
+//   "voltage"        — supply voltage out of range
+//   "temperature"    — internal temperature abnormal
+//   "optics_dirty"   — pollution/contamination of the optics window
+//                      (warning: clean soon; fault: data no longer reliable)
+//   "manipulation"   — safety scanner suspects tampering/covering
+//   "device_error"   — device-level fault the vendor doesn't break down
+//   "device_warning" — device-level warning the vendor doesn't break down
+// `detail` is human-readable, names the vendor, and may carry the raw value.
+struct DiagnosticIssue {
+    DiagSeverity severity = DiagSeverity::Fault;
+    std::string  code;
+    std::string  detail;
+};
+
+// Device self-diagnostics decoded from the data stream. valid stays false
+// until the driver has decoded one full scan; issues is empty while the
+// device reports healthy. Vendor-specific raw fields appear in `raw` keyed
+// by stable names ("olei.error_status", "sick.device_status",
+// "nano.general_state", "espe.error_status", "rplidar.health_status",
+// "rplidar.error_code", ...) — only fields present on the wire are set.
+struct Diagnostics {
+    bool        valid = false;
+    std::string model = "AUTO";
+    std::string firmware;                 // e.g. "fw 1.32 hw 18"; empty if unknown
+    uint32_t    device_timestamp_ms = 0;  // device clock; 0 if not on the wire
+
+    std::vector<DiagnosticIssue> issues;
+    std::map<std::string, uint32_t> raw;
+
+    bool has_fault() const {
+        for (const auto& i : issues)
+            if (i.severity == DiagSeverity::Fault) return true;
+        return false;
+    }
+    bool has_warning() const {
+        for (const auto& i : issues)
+            if (i.severity == DiagSeverity::Warning) return true;
+        return false;
+    }
+    bool healthy() const { return valid && !has_fault(); }
+};
+
+// Decode the diagnostic fields of one scan; sets valid = true.
+// Defined inline in lidar_interface.hpp (needs the ExtraInfo definition, and
+// every plugin .so must carry its own copy).
+Diagnostics decode_diagnostics(const ExtraInfo& info);
+
+// One-line log summary: "no data" / "ok" / "WARN: optics_dirty" /
+// "FAULT: voltage temperature | WARN: optics_dirty".
+inline std::string to_string(const Diagnostics& d) {
+    if (!d.valid)        return "no data";
+    if (d.issues.empty()) return "ok";
+    std::string faults, warnings;
+    for (const auto& i : d.issues)
+        (i.severity == DiagSeverity::Fault ? faults : warnings) += " " + i.code;
+    std::string s;
+    if (!faults.empty())   s += "FAULT:" + faults;
+    if (!warnings.empty()) s += (s.empty() ? "WARN:" : " | WARN:") + warnings;
+    return s;
+}
+
+namespace detail {
+// Minimal JSON string escaping (quotes, backslash, control characters) —
+// model/firmware come off the wire and may hold arbitrary bytes.
+inline std::string json_escape(const std::string& in) {
+    std::string out;
+    out.reserve(in.size());
+    for (unsigned char c : in) {
+        switch (c) {
+            case '"':  out += "\\\""; break;
+            case '\\': out += "\\\\"; break;
+            case '\n': out += "\\n";  break;
+            case '\r': out += "\\r";  break;
+            case '\t': out += "\\t";  break;
+            default:
+                if (c < 0x20) {
+                    char buf[8];
+                    std::snprintf(buf, sizeof(buf), "\\u%04X", c);
+                    out += buf;
+                } else {
+                    out += static_cast<char>(c);
+                }
+        }
+    }
+    return out;
+}
+} // namespace detail
+
+// Full JSON snapshot, e.g. for a REST/telemetry payload:
+//   {"valid":true,"model":"C1","firmware":"fw 1.32 hw 18",
+//    "device_timestamp_ms":0,"healthy":false,
+//    "issues":[{"severity":"fault","code":"voltage","detail":"..."}],
+//    "raw":{"olei.error_status":2}}
+inline std::string to_json(const Diagnostics& d) {
+    std::string s = "{\"valid\":";
+    s += d.valid ? "true" : "false";
+    s += ",\"model\":\"" + detail::json_escape(d.model) + "\"";
+    s += ",\"firmware\":\"" + detail::json_escape(d.firmware) + "\"";
+    s += ",\"device_timestamp_ms\":" + std::to_string(d.device_timestamp_ms);
+    s += ",\"healthy\":";
+    s += d.healthy() ? "true" : "false";
+    s += ",\"issues\":[";
+    for (size_t i = 0; i < d.issues.size(); ++i) {
+        const DiagnosticIssue& issue = d.issues[i];
+        if (i) s += ',';
+        s += "{\"severity\":\"";
+        s += to_string(issue.severity);
+        s += "\",\"code\":\"" + detail::json_escape(issue.code) + "\"";
+        s += ",\"detail\":\"" + detail::json_escape(issue.detail) + "\"}";
+    }
+    s += "],\"raw\":{";
+    bool first = true;
+    for (const auto& [key, value] : d.raw) {
+        if (!first) s += ',';
+        first = false;
+        s += "\"" + detail::json_escape(key) + "\":" + std::to_string(value);
+    }
+    s += "}}";
+    return s;
+}
+
+} // namespace xlidar

include/lidar_interface.hpp

@@ -0,0 +1,464 @@
+#pragma once
+// xlidar-driver — public driver interface.
+//
+// Every lidar driver plugin implements xlidar::LidarDriverInterface and
+// exports two extern "C" entry points (see "Plugin ABI" at the bottom):
+//
+//     get_driver_info(xlidar::DriverInfo*)            — static metadata
+//     create_driver_instance(const xlidar::DeviceConfig*)
+//                                                     — new driver instance
+//
+// Host applications never include plugin headers; they talk to plugins
+// exclusively through this header + lidar_manager.hpp.
+
+#include "lidar_diagnostics.hpp"
+
+#include <chrono>
+#include <cstdint>
+#include <functional>
+#include <map>
+#include <memory>
+#include <optional>
+#include <string>
+#include <vector>
+
+namespace xlidar {
+
+// ── Error codes ─────────────────────────────────────────────────────────────
+
+// Result of open() and the sticky status behind last_error(). Ok == 0 so
+// `if (err != ErrorCode::Ok)` reads naturally at call sites.
+enum class ErrorCode {
+    Ok = 0,
+
+    // Lifecycle misuse — the call was refused, the instance state is unchanged.
+    AlreadyOpen,          // open() called while already open
+    NotOpen,              // recv_scan()/spin_once() called before open()
+
+    // open() failures
+    SocketError,          // socket() creation failed
+    InvalidAddress,       // ip string is not a valid IPv4 address
+    PortInUse,            // bind: local port already taken (EADDRINUSE/EACCES)
+    BindFailed,           // bind failed for another reason
+    ConnectionRefused,    // TCP connect refused (device up, port closed)
+    ConnectionFailed,     // TCP connect failed (unreachable, no route, ...)
+    HandshakeFailed,      // connected, but the start-stream command failed
+    SerialError,          // serial port open/configure failed (serial drivers)
+    DeviceError,          // device rejected a command / reported a hard fault
+
+    // Runtime failures
+    Timeout,              // no (complete) scan within timeout_ms
+    DeviceDisconnected,   // peer closed the connection / socket or serial error
+
+    // Configuration errors
+    InvalidConfig,        // DeviceConfig is not usable by this driver
+};
+
+inline const char* to_string(ErrorCode e) {
+    switch (e) {
+        case ErrorCode::Ok:                 return "Ok";
+        case ErrorCode::AlreadyOpen:        return "AlreadyOpen";
+        case ErrorCode::NotOpen:            return "NotOpen";
+        case ErrorCode::SocketError:        return "SocketError";
+        case ErrorCode::InvalidAddress:     return "InvalidAddress";
+        case ErrorCode::PortInUse:          return "PortInUse";
+        case ErrorCode::BindFailed:         return "BindFailed";
+        case ErrorCode::ConnectionRefused:  return "ConnectionRefused";
+        case ErrorCode::ConnectionFailed:   return "ConnectionFailed";
+        case ErrorCode::HandshakeFailed:    return "HandshakeFailed";
+        case ErrorCode::SerialError:        return "SerialError";
+        case ErrorCode::DeviceError:        return "DeviceError";
+        case ErrorCode::Timeout:            return "Timeout";
+        case ErrorCode::DeviceDisconnected: return "DeviceDisconnected";
+        case ErrorCode::InvalidConfig:      return "InvalidConfig";
+    }
+    return "Unknown";
+}
+
+// ── Scan data ───────────────────────────────────────────────────────────────
+
+// ROS sensor_msgs/LaserScan-shaped output (radians, meters, seconds).
+// ranges[i] is at angle_min + i*angle_increment, in sweep order.
+struct LaserScan {
+    uint32_t timestamp_ms = 0;   // device clock (ms); 0 if not on the wire
+    float angle_min       = 0.f; // rad
+    float angle_max       = 0.f; // rad
+    float angle_increment = 0.f; // rad
+    float time_increment  = 0.f; // sec — not exposed by most devices, 0 then
+    float scan_time       = 0.f; // sec — not exposed by most devices, 0 then
+    float range_min       = 0.f; // m — from ModelConfig, not measured
+    float range_max       = 0.f; // m — from ModelConfig, not measured
+    std::vector<float> ranges;       // m
+    std::vector<float> intensities;  // 0-255 as float
+};
+
+// Diagnostic/header fields; fields the device family doesn't carry stay
+// std::nullopt (see docs/diagnostics.md for the per-family wire layout).
+struct ExtraInfo {
+    std::string detected_model = "AUTO";
+    uint8_t     error_status   = 0;    // OLEI Family A: BIT0=Monitor, BIT1=Voltage, BIT2=Temp
+    uint8_t     distance_scale_mm = 0; // 0 = not reported
+
+    // OLEI Family A only
+    std::optional<uint16_t> rotation_raw;
+
+    // OLEI Family C / V3 (GS1-5) only — raw passthroughs, unverified
+    std::optional<uint8_t>  distance_ratio_raw;
+    std::optional<uint16_t> scan_frequency_raw;
+    std::optional<uint16_t> input_status;
+    std::optional<uint16_t> output_status;
+    std::optional<uint32_t> field_status;
+    std::optional<uint32_t> status_flags;
+
+    // SICK TiM only — LMDscandata status pair (word0<<8)|word1:
+    // 0 ok, 1 error, 2 pollution warning, 4 pollution error.
+    std::optional<uint16_t> sick_device_status;
+
+    // SICK nanoScan3 only — General System State byte 0 (see kNanoState* bits).
+    std::optional<uint8_t> nano_general_state;
+
+    // ESPE LGA60 only — fault word from the newest "WSimu" area frame; the
+    // device only sends those when area data is polled, so usually nullopt.
+    std::optional<uint16_t> espe_error_status;
+
+    // RPLIDAR only — SDK health status (0 ok, 1 warning, 2 error) and the
+    // device error code that goes with it.
+    std::optional<uint8_t>  rplidar_health_status;
+    std::optional<uint16_t> rplidar_error_code;
+};
+
+struct ScanResult {
+    LaserScan scan;
+    ExtraInfo info;
+};
+
+// Decode the diagnostic fields of one scan into the vendor-neutral
+// Diagnostics structure; sets valid = true. Vendor bit layouts are decoded
+// here (constants in lidar_diagnostics.hpp) so hosts only ever see common
+// issue codes; the raw values ride along in Diagnostics::raw.
+inline Diagnostics decode_diagnostics(const ExtraInfo& info) {
+    Diagnostics d;
+    d.valid = true;
+    d.model = info.detected_model;
+
+    const auto add = [&d](DiagSeverity severity, const char* code, std::string detail) {
+        d.issues.push_back({severity, code, std::move(detail)});
+    };
+    char buf[48];
+
+    // OLEI Family A error byte (Family B/C don't carry it — stays 0).
+    if (info.error_status != 0) {
+        d.raw["olei.error_status"] = info.error_status;
+        if (info.error_status & kFaultMonitor)
+            add(DiagSeverity::Fault, "motor", "OLEI monitor/motor abnormal");
+        if (info.error_status & kFaultVoltage)
+            add(DiagSeverity::Fault, "voltage", "OLEI supply voltage out of range");
+        if (info.error_status & kFaultTemperature)
+            add(DiagSeverity::Fault, "temperature", "OLEI internal temperature abnormal");
+        if (const uint8_t rest = info.error_status
+                & static_cast<uint8_t>(~(kFaultMonitor | kFaultVoltage | kFaultTemperature))) {
+            std::snprintf(buf, sizeof(buf), "OLEI reserved error bits 0x%02X", rest);
+            add(DiagSeverity::Fault, "device_error", buf);
+        }
+    }
+
+    // OLEI raw passthroughs (meanings unverified — no issue decoding).
+    if (info.rotation_raw)       d.raw["olei.rotation"]       = *info.rotation_raw;
+    if (info.distance_ratio_raw) d.raw["olei.distance_ratio"] = *info.distance_ratio_raw;
+    if (info.scan_frequency_raw) d.raw["olei.scan_frequency"] = *info.scan_frequency_raw;
+    if (info.input_status)       d.raw["olei.input_status"]   = *info.input_status;
+    if (info.output_status)      d.raw["olei.output_status"]  = *info.output_status;
+    if (info.field_status)       d.raw["olei.field_status"]   = *info.field_status;
+    if (info.status_flags)       d.raw["olei.status_flags"]   = *info.status_flags;
+
+    // SICK TiM device status pair.
+    if (info.sick_device_status) {
+        d.raw["sick.device_status"] = *info.sick_device_status;
+        if (*info.sick_device_status & kSickStatusError)
+            add(DiagSeverity::Fault, "device_error", "SICK TiM device error");
+        if (*info.sick_device_status & kSickStatusPollutionWarning)
+            add(DiagSeverity::Warning, "optics_dirty", "SICK TiM pollution warning");
+        if (*info.sick_device_status & kSickStatusPollutionError)
+            add(DiagSeverity::Fault, "optics_dirty", "SICK TiM pollution error");
+    }
+
+    // SICK nanoScan3 general system state.
+    if (info.nano_general_state) {
+        d.raw["nano.general_state"] = *info.nano_general_state;
+        if (*info.nano_general_state & kNanoStateContaminationWarning)
+            add(DiagSeverity::Warning, "optics_dirty", "nanoScan3 contamination warning");
+        if (*info.nano_general_state & kNanoStateContaminationError)
+            add(DiagSeverity::Fault, "optics_dirty", "nanoScan3 contamination error");
+        if (*info.nano_general_state & kNanoStateManipulation)
+            add(DiagSeverity::Fault, "manipulation", "nanoScan3 manipulation suspected");
+    }
+
+    // ESPE fault word (bit meanings unverified).
+    if (info.espe_error_status) {
+        d.raw["espe.error_status"] = *info.espe_error_status;
+        if (*info.espe_error_status != 0) {
+            std::snprintf(buf, sizeof(buf), "ESPE fault word 0x%04X", *info.espe_error_status);
+            add(DiagSeverity::Fault, "device_error", buf);
+        }
+    }
+
+    // RPLIDAR SDK health.
+    if (info.rplidar_health_status) {
+        d.raw["rplidar.health_status"] = *info.rplidar_health_status;
+        if (info.rplidar_error_code) d.raw["rplidar.error_code"] = *info.rplidar_error_code;
+        if (*info.rplidar_health_status == kRplidarHealthError) {
+            std::snprintf(buf, sizeof(buf), "RPLIDAR health error, code 0x%04X",
+                          info.rplidar_error_code ? *info.rplidar_error_code : 0);
+            add(DiagSeverity::Fault, "device_error", buf);
+        } else if (*info.rplidar_health_status == kRplidarHealthWarning) {
+            add(DiagSeverity::Warning, "device_warning", "RPLIDAR health warning");
+        }
+    }
+
+    return d;
+}
+
+// Per-model configuration. scan_angle_* use the signed system [-180,180]:
+// 0 = ahead, + = left, - = right. range_min/max are datasheet placeholders.
+// The per-vendor MODEL_* presets live in each plugin.
+struct ModelConfig {
+    const char* name;
+    float       scan_angle_min;   // deg
+    float       scan_angle_max;   // deg
+    float       range_min_m = 0.05f;
+    float       range_max_m = 30.f;
+    // Added to the raw device angle so output 0° = ahead (e.g. LR-1F/1FMI
+    // report 0° at the back: +180; SICK TiM puts the front at 90°: -90).
+    float       angle_offset_deg = 0.f;
+    // Output remap window (see remap_scan_window in plugins/common): shifts
+    // the scan's angles onto [out_angle_min, out_angle_max] without dropping
+    // points.
+    bool        remap_angles  = false;
+    float       out_angle_min = 0.f;  // deg
+    float       out_angle_max = 0.f;  // deg
+    // Valid FOV window from DeviceConfig::angle_min/max_deg: points outside
+    // become NaN, geometry unchanged (apply_fov_window in plugins/common).
+    bool        fov_filter  = false;
+    float       fov_min_deg = -360.f;
+    float       fov_max_deg =  360.f;
+};
+
+using ScanCallback = std::function<void(const ScanResult&)>;
+
+// ── Driver metadata & instance configuration ────────────────────────────────
+
+// Transport a driver uses to reach the device. A driver declares exactly one
+// primary transport; drivers that can switch (e.g. ESPE TCP/UDP) declare the
+// default and honor DeviceConfig::transport.
+enum class Transport { Serial, Udp, Tcp };
+
+inline const char* to_string(Transport t) {
+    switch (t) {
+        case Transport::Serial: return "serial";
+        case Transport::Udp:    return "udp";
+        case Transport::Tcp:    return "tcp";
+    }
+    return "unknown";
+}
+
+// Parse the strings written by to_string(Transport); nullopt for anything else.
+inline std::optional<Transport> transport_from_string(const std::string& s) {
+    if (s == "serial") return Transport::Serial;
+    if (s == "udp")    return Transport::Udp;
+    if (s == "tcp")    return Transport::Tcp;
+    return std::nullopt;
+}
+
+// Static identity a plugin registers about itself (get_driver_info entry
+// point and LidarDriverInterface::get_driver_info()).
+struct DriverInfo {
+    std::string vendor;       // "Slamtec", "OLEI", "SICK", "ESPE"
+    std::string model;        // device category the driver targets, e.g. "C1"
+                              // or "TiM5xx/TiM7xx" — one driver may cover a
+                              // whole series
+    std::string driver_id;    // unique stable id, e.g. "rplidar_c1_driver"
+    std::string description;  // short doc: covered devices, transport, notes
+
+    // Extra metadata for hosts/UIs (not part of the required identity):
+    Transport transport = Transport::Udp;   // primary transport
+    bool      transport_selectable = false; // true → DeviceConfig::transport
+                                            //        may pick either TCP or UDP
+    std::vector<std::string> supported_models; // valid DeviceConfig::model values
+};
+
+// Settings for one lidar instance. `name` is the unique key across saves.
+// Which fields matter depends on the transport in effect:
+//   serial → serial_port + baudrate;  udp/tcp → ip + port.
+struct DeviceConfig {
+    std::string name      = "lidar";
+    std::string driver_id;            // plugin that owns this device
+    std::string model     = "AUTO";   // one of DriverInfo::supported_models
+
+    // Transport to reach the device. nullopt = the driver's declared default
+    // (DriverInfo::transport). A fixed-transport driver rejects a mismatch
+    // from open() with InvalidConfig; transport-selectable drivers (ESPE)
+    // switch between TCP and UDP through this field.
+    std::optional<Transport> transport;
+
+    // Network transports (udp: local bind address / tcp: device address)
+    std::string ip   = "0.0.0.0";
+    uint16_t    port = 0;             // 0 = driver default
+
+    // Serial transport
+    std::string serial_port = "/dev/ttyUSB0";
+    uint32_t    baudrate    = 460800;
+
+    bool inverted = false;   // unit mounted upside-down → mirror the scan
+
+    // Valid field-of-view window (deg, signed system: 0 = ahead, + = left).
+    // Points outside are reported as NaN (invalid), the scan geometry is
+    // unchanged. Defaults (±360) = off.
+    float angle_min_deg = -360.f;
+    float angle_max_deg =  360.f;
+
+    // Range override (m); 0 = keep the driver/model default.
+    float range_min_m = 0.f;
+    float range_max_m = 0.f;
+
+    // Legacy output remap window (deg): scan angles are linearly remapped
+    // onto [remap_angle_min_deg, remap_angle_max_deg] without dropping
+    // points. Defaults (±360) = off. Kept for pre-plugin lidarlib configs.
+    float remap_angle_min_deg = -360.f;
+    float remap_angle_max_deg =  360.f;
+
+    // Driver-specific options that don't warrant a first-class field
+    // (documented per plugin).
+    std::map<std::string, std::string> extra;
+
+    friend bool operator==(const DeviceConfig& a, const DeviceConfig& b) {
+        return a.name == b.name && a.driver_id == b.driver_id && a.model == b.model &&
+               a.transport == b.transport &&
+               a.ip == b.ip && a.port == b.port &&
+               a.serial_port == b.serial_port && a.baudrate == b.baudrate &&
+               a.inverted == b.inverted &&
+               a.angle_min_deg == b.angle_min_deg && a.angle_max_deg == b.angle_max_deg &&
+               a.range_min_m == b.range_min_m && a.range_max_m == b.range_max_m &&
+               a.remap_angle_min_deg == b.remap_angle_min_deg &&
+               a.remap_angle_max_deg == b.remap_angle_max_deg &&
+               a.extra == b.extra;
+    }
+    friend bool operator!=(const DeviceConfig& a, const DeviceConfig& b) { return !(a == b); }
+};
+
+// ── Driver interface ────────────────────────────────────────────────────────
+
+// Unified driver interface implemented by every plugin. Instances come from
+// LidarManager::create_lidar_device() (or a plugin's create_driver_instance
+// entry point directly). One instance == one physical device; instances are
+// fully independent — run each on its own thread without locking.
+class LidarDriverInterface {
+public:
+    virtual ~LidarDriverInterface() = default;
+
+    // Static metadata of the driver that produced this instance.
+    virtual DriverInfo get_driver_info() const = 0;
+
+    // ErrorCode::Ok on success. Calling open() on an already-open instance
+    // returns AlreadyOpen and leaves the connection untouched.
+    virtual ErrorCode open() = 0;
+    // Idempotent: safe to call before open() or more than once.
+    virtual void close() = 0;
+    // Block until one full scan; false on error/timeout (see last_error()).
+    // timeout_ms = 0 → block indefinitely. No default on purpose: drivers
+    // differ (OLEI 1000, SICK TiM 2000).
+    virtual bool recv_scan(ScanResult& out, int timeout_ms) = 0;
+    // The callback fires only from spin_once() — recv_scan() never invokes
+    // it. Pick one pump style: recv_scan() to poll, or callback + spin_once().
+    virtual void set_scan_callback(ScanCallback cb) = 0;
+    // Process one unit of input (may block on the socket/port while the
+    // device is silent); fires the scan callback when a scan completed.
+    // False on error.
+    virtual bool spin_once() = 0;
+    // Model name read from the wire where the protocol carries one
+    // ("AUTO"/configured name until then).
+    virtual const char* detected_model() const = 0;
+
+    virtual bool is_open() const = 0;
+    // Status of the most recent open()/recv_scan()/spin_once() call.
+    ErrorCode last_error() const { return last_error_; }
+
+    // Device self-diagnostics from the newest fully decoded scan. valid stays
+    // false until one scan has been seen. Updated by recv_scan()/spin_once();
+    // call from the same thread that pumps them.
+    virtual Diagnostics get_diagnostics() const { return {}; }
+
+    // True when the sensor is usable right now: connection open, at least one
+    // fault-free scan decoded, and that scan no older than max_age_ms
+    // (0 = skip the age check). Diagnostics only refresh from
+    // recv_scan()/spin_once(), so unless something is pumping them this goes
+    // stale and reports not-ready; call from the pump thread.
+    bool is_ready(int max_age_ms = 3000) const {
+        if (!is_open() || !get_diagnostics().healthy()) return false;
+        if (max_age_ms <= 0) return true;
+        return last_scan_time_.time_since_epoch().count() != 0
+            && std::chrono::steady_clock::now() - last_scan_time_
+                   <= std::chrono::milliseconds(max_age_ms);
+    }
+
+    // Pump recv_scan() until is_ready() or timeout_ms elapses; false on
+    // timeout (see last_error() for the underlying failure). Scans consumed
+    // while waiting are discarded and the scan callback does not fire —
+    // intended for startup, before handing the pump to the main loop.
+    bool wait_ready(int timeout_ms = 5000) {
+        const auto deadline = std::chrono::steady_clock::now()
+                            + std::chrono::milliseconds(timeout_ms);
+        ScanResult tmp;
+        while (!is_ready()) {
+            if (!is_open()) return false;
+            const auto left = std::chrono::duration_cast<std::chrono::milliseconds>(
+                deadline - std::chrono::steady_clock::now()).count();
+            if (left <= 0) return false;
+            recv_scan(tmp, static_cast<int>(left));
+        }
+        return true;
+    }
+
+protected:
+    ErrorCode set_error(ErrorCode e) { last_error_ = e; return e; }
+    // Drivers call this each time a full scan is decoded; feeds the freshness
+    // side of is_ready().
+    void mark_scan_decoded() { last_scan_time_ = std::chrono::steady_clock::now(); }
+
+private:
+    ErrorCode last_error_ = ErrorCode::Ok;
+    std::chrono::steady_clock::time_point last_scan_time_{};
+};
+
+} // namespace xlidar
+
+// ── Plugin ABI ──────────────────────────────────────────────────────────────
+//
+// Each plugin .so exports exactly these two symbols (C linkage, default
+// visibility — plugins are otherwise built with -fvisibility=hidden):
+//
+//     XLIDAR_PLUGIN_EXPORT void get_driver_info(xlidar::DriverInfo* out);
+//     XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface*
+//         create_driver_instance(const xlidar::DeviceConfig* cfg);
+//
+// create_driver_instance returns a heap-allocated instance (never nullptr for
+// a structurally valid cfg; config problems surface from open() as
+// InvalidConfig/SerialError/...). The host deletes it through the
+// LidarDriverInterface vtable, so the plugin must stay loaded for the
+// instance's whole lifetime — LidarManager guarantees that by keeping every
+// plugin open until the manager itself is destroyed.
+//
+// C linkage keeps the symbol names unmangled for dlsym(); the types crossing
+// the boundary are C++ (same toolchain for manager and plugins is required —
+// they are always built together in this repo).
+
+#define XLIDAR_PLUGIN_EXPORT extern "C" __attribute__((visibility("default")))
+
+extern "C" {
+using xlidar_get_driver_info_fn = void (*)(xlidar::DriverInfo*);
+using xlidar_create_driver_instance_fn =
+    xlidar::LidarDriverInterface* (*)(const xlidar::DeviceConfig*);
+}
+
+// Symbol names LidarManager resolves in every plugin.
+#define XLIDAR_GET_DRIVER_INFO_SYMBOL        "get_driver_info"
+#define XLIDAR_CREATE_DRIVER_INSTANCE_SYMBOL "create_driver_instance"

include/lidar_manager.hpp

@@ -0,0 +1,110 @@
+#pragma once
+// xlidar-driver — LidarManager: the host-facing facade of the SDK.
+//
+// The manager scans a plugin directory for driver .so files, registers their
+// DriverInfo, and creates driver instances by driver_id:
+//
+//     xlidar::LidarManager manager("/opt/xlidar/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 = "rplidar_c1_driver";
+//     cfg.serial_port = "/dev/ttyUSB0";
+//     auto lidar = manager.create_lidar_device(cfg);
+//
+// Plugins stay loaded until the manager is destroyed; the manager must
+// outlive every device instance it created (instances execute code that
+// lives inside the plugin .so).
+
+#include "lidar_interface.hpp"
+
+#include <map>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <vector>
+
+namespace xlidar {
+
+// One discovered plugin: its registered identity plus where it came from.
+struct PluginRegistry {
+    DriverInfo  info;
+    std::string file_path;   // absolute path of the plugin .so
+};
+
+// Configuration document (config.json): a named list of lidar devices.
+struct ManagerConfig {
+    std::vector<DeviceConfig> lidars;
+};
+
+// Returns defaults if the file doesn't exist (without creating it).
+// Legacy lidarlib entries ({"brand": "OLEI"|"SICK"|"ESPE", ...}) are
+// migrated on the fly: brand+model resolve to the matching driver_id and the
+// old angle window keys map to the remap fields.
+ManagerConfig load_config(const std::string& path);
+
+void save_config(const std::string& path, const ManagerConfig& cfg);
+
+class LidarManager {
+public:
+    // plugins_dir: directory holding the driver .so files. Nothing is
+    // touched until load_all_plugins() runs.
+    explicit LidarManager(std::string plugins_dir);
+    ~LidarManager();
+
+    LidarManager(const LidarManager&)            = delete;
+    LidarManager& operator=(const LidarManager&) = delete;
+
+    // Scan plugins_dir for *.so, dlopen each, resolve get_driver_info /
+    // create_driver_instance, and register the driver. Files that are not
+    // valid plugins (missing symbols, dlopen failure) are skipped with a
+    // message on stderr. Safe to call again to pick up newly added files
+    // (already-loaded driver_ids are kept, not reloaded). Returns the number
+    // of drivers registered in total.
+    size_t load_all_plugins();
+
+    // Registered drivers keyed by driver_id. Stable while the manager lives;
+    // load_all_plugins() may add entries.
+    const std::map<std::string, PluginRegistry>& available_drivers() const {
+        return available_;
+    }
+
+    bool has_driver(const std::string& driver_id) const {
+        return available_.count(driver_id) != 0;
+    }
+
+    // Create a device instance from the plugin registered for driver_id.
+    // nullptr if driver_id is unknown. The instance is independent and
+    // thread-safe to pump from its own thread; it must be destroyed before
+    // the manager.
+    std::unique_ptr<LidarDriverInterface>
+    create_lidar_device(const std::string& driver_id, const DeviceConfig& cfg);
+
+    // Convenience: driver_id taken from cfg.driver_id.
+    std::unique_ptr<LidarDriverInterface> create_lidar_device(const DeviceConfig& cfg) {
+        return create_lidar_device(cfg.driver_id, cfg);
+    }
+
+    // Convenience: one instance per entry of a config.json document (see
+    // load_config). Entries whose driver_id is not available are skipped
+    // with a message on stderr.
+    std::vector<std::unique_ptr<LidarDriverInterface>>
+    create_from_config_file(const std::string& path);
+
+    const std::string& plugins_dir() const { return plugins_dir_; }
+
+private:
+    struct LoadedPlugin {
+        void*                            handle = nullptr;  // dlopen handle
+        xlidar_create_driver_instance_fn create = nullptr;
+    };
+
+    std::string plugins_dir_;
+    std::map<std::string, PluginRegistry> available_;
+    std::map<std::string, LoadedPlugin>   loaded_;
+    mutable std::mutex mutex_;
+};
+
+} // namespace xlidar

include/lidarlib/config.hpp

@@ -1,63 +0,0 @@
-#pragma once
-#include "lidarlib/lidar.hpp"
-#include <memory>
-#include <string>
-#include <vector>
-
-namespace lidarlib {
-
-// Settings for one lidar. `name` is the unique key across saves.
-struct LidarConfig {
-    std::string name      = "lidar";
-    std::string ip        = "0.0.0.0";
-    uint16_t    port      = 2368;
-    std::string model     = "AUTO";
-    bool        inverted  = false;    // unit mounted upside-down → mirror the scan
-    std::string brand     = "OLEI";   // "OLEI", "SICK" or "ESPE"
-    bool        use_udp   = false;    // ESPE only: UDP instead of TCP transport
-
-    // Output angle window (deg): scan angles are remapped onto
-    // [angle_min_deg, angle_max_deg] without dropping points.
-    // Defaults (±360) = off.
-    float angle_min_deg = -360.f;
-    float angle_max_deg =  360.f;
-
-    friend bool operator==(const LidarConfig& a, const LidarConfig& b) {
-        return a.name == b.name && a.ip == b.ip && a.port == b.port &&
-               a.model == b.model && a.inverted == b.inverted && a.brand == b.brand &&
-               a.use_udp == b.use_udp &&
-               a.angle_min_deg == b.angle_min_deg && a.angle_max_deg == b.angle_max_deg;
-    }
-    friend bool operator!=(const LidarConfig& a, const LidarConfig& b) { return !(a == b); }
-};
-
-struct Config {
-    std::vector<LidarConfig> lidars = {
-        {"front", "0.0.0.0", 2368, "AUTO", false},
-        {"rear",  "0.0.0.0", 2369, "AUTO", true},
-    };
-};
-
-// nullptr if `name` doesn't match any known model.
-const ModelConfig* model_by_name(const std::string& name);
-
-const std::vector<std::string>& model_names();
-
-const std::vector<std::string>& brand_names();
-
-// Subset of model_names() valid for `brand`; empty if unknown.
-const std::vector<std::string>& model_names_for_brand(const std::string& brand);
-
-// Returns defaults if the file doesn't exist (without creating it).
-Config load_config(const std::string& path);
-
-void save_config(const std::string& path, const Config& cfg);
-
-// Build a ready-to-open lidar from one LidarConfig — the only entry point an
-// app needs. brand "SICK" → SICK driver (model "SICK-nanoScan3" → UDP
-// NanoScanDriver, others → TCP SickDriver); brand "ESPE" → TCP EspeDriver;
-// anything else → OLEI UDP.
-// Unknown/cross-brand model falls back to the brand default. Never nullptr.
-std::unique_ptr<Lidar> make_lidar(const LidarConfig& cfg);
-
-} // namespace lidarlib

include/lidarlib/diagnostics.hpp

@@ -1,119 +0,0 @@
-#pragma once
-#include <cstdint>
-#include <cstdio>
-#include <optional>
-#include <string>
-
-namespace lidarlib {
-
-struct ExtraInfo;  // lidar.hpp
-
-// Family A (0xFAF0) error_status bits, header byte [5]. Bits 3-7 are reserved
-// on the wire; a nonzero reserved bit is still reported as a fault.
-inline constexpr uint8_t kFaultMonitor     = 1u << 0;  // monitor / motor abnormal
-inline constexpr uint8_t kFaultVoltage     = 1u << 1;  // supply voltage out of range
-inline constexpr uint8_t kFaultTemperature = 1u << 2;  // internal temperature abnormal
-
-// SICK TiM LMDscandata device status (low word; Telegram Listing).
-inline constexpr uint16_t kSickStatusError            = 1u << 0;
-inline constexpr uint16_t kSickStatusPollutionWarning = 1u << 1;
-inline constexpr uint16_t kSickStatusPollutionError   = 1u << 2;
-
-// SICK nanoScan3 General System State byte 0 (layout from sick_safetyscanners;
-// NOT verified on real hardware).
-inline constexpr uint8_t kNanoStateRunMode              = 1u << 0;
-inline constexpr uint8_t kNanoStateStandby              = 1u << 1;
-inline constexpr uint8_t kNanoStateContaminationWarning = 1u << 2;
-inline constexpr uint8_t kNanoStateContaminationError   = 1u << 3;
-inline constexpr uint8_t kNanoStateReferenceContour     = 1u << 4;
-inline constexpr uint8_t kNanoStateManipulation         = 1u << 5;
-
-// Device self-diagnostics decoded from the data stream. Fields the family
-// doesn't carry stay std::nullopt (see docs/diagnostics.md for the per-family
-// wire layout). valid stays false until the driver has decoded one full scan.
-struct Diagnostics {
-    bool        valid = false;
-    std::string model = "AUTO";
-    uint32_t    device_timestamp_ms = 0;  // device clock; 0 if not on the wire
-
-    // Family A error byte (0 = no fault; Family B/C don't carry it)
-    uint8_t error_status = 0;
-
-    bool monitor_fault()     const { return (error_status & kFaultMonitor)     != 0; }
-    bool voltage_fault()     const { return (error_status & kFaultVoltage)     != 0; }
-    bool temperature_fault() const { return (error_status & kFaultTemperature) != 0; }
-
-    // Family A only — raw motor speed field, unit unverified
-    std::optional<uint16_t> rotation_raw;
-
-    // Family C / V3 (GS1-5) only — raw passthroughs, bit meanings unverified
-    std::optional<uint16_t> scan_frequency_raw;
-    std::optional<uint16_t> input_status;
-    std::optional<uint16_t> output_status;
-    std::optional<uint32_t> field_status;
-    std::optional<uint32_t> status_flags;
-
-    // SICK TiM — LMDscandata status pair (word0<<8)|word1
-    std::optional<uint16_t> sick_device_status;
-
-    bool sick_error()        const { return sick_device_status && (*sick_device_status & kSickStatusError); }
-    bool pollution_warning() const { return sick_device_status && (*sick_device_status & kSickStatusPollutionWarning); }
-    bool pollution_error()   const { return sick_device_status && (*sick_device_status & kSickStatusPollutionError); }
-
-    // SICK nanoScan3 — General System State byte 0
-    std::optional<uint8_t> nano_general_state;
-
-    bool contamination_warning() const { return nano_general_state && (*nano_general_state & kNanoStateContaminationWarning); }
-    bool contamination_error()   const { return nano_general_state && (*nano_general_state & kNanoStateContaminationError); }
-    bool manipulation()          const { return nano_general_state && (*nano_general_state & kNanoStateManipulation); }
-
-    // ESPE LGA60 — raw fault word from area frames (bit meanings unverified);
-    // only present when the host polls area data.
-    std::optional<uint16_t> espe_error_status;
-
-    bool espe_fault() const { return espe_error_status && *espe_error_status != 0; }
-
-    // Fault = device says something is wrong now; warning = degraded but
-    // still measuring (dirty optics) — schedule cleaning/service.
-    bool has_fault() const {
-        return error_status != 0 || sick_error() || pollution_error()
-            || contamination_error() || manipulation() || espe_fault();
-    }
-    bool has_warning() const { return pollution_warning() || contamination_warning(); }
-    bool healthy()     const { return valid && !has_fault(); }
-};
-
-// Decode the diagnostic fields of one scan; sets valid = true.
-Diagnostics decode_diagnostics(const ExtraInfo& info);
-
-// One-line log summary: "no data" / "ok" / "WARN: pollution" /
-// "FAULT: voltage temperature".
-inline std::string to_string(const Diagnostics& d) {
-    if (!d.valid)       return "no data";
-    if (!d.has_fault()) return d.has_warning()
-        ? std::string("WARN:") + (d.pollution_warning() ? " pollution" : "")
-                                + (d.contamination_warning() ? " contamination" : "")
-        : "ok";
-
-    std::string s = "FAULT:";
-    if (d.monitor_fault())       s += " monitor";
-    if (d.voltage_fault())       s += " voltage";
-    if (d.temperature_fault())   s += " temperature";
-    if (d.sick_error())          s += " device";
-    if (d.pollution_error())     s += " pollution";
-    if (d.contamination_error()) s += " contamination";
-    if (d.manipulation())        s += " manipulation";
-    if (d.espe_fault()) {
-        char buf[24];
-        std::snprintf(buf, sizeof(buf), " espe(0x%04X)", *d.espe_error_status);
-        s += buf;
-    }
-    if (uint8_t rest = d.error_status & ~(kFaultMonitor | kFaultVoltage | kFaultTemperature)) {
-        char buf[24];
-        std::snprintf(buf, sizeof(buf), " reserved(0x%02X)", rest);
-        s += buf;
-    }
-    return s;
-}
-
-} // namespace lidarlib

include/lidarlib/error.hpp

@@ -1,46 +0,0 @@
-#pragma once
-
-namespace lidarlib {
-
-// Result of open() and the sticky status behind last_error(). Ok == 0 so
-// `if (err != ErrorCode::Ok)` reads naturally at call sites.
-enum class ErrorCode {
-    Ok = 0,
-
-    // Lifecycle misuse — the call was refused, the instance state is unchanged.
-    AlreadyOpen,          // open() called while already open
-    NotOpen,              // recv_scan()/spin_once() called before open()
-
-    // open() failures
-    SocketError,          // socket() creation failed
-    InvalidAddress,       // ip string is not a valid IPv4 address
-    PortInUse,            // bind: local port already taken (EADDRINUSE/EACCES)
-    BindFailed,           // bind failed for another reason
-    ConnectionRefused,    // TCP connect refused (device up, port closed)
-    ConnectionFailed,     // TCP connect failed (unreachable, no route, ...)
-    HandshakeFailed,      // connected, but the start-stream command failed
-
-    // Runtime failures
-    Timeout,              // no (complete) scan within timeout_ms
-    DeviceDisconnected,   // peer closed the connection / socket recv error
-};
-
-inline const char* to_string(ErrorCode e) {
-    switch (e) {
-        case ErrorCode::Ok:                 return "Ok";
-        case ErrorCode::AlreadyOpen:        return "AlreadyOpen";
-        case ErrorCode::NotOpen:            return "NotOpen";
-        case ErrorCode::SocketError:        return "SocketError";
-        case ErrorCode::InvalidAddress:     return "InvalidAddress";
-        case ErrorCode::PortInUse:          return "PortInUse";
-        case ErrorCode::BindFailed:         return "BindFailed";
-        case ErrorCode::ConnectionRefused:  return "ConnectionRefused";
-        case ErrorCode::ConnectionFailed:   return "ConnectionFailed";
-        case ErrorCode::HandshakeFailed:    return "HandshakeFailed";
-        case ErrorCode::Timeout:            return "Timeout";
-        case ErrorCode::DeviceDisconnected: return "DeviceDisconnected";
-    }
-    return "Unknown";
-}
-
-} // namespace lidarlib

include/lidarlib/lidar.hpp

@@ -1,236 +0,0 @@
-#pragma once
-#include "lidarlib/diagnostics.hpp"
-#include "lidarlib/error.hpp"
-#include <chrono>
-#include <cstdint>
-#include <vector>
-#include <string>
-#include <functional>
-#include <optional>
-
-namespace lidarlib {
-
-// ROS sensor_msgs/LaserScan-shaped output (radians, meters, seconds).
-// ranges[i] is at angle_min + i*angle_increment, in sweep order.
-struct LaserScan {
-    uint32_t timestamp_ms = 0;   // device clock (ms); 0 if not on the wire
-    float angle_min       = 0.f; // rad
-    float angle_max       = 0.f; // rad
-    float angle_increment = 0.f; // rad
-    float time_increment  = 0.f; // sec — not exposed by devices, always 0
-    float scan_time       = 0.f; // sec — not exposed by devices, always 0
-    float range_min       = 0.f; // m — from ModelConfig, not measured
-    float range_max       = 0.f; // m — from ModelConfig, not measured
-    std::vector<float> ranges;       // m
-    std::vector<float> intensities;  // 0-255 as float
-};
-
-// Diagnostic/header fields; fields the family doesn't carry stay std::nullopt.
-struct ExtraInfo {
-    std::string detected_model = "AUTO";
-    uint8_t     error_status   = 0;    // Family A: BIT0=Monitor, BIT1=Voltage, BIT2=Temp
-    uint8_t     distance_scale_mm = 0; // 0 = not reported
-
-    // Family A only
-    std::optional<uint16_t> rotation_raw;
-
-    // Family C / V3 (GS1-5) only — raw passthroughs, unverified
-    std::optional<uint8_t>  distance_ratio_raw;
-    std::optional<uint16_t> scan_frequency_raw;
-    std::optional<uint16_t> input_status;
-    std::optional<uint16_t> output_status;
-    std::optional<uint32_t> field_status;
-    std::optional<uint32_t> status_flags;
-
-    // SICK TiM only — LMDscandata status pair (word0<<8)|word1:
-    // 0 ok, 1 error, 2 pollution warning, 4 pollution error.
-    std::optional<uint16_t> sick_device_status;
-
-    // SICK nanoScan3 only — General System State byte 0 (see kNanoState* bits).
-    std::optional<uint8_t> nano_general_state;
-
-    // ESPE LGA60 only — fault word from the newest "WSimu" area frame; the
-    // device only sends those when area data is polled, so usually nullopt.
-    std::optional<uint16_t> espe_error_status;
-};
-
-struct ScanResult {
-    LaserScan scan;
-    ExtraInfo info;
-};
-
-// Per-model configuration. scan_angle_* use the signed system [-180,180]:
-// 0 = ahead, + = left, - = right. range_min/max are datasheet placeholders.
-struct ModelConfig {
-    const char* name;
-    float       scan_angle_min;   // deg
-    float       scan_angle_max;   // deg
-    float       range_min_m = 0.05f;
-    float       range_max_m = 30.f;
-    // Added to the raw device angle so output 0° = ahead (LR-1F/1FMI report 0°
-    // at the back: +180; SICK TiM puts the front at 90°: -90).
-    float       angle_offset_deg = 0.f;
-    // Output remap window (see make_lidar / remap_scan_window): shifts the
-    // scan's angles onto [out_angle_min, out_angle_max] without dropping points.
-    bool        remap_angles  = false;
-    float       out_angle_min = 0.f;  // deg
-    float       out_angle_max = 0.f;  // deg
-};
-
-inline constexpr ModelConfig MODEL_VB     { "VB",      -135.f, 135.f, 0.05f, 30.f };  // 2D 270°
-inline constexpr ModelConfig MODEL_VF     { "VF",      -180.f, 180.f, 0.05f, 30.f };  // 2D 360°
-inline constexpr ModelConfig MODEL_LR1F   { "LR-1F",   -180.f, 180.f, 0.05f, 50.f, 180.f };  // 2D 360° 50m; device 0° = rear
-inline constexpr ModelConfig MODEL_LR1FMI { "LR-1FMI", -180.f, 180.f, 0.05f, 30.f, 180.f };  // 2D 360° (Family B); device 0° = rear
-inline constexpr ModelConfig MODEL_LR1BS5 { "LR-1BS5", -180.f, 180.f, 0.05f, 30.f };  // 2D 360° (Family B)
-inline constexpr ModelConfig MODEL_LR16F  { "LR-16F",  -135.f, 135.f, 0.05f, 30.f };  // 3D 16 line
-inline constexpr ModelConfig MODEL_GS15   { "GS1-5",   -180.f, 180.f, 0.05f, 30.f };  // 2D 360°
-
-// Model unknown ahead of time: Family B/C packets carry enough to auto-detect;
-// Family A doesn't, so the wide default FOV is kept.
-inline constexpr ModelConfig MODEL_AUTO   { "AUTO",    -180.f, 180.f, 0.05f, 30.f };
-
-using ScanCallback = std::function<void(const ScanResult&)>;
-
-// Unified driver interface returned by make_lidar(); OLEI and SICK drivers
-// both derive from it.
-class Lidar {
-public:
-    virtual ~Lidar() = default;
-
-    // ErrorCode::Ok on success. Calling open() on an already-open instance
-    // returns AlreadyOpen and leaves the connection untouched.
-    virtual ErrorCode open() = 0;
-    // Idempotent: safe to call before open() or more than once.
-    virtual void close() = 0;
-    // Block until one full scan; false on error/timeout (see last_error()).
-    // timeout_ms = 0 → block indefinitely. No default on purpose: drivers
-    // differ (OLEI 1000, SICK 2000).
-    virtual bool recv_scan(ScanResult& out, int timeout_ms) = 0;
-    // The callback fires only from spin_once() — recv_scan() never invokes
-    // it. Pick one pump style: recv_scan() to poll, or callback + spin_once().
-    virtual void set_scan_callback(ScanCallback cb) = 0;
-    // Process one unit of input (may block on the socket while the device is
-    // silent); fires the scan callback when a scan completed. False on error.
-    virtual bool spin_once() = 0;
-    virtual const char* detected_model() const = 0;
-
-    virtual bool is_open() const = 0;
-    // Status of the most recent open()/recv_scan()/spin_once() call.
-    ErrorCode last_error() const { return last_error_; }
-
-    // Device self-diagnostics from the newest fully decoded scan. valid stays
-    // false until one scan has been seen. Updated by recv_scan()/spin_once();
-    // call from the same thread that pumps them.
-    virtual Diagnostics get_diagnostics() const { return {}; }
-
-    // True when the sensor is usable right now: connection open, at least one
-    // fault-free scan decoded, and that scan no older than max_age_ms
-    // (0 = skip the age check). Diagnostics only refresh from
-    // recv_scan()/spin_once(), so unless something is pumping them this goes
-    // stale and reports not-ready; call from the pump thread.
-    bool is_ready(int max_age_ms = 3000) const {
-        if (!is_open() || !get_diagnostics().healthy()) return false;
-        if (max_age_ms <= 0) return true;
-        return last_scan_time_.time_since_epoch().count() != 0
-            && std::chrono::steady_clock::now() - last_scan_time_
-                   <= std::chrono::milliseconds(max_age_ms);
-    }
-
-    // Pump recv_scan() until is_ready() or timeout_ms elapses; false on
-    // timeout (see last_error() for the underlying failure). Scans consumed
-    // while waiting are discarded and the scan callback does not fire —
-    // intended for startup, before handing the pump to the main loop.
-    bool wait_ready(int timeout_ms = 5000) {
-        const auto deadline = std::chrono::steady_clock::now()
-                            + std::chrono::milliseconds(timeout_ms);
-        ScanResult tmp;
-        while (!is_ready()) {
-            if (!is_open()) return false;
-            const auto left = std::chrono::duration_cast<std::chrono::milliseconds>(
-                deadline - std::chrono::steady_clock::now()).count();
-            if (left <= 0) return false;
-            recv_scan(tmp, static_cast<int>(left));
-        }
-        return true;
-    }
-
-protected:
-    ErrorCode set_error(ErrorCode e) { last_error_ = e; return e; }
-    // Drivers call this each time a full scan is decoded; feeds the freshness
-    // side of is_ready().
-    void mark_scan_decoded() { last_scan_time_ = std::chrono::steady_clock::now(); }
-
-private:
-    ErrorCode last_error_ = ErrorCode::Ok;
-    std::chrono::steady_clock::time_point last_scan_time_{};
-};
-
-// OLEI UDP driver.
-class Driver : public Lidar {
-public:
-    using ScanCallback = lidarlib::ScanCallback;
-
-    // ip: local bind address; port: UDP port the lidar sends to;
-    // inverted: unit mounted upside-down → mirror every angle.
-    explicit Driver(const ModelConfig& cfg,
-                    const std::string& ip       = "0.0.0.0",
-                    uint16_t           port     = 2368,
-                    bool                inverted = false);
-    ~Driver();
-
-    Driver(const Driver&)            = delete;
-    Driver& operator=(const Driver&) = delete;
-
-    ErrorCode open() override;
-    void close() override;
-    bool recv_scan(ScanResult& out, int timeout_ms = 1000) override;
-    void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
-    bool spin_once() override;
-    bool is_open() const override { return sock_fd_ >= 0; }
-
-    // Model name read from the Family B/C header; "AUTO" until one is seen.
-    const char* detected_model() const override { return detected_model_name_.c_str(); }
-
-    Diagnostics get_diagnostics() const override { return latest_diag_; }
-
-private:
-    bool poll_packet();  // one recvfrom() + dispatch to the family parser
-    bool parse_family_a(const uint8_t* buf, int len);  // ID=0xFAF0
-    bool parse_family_b(const uint8_t* buf, int len);  // ID=0xFEF0
-    bool parse_family_c(const uint8_t* buf, int len);  // Magic=0xFEAC (GS1-5)
-
-    void push_point(float signed_angle_deg, float dist_m, uint8_t intensity);
-    void flush_scan();
-
-    ModelConfig  cfg_;
-    std::string  ip_;
-    uint16_t     port_;
-    bool         inverted_  = false;
-    int          sock_fd_   = -1;
-    ScanCallback cb_;
-
-    // Per-revolution accumulation buffers (index-aligned)
-    std::vector<float>   pending_angle_deg_;
-    std::vector<float>   pending_dist_m_;
-    std::vector<uint8_t> pending_intensity_;
-    uint32_t              pending_ts_   = 0;
-    uint8_t                pending_err_  = 0;
-    float                  last_angle_   = -1.f;  // wrap detection, device space [0,360)
-
-    ExtraInfo pending_info_;
-
-    // Snapshot for get_diagnostics(); refreshed by flush_scan().
-    Diagnostics latest_diag_;
-
-    ScanResult ready_result_;
-    bool       scan_ready_ = false;
-
-    // Per-instance so two drivers on two threads don't race.
-    uint8_t  recv_buf_[4096];
-
-    bool        auto_detect_        = false;
-    bool        model_locked_       = false;
-    std::string detected_model_name_ = "AUTO";
-};
-
-} // namespace lidarlib

include/lidarlib/lidarlib.hpp

@@ -1,7 +0,0 @@
-#pragma once
-// One-include convenience header for the whole public API.
-#include "lidarlib/error.hpp"
-#include "lidarlib/lidar.hpp"
-#include "lidarlib/sick_lidar.hpp"
-#include "lidarlib/espe_lidar.hpp"
-#include "lidarlib/config.hpp"

include/lidarlib/sick_lidar.hpp

@@ -1,118 +0,0 @@
-#pragma once
-#include "lidarlib/lidar.hpp"
-#include <cstdint>
-#include <string>
-#include <vector>
-
-namespace lidarlib {
-
-// SICK TiM presets. FOV/range from datasheets; scan_angle_* are informational
-// only and do NOT filter points. angle_offset_deg = -90 because the TiM wire
-// frame puts 90° at the device front.
-inline constexpr ModelConfig MODEL_SICK_TIM5XX { "SICK-TIM5xx", -135.f, 135.f, 0.05f, 10.f, -90.f }; // TiM551/561, 270°, 10m
-inline constexpr ModelConfig MODEL_SICK_TIM571 { "SICK-TIM571", -135.f, 135.f, 0.05f, 25.f, -90.f }; // TiM571,      270°, 25m
-inline constexpr ModelConfig MODEL_SICK_TIM7XX { "SICK-TIM7xx", -135.f, 135.f, 0.05f, 25.f, -90.f }; // TiM781,      270°, 25m
-
-// SICK TiM5xx/7xx over SOPAS/CoLa-A (TCP, default port 2111).
-// Verified against a real TiM781S (FW V5.11). NOT verified: NumEncoders > 0,
-// the 8-bit channel branch, and the TIM5xx/TIM571 FOV/range numbers.
-class SickDriver : public Lidar {
-public:
-    using ScanCallback = lidarlib::ScanCallback;
-
-    // inverted: unit mounted upside-down → mirror the scan.
-    explicit SickDriver(const ModelConfig& cfg,
-                         const std::string& ip,
-                         uint16_t            port     = 2111,
-                         bool                inverted = false);
-    ~SickDriver();
-
-    SickDriver(const SickDriver&)            = delete;
-    SickDriver& operator=(const SickDriver&) = delete;
-
-    // Connect + send "sEN LMDscandata 1" to start continuous scan output.
-    ErrorCode open() override;
-    void close() override;
-    bool recv_scan(ScanResult& out, int timeout_ms = 2000) override;
-    void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
-    bool spin_once() override;
-    bool is_open() const override { return sock_fd_ >= 0; }
-
-    // No model string on the wire — returns the configured name.
-    const char* detected_model() const override { return detected_model_name_.c_str(); }
-
-    Diagnostics get_diagnostics() const override { return latest_diag_; }
-
-private:
-    bool send_telegram(const std::string& body);
-    bool read_telegram(std::string& out, int timeout_ms);
-    bool parse_lmdscandata(const std::string& telegram, ScanResult& out);
-
-    ModelConfig  cfg_;
-    std::string  detected_model_name_;  // owned copy of cfg_.name (stable lifetime)
-    std::string  ip_;
-    uint16_t     port_;
-    bool         inverted_ = false;
-    int          sock_fd_ = -1;
-    ScanCallback cb_;
-
-    // Snapshot for get_diagnostics(); refreshed by parse_lmdscandata().
-    Diagnostics latest_diag_;
-
-    // Leftover TCP bytes carried across telegram boundaries; per-instance.
-    std::string recv_buf_;
-};
-
-inline constexpr ModelConfig MODEL_SICK_NANOSCAN3 { "SICK-nanoScan3", -137.5f, 137.5f, 0.05f, 40.f };
-
-// SICK nanoScan3 / microScan3 safety-scanner binary UDP output. Layout ported
-// from SICK's open-source sick_safetyscanners; NOT verified on real hardware.
-// Passive UDP receiver: the sensor's UDP target must be configured up front in
-// SICK Safety Designer — this class does no CoLa2/TCP handshake.
-class NanoScanDriver : public Lidar {
-public:
-    using ScanCallback = lidarlib::ScanCallback;
-
-    // ip: local bind address; port: local UDP port the sensor sends to;
-    // inverted: unit mounted upside-down → mirror the scan.
-    explicit NanoScanDriver(const ModelConfig& cfg,
-                            const std::string& ip       = "0.0.0.0",
-                            uint16_t            port     = 6060,
-                            bool                inverted = false);
-    ~NanoScanDriver();
-
-    NanoScanDriver(const NanoScanDriver&)            = delete;
-    NanoScanDriver& operator=(const NanoScanDriver&) = delete;
-
-    ErrorCode open() override;
-    void close() override;
-    bool recv_scan(ScanResult& out, int timeout_ms = 1000) override;
-    void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
-    bool spin_once() override;
-    bool is_open() const override { return sock_fd_ >= 0; }
-
-    // No model string on the wire — returns the configured name.
-    const char* detected_model() const override { return detected_model_name_.c_str(); }
-
-    Diagnostics get_diagnostics() const override { return latest_diag_; }
-
-private:
-    int  recv_datagram(int timeout_ms);
-    bool parse_packet(const uint8_t* buf, int len, ScanResult& out);
-
-    ModelConfig  cfg_;
-    std::string  detected_model_name_;  // owned copy of cfg_.name (stable lifetime)
-    std::string  ip_;
-    uint16_t     port_;
-    bool         inverted_ = false;
-    int          sock_fd_ = -1;
-    ScanCallback cb_;
-
-    // Snapshot for get_diagnostics(); refreshed by parse_packet().
-    Diagnostics latest_diag_;
-
-    // Per-instance; sized for a full safety-data packet (max ~2751 beams).
-    std::vector<uint8_t> recv_buf_;
-};
-
-} // namespace lidarlib

plugins/CMakeLists.txt

@@ -0,0 +1,29 @@
+# Driver plugins. Every plugin is a self-contained MODULE library named
+# <dir>.so (no "lib" prefix), exporting exactly the two C entry points
+# declared in include/lidar_interface.hpp. Plugins land in build/plugins/.
+
+set(XLIDAR_PLUGIN_OUTPUT_DIR ${CMAKE_BINARY_DIR}/plugins)
+set(XLIDAR_PLUGIN_COMMON_DIR ${CMAKE_CURRENT_SOURCE_DIR}/common)
+
+# xlidar_add_plugin(<name> <sources...>) — shared boilerplate for one plugin.
+function(xlidar_add_plugin name)
+    add_library(${name} MODULE ${ARGN})
+    set_target_properties(${name} PROPERTIES
+        PREFIX ""                                       # driver_olei.so, not libdriver_olei.so
+        LIBRARY_OUTPUT_DIRECTORY ${XLIDAR_PLUGIN_OUTPUT_DIR}
+        CXX_VISIBILITY_PRESET hidden                    # only the two entry points are visible
+        VISIBILITY_INLINES_HIDDEN ON
+        POSITION_INDEPENDENT_CODE ON
+    )
+    target_include_directories(${name} PRIVATE
+        ${CMAKE_SOURCE_DIR}/include
+        ${XLIDAR_PLUGIN_COMMON_DIR}
+    )
+    target_link_libraries(${name} PRIVATE Threads::Threads)
+endfunction()
+
+add_subdirectory(driver_olei)
+add_subdirectory(driver_sick_tim)
+add_subdirectory(driver_sick_safety)
+add_subdirectory(driver_espe)
+add_subdirectory(driver_rplidar)

plugins/common/plugin_helpers.hpp

@@ -0,0 +1,199 @@
+// Internal helpers shared by the plugin TUs — not part of the public API.
+// Header-only on purpose: every plugin .so carries its own copy, so plugins
+// never link against each other or against liblidar_manager.
+#pragma once
+#include "lidar_interface.hpp"
+
+#include <algorithm>
+#include <cerrno>
+#include <cmath>
+#include <cstdint>
+#include <cstdio>
+#include <cstring>
+#include <fcntl.h>
+#include <limits>
+#include <netinet/in.h>
+#include <netinet/tcp.h>
+#include <string>
+#include <sys/select.h>
+#include <sys/socket.h>
+#include <utility>
+
+namespace xlidar {
+
+inline constexpr float kDeg2Rad = 3.14159265358979323846f / 180.f;
+
+// True when the requested DeviceConfig::transport is one this driver can
+// serve: unset always matches (driver default); otherwise the declared
+// transport, or — for transport-selectable drivers — either side of the
+// TCP/UDP pair.
+inline bool transport_supported(const DriverInfo& info, const DeviceConfig& cfg) {
+    if (!cfg.transport || *cfg.transport == info.transport) return true;
+    if (info.transport_selectable)
+        return (*cfg.transport == Transport::Udp && info.transport == Transport::Tcp) ||
+               (*cfg.transport == Transport::Tcp && info.transport == Transport::Udp);
+    return false;
+}
+
+// Stand-in returned by create_driver_instance() when a structurally valid
+// config still can't be served (e.g. transport mismatch): the plugin ABI
+// forbids returning nullptr there, so the error surfaces from open() as
+// InvalidConfig instead of the setting being silently ignored.
+class InvalidConfigDriver : public LidarDriverInterface {
+public:
+    InvalidConfigDriver(DriverInfo info, std::string reason)
+        : info_(std::move(info)), reason_(std::move(reason)) {}
+
+    DriverInfo get_driver_info() const override { return info_; }
+    ErrorCode open() override {
+        std::fprintf(stderr, "[xlidar] %s: %s\n", info_.driver_id.c_str(), reason_.c_str());
+        return set_error(ErrorCode::InvalidConfig);
+    }
+    void close() override {}
+    bool recv_scan(ScanResult&, int) override {
+        set_error(ErrorCode::NotOpen);
+        return false;
+    }
+    void set_scan_callback(ScanCallback) override {}
+    bool spin_once() override {
+        set_error(ErrorCode::NotOpen);
+        return false;
+    }
+    const char* detected_model() const override { return info_.model.c_str(); }
+    bool is_open() const override { return false; }
+
+private:
+    DriverInfo  info_;
+    std::string reason_;
+};
+
+// Remap a finished scan's angular window onto [min_deg, max_deg]. Only
+// angle_min/angle_max/angle_increment are rewritten; points are untouched.
+inline void remap_scan_window(LaserScan& scan, float min_deg, float max_deg) {
+    const float new_min  = min_deg * kDeg2Rad;
+    const float new_max  = max_deg * kDeg2Rad;
+    const float old_span = scan.angle_max - scan.angle_min;
+    if (old_span > 0.f)
+        scan.angle_increment *= (new_max - new_min) / old_span;
+    scan.angle_min = new_min;
+    scan.angle_max = new_max;
+}
+
+// Mirror a finished scan for a unit mounted upside-down: reverse the point
+// order and negate the angular window. Apply before remap_scan_window().
+inline void invert_scan(LaserScan& scan) {
+    std::reverse(scan.ranges.begin(), scan.ranges.end());
+    std::reverse(scan.intensities.begin(), scan.intensities.end());
+    const float new_min = -scan.angle_max;
+    scan.angle_max = -scan.angle_min;
+    scan.angle_min = new_min;
+}
+
+// Valid FOV window (DeviceConfig::angle_min/max_deg): points whose signed
+// angle falls outside [min_deg, max_deg] become NaN; the scan geometry is
+// unchanged. Apply after invert_scan(), before remap_scan_window() (it needs
+// the real angles).
+inline void apply_fov_window(LaserScan& scan, float min_deg, float max_deg) {
+    const float min_rad = min_deg * kDeg2Rad;
+    const float max_rad = max_deg * kDeg2Rad;
+    constexpr float kPi = 3.14159265358979323846f;
+    for (size_t i = 0; i < scan.ranges.size(); ++i) {
+        // Normalize into (-pi, pi]: OLEI scans unwrap continuously and may
+        // exceed the seam.
+        float a = scan.angle_min + static_cast<float>(i) * scan.angle_increment;
+        a = std::fmod(a, 2.f * kPi);
+        if (a >  kPi) a -= 2.f * kPi;
+        if (a < -kPi) a += 2.f * kPi;
+        if (a < min_rad || a > max_rad)
+            scan.ranges[i] = std::numeric_limits<float>::quiet_NaN();
+    }
+}
+
+// Apply the generic DeviceConfig windows/overrides onto a model preset —
+// every plugin's create_driver_instance() funnels through this.
+inline ModelConfig apply_device_config(const ModelConfig& preset, const DeviceConfig& cfg) {
+    ModelConfig mc = preset;
+    if (cfg.range_min_m > 0.f) mc.range_min_m = cfg.range_min_m;
+    if (cfg.range_max_m > 0.f) mc.range_max_m = cfg.range_max_m;
+    if (cfg.angle_min_deg > -360.f || cfg.angle_max_deg < 360.f) {
+        mc.fov_filter  = true;
+        mc.fov_min_deg = cfg.angle_min_deg;
+        mc.fov_max_deg = cfg.angle_max_deg;
+    }
+    if (cfg.remap_angle_min_deg > -360.f || cfg.remap_angle_max_deg < 360.f) {
+        mc.remap_angles  = true;
+        mc.out_angle_min = cfg.remap_angle_min_deg;
+        mc.out_angle_max = cfg.remap_angle_max_deg;
+    }
+    return mc;
+}
+
+// Standard finalize sequence shared by the drivers; call once per completed
+// scan, after ranges/intensities/angles are filled in device order.
+inline void finalize_scan(LaserScan& scan, const ModelConfig& cfg, bool inverted) {
+    if (inverted)
+        invert_scan(scan);
+    if (cfg.fov_filter)
+        apply_fov_window(scan, cfg.fov_min_deg, cfg.fov_max_deg);
+    if (cfg.remap_angles)
+        remap_scan_window(scan, cfg.out_angle_min, cfg.out_angle_max);
+}
+
+// Little-endian readers (bounds are the caller's responsibility).
+inline uint8_t  le_u8 (const uint8_t* p) { return p[0]; }
+inline uint16_t le16(const uint8_t* p) {
+    return static_cast<uint16_t>(p[0] | (p[1] << 8));
+}
+inline uint32_t le32(const uint8_t* p) {
+    return static_cast<uint32_t>(p[0])
+         | (static_cast<uint32_t>(p[1]) <<  8)
+         | (static_cast<uint32_t>(p[2]) << 16)
+         | (static_cast<uint32_t>(p[3]) << 24);
+}
+inline int32_t  le_i32(const uint8_t* p) { return static_cast<int32_t>(le32(p)); }
+
+inline float bits_to_float(uint32_t bits) {
+    float f;
+    std::memcpy(&f, &bits, sizeof(f));
+    return f;
+}
+
+// Non-blocking connect with a bounded timeout — a blocking connect() to an
+// unreachable device would stall for the OS default (~2 min on Linux).
+// Enables TCP_NODELAY on success; the fd is returned to blocking mode either
+// way. The caller owns the fd and closes it on failure.
+inline ErrorCode connect_tcp_with_timeout(int fd, const sockaddr_in& addr, int timeout_ms) {
+    int flags = ::fcntl(fd, F_GETFL, 0);
+    ::fcntl(fd, F_SETFL, flags | O_NONBLOCK);
+
+    ErrorCode conn_err = ErrorCode::Ok;
+    int rc = ::connect(fd, reinterpret_cast<const sockaddr*>(&addr), sizeof(addr));
+    if (rc < 0 && errno != EINPROGRESS) {
+        conn_err = (errno == ECONNREFUSED) ? ErrorCode::ConnectionRefused
+                                           : ErrorCode::ConnectionFailed;
+    } else if (rc < 0) {
+        fd_set wfds; FD_ZERO(&wfds); FD_SET(fd, &wfds);
+        timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 };
+        rc = ::select(fd + 1, nullptr, &wfds, nullptr, &tv);
+        if (rc == 0) {
+            conn_err = ErrorCode::Timeout;
+        } else if (rc < 0) {
+            conn_err = ErrorCode::ConnectionFailed;
+        } else {
+            int err = 0; socklen_t errlen = sizeof(err);
+            ::getsockopt(fd, SOL_SOCKET, SO_ERROR, &err, &errlen);
+            if (err != 0)
+                conn_err = (err == ECONNREFUSED) ? ErrorCode::ConnectionRefused
+                                                 : ErrorCode::ConnectionFailed;
+        }
+    }
+    ::fcntl(fd, F_SETFL, flags);
+
+    if (conn_err == ErrorCode::Ok) {
+        int nodelay = 1;
+        ::setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, &nodelay, sizeof(nodelay));
+    }
+    return conn_err;
+}
+
+} // namespace xlidar

plugins/driver_espe/CMakeLists.txt

@@ -0,0 +1 @@
+xlidar_add_plugin(driver_espe espe_driver.cpp)

plugins/driver_espe/espe_driver.cpp

@@ -1,19 +1,19 @@
-#include "lidarlib/espe_lidar.hpp"
+// ESPE LGA60 — "HISN" range frames + "WSimu" area frames over TCP/UDP.
-#include "lidar_bytes.hpp"
+#include "espe_driver.hpp"
-#include "lidar_net.hpp"
+#include "plugin_helpers.hpp"
 
 #include <algorithm>
 #include <cerrno>
 #include <cmath>
 #include <cstring>
 #include <limits>
-#include <sys/socket.h>
 #include <arpa/inet.h>
-#include <unistd.h>
-#include <sys/select.h>
 #include <netinet/in.h>
+#include <sys/select.h>
+#include <sys/socket.h>
+#include <unistd.h>
 
-namespace lidarlib {
+namespace xlidar {
 
 namespace {
 // "RAuto" + fixed tail — puts the device into continuous measurement output.
@@ -219,10 +219,7 @@ void EspeDriver::finish_scan() {
     scan.range_min = cfg_.range_min_m;
     scan.range_max = cfg_.range_max_m;
 
-    if (inverted_)
+    finalize_scan(scan, cfg_, inverted_);
-        invert_scan(scan);
-    if (cfg_.remap_angles)
-        remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max);
 
     ExtraInfo& info = ready_result_.info;
     info = ExtraInfo{};
@@ -263,4 +260,45 @@ bool EspeDriver::spin_once() {
     return true;
 }
 
-} // namespace lidarlib
+// ── plugin registration ─────────────────────────────────────────────────────
+
+namespace {
+
+const DriverInfo kDriverInfo = [] {
+    DriverInfo info;
+    info.vendor      = "ESPE";
+    info.model       = "LGA60";
+    info.driver_id   = "espe_lga60_driver";
+    info.description = "ESPE LGA60 320° laser scanner — TCP by default, UDP via "
+                       "DeviceConfig::transport; open() sends the RAuto start "
+                       "command; device parameters come from the vendor Windows "
+                       "tool. Default port 8080 (vendor default IP 192.168.1.88). "
+                       "Ported from the vendor ROS driver; not verified on real "
+                       "hardware.";
+    info.transport   = Transport::Tcp;
+    info.transport_selectable = true;   // transport = udp switches to UDP
+    info.supported_models = {"ESPE-LGA60"};
+    return info;
+}();
+
+} // namespace
+
+DriverInfo EspeDriver::get_driver_info() const { return kDriverInfo; }
+
+} // namespace xlidar
+
+XLIDAR_PLUGIN_EXPORT void get_driver_info(xlidar::DriverInfo* out) {
+    *out = xlidar::kDriverInfo;
+}
+
+XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface*
+create_driver_instance(const xlidar::DeviceConfig* cfg) {
+    using namespace xlidar;
+    if (!transport_supported(kDriverInfo, *cfg))
+        return new InvalidConfigDriver(kDriverInfo,
+            std::string("unsupported transport '") + to_string(*cfg->transport) + "'");
+    const uint16_t port    = cfg->port ? cfg->port : 8080;
+    const bool     use_udp = cfg->transport == Transport::Udp;
+    return new EspeDriver(apply_device_config(MODEL_ESPE_LGA60, *cfg),
+                          cfg->ip, port, use_udp, cfg->inverted);
+}

plugins/driver_espe/espe_driver.hpp

@@ -1,9 +1,13 @@
+// ESPE LGA60 laser scanner over TCP or UDP — plugin-private header.
 #pragma once
-#include "lidarlib/lidar.hpp"
+#include "lidar_interface.hpp"
-#include <cstdint>
-#include <string>
 
-namespace lidarlib {
+#include <cstdint>
+#include <optional>
+#include <string>
+#include <vector>
+
+namespace xlidar {
 
 // ESPE LGA60-320: 320° FOV, device sweeps 20°..340° with 0° at the rear
 // (angle_offset_deg = -180 so output 0° = ahead). Range per datasheet page;
@@ -14,10 +18,8 @@ inline constexpr ModelConfig MODEL_ESPE_LGA60 { "ESPE-LGA60", -160.f, 160.f, 0.0
 // ROS driver; NOT verified on real hardware. open() sends the "RAuto" start
 // command; device parameters (spin rate, resolution, filters) are whatever
 // the vendor Windows config tool programmed — this driver does not set them.
-class EspeDriver : public Lidar {
+class EspeDriver : public LidarDriverInterface {
 public:
-    using ScanCallback = lidarlib::ScanCallback;
-
     // ip: device address; use_udp selects the transport the device is
     // configured for (vendor default is TCP); inverted: unit mounted
     // upside-down → mirror the scan.
@@ -31,6 +33,8 @@ public:
     EspeDriver(const EspeDriver&)            = delete;
     EspeDriver& operator=(const EspeDriver&) = delete;
 
+    DriverInfo get_driver_info() const override;
+
     // Connect + send the start-capture command.
     ErrorCode open() override;
     void close() override;
@@ -80,4 +84,4 @@ private:
     bool       scan_ready_ = false;
 };
 
-} // namespace lidarlib
+} // namespace xlidar

plugins/driver_olei/CMakeLists.txt

@@ -0,0 +1 @@
+xlidar_add_plugin(driver_olei olei_driver.cpp)

plugins/driver_olei/olei_driver.cpp

@@ -1,16 +1,17 @@
-#include "lidarlib/lidar.hpp"
+// OLEI 2D lidars over UDP — Family A (0xFAF0), Family B (0xFEF0) and
-#include "lidar_bytes.hpp"
+// Family C / Protocol V3 (0xFEAC, GS1-5) packet parsing.
+#include "olei_driver.hpp"
+#include "plugin_helpers.hpp"
 
 #include <cerrno>
 #include <cstring>
 #include <cmath>
-#include <stdexcept>
-#include <sys/socket.h>
 #include <arpa/inet.h>
-#include <unistd.h>
 #include <sys/select.h>
+#include <sys/socket.h>
+#include <unistd.h>
 
-namespace lidarlib {
+namespace xlidar {
 
 // Normalize into (-180, 180]: 0 = ahead, + = left, - = right.
 static inline float to_signed_deg(float deg) {
@@ -39,32 +40,16 @@ static constexpr uint16_t FRAME_ID_A = 0xFAF0;  // 2D Ethernet (VB, VF, LR-1F)
 static constexpr uint16_t FRAME_ID_B = 0xFEF0;  // LR-1BS5 / LR-1BS2 Ethernet variant
 static constexpr uint16_t FRAME_ID_C = 0xFEAC;  // Protocol V3 (GS1-5)
 
-Diagnostics decode_diagnostics(const ExtraInfo& info) {
+OleiDriver::OleiDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port,
-    Diagnostics d;
+                       bool inverted)
-    d.valid              = true;
-    d.model              = info.detected_model;
-    d.error_status       = info.error_status;
-    d.rotation_raw       = info.rotation_raw;
-    d.scan_frequency_raw = info.scan_frequency_raw;
-    d.input_status       = info.input_status;
-    d.output_status      = info.output_status;
-    d.field_status       = info.field_status;
-    d.status_flags       = info.status_flags;
-    d.sick_device_status = info.sick_device_status;
-    d.nano_general_state = info.nano_general_state;
-    d.espe_error_status  = info.espe_error_status;
-    return d;
-}
-
-Driver::Driver(const ModelConfig& cfg, const std::string& ip, uint16_t port, bool inverted)
     : cfg_(cfg), ip_(ip), port_(port), inverted_(inverted)
 {
     auto_detect_ = (std::strcmp(cfg.name, "AUTO") == 0);
 }
 
-Driver::~Driver() { close(); }
+OleiDriver::~OleiDriver() { close(); }
 
-ErrorCode Driver::open() {
+ErrorCode OleiDriver::open() {
     if (is_open()) return set_error(ErrorCode::AlreadyOpen);
 
     sockaddr_in addr{};
@@ -105,14 +90,14 @@ ErrorCode Driver::open() {
     return set_error(ErrorCode::Ok);
 }
 
-void Driver::close() {
+void OleiDriver::close() {
     if (sock_fd_ >= 0) {
         ::close(sock_fd_);
         sock_fd_ = -1;
     }
 }
 
-bool Driver::recv_scan(ScanResult& out, int timeout_ms) {
+bool OleiDriver::recv_scan(ScanResult& out, int timeout_ms) {
     if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }
     scan_ready_ = false;
 
@@ -133,7 +118,7 @@ bool Driver::recv_scan(ScanResult& out, int timeout_ms) {
     return true;
 }
 
-bool Driver::spin_once() {
+bool OleiDriver::spin_once() {
     if (!poll_packet()) return false;
     if (scan_ready_) {
         scan_ready_ = false;
@@ -142,7 +127,7 @@ bool Driver::spin_once() {
     return true;
 }
 
-bool Driver::poll_packet() {
+bool OleiDriver::poll_packet() {
     if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }
     uint8_t* buf = recv_buf_;
     sockaddr_in from{};
@@ -165,7 +150,7 @@ bool Driver::poll_packet() {
 }
 
 // Append with angle-unwrapping so the ±180° seam stays a continuous ramp.
-void Driver::push_point(float signed_angle_deg, float dist_m, uint8_t intensity) {
+void OleiDriver::push_point(float signed_angle_deg, float dist_m, uint8_t intensity) {
     float angle = signed_angle_deg;
     if (!pending_angle_deg_.empty()) {
         float prev = pending_angle_deg_.back();
@@ -177,7 +162,7 @@ void Driver::push_point(float signed_angle_deg, float dist_m, uint8_t intensity)
     pending_intensity_.push_back(intensity);
 }
 
-void Driver::flush_scan() {
+void OleiDriver::flush_scan() {
     if (pending_angle_deg_.empty()) return;
 
     const size_t n = pending_angle_deg_.size();
@@ -195,8 +180,8 @@ void Driver::flush_scan() {
     scan.ranges.assign(pending_dist_m_.begin(), pending_dist_m_.end());
     scan.intensities.assign(pending_intensity_.begin(), pending_intensity_.end());
 
-    if (cfg_.remap_angles)
+    // Inversion already happened per point (maybe_invert), so inverted=false.
-        remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max);
+    finalize_scan(scan, cfg_, /*inverted=*/false);
 
     ExtraInfo& info  = ready_result_.info;
     info             = pending_info_;
@@ -215,7 +200,7 @@ void Driver::flush_scan() {
 }
 
 // Family A (0xFAF0): 20B header + 3B blocks (u16 dist, u8 intensity).
-bool Driver::parse_family_a(const uint8_t* buf, int len) {
+bool OleiDriver::parse_family_a(const uint8_t* buf, int len) {
     static constexpr int HEADER_LEN = 20;
     static constexpr int BLOCK_LEN  =  3;
 
@@ -267,7 +252,7 @@ bool Driver::parse_family_a(const uint8_t* buf, int len) {
 
 // Family B (0xFEF0): 40B header (model string at [7-16]) + 8B blocks
 // (u16 angle ×0.01°, u16 dist, u16 signal). No timestamp/error on the wire.
-bool Driver::parse_family_b(const uint8_t* buf, int len) {
+bool OleiDriver::parse_family_b(const uint8_t* buf, int len) {
     static constexpr int HEADER_LEN = 40;
     static constexpr int BLOCK_LEN  =  8;
 
@@ -338,7 +323,7 @@ bool Driver::parse_family_b(const uint8_t* buf, int len) {
 // Family C / Protocol V3 (0xFEAC, GS1-5): 48B header + 2 or 4B points depending
 // on Types. Ported from the C# driver OleiGS15Driver.cs; NOT verified on real
 // hardware. Angle = (FirstIndex + i) * (360 / NumPointsScan) - 180.
-bool Driver::parse_family_c(const uint8_t* buf, int len) {
+bool OleiDriver::parse_family_c(const uint8_t* buf, int len) {
     static constexpr int HEADER_LEN = 48;
     if (len < HEADER_LEN) return false;
 
@@ -424,4 +409,52 @@ bool Driver::parse_family_c(const uint8_t* buf, int len) {
     return true;
 }
 
-} // namespace lidarlib
+// ── plugin registration ─────────────────────────────────────────────────────
+
+namespace {
+
+const DriverInfo kDriverInfo = [] {
+    DriverInfo info;
+    info.vendor      = "OLEI";
+    info.model       = "2D series (VB/VF/LR-1x/GS1-5)";
+    info.driver_id   = "olei_lidar_driver";
+    info.description = "OLEI 2D lidars over UDP — auto-detects the Family A/B/C "
+                       "protocol per packet; model AUTO self-detects from the "
+                       "stream (Family B/C). Default port 2368.";
+    info.transport   = Transport::Udp;
+    info.supported_models = {"AUTO", "VB", "VF", "LR-1F", "LR-1FMI", "LR-1BS5",
+                             "LR-16F", "GS1-5"};
+    return info;
+}();
+
+const ModelConfig* model_by_name(const std::string& name) {
+    static constexpr const ModelConfig* kModels[] = {
+        &MODEL_AUTO, &MODEL_VB, &MODEL_VF, &MODEL_LR1F, &MODEL_LR1FMI,
+        &MODEL_LR1BS5, &MODEL_LR16F, &MODEL_GS15,
+    };
+    for (const ModelConfig* m : kModels)
+        if (name == m->name) return m;
+    return nullptr;
+}
+
+} // namespace
+
+DriverInfo OleiDriver::get_driver_info() const { return kDriverInfo; }
+
+} // namespace xlidar
+
+XLIDAR_PLUGIN_EXPORT void get_driver_info(xlidar::DriverInfo* out) {
+    *out = xlidar::kDriverInfo;
+}
+
+XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface*
+create_driver_instance(const xlidar::DeviceConfig* cfg) {
+    using namespace xlidar;
+    if (!transport_supported(kDriverInfo, *cfg))
+        return new InvalidConfigDriver(kDriverInfo,
+            std::string("unsupported transport '") + to_string(*cfg->transport) + "'");
+    const ModelConfig* preset = model_by_name(cfg->model);
+    if (!preset) preset = &MODEL_AUTO;  // unknown model → auto-detect
+    const uint16_t port = cfg->port ? cfg->port : 2368;
+    return new OleiDriver(apply_device_config(*preset, *cfg), cfg->ip, port, cfg->inverted);
+}

plugins/driver_olei/olei_driver.hpp

@@ -0,0 +1,89 @@
+// OLEI 2D lidars over UDP (Family A / B / C protocols) — plugin-private header.
+#pragma once
+#include "lidar_interface.hpp"
+
+#include <string>
+#include <vector>
+
+namespace xlidar {
+
+// Model presets. Family B/C packets carry enough to auto-detect the model;
+// Family A doesn't, so MODEL_AUTO keeps the wide default FOV.
+inline constexpr ModelConfig MODEL_VB     { "VB",      -135.f, 135.f, 0.05f, 30.f };  // 2D 270°
+inline constexpr ModelConfig MODEL_VF     { "VF",      -180.f, 180.f, 0.05f, 30.f };  // 2D 360°
+inline constexpr ModelConfig MODEL_LR1F   { "LR-1F",   -180.f, 180.f, 0.05f, 50.f, 180.f };  // 2D 360° 50m; device 0° = rear
+inline constexpr ModelConfig MODEL_LR1FMI { "LR-1FMI", -180.f, 180.f, 0.05f, 30.f, 180.f };  // 2D 360° (Family B); device 0° = rear
+inline constexpr ModelConfig MODEL_LR1BS5 { "LR-1BS5", -180.f, 180.f, 0.05f, 30.f };  // 2D 360° (Family B)
+inline constexpr ModelConfig MODEL_LR16F  { "LR-16F",  -135.f, 135.f, 0.05f, 30.f };  // 3D 16 line
+inline constexpr ModelConfig MODEL_GS15   { "GS1-5",   -180.f, 180.f, 0.05f, 30.f };  // 2D 360° (Family C/V3)
+inline constexpr ModelConfig MODEL_AUTO   { "AUTO",    -180.f, 180.f, 0.05f, 30.f };
+
+// OLEI UDP driver.
+class OleiDriver : public LidarDriverInterface {
+public:
+    // ip: local bind address; port: UDP port the lidar sends to;
+    // inverted: unit mounted upside-down → mirror every angle.
+    explicit OleiDriver(const ModelConfig& cfg,
+                        const std::string& ip       = "0.0.0.0",
+                        uint16_t           port     = 2368,
+                        bool               inverted = false);
+    ~OleiDriver();
+
+    OleiDriver(const OleiDriver&)            = delete;
+    OleiDriver& operator=(const OleiDriver&) = delete;
+
+    DriverInfo get_driver_info() const override;
+
+    ErrorCode open() override;
+    void close() override;
+    bool recv_scan(ScanResult& out, int timeout_ms = 1000) override;
+    void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
+    bool spin_once() override;
+    bool is_open() const override { return sock_fd_ >= 0; }
+
+    // Model name read from the Family B/C header; "AUTO" until one is seen.
+    const char* detected_model() const override { return detected_model_name_.c_str(); }
+
+    Diagnostics get_diagnostics() const override { return latest_diag_; }
+
+private:
+    bool poll_packet();  // one recvfrom() + dispatch to the family parser
+    bool parse_family_a(const uint8_t* buf, int len);  // ID=0xFAF0
+    bool parse_family_b(const uint8_t* buf, int len);  // ID=0xFEF0
+    bool parse_family_c(const uint8_t* buf, int len);  // Magic=0xFEAC (GS1-5)
+
+    void push_point(float signed_angle_deg, float dist_m, uint8_t intensity);
+    void flush_scan();
+
+    ModelConfig  cfg_;
+    std::string  ip_;
+    uint16_t     port_;
+    bool         inverted_  = false;
+    int          sock_fd_   = -1;
+    ScanCallback cb_;
+
+    // Per-revolution accumulation buffers (index-aligned)
+    std::vector<float>   pending_angle_deg_;
+    std::vector<float>   pending_dist_m_;
+    std::vector<uint8_t> pending_intensity_;
+    uint32_t              pending_ts_   = 0;
+    uint8_t                pending_err_  = 0;
+    float                  last_angle_   = -1.f;  // wrap detection, device space [0,360)
+
+    ExtraInfo pending_info_;
+
+    // Snapshot for get_diagnostics(); refreshed by flush_scan().
+    Diagnostics latest_diag_;
+
+    ScanResult ready_result_;
+    bool       scan_ready_ = false;
+
+    // Per-instance so two drivers on two threads don't race.
+    uint8_t  recv_buf_[4096];
+
+    bool        auto_detect_        = false;
+    bool        model_locked_       = false;
+    std::string detected_model_name_ = "AUTO";
+};
+
+} // namespace xlidar

plugins/driver_rplidar/CMakeLists.txt

@@ -0,0 +1,35 @@
+# The rplidar plugin compiles the vendor SDK straight into the plugin .so.
+# XLIDAR_RPLIDAR_SDK_DIR must point at a directory holding include/ + src/;
+# when unset, common local layouts are probed. Without an SDK the plugin is
+# skipped (the rest of the build is unaffected).
+
+if(NOT XLIDAR_RPLIDAR_SDK_DIR)
+    foreach(candidate
+            ${CMAKE_SOURCE_DIR}/third_party/rplidar_sdk
+            ${CMAKE_SOURCE_DIR}/../rplidar_sdk/sdk
+            ${CMAKE_SOURCE_DIR}/../xloc-monorepo/xlocd/deps/rplidar_sdk)
+        if(EXISTS ${candidate}/include/sl_lidar.h)
+            set(XLIDAR_RPLIDAR_SDK_DIR ${candidate})
+            break()
+        endif()
+    endforeach()
+endif()
+
+if(NOT XLIDAR_RPLIDAR_SDK_DIR OR NOT EXISTS ${XLIDAR_RPLIDAR_SDK_DIR}/include/sl_lidar.h)
+    message(WARNING "driver_rplidar: Slamtec SDK not found "
+                    "(set -DXLIDAR_RPLIDAR_SDK_DIR=...) — plugin skipped")
+    return()
+endif()
+
+message(STATUS "driver_rplidar: using SDK at ${XLIDAR_RPLIDAR_SDK_DIR}")
+
+file(GLOB_RECURSE RPLIDAR_SDK_SOURCES CONFIGURE_DEPENDS
+    ${XLIDAR_RPLIDAR_SDK_DIR}/src/*.cpp)
+# The SDK ships win32/macOS arch files; keep Linux only.
+list(FILTER RPLIDAR_SDK_SOURCES EXCLUDE REGEX "arch/(win32|macOS)/")
+
+xlidar_add_plugin(driver_rplidar rplidar_driver.cpp ${RPLIDAR_SDK_SOURCES})
+target_include_directories(driver_rplidar SYSTEM PRIVATE
+    ${XLIDAR_RPLIDAR_SDK_DIR}/include
+    ${XLIDAR_RPLIDAR_SDK_DIR}/src
+)

plugins/driver_rplidar/rplidar_driver.cpp

@@ -0,0 +1,329 @@
+// Slamtec RPLIDAR over serial (C1 defaults), built on the vendor SDK
+// (sl_lidar.h). Ported from xlocd's embedded rplidar driver — the scan math
+// (angle/distance decoding, inversion, FOV window, NaN invalid points) is
+// kept identical.
+//
+// Unlike the network drivers, angles are reported in the DEVICE frame
+// [0, 2π), 0 = ahead, ascending — exactly what the SDK's ascendScanData
+// yields (and what xlocd's engine expects).
+#include "lidar_interface.hpp"
+#include "plugin_helpers.hpp"
+
+#include <chrono>
+#include <cmath>
+#include <cstdio>
+#include <filesystem>
+#include <limits>
+#include <string>
+#include <vector>
+
+#include "sl_lidar.h"
+
+namespace xlidar {
+
+namespace {
+
+// Node buffer for one grab. 8192 is the SDK-recommended size, far above the
+// ~400-500 points/rev of a C1 in DenseBoost mode.
+constexpr std::size_t kMaxNodesPerScan = 8192;
+constexpr float kPi = 3.14159265358979323846F;
+constexpr float kTwoPi = 2.0F * kPi;
+// C1 default range (datasheet: 12 m on white; 16 m ceiling matches the
+// common rplidar_ros configuration). Overridable via DeviceConfig range_*.
+constexpr float kDefaultRangeMinM = 0.05F;
+constexpr float kDefaultRangeMaxM = 16.0F;
+// Nominal rotation period (~10 Hz) for the first frame, before a real
+// grab-to-grab interval has been measured.
+constexpr float kDefaultScanTimeS = 0.1F;
+constexpr int   kDefaultGrabTimeoutMs = 2000;  // SDK default
+
+inline constexpr ModelConfig MODEL_RPLIDAR_C1 { "C1", -180.f, 180.f, kDefaultRangeMinM, kDefaultRangeMaxM };
+
+// HQ node angle: angle_z_q14 is [0..360) fixed-point Q14 on a 90° scale.
+float node_angle_rad(const sl_lidar_response_measurement_node_hq_t& node) {
+    return static_cast<float>(node.angle_z_q14) * 90.0F / (1 << 14) * kDeg2Rad;
+}
+
+// HQ node distance: dist_mm_q2 is mm in Q2 (1/4 mm) -> metres.
+float node_distance_m(const sl_lidar_response_measurement_node_hq_t& node) {
+    return static_cast<float>(node.dist_mm_q2) / 4.0F / 1000.0F;
+}
+
+// Device angle [0, 2π) -> signed (-180, 180] degrees (0 = ahead, + = left),
+// to compare against the configured FOV window.
+float to_signed_deg(float angle_rad) {
+    float deg = angle_rad / kDeg2Rad;
+    if (deg > 180.0F) deg -= 360.0F;
+    return deg;
+}
+
+const DriverInfo kDriverInfo = [] {
+    DriverInfo info;
+    info.vendor      = "Slamtec";
+    info.model       = "C1";
+    info.driver_id   = "rplidar_c1_driver";
+    info.description = "Slamtec RPLIDAR over serial, built on the vendor SDK — "
+                       "defaults match the C1 (CP2102N UART bridge, baud "
+                       "460800); other SDK-compatible serial models (A/S "
+                       "series) work with the matching baud rate. Health check "
+                       "at open(); model/firmware auto-detected.";
+    info.transport   = Transport::Serial;
+    info.supported_models = {"AUTO", "C1"};
+    return info;
+}();
+
+} // namespace
+
+class RplidarDriver : public LidarDriverInterface {
+public:
+    RplidarDriver(const ModelConfig& cfg, std::string serial_port, uint32_t baudrate,
+                  bool inverted)
+        : cfg_(cfg), serial_port_(std::move(serial_port)), baudrate_(baudrate),
+          inverted_(inverted) {}
+
+    ~RplidarDriver() override { close(); }
+
+    RplidarDriver(const RplidarDriver&)            = delete;
+    RplidarDriver& operator=(const RplidarDriver&) = delete;
+
+    DriverInfo get_driver_info() const override { return kDriverInfo; }
+
+    // Full connect sequence; each step maps to one ErrorCode:
+    // device present (SerialError) -> serial channel (SerialError) -> SDK
+    // driver (SerialError) -> connect (ConnectionFailed) -> device info
+    // (non-fatal, fills model/firmware) -> health check (DeviceError on
+    // fault) -> motor + startScan typical mode (HandshakeFailed).
+    ErrorCode open() override {
+        if (is_open()) return set_error(ErrorCode::AlreadyOpen);
+
+        std::error_code fs_error;
+        if (!std::filesystem::exists(serial_port_, fs_error))
+            return set_error(ErrorCode::SerialError);
+
+        auto channel = sl::createSerialPortChannel(serial_port_, static_cast<int>(baudrate_));
+        if (!channel) return set_error(ErrorCode::SerialError);
+        channel_ = *channel;
+
+        auto lidar = sl::createLidarDriver();
+        if (!lidar) { disconnect(); return set_error(ErrorCode::SerialError); }
+        lidar_ = *lidar;
+
+        if (!SL_IS_OK(lidar_->connect(channel_))) {
+            disconnect();
+            return set_error(ErrorCode::ConnectionFailed);
+        }
+
+        // Identification — failure here is non-fatal (fields stay empty).
+        detected_model_name_ = cfg_.name;
+        firmware_.clear();
+        sl_lidar_response_device_info_t info{};
+        if (SL_IS_OK(lidar_->getDeviceInfo(info))) {
+            char model_buf[32];
+            std::snprintf(model_buf, sizeof(model_buf), "slamtec-0x%02X",
+                          static_cast<unsigned>(info.model));
+            char firmware_buf[48];
+            std::snprintf(firmware_buf, sizeof(firmware_buf), "fw %u.%02u hw %u",
+                          static_cast<unsigned>(info.firmware_version >> 8),
+                          static_cast<unsigned>(info.firmware_version & 0xFF),
+                          static_cast<unsigned>(info.hardware_version));
+            detected_model_name_ = model_buf;
+            firmware_            = firmware_buf;
+        }
+
+        // Mandatory health check: a self-reported Fault means the data is not
+        // usable; Warning still runs but stays visible in diagnostics.
+        sl_lidar_response_device_health_t health{};
+        if (!SL_IS_OK(lidar_->getHealth(health)) || health.status == SL_LIDAR_STATUS_ERROR) {
+            health_status_ = SL_LIDAR_STATUS_ERROR;
+            health_error_code_ = static_cast<uint16_t>(health.error_code);
+            refresh_diag_from_health();
+            disconnect();
+            return set_error(ErrorCode::DeviceError);
+        }
+        health_status_     = health.status;
+        health_error_code_ = static_cast<uint16_t>(health.error_code);
+        refresh_diag_from_health();
+
+        // C1 spins the motor on the scan command; setMotorSpeed stays for
+        // DTR-controlled models (A-series).
+        (void)lidar_->setMotorSpeed();
+        sl::LidarScanMode scan_mode{};
+        if (!SL_IS_OK(lidar_->startScan(false, true, 0, &scan_mode))) {
+            (void)lidar_->setMotorSpeed(0);
+            disconnect();
+            return set_error(ErrorCode::HandshakeFailed);
+        }
+
+        have_last_grab_ = false;
+        return set_error(ErrorCode::Ok);
+    }
+
+    void close() override {
+        if (lidar_ != nullptr) {
+            (void)lidar_->stop();
+            (void)lidar_->setMotorSpeed(0);
+        }
+        disconnect();
+    }
+
+    // Blocks until the SDK hands over one full revolution.
+    bool recv_scan(ScanResult& out, int timeout_ms) override {
+        if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }
+
+        std::vector<sl_lidar_response_measurement_node_hq_t> nodes(kMaxNodesPerScan);
+        std::size_t count = nodes.size();
+        const auto grabbed = lidar_->grabScanDataHq(
+            nodes.data(), count,
+            timeout_ms > 0 ? static_cast<sl_u32>(timeout_ms) : kDefaultGrabTimeoutMs);
+        if (!SL_IS_OK(grabbed) || count < 2) {
+            set_error(grabbed == SL_RESULT_OPERATION_TIMEOUT ? ErrorCode::Timeout
+                                                             : ErrorCode::DeviceDisconnected);
+            return false;
+        }
+        (void)lidar_->ascendScanData(nodes.data(), count);
+
+        // Real rotation period = interval between consecutive grabs (~86 ms
+        // on a C1); the first frame uses the nominal value.
+        const auto grab_time = std::chrono::steady_clock::now();
+        const float scan_time = have_last_grab_
+            ? std::chrono::duration<float>(grab_time - last_grab_).count()
+            : kDefaultScanTimeS;
+        last_grab_      = grab_time;
+        have_last_grab_ = true;
+
+        const float angle_first = node_angle_rad(nodes.front());
+        const float angle_last  = node_angle_rad(nodes[count - 1]);
+        if (angle_last <= angle_first) {
+            set_error(ErrorCode::Timeout);  // malformed revolution — treat as a miss
+            return false;
+        }
+
+        LaserScan& scan = out.scan;
+        scan = LaserScan{};
+        // Inverted mount -> mirror the angles (angle' = 2π - angle) and walk
+        // the nodes backwards to keep ascending order.
+        if (inverted_) {
+            scan.angle_min = kTwoPi - angle_last;
+            scan.angle_max = kTwoPi - angle_first;
+        } else {
+            scan.angle_min = angle_first;
+            scan.angle_max = angle_last;
+        }
+        scan.angle_increment = (scan.angle_max - scan.angle_min) / static_cast<float>(count - 1);
+        scan.scan_time       = scan_time;
+        scan.time_increment  = scan_time / static_cast<float>(count);
+        scan.range_min       = cfg_.range_min_m;
+        scan.range_max       = cfg_.range_max_m;
+
+        // Valid FOV window — only filter when narrower than the full circle.
+        const bool apply_angle_window =
+            cfg_.fov_filter && (cfg_.fov_min_deg > -180.0F || cfg_.fov_max_deg < 180.0F);
+
+        scan.ranges.reserve(count);
+        scan.intensities.reserve(count);
+        for (std::size_t i = 0; i < count; ++i) {
+            const std::size_t node_index = inverted_ ? count - 1 - i : i;
+            // dist = 0 is the SDK's "no return" sentinel; together with
+            // out-of-range / out-of-window points it becomes NaN.
+            const float distance = node_distance_m(nodes[node_index]);
+            bool valid = nodes[node_index].dist_mm_q2 != 0 &&
+                         distance >= scan.range_min && distance <= scan.range_max;
+            if (valid && apply_angle_window) {
+                const float grid_angle = scan.angle_min + scan.angle_increment * static_cast<float>(i);
+                const float signed_deg = to_signed_deg(grid_angle);
+                valid = signed_deg >= cfg_.fov_min_deg && signed_deg <= cfg_.fov_max_deg;
+            }
+            scan.ranges.push_back(valid ? distance : std::numeric_limits<float>::quiet_NaN());
+            scan.intensities.push_back(static_cast<float>(nodes[node_index].quality));
+        }
+
+        if (cfg_.remap_angles)
+            remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max);
+
+        ExtraInfo& info = out.info;
+        info = ExtraInfo{};
+        info.detected_model        = detected_model_name_;
+        info.rplidar_health_status = health_status_;
+        info.rplidar_error_code    = health_error_code_;
+
+        latest_diag_          = decode_diagnostics(info);
+        latest_diag_.firmware = firmware_;
+        mark_scan_decoded();
+
+        set_error(ErrorCode::Ok);
+        return true;
+    }
+
+    void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
+
+    // One unit of input == one revolution for this device.
+    bool spin_once() override {
+        ScanResult result;
+        if (!recv_scan(result, kDefaultGrabTimeoutMs)) return false;
+        if (cb_) cb_(result);
+        return true;
+    }
+
+    bool is_open() const override { return lidar_ != nullptr; }
+
+    const char* detected_model() const override { return detected_model_name_.c_str(); }
+
+    Diagnostics get_diagnostics() const override { return latest_diag_; }
+
+private:
+    // Health snapshot -> diagnostics, so a fault is visible before the first
+    // scan (valid = true means "health was read", not "a scan was decoded").
+    void refresh_diag_from_health() {
+        ExtraInfo info;
+        info.detected_model        = detected_model_name_;
+        info.rplidar_health_status = health_status_;
+        info.rplidar_error_code    = health_error_code_;
+        latest_diag_          = decode_diagnostics(info);
+        latest_diag_.firmware = firmware_;
+    }
+
+    // The SDK factories hand out raw pointers and require the caller to
+    // delete them (see sl_lidar_driver.h) — this is the only place doing so.
+    void disconnect() {
+        if (lidar_ != nullptr) { delete lidar_; lidar_ = nullptr; }
+        if (channel_ != nullptr) { delete channel_; channel_ = nullptr; }
+    }
+
+    ModelConfig  cfg_;
+    std::string  serial_port_;
+    uint32_t     baudrate_;
+    bool         inverted_ = false;
+    ScanCallback cb_;
+
+    std::string detected_model_name_ = "AUTO";
+    std::string firmware_;
+    std::optional<uint8_t>  health_status_;
+    std::optional<uint16_t> health_error_code_;
+
+    Diagnostics latest_diag_;
+
+    std::chrono::steady_clock::time_point last_grab_{};
+    bool have_last_grab_ = false;
+
+    sl::ILidarDriver* lidar_   = nullptr;
+    sl::IChannel*     channel_ = nullptr;
+};
+
+} // namespace xlidar
+
+XLIDAR_PLUGIN_EXPORT void get_driver_info(xlidar::DriverInfo* out) {
+    *out = xlidar::kDriverInfo;
+}
+
+XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface*
+create_driver_instance(const xlidar::DeviceConfig* cfg) {
+    using namespace xlidar;
+    if (!transport_supported(kDriverInfo, *cfg))
+        return new InvalidConfigDriver(kDriverInfo,
+            std::string("unsupported transport '") + to_string(*cfg->transport) + "'");
+    // "AUTO" and "C1" share the same preset; the real model is read from the
+    // device at open().
+    const uint32_t baud = cfg->baudrate ? cfg->baudrate : 460800;
+    return new RplidarDriver(apply_device_config(MODEL_RPLIDAR_C1, *cfg),
+                             cfg->serial_port, baud, cfg->inverted);
+}

plugins/driver_sick_safety/CMakeLists.txt

@@ -0,0 +1 @@
+xlidar_add_plugin(driver_sick_safety sick_safety_driver.cpp)

plugins/driver_sick_safety/sick_safety_driver.cpp

@@ -0,0 +1,264 @@
+// SICK nanoScan3 / microScan3 — binary safety-data UDP packets, with
+// application-layer "MS3 " fragment reassembly.
+#include "sick_safety_driver.hpp"
+#include "plugin_helpers.hpp"
+
+#include <cerrno>
+#include <cmath>
+#include <cstring>
+#include <limits>
+#include <arpa/inet.h>
+#include <netinet/in.h>
+#include <sys/select.h>
+#include <sys/socket.h>
+#include <unistd.h>
+
+namespace xlidar {
+
+namespace {
+constexpr size_t kNanoRecvBufSize = 65536;
+// nanoScan3 DerivedValues store angles as int32 in 1/4194304 degree.
+constexpr double kNanoAngleResolution = 4194304.0;
+} // namespace
+
+SickSafetyDriver::SickSafetyDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port,
+                                   bool inverted)
+    : cfg_(cfg), detected_model_name_(cfg.name ? cfg.name : ""), ip_(ip), port_(port),
+      inverted_(inverted), recv_buf_(kNanoRecvBufSize) {}
+
+SickSafetyDriver::~SickSafetyDriver() { close(); }
+
+ErrorCode SickSafetyDriver::open() {
+    if (is_open()) return set_error(ErrorCode::AlreadyOpen);
+
+    sockaddr_in addr{};
+    addr.sin_family = AF_INET;
+    addr.sin_port   = htons(port_);
+    if (ip_ == "0.0.0.0" || ip_.empty()) {
+        addr.sin_addr.s_addr = INADDR_ANY;
+    } else if (::inet_pton(AF_INET, ip_.c_str(), &addr.sin_addr) != 1) {
+        return set_error(ErrorCode::InvalidAddress);
+    }
+
+    sock_fd_ = ::socket(AF_INET, SOCK_DGRAM, 0);
+    if (sock_fd_ < 0) return set_error(ErrorCode::SocketError);
+
+    // No SO_REUSEADDR: UDP has no TIME_WAIT, and on Linux it would let two
+    // sockets bind the same port, hiding PortInUse from the second app.
+    if (::bind(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0) {
+        int err = errno;
+        ::close(sock_fd_);
+        sock_fd_ = -1;
+        return set_error((err == EADDRINUSE || err == EACCES) ? ErrorCode::PortInUse
+                                                              : ErrorCode::BindFailed);
+    }
+    latest_diag_ = Diagnostics{};
+    return set_error(ErrorCode::Ok);
+}
+
+void SickSafetyDriver::close() {
+    if (sock_fd_ >= 0) {
+        ::close(sock_fd_);
+        sock_fd_ = -1;
+    }
+}
+
+int SickSafetyDriver::recv_datagram(int timeout_ms) {
+    if (!is_open()) { set_error(ErrorCode::NotOpen); return -1; }
+
+    if (timeout_ms > 0) {
+        fd_set fds; FD_ZERO(&fds); FD_SET(sock_fd_, &fds);
+        timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 };
+        int r = ::select(sock_fd_ + 1, &fds, nullptr, nullptr, &tv);
+        if (r <= 0) {
+            set_error(r == 0 ? ErrorCode::Timeout : ErrorCode::DeviceDisconnected);
+            return -1;
+        }
+    }
+
+    ssize_t n = ::recv(sock_fd_, recv_buf_.data(), recv_buf_.size(), 0);
+    if (n <= 0) { set_error(ErrorCode::DeviceDisconnected); return -1; }
+    return static_cast<int>(n);
+}
+
+// A scan is split across datagrams at the application layer. Each starts with
+// a 24-byte fragment header: "MS3 " @0, u32 totalLength @8, u32 scanNumber @12,
+// u32 fragmentOffset @16. Reassemble until totalLength bytes; a lost fragment
+// drops that scan and we resync on the next scanNumber.
+bool SickSafetyDriver::recv_scan(ScanResult& out, int timeout_ms) {
+    std::vector<uint8_t> tele;
+    std::vector<uint8_t> have;  // per-byte coverage so duplicate fragments don't count twice
+    uint32_t cur_scan = 0, total = 0, got = 0;
+    bool assembling = false;
+
+    for (;;) {
+        int n = recv_datagram(timeout_ms);
+        if (n < 0) return false;
+        const uint8_t* d = recv_buf_.data();
+
+        if (n < 24 || std::memcmp(d, "MS3 ", 4) != 0) {
+            if (parse_packet(d, n, out)) { set_error(ErrorCode::Ok); return true; }
+            continue;
+        }
+
+        uint32_t tl    = le32(d + 8);
+        uint32_t scan  = le32(d + 12);
+        uint32_t foff  = le32(d + 16);
+        const uint8_t* pl = d + 24;
+        uint32_t pl_len   = static_cast<uint32_t>(n) - 24;
+        if (tl == 0 || tl > kNanoRecvBufSize) continue;
+
+        if (!assembling || scan != cur_scan || tl != total) {
+            cur_scan = scan; total = tl; got = 0;
+            tele.assign(total, 0);
+            have.assign(total, 0);
+            assembling = true;
+        }
+
+        if (static_cast<uint64_t>(foff) + pl_len <= total) {
+            std::memcpy(tele.data() + foff, pl, pl_len);
+            for (uint32_t b = 0; b < pl_len; ++b)
+                if (!have[foff + b]) { have[foff + b] = 1; ++got; }
+        }
+
+        if (got >= total) {
+            assembling = false;
+            if (parse_packet(tele.data(), static_cast<int>(total), out)) {
+                set_error(ErrorCode::Ok);
+                return true;
+            }
+        }
+    }
+}
+
+bool SickSafetyDriver::spin_once() {
+    int n = recv_datagram(0);
+    if (n < 0) return false;
+
+    ScanResult result;
+    if (!parse_packet(recv_buf_.data(), n, result)) return true;
+    if (cb_) cb_(result);
+    return true;
+}
+
+// SICK safety-scanner data packet (LE), layout ported from sick_safetyscanners:
+// DataHeader offset table at fixed offsets (derivedValues @36, measurementData
+// @40); DerivedValues holds multiplicationFactor/startAngle/resolution;
+// MeasurementData is u32 numBeams then 4 B/beam (u16 dist, u8 reflect, u8 status).
+bool SickSafetyDriver::parse_packet(const uint8_t* buf, int len, ScanResult& out) {
+    if (len < 52) return false;
+
+    uint16_t gss_off = le16(buf + 32);  // General System State block
+    uint16_t gss_size = le16(buf + 34);
+    uint16_t dv_off  = le16(buf + 36);
+    uint16_t dv_size = le16(buf + 38);
+    uint16_t md_off  = le16(buf + 40);
+    uint16_t md_size = le16(buf + 42);
+
+    if (dv_off == 0 || dv_size == 0 || md_off == 0 || md_size == 0) return false;
+    if (static_cast<int>(dv_off) + 20 > len) return false;
+    if (static_cast<int>(md_off) + 4  > len) return false;
+
+    const uint8_t* dv = buf + dv_off;
+    uint16_t mult_factor = le16(dv + 0);
+    int32_t  start_raw   = le_i32(dv + 8);
+    int32_t  res_raw     = le_i32(dv + 12);
+    if (mult_factor == 0) mult_factor = 1;
+
+    double start_deg = static_cast<double>(start_raw) / kNanoAngleResolution;
+    double res_deg   = static_cast<double>(res_raw)   / kNanoAngleResolution;
+
+    const uint8_t* md   = buf + md_off;
+    uint32_t num_beams  = le32(md + 0);
+    if (num_beams == 0 || num_beams > 2751) return false;  // 2751 = sensor max
+    if (static_cast<int64_t>(md_off) + 4 + static_cast<int64_t>(num_beams) * 4 > len)
+        return false;
+
+    LaserScan& scan = out.scan;
+    scan.ranges.assign(num_beams, 0.f);
+    scan.intensities.assign(num_beams, 0.f);
+
+    for (uint32_t i = 0; i < num_beams; ++i) {
+        const uint8_t* p   = md + 4 + i * 4;
+        uint16_t distance  = le16(p + 0);
+        uint8_t  reflect   = le_u8(p + 2);
+        uint8_t  status    = le_u8(p + 3);
+        bool     valid     = (status & 0x01) != 0;
+        bool     infinite  = (status & 0x02) != 0;
+
+        if (!valid || infinite) {
+            scan.ranges[i] = std::numeric_limits<float>::infinity();
+        } else {
+            scan.ranges[i] = static_cast<float>(distance) *
+                             static_cast<float>(mult_factor) * 1e-3f;  // mm -> m
+        }
+        scan.intensities[i] = static_cast<float>(reflect);
+    }
+
+    scan.angle_min       = (static_cast<float>(start_deg) + cfg_.angle_offset_deg) * kDeg2Rad;
+    scan.angle_increment = static_cast<float>(res_deg   * kDeg2Rad);
+    scan.angle_max       = scan.angle_min +
+        scan.angle_increment * static_cast<float>(num_beams - 1);
+    scan.time_increment  = 0.f;
+    scan.scan_time       = 0.f;
+    scan.range_min       = cfg_.range_min_m;
+    scan.range_max       = cfg_.range_max_m;
+    // Raw device time from the DataHeader — an opaque tag, not ms since power-on.
+    scan.timestamp_ms    = le32(buf + 28);
+
+    finalize_scan(scan, cfg_, inverted_);
+
+    ExtraInfo& info = out.info;
+    info = ExtraInfo{};
+    info.detected_model = cfg_.name;
+    // Byte 0 holds the run/standby/contamination/manipulation flags
+    // (kNanoState*); the block is absent when not configured in the sensor.
+    if (gss_off != 0 && gss_size != 0 && static_cast<int>(gss_off) < len)
+        info.nano_general_state = buf[gss_off];
+
+    latest_diag_ = decode_diagnostics(info);
+    latest_diag_.device_timestamp_ms = scan.timestamp_ms;
+    mark_scan_decoded();
+
+    return true;
+}
+
+// ── plugin registration ─────────────────────────────────────────────────────
+
+namespace {
+
+const DriverInfo kDriverInfo = [] {
+    DriverInfo info;
+    info.vendor      = "SICK";
+    info.model       = "nanoScan3/microScan3";
+    info.driver_id   = "sick_nanoscan3_driver";
+    info.description = "SICK safety laser scanners (nanoScan3/microScan3 family) "
+                       "— passive receiver of the binary safety-data UDP output; "
+                       "the sensor's UDP target must be configured in SICK Safety "
+                       "Designer. Default local port 6060. Not verified on real "
+                       "hardware.";
+    info.transport   = Transport::Udp;
+    info.supported_models = {"SICK-nanoScan3"};
+    return info;
+}();
+
+} // namespace
+
+DriverInfo SickSafetyDriver::get_driver_info() const { return kDriverInfo; }
+
+} // namespace xlidar
+
+XLIDAR_PLUGIN_EXPORT void get_driver_info(xlidar::DriverInfo* out) {
+    *out = xlidar::kDriverInfo;
+}
+
+XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface*
+create_driver_instance(const xlidar::DeviceConfig* cfg) {
+    using namespace xlidar;
+    if (!transport_supported(kDriverInfo, *cfg))
+        return new InvalidConfigDriver(kDriverInfo,
+            std::string("unsupported transport '") + to_string(*cfg->transport) + "'");
+    const uint16_t port = cfg->port ? cfg->port : 6060;
+    return new SickSafetyDriver(apply_device_config(MODEL_SICK_NANOSCAN3, *cfg),
+                                cfg->ip, port, cfg->inverted);
+}

plugins/driver_sick_safety/sick_safety_driver.hpp

@@ -0,0 +1,63 @@
+// SICK nanoScan3 / microScan3 safety scanners over UDP — plugin-private header.
+#pragma once
+#include "lidar_interface.hpp"
+
+#include <cstdint>
+#include <string>
+#include <vector>
+
+namespace xlidar {
+
+inline constexpr ModelConfig MODEL_SICK_NANOSCAN3 { "SICK-nanoScan3", -137.5f, 137.5f, 0.05f, 40.f };
+
+// SICK nanoScan3 / microScan3 safety-scanner binary UDP output. Layout ported
+// from SICK's open-source sick_safetyscanners; NOT verified on real hardware.
+// Passive UDP receiver: the sensor's UDP target must be configured up front in
+// SICK Safety Designer — this class does no CoLa2/TCP handshake.
+class SickSafetyDriver : public LidarDriverInterface {
+public:
+    // ip: local bind address; port: local UDP port the sensor sends to;
+    // inverted: unit mounted upside-down → mirror the scan.
+    explicit SickSafetyDriver(const ModelConfig& cfg,
+                              const std::string& ip       = "0.0.0.0",
+                              uint16_t            port     = 6060,
+                              bool                inverted = false);
+    ~SickSafetyDriver();
+
+    SickSafetyDriver(const SickSafetyDriver&)            = delete;
+    SickSafetyDriver& operator=(const SickSafetyDriver&) = delete;
+
+    DriverInfo get_driver_info() const override;
+
+    ErrorCode open() override;
+    void close() override;
+    bool recv_scan(ScanResult& out, int timeout_ms = 1000) override;
+    void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
+    bool spin_once() override;
+    bool is_open() const override { return sock_fd_ >= 0; }
+
+    // No model string on the wire — returns the configured name.
+    const char* detected_model() const override { return detected_model_name_.c_str(); }
+
+    Diagnostics get_diagnostics() const override { return latest_diag_; }
+
+private:
+    int  recv_datagram(int timeout_ms);
+    bool parse_packet(const uint8_t* buf, int len, ScanResult& out);
+
+    ModelConfig  cfg_;
+    std::string  detected_model_name_;  // owned copy of cfg_.name (stable lifetime)
+    std::string  ip_;
+    uint16_t     port_;
+    bool         inverted_ = false;
+    int          sock_fd_ = -1;
+    ScanCallback cb_;
+
+    // Snapshot for get_diagnostics(); refreshed by parse_packet().
+    Diagnostics latest_diag_;
+
+    // Per-instance; sized for a full safety-data packet (max ~2751 beams).
+    std::vector<uint8_t> recv_buf_;
+};
+
+} // namespace xlidar

plugins/driver_sick_tim/CMakeLists.txt

@@ -0,0 +1 @@
+xlidar_add_plugin(driver_sick_tim sick_tim_driver.cpp)

plugins/driver_sick_tim/sick_tim_driver.cpp

@@ -1,21 +1,20 @@
-#include "lidarlib/sick_lidar.hpp"
+// SICK TiM 5xx/7xx — SOPAS/CoLa-A ASCII telegrams over TCP ("sSN/sRA
-#include "lidar_bytes.hpp"
+// LMDscandata" parsing).
-#include "lidar_net.hpp"
+#include "sick_tim_driver.hpp"
+#include "plugin_helpers.hpp"
 
 #include <cctype>
 #include <cerrno>
-#include <cmath>
 #include <cstdlib>
 #include <cstring>
-#include <limits>
 #include <vector>
-#include <sys/socket.h>
 #include <arpa/inet.h>
-#include <unistd.h>
-#include <sys/select.h>
 #include <netinet/in.h>
+#include <sys/select.h>
+#include <sys/socket.h>
+#include <unistd.h>
 
-namespace lidarlib {
+namespace xlidar {
 
 namespace {
 constexpr char  kStx              = 0x02;
@@ -41,20 +40,16 @@ std::vector<std::string> tokenize(const std::string& s) {
     }
     return out;
 }
-
-constexpr size_t kNanoRecvBufSize = 65536;
-// nanoScan3 DerivedValues store angles as int32 in 1/4194304 degree.
-constexpr double kNanoAngleResolution = 4194304.0;
 } // namespace
 
-SickDriver::SickDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port,
+SickTimDriver::SickTimDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port,
-                       bool inverted)
+                               bool inverted)
     : cfg_(cfg), detected_model_name_(cfg.name ? cfg.name : ""), ip_(ip), port_(port),
       inverted_(inverted) {}
 
-SickDriver::~SickDriver() { close(); }
+SickTimDriver::~SickTimDriver() { close(); }
 
-ErrorCode SickDriver::open() {
+ErrorCode SickTimDriver::open() {
     if (is_open()) return set_error(ErrorCode::AlreadyOpen);
 
     sockaddr_in addr{};
@@ -84,7 +79,7 @@ ErrorCode SickDriver::open() {
     return set_error(ErrorCode::Ok);
 }
 
-void SickDriver::close() {
+void SickTimDriver::close() {
     if (sock_fd_ >= 0) {
         send_telegram("sEN LMDscandata 0");  // best-effort
         ::close(sock_fd_);
@@ -92,7 +87,7 @@ void SickDriver::close() {
     }
 }
 
-bool SickDriver::send_telegram(const std::string& body) {
+bool SickTimDriver::send_telegram(const std::string& body) {
     if (sock_fd_ < 0) return false;
     std::string framed;
     framed.reserve(body.size() + 2);
@@ -111,7 +106,7 @@ bool SickDriver::send_telegram(const std::string& body) {
 
 // CoLa-A has no length prefix, so ETX is the only frame boundary; recv_buf_
 // carries leftover bytes across calls.
-bool SickDriver::read_telegram(std::string& out, int timeout_ms) {
+bool SickTimDriver::read_telegram(std::string& out, int timeout_ms) {
     if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }
 
     for (;;) {
@@ -144,7 +139,7 @@ bool SickDriver::read_telegram(std::string& out, int timeout_ms) {
     }
 }
 
-bool SickDriver::recv_scan(ScanResult& out, int timeout_ms) {
+bool SickTimDriver::recv_scan(ScanResult& out, int timeout_ms) {
     for (;;) {
         std::string telegram;
         if (!read_telegram(telegram, timeout_ms)) return false;
@@ -153,7 +148,7 @@ bool SickDriver::recv_scan(ScanResult& out, int timeout_ms) {
     }
 }
 
-bool SickDriver::spin_once() {
+bool SickTimDriver::spin_once() {
     std::string telegram;
     if (!read_telegram(telegram, 0)) return false;
 
@@ -165,7 +160,7 @@ bool SickDriver::spin_once() {
 
 // CoLa-A "sSN/sRA LMDscandata": space-separated ASCII hex tokens, field order
 // per SICK's Telegram Listing. "DIST1" → ranges, "RSSI1" → intensities.
-bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out) {
+bool SickTimDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out) {
     std::vector<std::string> tok = tokenize(telegram);
     if (tok.size() < 20) return false;
     if (tok[0] != "sSN" && tok[0] != "sRA") return false;
@@ -259,10 +254,7 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out)
     if (scan.intensities.size() != scan.ranges.size())
         scan.intensities.assign(scan.ranges.size(), 0.f);
 
-    if (inverted_)
+    finalize_scan(scan, cfg_, inverted_);
-        invert_scan(scan);
-    if (cfg_.remap_angles)
-        remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max);
 
     ExtraInfo& info = out.info;
     info = ExtraInfo{};
@@ -280,211 +272,51 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out)
     return true;
 }
 
-// ── NanoScanDriver — SICK nanoScan3/microScan3 safety-scanner UDP output ────
+// ── plugin registration ─────────────────────────────────────────────────────
 
-NanoScanDriver::NanoScanDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port,
+namespace {
-                               bool inverted)
-    : cfg_(cfg), detected_model_name_(cfg.name ? cfg.name : ""), ip_(ip), port_(port),
-      inverted_(inverted), recv_buf_(kNanoRecvBufSize) {}
 
-NanoScanDriver::~NanoScanDriver() { close(); }
+const DriverInfo kDriverInfo = [] {
+    DriverInfo info;
+    info.vendor      = "SICK";
+    info.model       = "TiM5xx/TiM7xx";
+    info.driver_id   = "sick_tim_driver";
+    info.description = "SICK TiM 2D lidars (TiM551/561/571/781, ...) over "
+                       "SOPAS/CoLa-A ASCII telegrams on TCP. open() starts the "
+                       "LMDscandata stream. Default port 2111. Verified on a "
+                       "real TiM781S.";
+    info.transport   = Transport::Tcp;
+    info.supported_models = {"SICK-TIM5xx", "SICK-TIM571", "SICK-TIM7xx"};
+    return info;
+}();
 
-ErrorCode NanoScanDriver::open() {
+const ModelConfig* model_by_name(const std::string& name) {
-    if (is_open()) return set_error(ErrorCode::AlreadyOpen);
+    static constexpr const ModelConfig* kModels[] = {
-
+        &MODEL_SICK_TIM5XX, &MODEL_SICK_TIM571, &MODEL_SICK_TIM7XX,
-    sockaddr_in addr{};
+    };
-    addr.sin_family = AF_INET;
+    for (const ModelConfig* m : kModels)
-    addr.sin_port   = htons(port_);
+        if (name == m->name) return m;
-    if (ip_ == "0.0.0.0" || ip_.empty()) {
+    return nullptr;
-        addr.sin_addr.s_addr = INADDR_ANY;
-    } else if (::inet_pton(AF_INET, ip_.c_str(), &addr.sin_addr) != 1) {
-        return set_error(ErrorCode::InvalidAddress);
-    }
-
-    sock_fd_ = ::socket(AF_INET, SOCK_DGRAM, 0);
-    if (sock_fd_ < 0) return set_error(ErrorCode::SocketError);
-
-    // No SO_REUSEADDR: UDP has no TIME_WAIT, and on Linux it would let two
-    // sockets bind the same port, hiding PortInUse from the second app.
-    if (::bind(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0) {
-        int err = errno;
-        ::close(sock_fd_);
-        sock_fd_ = -1;
-        return set_error((err == EADDRINUSE || err == EACCES) ? ErrorCode::PortInUse
-                                                              : ErrorCode::BindFailed);
-    }
-    latest_diag_ = Diagnostics{};
-    return set_error(ErrorCode::Ok);
 }
 
-void NanoScanDriver::close() {
+} // namespace
-    if (sock_fd_ >= 0) {
+
-        ::close(sock_fd_);
+DriverInfo SickTimDriver::get_driver_info() const { return kDriverInfo; }
-        sock_fd_ = -1;
+
-    }
+} // namespace xlidar
+
+XLIDAR_PLUGIN_EXPORT void get_driver_info(xlidar::DriverInfo* out) {
+    *out = xlidar::kDriverInfo;
 }
 
-int NanoScanDriver::recv_datagram(int timeout_ms) {
+XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface*
-    if (!is_open()) { set_error(ErrorCode::NotOpen); return -1; }
+create_driver_instance(const xlidar::DeviceConfig* cfg) {
-
+    using namespace xlidar;
-    if (timeout_ms > 0) {
+    if (!transport_supported(kDriverInfo, *cfg))
-        fd_set fds; FD_ZERO(&fds); FD_SET(sock_fd_, &fds);
+        return new InvalidConfigDriver(kDriverInfo,
-        timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 };
+            std::string("unsupported transport '") + to_string(*cfg->transport) + "'");
-        int r = ::select(sock_fd_ + 1, &fds, nullptr, nullptr, &tv);
+    const ModelConfig* preset = model_by_name(cfg->model);
-        if (r <= 0) {
+    if (!preset) preset = &MODEL_SICK_TIM571;  // brand default
-            set_error(r == 0 ? ErrorCode::Timeout : ErrorCode::DeviceDisconnected);
+    const uint16_t port = cfg->port ? cfg->port : 2111;
-            return -1;
+    return new SickTimDriver(apply_device_config(*preset, *cfg), cfg->ip, port, cfg->inverted);
-        }
-    }
-
-    ssize_t n = ::recv(sock_fd_, recv_buf_.data(), recv_buf_.size(), 0);
-    if (n <= 0) { set_error(ErrorCode::DeviceDisconnected); return -1; }
-    return static_cast<int>(n);
 }
-
-// A scan is split across datagrams at the application layer. Each starts with
-// a 24-byte fragment header: "MS3 " @0, u32 totalLength @8, u32 scanNumber @12,
-// u32 fragmentOffset @16. Reassemble until totalLength bytes; a lost fragment
-// drops that scan and we resync on the next scanNumber.
-bool NanoScanDriver::recv_scan(ScanResult& out, int timeout_ms) {
-    std::vector<uint8_t> tele;
-    std::vector<uint8_t> have;  // per-byte coverage so duplicate fragments don't count twice
-    uint32_t cur_scan = 0, total = 0, got = 0;
-    bool assembling = false;
-
-    for (;;) {
-        int n = recv_datagram(timeout_ms);
-        if (n < 0) return false;
-        const uint8_t* d = recv_buf_.data();
-
-        if (n < 24 || std::memcmp(d, "MS3 ", 4) != 0) {
-            if (parse_packet(d, n, out)) { set_error(ErrorCode::Ok); return true; }
-            continue;
-        }
-
-        uint32_t tl    = le32(d + 8);
-        uint32_t scan  = le32(d + 12);
-        uint32_t foff  = le32(d + 16);
-        const uint8_t* pl = d + 24;
-        uint32_t pl_len   = static_cast<uint32_t>(n) - 24;
-        if (tl == 0 || tl > kNanoRecvBufSize) continue;
-
-        if (!assembling || scan != cur_scan || tl != total) {
-            cur_scan = scan; total = tl; got = 0;
-            tele.assign(total, 0);
-            have.assign(total, 0);
-            assembling = true;
-        }
-
-        if (static_cast<uint64_t>(foff) + pl_len <= total) {
-            std::memcpy(tele.data() + foff, pl, pl_len);
-            for (uint32_t b = 0; b < pl_len; ++b)
-                if (!have[foff + b]) { have[foff + b] = 1; ++got; }
-        }
-
-        if (got >= total) {
-            assembling = false;
-            if (parse_packet(tele.data(), static_cast<int>(total), out)) {
-                set_error(ErrorCode::Ok);
-                return true;
-            }
-        }
-    }
-}
-
-bool NanoScanDriver::spin_once() {
-    int n = recv_datagram(0);
-    if (n < 0) return false;
-
-    ScanResult result;
-    if (!parse_packet(recv_buf_.data(), n, result)) return true;
-    if (cb_) cb_(result);
-    return true;
-}
-
-// SICK safety-scanner data packet (LE), layout ported from sick_safetyscanners:
-// DataHeader offset table at fixed offsets (derivedValues @36, measurementData
-// @40); DerivedValues holds multiplicationFactor/startAngle/resolution;
-// MeasurementData is u32 numBeams then 4 B/beam (u16 dist, u8 reflect, u8 status).
-bool NanoScanDriver::parse_packet(const uint8_t* buf, int len, ScanResult& out) {
-    if (len < 52) return false;
-
-    uint16_t gss_off = le16(buf + 32);  // General System State block
-    uint16_t gss_size = le16(buf + 34);
-    uint16_t dv_off  = le16(buf + 36);
-    uint16_t dv_size = le16(buf + 38);
-    uint16_t md_off  = le16(buf + 40);
-    uint16_t md_size = le16(buf + 42);
-
-    if (dv_off == 0 || dv_size == 0 || md_off == 0 || md_size == 0) return false;
-    if (static_cast<int>(dv_off) + 20 > len) return false;
-    if (static_cast<int>(md_off) + 4  > len) return false;
-
-    const uint8_t* dv = buf + dv_off;
-    uint16_t mult_factor = le16(dv + 0);
-    int32_t  start_raw   = le_i32(dv + 8);
-    int32_t  res_raw     = le_i32(dv + 12);
-    if (mult_factor == 0) mult_factor = 1;
-
-    double start_deg = static_cast<double>(start_raw) / kNanoAngleResolution;
-    double res_deg   = static_cast<double>(res_raw)   / kNanoAngleResolution;
-
-    const uint8_t* md   = buf + md_off;
-    uint32_t num_beams  = le32(md + 0);
-    if (num_beams == 0 || num_beams > 2751) return false;  // 2751 = sensor max
-    if (static_cast<int64_t>(md_off) + 4 + static_cast<int64_t>(num_beams) * 4 > len)
-        return false;
-
-    LaserScan& scan = out.scan;
-    scan.ranges.assign(num_beams, 0.f);
-    scan.intensities.assign(num_beams, 0.f);
-
-    for (uint32_t i = 0; i < num_beams; ++i) {
-        const uint8_t* p   = md + 4 + i * 4;
-        uint16_t distance  = le16(p + 0);
-        uint8_t  reflect   = le_u8(p + 2);
-        uint8_t  status    = le_u8(p + 3);
-        bool     valid     = (status & 0x01) != 0;
-        bool     infinite  = (status & 0x02) != 0;
-
-        if (!valid || infinite) {
-            scan.ranges[i] = std::numeric_limits<float>::infinity();
-        } else {
-            scan.ranges[i] = static_cast<float>(distance) *
-                             static_cast<float>(mult_factor) * 1e-3f;  // mm -> m
-        }
-        scan.intensities[i] = static_cast<float>(reflect);
-    }
-
-    scan.angle_min       = (static_cast<float>(start_deg) + cfg_.angle_offset_deg) * kDeg2Rad;
-    scan.angle_increment = static_cast<float>(res_deg   * kDeg2Rad);
-    scan.angle_max       = scan.angle_min +
-        scan.angle_increment * static_cast<float>(num_beams - 1);
-    scan.time_increment  = 0.f;
-    scan.scan_time       = 0.f;
-    scan.range_min       = cfg_.range_min_m;
-    scan.range_max       = cfg_.range_max_m;
-    // Raw device time from the DataHeader — an opaque tag, not ms since power-on.
-    scan.timestamp_ms    = le32(buf + 28);
-
-    if (inverted_)
-        invert_scan(scan);
-    if (cfg_.remap_angles)
-        remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max);
-
-    ExtraInfo& info = out.info;
-    info = ExtraInfo{};
-    info.detected_model = cfg_.name;
-    // Byte 0 holds the run/standby/contamination/manipulation flags
-    // (kNanoState*); the block is absent when not configured in the sensor.
-    if (gss_off != 0 && gss_size != 0 && static_cast<int>(gss_off) < len)
-        info.nano_general_state = buf[gss_off];
-
-    latest_diag_ = decode_diagnostics(info);
-    latest_diag_.device_timestamp_ms = scan.timestamp_ms;
-    mark_scan_decoded();
-
-    return true;
-}
-
-} // namespace lidarlib

plugins/driver_sick_tim/sick_tim_driver.hpp

@@ -0,0 +1,67 @@
+// SICK TiM 5xx/7xx over SOPAS/CoLa-A (TCP) — plugin-private header.
+#pragma once
+#include "lidar_interface.hpp"
+
+#include <cstdint>
+#include <string>
+
+namespace xlidar {
+
+// SICK TiM presets. FOV/range from datasheets; scan_angle_* are informational
+// only and do NOT filter points. angle_offset_deg = -90 because the TiM wire
+// frame puts 90° at the device front.
+inline constexpr ModelConfig MODEL_SICK_TIM5XX { "SICK-TIM5xx", -135.f, 135.f, 0.05f, 10.f, -90.f }; // TiM551/561, 270°, 10m
+inline constexpr ModelConfig MODEL_SICK_TIM571 { "SICK-TIM571", -135.f, 135.f, 0.05f, 25.f, -90.f }; // TiM571,      270°, 25m
+inline constexpr ModelConfig MODEL_SICK_TIM7XX { "SICK-TIM7xx", -135.f, 135.f, 0.05f, 25.f, -90.f }; // TiM781,      270°, 25m
+
+// SICK TiM5xx/7xx over SOPAS/CoLa-A (TCP, default port 2111).
+// Verified against a real TiM781S (FW V5.11). NOT verified: NumEncoders > 0,
+// the 8-bit channel branch, and the TIM5xx/TIM571 FOV/range numbers.
+class SickTimDriver : public LidarDriverInterface {
+public:
+    // inverted: unit mounted upside-down → mirror the scan.
+    explicit SickTimDriver(const ModelConfig& cfg,
+                            const std::string& ip,
+                            uint16_t            port     = 2111,
+                            bool                inverted = false);
+    ~SickTimDriver();
+
+    SickTimDriver(const SickTimDriver&)            = delete;
+    SickTimDriver& operator=(const SickTimDriver&) = delete;
+
+    DriverInfo get_driver_info() const override;
+
+    // Connect + send "sEN LMDscandata 1" to start continuous scan output.
+    ErrorCode open() override;
+    void close() override;
+    bool recv_scan(ScanResult& out, int timeout_ms = 2000) override;
+    void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
+    bool spin_once() override;
+    bool is_open() const override { return sock_fd_ >= 0; }
+
+    // No model string on the wire — returns the configured name.
+    const char* detected_model() const override { return detected_model_name_.c_str(); }
+
+    Diagnostics get_diagnostics() const override { return latest_diag_; }
+
+private:
+    bool send_telegram(const std::string& body);
+    bool read_telegram(std::string& out, int timeout_ms);
+    bool parse_lmdscandata(const std::string& telegram, ScanResult& out);
+
+    ModelConfig  cfg_;
+    std::string  detected_model_name_;  // owned copy of cfg_.name (stable lifetime)
+    std::string  ip_;
+    uint16_t     port_;
+    bool         inverted_ = false;
+    int          sock_fd_ = -1;
+    ScanCallback cb_;
+
+    // Snapshot for get_diagnostics(); refreshed by parse_lmdscandata().
+    Diagnostics latest_diag_;
+
+    // Leftover TCP bytes carried across telegram boundaries; per-instance.
+    std::string recv_buf_;
+};
+
+} // namespace xlidar

src/CMakeLists.txt

@@ -0,0 +1,13 @@
+# liblidar_manager.so — the host-facing facade (plugin loader + config).
+add_library(lidar_manager SHARED lidar_manager.cpp)
+set_target_properties(lidar_manager PROPERTIES
+    OUTPUT_NAME lidar_manager
+    VERSION ${PROJECT_VERSION}
+    SOVERSION ${PROJECT_VERSION_MAJOR}
+    POSITION_INDEPENDENT_CODE ON
+)
+target_include_directories(lidar_manager PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include/xlidar>
+)
+target_link_libraries(lidar_manager PUBLIC Threads::Threads PRIVATE ${CMAKE_DL_LIBS})

src/lidar_bytes.hpp

@@ -1,53 +0,0 @@
-// Internal helpers shared by the driver TUs — not part of the public API.
-#pragma once
-#include "lidarlib/lidar.hpp"
-#include <algorithm>
-#include <cstdint>
-#include <cstring>
-
-namespace lidarlib {
-
-inline constexpr float kDeg2Rad = 3.14159265358979323846f / 180.f;
-
-// Remap a finished scan's angular window onto [min_deg, max_deg]. Only
-// angle_min/angle_max/angle_increment are rewritten; points are untouched.
-inline void remap_scan_window(LaserScan& scan, float min_deg, float max_deg) {
-    const float new_min  = min_deg * kDeg2Rad;
-    const float new_max  = max_deg * kDeg2Rad;
-    const float old_span = scan.angle_max - scan.angle_min;
-    if (old_span > 0.f)
-        scan.angle_increment *= (new_max - new_min) / old_span;
-    scan.angle_min = new_min;
-    scan.angle_max = new_max;
-}
-
-// Mirror a finished scan for a unit mounted upside-down: reverse the point
-// order and negate the angular window. Apply before remap_scan_window().
-inline void invert_scan(LaserScan& scan) {
-    std::reverse(scan.ranges.begin(), scan.ranges.end());
-    std::reverse(scan.intensities.begin(), scan.intensities.end());
-    const float new_min = -scan.angle_max;
-    scan.angle_max = -scan.angle_min;
-    scan.angle_min = new_min;
-}
-
-// Little-endian readers (bounds are the caller's responsibility).
-inline uint8_t  le_u8 (const uint8_t* p) { return p[0]; }
-inline uint16_t le16(const uint8_t* p) {
-    return static_cast<uint16_t>(p[0] | (p[1] << 8));
-}
-inline uint32_t le32(const uint8_t* p) {
-    return static_cast<uint32_t>(p[0])
-         | (static_cast<uint32_t>(p[1]) <<  8)
-         | (static_cast<uint32_t>(p[2]) << 16)
-         | (static_cast<uint32_t>(p[3]) << 24);
-}
-inline int32_t  le_i32(const uint8_t* p) { return static_cast<int32_t>(le32(p)); }
-
-inline float bits_to_float(uint32_t bits) {
-    float f;
-    std::memcpy(&f, &bits, sizeof(f));
-    return f;
-}
-
-} // namespace lidarlib

src/lidar_config.cpp

@@ -1,176 +0,0 @@
-#include "lidarlib/config.hpp"
-#include "lidarlib/sick_lidar.hpp"
-#include "lidarlib/espe_lidar.hpp"
-#include "json_mini.hpp"
-#include <algorithm>
-#include <fstream>
-#include <sstream>
-#include <stdexcept>
-#include <utility>
-
-namespace lidarlib {
-
-namespace {
-
-struct ModelEntry { const char* name; const ModelConfig* cfg; const char* brand; };
-
-constexpr ModelEntry kModels[] = {
-    { "AUTO",          &MODEL_AUTO,         "OLEI" },
-    { "VB",            &MODEL_VB,           "OLEI" },
-    { "VF",            &MODEL_VF,           "OLEI" },
-    { "LR-1F",         &MODEL_LR1F,         "OLEI" },
-    { "LR-1FMI",       &MODEL_LR1FMI,       "OLEI" },
-    { "LR-1BS5",       &MODEL_LR1BS5,       "OLEI" },
-    { "LR-16F",        &MODEL_LR16F,        "OLEI" },
-    { "GS1-5",         &MODEL_GS15,         "OLEI" },
-    { "SICK-TIM5xx",    &MODEL_SICK_TIM5XX,    "SICK" },
-    { "SICK-TIM571",    &MODEL_SICK_TIM571,    "SICK" },
-    { "SICK-TIM7xx",    &MODEL_SICK_TIM7XX,    "SICK" },
-    { "SICK-nanoScan3", &MODEL_SICK_NANOSCAN3, "SICK" },
-    { "ESPE-LGA60",     &MODEL_ESPE_LGA60,     "ESPE" },
-};
-
-json::Value to_json(const LidarConfig& c) {
-    json::Value v = json::Value::make_object();
-    v.set("name",     json::Value::make_string(c.name));
-    v.set("ip",       json::Value::make_string(c.ip));
-    v.set("port",     json::Value::make_number(c.port));
-    v.set("brand",    json::Value::make_string(c.brand));
-    v.set("model",    json::Value::make_string(c.model));
-    v.set("inverted", json::Value::make_bool(c.inverted));
-    v.set("use_udp",  json::Value::make_bool(c.use_udp));
-    v.set("angle_min_deg", json::Value::make_number(c.angle_min_deg));
-    v.set("angle_max_deg", json::Value::make_number(c.angle_max_deg));
-    return v;
-}
-
-LidarConfig lidar_from_json(const json::Value& v, const LidarConfig& def) {
-    LidarConfig c = def;
-    c.name     = v.get_string("name", def.name);
-    c.ip       = v.get_string("ip", def.ip);
-    c.port     = static_cast<uint16_t>(v.get_number("port", def.port));
-    c.brand    = v.get_string("brand", def.brand);
-    c.model    = v.get_string("model", def.model);
-    c.inverted = v.get_bool("inverted", def.inverted);
-    c.use_udp  = v.get_bool("use_udp", def.use_udp);
-    c.angle_min_deg = static_cast<float>(v.get_number("angle_min_deg", def.angle_min_deg));
-    c.angle_max_deg = static_cast<float>(v.get_number("angle_max_deg", def.angle_max_deg));
-    return c;
-}
-
-} // namespace
-
-const ModelConfig* model_by_name(const std::string& name) {
-    for (const auto& e : kModels)
-        if (name == e.name) return e.cfg;
-    return nullptr;
-}
-
-namespace {
-const ModelConfig* model_by_name_for_brand(const std::string& name, const std::string& brand) {
-    for (const auto& e : kModels)
-        if (name == e.name && brand == e.brand) return e.cfg;
-    return nullptr;
-}
-} // namespace
-
-const std::vector<std::string>& model_names() {
-    static const std::vector<std::string> names = [] {
-        std::vector<std::string> v;
-        for (const auto& e : kModels) v.push_back(e.name);
-        return v;
-    }();
-    return names;
-}
-
-const std::vector<std::string>& brand_names() {
-    static const std::vector<std::string> names = {"OLEI", "SICK", "ESPE"};
-    return names;
-}
-
-const std::vector<std::string>& model_names_for_brand(const std::string& brand) {
-    static const std::vector<std::string> empty;
-    static const auto by_brand = [] {
-        std::vector<std::pair<std::string, std::vector<std::string>>> m;
-        for (const auto& e : kModels) {
-            auto it = std::find_if(m.begin(), m.end(),
-                                    [&](const auto& p) { return p.first == e.brand; });
-            if (it == m.end()) { m.push_back({e.brand, {}}); it = m.end() - 1; }
-            it->second.push_back(e.name);
-        }
-        return m;
-    }();
-    for (const auto& p : by_brand)
-        if (p.first == brand) return p.second;
-    return empty;
-}
-
-Config load_config(const std::string& path) {
-    Config cfg;
-    std::ifstream f(path);
-    if (!f) return cfg;
-
-    std::ostringstream ss;
-    ss << f.rdbuf();
-    json::Value root;
-    try {
-        root = json::parse(ss.str());
-    } catch (const json::ParseError&) {
-        return cfg; // malformed file -> defaults
-    }
-
-    const json::Value* lidars = root.find("lidars");
-    if (!lidars || lidars->type != json::Type::Array) return cfg;
-
-    static const LidarConfig kBlankDefault{};
-    cfg.lidars.clear();
-    for (const auto& entry : lidars->arr)
-        cfg.lidars.push_back(lidar_from_json(entry, kBlankDefault));
-    return cfg;
-}
-
-void save_config(const std::string& path, const Config& cfg) {
-    json::Value root = json::Value::make_object();
-    json::Value arr; arr.type = json::Type::Array;
-    for (const auto& lidar : cfg.lidars) arr.arr.push_back(to_json(lidar));
-    root.set("lidars", arr);
-
-    std::ofstream f(path, std::ios::trunc);
-    if (!f) throw std::runtime_error("khong the ghi file config: " + path);
-    f << root.dump() << "\n";
-}
-
-std::unique_ptr<Lidar> make_lidar(const LidarConfig& cfg) {
-    // Anything but the exact strings "SICK"/"ESPE" is OLEI (keeps brand-less
-    // configs working). Brand name and fallback model are decided together so
-    // a new brand adds exactly one branch here plus one construction case.
-    const bool is_sick = (cfg.brand == "SICK");
-    const bool is_espe = (cfg.brand == "ESPE");
-
-    const char*        brand;
-    const ModelConfig* fallback;
-    if (is_sick)      { brand = "SICK"; fallback = &MODEL_SICK_TIM571; }
-    else if (is_espe) { brand = "ESPE"; fallback = &MODEL_ESPE_LGA60; }
-    else              { brand = "OLEI"; fallback = &MODEL_AUTO; }
-
-    const ModelConfig* model = model_by_name_for_brand(cfg.model, brand);
-    if (!model) model = fallback;
-
-    ModelConfig mc = *model;
-    if (cfg.angle_min_deg > -360.f || cfg.angle_max_deg < 360.f) {
-        mc.remap_angles  = true;
-        mc.out_angle_min = cfg.angle_min_deg;
-        mc.out_angle_max = cfg.angle_max_deg;
-    }
-
-    if (is_sick) {
-        if (model == &MODEL_SICK_NANOSCAN3)
-            return std::make_unique<NanoScanDriver>(mc, cfg.ip, cfg.port, cfg.inverted);
-        return std::make_unique<SickDriver>(mc, cfg.ip, cfg.port, cfg.inverted);
-    }
-    if (is_espe)
-        return std::make_unique<EspeDriver>(mc, cfg.ip, cfg.port, cfg.use_udp, cfg.inverted);
-    return std::make_unique<Driver>(mc, cfg.ip, cfg.port, cfg.inverted);
-}
-
-} // namespace lidarlib

src/lidar_manager.cpp

@@ -0,0 +1,246 @@
+// xlidar-driver — LidarManager implementation: plugin discovery (dlopen) and
+// config.json load/save with legacy-lidarlib migration.
+#include "lidar_manager.hpp"
+#include "json_mini.hpp"
+
+#include <algorithm>
+#include <cstdio>
+#include <dirent.h>
+#include <dlfcn.h>
+#include <fstream>
+#include <sstream>
+#include <stdexcept>
+
+namespace xlidar {
+
+namespace {
+
+bool ends_with(const std::string& s, const std::string& suffix) {
+    return s.size() >= suffix.size() &&
+           s.compare(s.size() - suffix.size(), suffix.size(), suffix) == 0;
+}
+
+// ── config.json ──────────────────────────────────────────────────────────────
+
+json::Value to_json(const DeviceConfig& c) {
+    json::Value v = json::Value::make_object();
+    v.set("name",        json::Value::make_string(c.name));
+    v.set("driver_id",   json::Value::make_string(c.driver_id));
+    v.set("model",       json::Value::make_string(c.model));
+    // Omitted when unset — the driver's declared default transport applies.
+    if (c.transport)
+        v.set("transport", json::Value::make_string(to_string(*c.transport)));
+    v.set("ip",          json::Value::make_string(c.ip));
+    v.set("port",        json::Value::make_number(c.port));
+    v.set("serial_port", json::Value::make_string(c.serial_port));
+    v.set("baudrate",    json::Value::make_number(c.baudrate));
+    v.set("inverted",    json::Value::make_bool(c.inverted));
+    v.set("angle_min_deg", json::Value::make_number(c.angle_min_deg));
+    v.set("angle_max_deg", json::Value::make_number(c.angle_max_deg));
+    v.set("range_min_m",   json::Value::make_number(c.range_min_m));
+    v.set("range_max_m",   json::Value::make_number(c.range_max_m));
+    v.set("remap_angle_min_deg", json::Value::make_number(c.remap_angle_min_deg));
+    v.set("remap_angle_max_deg", json::Value::make_number(c.remap_angle_max_deg));
+    if (!c.extra.empty()) {
+        json::Value extra = json::Value::make_object();
+        for (const auto& [k, val] : c.extra) extra.set(k, json::Value::make_string(val));
+        v.set("extra", extra);
+    }
+    return v;
+}
+
+// Legacy lidarlib entries carried {"brand": "OLEI"|"SICK"|"ESPE"} instead of
+// driver_id; brand + model pick the plugin, and the old angle window keys
+// were a remap window, not a FOV filter.
+std::string legacy_driver_id(const std::string& brand, const std::string& model) {
+    if (brand == "SICK") {
+        return (model == "SICK-nanoScan3") ? "sick_nanoscan3_driver" : "sick_tim_driver";
+    }
+    if (brand == "ESPE") return "espe_lga60_driver";
+    return "olei_lidar_driver";  // lidarlib treated anything else as OLEI
+}
+
+DeviceConfig lidar_from_json(const json::Value& v) {
+    DeviceConfig c;
+    c.name        = v.get_string("name", c.name);
+    c.driver_id   = v.get_string("driver_id");
+    c.model       = v.get_string("model", c.model);
+    c.ip          = v.get_string("ip", c.ip);
+    c.port        = static_cast<uint16_t>(v.get_number("port", c.port));
+    c.serial_port = v.get_string("serial_port", c.serial_port);
+    c.baudrate    = static_cast<uint32_t>(v.get_number("baudrate", c.baudrate));
+    c.inverted    = v.get_bool("inverted", c.inverted);
+
+    // "transport": "serial" | "udp" | "tcp"; unknown strings fall back to the
+    // driver default. Legacy files carried a use_udp bool instead.
+    if (const std::string t = v.get_string("transport"); !t.empty()) {
+        c.transport = transport_from_string(t);
+        if (!c.transport)
+            std::fprintf(stderr, "[xlidar] lidar '%s': unknown transport '%s' — using driver default\n",
+                         c.name.c_str(), t.c_str());
+    } else if (v.get_bool("use_udp", false)) {
+        c.transport = Transport::Udp;
+    }
+    c.range_min_m = static_cast<float>(v.get_number("range_min_m", c.range_min_m));
+    c.range_max_m = static_cast<float>(v.get_number("range_max_m", c.range_max_m));
+
+    const bool legacy = c.driver_id.empty() && v.find("brand") != nullptr;
+    if (legacy) {
+        c.driver_id = legacy_driver_id(v.get_string("brand"), c.model);
+        c.remap_angle_min_deg =
+            static_cast<float>(v.get_number("angle_min_deg", c.remap_angle_min_deg));
+        c.remap_angle_max_deg =
+            static_cast<float>(v.get_number("angle_max_deg", c.remap_angle_max_deg));
+    } else {
+        c.angle_min_deg = static_cast<float>(v.get_number("angle_min_deg", c.angle_min_deg));
+        c.angle_max_deg = static_cast<float>(v.get_number("angle_max_deg", c.angle_max_deg));
+        c.remap_angle_min_deg =
+            static_cast<float>(v.get_number("remap_angle_min_deg", c.remap_angle_min_deg));
+        c.remap_angle_max_deg =
+            static_cast<float>(v.get_number("remap_angle_max_deg", c.remap_angle_max_deg));
+    }
+
+    if (const json::Value* extra = v.find("extra"); extra && extra->type == json::Type::Object) {
+        for (const auto& [k, val] : extra->obj)
+            if (val.type == json::Type::String) c.extra[k] = val.str;
+    }
+    return c;
+}
+
+} // namespace
+
+ManagerConfig load_config(const std::string& path) {
+    ManagerConfig cfg;
+    std::ifstream f(path);
+    if (!f) return cfg;
+
+    std::ostringstream ss;
+    ss << f.rdbuf();
+    json::Value root;
+    try {
+        root = json::parse(ss.str());
+    } catch (const json::ParseError&) {
+        return cfg;  // malformed file -> defaults
+    }
+
+    const json::Value* lidars = root.find("lidars");
+    if (!lidars || lidars->type != json::Type::Array) return cfg;
+
+    for (const auto& entry : lidars->arr)
+        cfg.lidars.push_back(lidar_from_json(entry));
+    return cfg;
+}
+
+void save_config(const std::string& path, const ManagerConfig& cfg) {
+    json::Value root = json::Value::make_object();
+    json::Value arr; arr.type = json::Type::Array;
+    for (const auto& lidar : cfg.lidars) arr.arr.push_back(to_json(lidar));
+    root.set("lidars", arr);
+
+    std::ofstream f(path, std::ios::trunc);
+    if (!f) throw std::runtime_error("cannot write config file: " + path);
+    f << root.dump() << "\n";
+}
+
+// ── plugin loading ───────────────────────────────────────────────────────────
+
+LidarManager::LidarManager(std::string plugins_dir)
+    : plugins_dir_(std::move(plugins_dir)) {}
+
+LidarManager::~LidarManager() {
+    // Instances created by the plugins must already be gone by contract.
+    for (auto& [id, plugin] : loaded_)
+        if (plugin.handle) ::dlclose(plugin.handle);
+}
+
+size_t LidarManager::load_all_plugins() {
+    std::lock_guard<std::mutex> lock(mutex_);
+
+    DIR* dir = ::opendir(plugins_dir_.c_str());
+    if (!dir) {
+        std::fprintf(stderr, "[xlidar] plugins dir not readable: %s\n", plugins_dir_.c_str());
+        return available_.size();
+    }
+
+    std::vector<std::string> files;
+    while (const dirent* entry = ::readdir(dir)) {
+        std::string name = entry->d_name;
+        if (ends_with(name, ".so")) files.push_back(name);
+    }
+    ::closedir(dir);
+    std::sort(files.begin(), files.end());  // deterministic registration order
+
+    for (const std::string& file : files) {
+        const std::string path = plugins_dir_ + "/" + file;
+
+        // Already registered from this path? (repeat load_all_plugins call)
+        bool known = false;
+        for (const auto& [id, reg] : available_)
+            if (reg.file_path == path) { known = true; break; }
+        if (known) continue;
+
+        void* handle = ::dlopen(path.c_str(), RTLD_NOW | RTLD_LOCAL);
+        if (!handle) {
+            std::fprintf(stderr, "[xlidar] skip %s: %s\n", file.c_str(), ::dlerror());
+            continue;
+        }
+
+        auto info_fn = reinterpret_cast<xlidar_get_driver_info_fn>(
+            ::dlsym(handle, XLIDAR_GET_DRIVER_INFO_SYMBOL));
+        auto create_fn = reinterpret_cast<xlidar_create_driver_instance_fn>(
+            ::dlsym(handle, XLIDAR_CREATE_DRIVER_INSTANCE_SYMBOL));
+        if (!info_fn || !create_fn) {
+            std::fprintf(stderr, "[xlidar] skip %s: missing %s/%s\n", file.c_str(),
+                         XLIDAR_GET_DRIVER_INFO_SYMBOL, XLIDAR_CREATE_DRIVER_INSTANCE_SYMBOL);
+            ::dlclose(handle);
+            continue;
+        }
+
+        DriverInfo info;
+        info_fn(&info);
+        if (info.driver_id.empty()) {
+            std::fprintf(stderr, "[xlidar] skip %s: empty driver_id\n", file.c_str());
+            ::dlclose(handle);
+            continue;
+        }
+        if (available_.count(info.driver_id)) {
+            std::fprintf(stderr, "[xlidar] skip %s: duplicate driver_id '%s' (kept %s)\n",
+                         file.c_str(), info.driver_id.c_str(),
+                         available_[info.driver_id].file_path.c_str());
+            ::dlclose(handle);
+            continue;
+        }
+
+        available_[info.driver_id] = PluginRegistry{info, path};
+        loaded_[info.driver_id]    = LoadedPlugin{handle, create_fn};
+    }
+    return available_.size();
+}
+
+std::unique_ptr<LidarDriverInterface>
+LidarManager::create_lidar_device(const std::string& driver_id, const DeviceConfig& cfg) {
+    xlidar_create_driver_instance_fn create = nullptr;
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        auto it = loaded_.find(driver_id);
+        if (it == loaded_.end()) {
+            std::fprintf(stderr, "[xlidar] unknown driver_id '%s' (plugins dir: %s)\n",
+                         driver_id.c_str(), plugins_dir_.c_str());
+            return nullptr;
+        }
+        create = it->second.create;
+    }
+    return std::unique_ptr<LidarDriverInterface>(create(&cfg));
+}
+
+std::vector<std::unique_ptr<LidarDriverInterface>>
+LidarManager::create_from_config_file(const std::string& path) {
+    std::vector<std::unique_ptr<LidarDriverInterface>> devices;
+    for (const DeviceConfig& cfg : load_config(path).lidars) {
+        auto device = create_lidar_device(cfg);
+        if (device) devices.push_back(std::move(device));
+    }
+    return devices;
+}
+
+} // namespace xlidar

src/lidar_net.hpp

@@ -1,51 +0,0 @@
-// Internal socket helpers shared by the TCP driver TUs — not part of the public API.
-#pragma once
-#include "lidarlib/error.hpp"
-#include <cerrno>
-#include <fcntl.h>
-#include <netinet/in.h>
-#include <netinet/tcp.h>
-#include <sys/select.h>
-#include <sys/socket.h>
-
-namespace lidarlib {
-
-// Non-blocking connect with a bounded timeout — a blocking connect() to an
-// unreachable device would stall for the OS default (~2 min on Linux).
-// Enables TCP_NODELAY on success; the fd is returned to blocking mode either
-// way. The caller owns the fd and closes it on failure.
-inline ErrorCode connect_tcp_with_timeout(int fd, const sockaddr_in& addr, int timeout_ms) {
-    int flags = ::fcntl(fd, F_GETFL, 0);
-    ::fcntl(fd, F_SETFL, flags | O_NONBLOCK);
-
-    ErrorCode conn_err = ErrorCode::Ok;
-    int rc = ::connect(fd, reinterpret_cast<const sockaddr*>(&addr), sizeof(addr));
-    if (rc < 0 && errno != EINPROGRESS) {
-        conn_err = (errno == ECONNREFUSED) ? ErrorCode::ConnectionRefused
-                                           : ErrorCode::ConnectionFailed;
-    } else if (rc < 0) {
-        fd_set wfds; FD_ZERO(&wfds); FD_SET(fd, &wfds);
-        timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 };
-        rc = ::select(fd + 1, nullptr, &wfds, nullptr, &tv);
-        if (rc == 0) {
-            conn_err = ErrorCode::Timeout;
-        } else if (rc < 0) {
-            conn_err = ErrorCode::ConnectionFailed;
-        } else {
-            int err = 0; socklen_t errlen = sizeof(err);
-            ::getsockopt(fd, SOL_SOCKET, SO_ERROR, &err, &errlen);
-            if (err != 0)
-                conn_err = (err == ECONNREFUSED) ? ErrorCode::ConnectionRefused
-                                                 : ErrorCode::ConnectionFailed;
-        }
-    }
-    ::fcntl(fd, F_SETFL, flags);
-
-    if (conn_err == ErrorCode::Ok) {
-        int nodelay = 1;
-        ::setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, &nodelay, sizeof(nodelay));
-    }
-    return conn_err;
-}
-
-} // namespace lidarlib