fix: driver LGA60

Every driver must normalize its device's native scale into ROS REP-103
right-handed CCW (0 = front, + = left). Spells out the three stackable
device quirks to absorb — shifted 0° reference, clockwise scale,
upside-down mounting — and warns that a CW scale decoded as CCW mirrors
the world invisibly on single-lidar devices: verify handedness against
the real room, not just against motion.

Co-Authored-By: Claude Opus 4.8 (1M context) <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,73 +9,44 @@ 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
+add_subdirectory(src)
-    src/olei_lidar.cpp
+add_subdirectory(plugins)
-    src/sick_lidar.cpp
-    src/lidar_config.cpp
-)
 
-add_library(lidarlib ${LIDARLIB_SOURCES})
+if(XLIDAR_BUILD_EXAMPLES)
-set_target_properties(lidarlib PROPERTIES
+  add_subdirectory(examples)
-    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(LIDARLIB_BUILD_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(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,166 +1,351 @@
-# Lidarlib
+# xlidar-driver
 
-Thư viện C++17 cho lidar **OLEI** (UDP) và **SICK** (TCP/UDP). Build bằng CMake
+SDK driver lidar 2D cho Linux, kiến trúc **plugin nạp động**: mỗi driver là
-ra shared lib, hỗ trợ `find_package(lidarlib)`. Mọi driver dùng chung một
+một file `.so` độc lập, cùng implement một interface chung
-interface `lidarlib::Lidar` và một hàm khởi tạo duy nhất `lidarlib::make_lidar()`.
+`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ị.
 
-- Tự nhận diện họ giao thức OLEI (Family A/B/C) theo từng gói tin
+Driver đi kèm: **Slamtec RPLIDAR** (serial), **OLEI** (UDP), **SICK TiM**
-- Tự dò model (`MODEL_AUTO`) với Family B/C
+(TCP/CoLa-A), **SICK nanoScan3** (UDP safety), **ESPE LGA60** (TCP/UDP).
-- Chạy nhiều lidar song song (mỗi instance độc lập, an toàn đa luồng)
+Output thống nhất theo định dạng ROS `sensor_msgs/LaserScan`.
-- Output chuẩn ROS `sensor_msgs/LaserScan` (radian, mét)
-- Không có UI — tự viết giao diện trên API này
 
-## Build
+## Kiến trúc
+
+```
+xlidar_driver/
+├── CMakeLists.txt
+├── include/                    # API public — host chỉ include từ đây
+│   ├── lidar_interface.hpp     #   LidarDriverInterface, DriverInfo, DeviceConfig,
+│   │                           #   LaserScan/ScanResult, ErrorCode, plugin ABI
+│   ├── lidar_diagnostics.hpp   #   Diagnostics + bit lỗi từng hãng
+│   └── lidar_manager.hpp       #   LidarManager, PluginRegistry, config.json
+├── src/
+│   ├── CMakeLists.txt          # → liblidar_manager.so
+│   ├── lidar_manager.cpp       #   dlopen/dlsym + load/save config.json
+│   └── json_mini.hpp
+├── plugins/                    # mỗi thư mục → một plugin .so
+│   ├── CMakeLists.txt          #   helper xlidar_add_plugin()
+│   ├── common/plugin_helpers.hpp
+│   ├── driver_rplidar/         # → driver_rplidar.so     (rplidar_c1_driver)
+│   ├── driver_olei/            # → driver_olei.so        (olei_lidar_driver)
+│   ├── driver_sick_tim/        # → driver_sick_tim.so    (sick_tim_driver)
+│   ├── driver_sick_safety/     # → driver_sick_safety.so (sick_nanoscan3_driver)
+│   └── driver_espe/            # → driver_espe.so        (espe_lga60_driver)
+├── third_party/
+│   └── rplidar_sdk/            # SDK Slamtec, build thẳng vào driver_rplidar.so
+├── examples/                   # list_drivers, example, lidar_app
+└── docs/diagnostics.md         # nghiên cứu layout dữ liệu chẩn đoán
+```
+
+- **LidarManager** quét thư mục plugin, `dlopen` từng `.so`, resolve hai
+  entry point C rồi đăng ký vào `available_drivers()` — map
+  `<driver_id, PluginRegistry>` trong đó `PluginRegistry = {DriverInfo,
+  file_path}`.
+- **Plugin ABI** — mỗi plugin export đúng hai symbol C:
+
+  ```cpp
+  XLIDAR_PLUGIN_EXPORT void get_driver_info(xlidar::DriverInfo* out);
+  XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface*
+      create_driver_instance(const xlidar::DeviceConfig* cfg);
+  ```
+
+- **DriverInfo** — plugin tự đăng ký danh tính: `vendor`, `model` (dòng
+  thiết bị driver phụ trách — một driver có thể cover cả series), `driver_id`
+  (định danh duy nhất, ví dụ `rplidar_c1_driver`), `description` (mô tả ngắn:
+  thiết bị hỗ trợ, transport, ghi chú), kèm metadata cho UI: `transport`
+  (`serial`/`udp`/`tcp`), `transport_selectable`, `supported_models`.
+- Plugin được giữ nguyên trong bộ nhớ tới khi manager bị hủy — **manager
+  phải sống lâu hơn mọi instance nó tạo ra**.
+
+## Cài đặt
+
+Yêu cầu: Linux, CMake ≥ 3.16, C++17. Không có dependency ngoài (pthread,
+dl); SDK của hãng (Slamtec cho plugin rplidar) đã nằm sẵn trong
+`third_party/` — clone là build được.
 
 ```bash
 cmake -S . -B build -DCMAKE_BUILD_TYPE=Release
 cmake --build build -j"$(nproc)"
 ```
 
-Sinh ra `build/liblidarlib.so` (chỉ phụ thuộc pthread) và các binary demo
+Kết quả: `build/src/liblidar_manager.so`, plugin trong `build/plugins/*.so`,
-(`example`, `test_dual`, `sick_example`, `nanoscan_example`, `lidar_app`).
+demo trong `build/examples/`.
 
-Tùy chọn: `-DLIDARLIB_BUILD_EXAMPLES=OFF` (tắt demo),
+Tùy chọn: `-DXLIDAR_BUILD_EXAMPLES=OFF`. Hỗ trợ `cmake --install` +
-`-DBUILD_SHARED_LIBS=OFF` (static lib).
+`find_package(xlidar_driver)` (target `xlidar::lidar_manager`).
 
-Cài đặt và dùng từ project khác:
+## Sử dụng
 
-```bash
+### Khám phá driver và đọc scan
-cmake --install build --prefix "$HOME/.local"   # hoặc sudo với /usr/local
-```
-
-```cmake
-find_package(lidarlib REQUIRED)
-target_link_libraries(my_app PRIVATE lidarlib::lidarlib)
-```
-
-## Quick start
 
 ```cpp
-#include "lidarlib/lidarlib.hpp"   // toàn bộ API trong 1 include
+#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();
-lidar->open();
 
-lidarlib::ScanResult r;
+for (const auto& [id, plugin] : manager.available_drivers())
-lidar->recv_scan(r, 1000);
+    printf("%s — %s\n", id.c_str(), plugin.info.description.c_str());
-// r.scan : LaserScan — format sensor_msgs/LaserScan của ROS, chung mọi lidar
+
-// r.info : ExtraInfo — thông tin thêm tuỳ family/model
+xlidar::DeviceConfig cfg;
-printf("%zu diem, model=%s\n", r.scan.ranges.size(), r.info.detected_model.c_str());
+cfg.driver_id = "sick_tim_driver";
+cfg.model     = "SICK-TIM7xx";
+cfg.ip        = "192.168.0.1";       // port 0 = port mặc định của driver
+
+auto lidar = manager.create_lidar_device(cfg);   // nullptr nếu driver_id lạ
+if (lidar->open() != xlidar::ErrorCode::Ok) {
+    fprintf(stderr, "open: %s\n", xlidar::to_string(lidar->last_error()));
+    return 1;
+}
+
+xlidar::ScanResult r;
+if (lidar->recv_scan(r, 1000)) {
+    // r.scan : LaserScan  — điểm đo, format ROS
+    // r.info : ExtraInfo  — metadata tuỳ model
+}
 ```
 
-Có thể khởi tạo driver trực tiếp thay vì qua `make_lidar()`:
+Driver serial (rplidar) dùng `cfg.serial_port` + `cfg.baudrate` thay cho
+`ip`/`port`. Xem `examples/example.cpp`.
+
+### Chế độ callback
 
 ```cpp
-lidarlib::Driver      olei(lidarlib::MODEL_AUTO, "192.168.100.100", 2368);
+lidar->set_scan_callback([](const xlidar::ScanResult& r) { /* mỗi vòng quét */ });
-lidarlib::SickDriver  tim (lidarlib::MODEL_SICK_TIM571, "192.168.0.1", 2111);
+while (running) lidar->spin_once();
-lidarlib::NanoScanDriver nano(lidarlib::MODEL_SICK_NANOSCAN3, "0.0.0.0", 6060);
 ```
 
-Ngoài `recv_scan()` blocking còn có callback: `set_scan_callback()` +
+Callback chỉ phát từ `spin_once()` — chọn một kiểu bơm dữ liệu:
-`spin_once()` trong vòng lặp riêng.
+`recv_scan()` (poll) hoặc callback + `spin_once()`.
 
-## Cấu hình (config.json)
+### Sẵn sàng & chẩn đoán thiết bị
 
-`lidar_app` là app mẫu headless: đọc `config.json`, mở từng lidar bằng
+API chẩn đoán **chung cho mọi hãng**: `Diagnostics` trả về danh sách
-`make_lidar()`, một thread mỗi con.
+`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()`.
 
-```bash
+```cpp
-./build/lidar_app [my_config.json]
+if (!lidar->wait_ready(5000)) { /* chưa có scan sạch nào trong 5s */ }
+
+xlidar::Diagnostics d = lidar->get_diagnostics();
+for (const xlidar::DiagnosticIssue& issue : d.issues) {
+    // issue.severity : DiagSeverity::Fault | DiagSeverity::Warning
+    // issue.code     : "motor" | "voltage" | "temperature" | "optics_dirty"
+    //                  | "manipulation" | "device_error" | "device_warning"
+    // issue.detail   : mô tả kèm tên hãng + giá trị thô, VD "ESPE fault word 0x0004"
+    printf("[%s] %s — %s\n", xlidar::to_string(issue.severity),
+           issue.code.c_str(), issue.detail.c_str());
+}
+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 fault. `is_ready(max_age_ms)` = đang
+mở + healthy + scan mới nhất chưa quá hạn — diagnostics chỉ refresh qua
+`recv_scan()`/`spin_once()`, nên gọi từ chính thread bơm dữ liệu. Ý 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ả gần nhất.
+
+| ErrorCode | Ý nghĩa |
+|---|---|
+| `Ok` | Thành công |
+| `AlreadyOpen` | `open()` khi đang mở — kết nối cũ giữ nguyên |
+| `NotOpen` | `recv_scan()`/`spin_once()` khi chưa `open()` |
+| `InvalidAddress` | Chuỗi IP không hợp lệ |
+| `PortInUse` / `BindFailed` / `SocketError` | Lỗi bind/socket (driver UDP) |
+| `ConnectionRefused` / `ConnectionFailed` / `Timeout` | TCP connect bị từ chối / không tới được / quá hạn |
+| `HandshakeFailed` | Nối được nhưng lệnh start-stream/start-scan thất bại |
+| `SerialError` | Cổng serial không tồn tại / không mở được (rplidar) |
+| `DeviceError` | Thiết bị tự báo fault (rplidar health check lúc `open()`) |
+| `DeviceDisconnected` | Thiết bị đóng kết nối / lỗi recv giữa chừng |
+| `InvalidConfig` | `DeviceConfig` không dùng được với driver |
+
+Lifecycle an toàn với mọi thứ tự gọi: `close()` idempotent, `open()` lặp trả
+`AlreadyOpen`, sau `close()` mở lại được. Mỗi instance độc lập hoàn toàn —
+mỗi lidar một thread, không cần khóa.
+
+### Cấu hình JSON
+
+`lidar_app` (binary demo) đọc `config.json`, mở mỗi lidar một thread; API:
+`xlidar::load_config(path)` / `save_config(path, cfg)` /
+`manager.create_from_config_file(path)`.
+
 ```json
 {
   "lidars": [
-    {"name":"front", "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":"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) hoặc `"SICK"` |
+| `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 (`AUTO` / `SICK-TIM571`). Với SICK, `"SICK-nanoScan3"` → driver UDP, còn lại → driver TCP |
+| `model` | Một trong `supported_models` của driver; tên lạ → mặc định của driver |
-| `inverted` | Chỉ OLEI: `true` nếu lidar lắp úp ngược, driver tự đảo góc về hệ quy chiếu xe |
+| `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 |
-| `angle_min_deg` / `angle_max_deg` | Tuỳ chọn: remap tuyến tính góc output sang cửa sổ này (độ). Không cắt điểm nào, chỉ ghi lại `angle_min/max/increment`. Bỏ trống (±360) = tắt |
+| `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 |
+| `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
+## Driver đi kèm
 
-### OLEI (`brand = "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) | Ghi chú |
+### 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 | 2D 270°, 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 | 2D 360°, Family A |
+| `VB` | −135…135 | 0.05…30 | Family A |
-| `MODEL_LR1F` | -180…180 | 0.05…50 | Family A; 0° thô của máy chỉ về đ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° thô chỉ về đuôi (offset +180°) |
+| `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 | Không biết trước model; tự dò với Family B (chuỗi tên) và C (magic). Family A không mang tên model nên giữ FOV rộng |
+| `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 mỗi gói:
+**SICK** — TiM qua TCP/SOPAS (CoLa-A) port 2111: `open()` gửi
-**Family A** `0xFAF0` (header 20B, 3B/điểm, CRC32) ·
+`sEN LMDscandata 1` để start stream; frame wire đặt 90° ở phía trước nên
-**Family B** `0xFEF0` (header 40B kèm tên model ASCII, 8B/điểm) ·
+driver offset −90° để 0° = phía trước. nanoScan3 là receiver UDP thụ động
-**Family C/V3** `0xFEAC` (header 48B, 2/4B/điểm — port từ driver C#, chưa verify).
+port 6060 — đích UDP phải cấu hình sẵn bằng SICK Safety Designer, driver
+không handshake CoLa2/TCP.
 
-Tên model thật đọc từ packet xem qua `detected_model()` hoặc
+| Model | FOV (°) | Range (m) | Ghi chú |
-`result.info.detected_model`.
-
-### SICK (`brand = "SICK"`)
-
-| Constant | FOV (°) | Range (m) | Transport |
 |---|---|---|---|
-| `MODEL_SICK_TIM5XX` | -135…135 | 0.05…10 | TCP/SOPAS (CoLa-A), port 2111 |
+| `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 |
+| `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 `sEN LMDscandata 1` để bắt đầu stream.
+**ESPE LGA60** (TCP mặc định / UDP, port 8080; IP mặc định của hãng
-Hệ góc trên dây đặt 90° = trước mặt nên preset có `angle_offset_deg = -90`,
+192.168.1.88) — FOV 320°: thiết bị quét 20°→340° với 0° hướng đuôi (offset
-output ra -135…135° với 0° = phía trước. **Đã verify trên TiM781S thật**
+−180°), preset −160…160°, range 0.05…50 m. Frame đo `HISN` (header 16 B
-(811 điểm/scan, increment 0.333°, DIST1/RSSI1 đúng layout). Chưa verify:
+big-endian; điểm = distance mm + intensity, distance 50 000 mm = không có
-encoder, kênh 8-bit, thông số TiM5xx/571.
+phản hồi → ∞); frame vùng `WSimu` (nếu thiết bị gửi) được đọc lấy từ lỗi.
+Mỗi lượt quét được thiết bị chia thành NHIỀU packet `HISN`: các bộ đếm điểm
+trong header là của riêng packet đó, còn vòng quét là cửa sổ cố định
+20°→340° = 320°/bước điểm — driver ghép các packet theo `start_angle` và chỉ
+phát scan khi packet đóng ở 340°. Điểm không packet nào gửi = NaN. Bước góc
+lấy từ packet mở vòng (span/số điểm) rồi giữ nguyên.
+Chu kỳ quay đo host-side giữa hai vòng: `scan_time` là thời gian QUÉT
+(period × 320/360, đúng như driver ROS của hãng dùng để đóng dấu), nên spin
+rate suy ra từ `1/scan_time` cao hơn cơ khí 360/320; vòng đầu chưa đo được
+thì để 0. Trường `time` 16-bit trong header là bộ đếm thiết bị đơn vị chưa
+xác định (header của hãng ghi "chưa kích hoạt") → `timestamp_ms` = 0.
+Độ 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.
 
-**nanoScan3 (`NanoScanDriver`)** — UDP receiver thụ động: chỉ bind cổng và
+**RPLIDAR** (serial, mặc định `/dev/ttyUSB0` @ 460800) — preset C1: range
-parse datagram; **đích UDP phải cấu hình sẵn trong SICK Safety Designer**
+0.05…16 m (datasheet 12 m trên nền trắng), ~10 Hz, ~400–500 điểm/vòng
-(driver không bắt tay CoLa2). Layout port từ `sick_safetyscanners` (Apache-2.0).
+(DenseBoost); A/S series dùng được với baud tương ứng. `open()` chạy health
-**Chưa verify phần cứng thật** — mới test bằng gói tổng hợp qua loopback.
+check (Error → `DeviceError`) và tự nhận model/firmware. Riêng driver này
+đo được `time_increment`/`scan_time` thực.
 
-## Output
+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).
 
-`ScanResult { LaserScan scan; ExtraInfo info; }` mỗi vòng quét:
+## Kiểu dữ liệu
 
-- **`LaserScan`** — cùng field/đơn vị với ROS: `angle_min/max/increment` (rad,
+### `ScanResult` = `{ LaserScan scan; ExtraInfo info; }`
-  đã unwrap liên tục, không giới hạn ±π), `ranges[]` (m), `intensities[]`
-  (0-255), `timestamp_ms` (đồng hồ thiết bị, 0 nếu family không có).
-  `range_min/max` lấy từ `ModelConfig` (đặt sẵn, không đo mỗi scan);
-  `time_increment/scan_time` luôn 0.
-- **`ExtraInfo`** — field tuỳ family: `detected_model`, `error_status` (Family A),
-  `distance_scale_mm` (A/B), và các trường raw của Family C (chưa verify).
-  Field thiết bị không có giữ `std::nullopt`.
 
-## Cấu trúc source
+`LaserScan` cùng field và đơn vị với ROS `sensor_msgs/LaserScan`:
 
-| File | Vai trò |
+| Field | Ý nghĩa |
 |---|---|
-| `include/lidarlib/lidar.hpp` | Data model, interface `Lidar`, driver OLEI, các `MODEL_*` OLEI |
+| `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π) |
-| `include/lidarlib/sick_lidar.hpp` | `SickDriver`, `NanoScanDriver`, các `MODEL_SICK_*` |
+| `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) |
-| `include/lidarlib/config.hpp` | `LidarConfig`, load/save JSON, `make_lidar()` |
+| `intensities` | Cường độ 0–255 |
-| `src/olei_lidar.cpp` | Parse Family A/B/C, CRC, gom scan |
+| `range_min` / `range_max` | Dải đo hợp lệ (m) |
-| `src/sick_lidar.cpp` | Parse CoLa-A (TiM) + safety-data UDP (nanoScan3) |
+| `timestamp_ms` | Đồng hồ thiết bị (ms); 0 nếu giao thức không có |
-| `src/lidar_config.cpp` | Bảng model/brand, config JSON, factory |
+| `time_increment` / `scan_time` | Chỉ rplidar (chu kỳ grab thực) và ESPE (chu kỳ vòng quét, × 320/360) đo được; driver khác = 0 |
-| `examples/` | Demo: 1 lidar, 2 lidar song song, SICK TiM, nanoScan3, app khung |
 
-## Ghi chú
+`ExtraInfo`: metadata thô tuỳ giao thức — field thiết bị không có giữ
+`std::nullopt`:
 
-- Nếu port UDP đã bị app khác giữ (không bật `SO_REUSEPORT`), `open()` sẽ thất
+| Field | Nguồn | Ý nghĩa |
-  bại. Kiểm tra: `ss -lunp | grep 2368`.
+|---|---|---|
-- `inverted` đã verify bằng sniff sống: `false` góc tăng dần, `true` góc giảm
+| `detected_model` | mọi driver | Tên model thực đọc từ dữ liệu (hoặc tên cấu hình) |
-  dần cùng bước.
+| `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; 0 = không báo |
+| `rotation_raw` | OLEI Family A | Tốc độ motor (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 `(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 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` (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()`.
+
+## Viết một plugin mới
+
+1. Tạo `plugins/driver_<tên>/` với `<tên>_driver.cpp` + `CMakeLists.txt`
+   (`xlidar_add_plugin(driver_<tên> <tên>_driver.cpp)`), thêm
+   `add_subdirectory` vào `plugins/CMakeLists.txt`.
+2. Implement class kế thừa `xlidar::LidarDriverInterface` — đủ
+   `open`/`close`/`recv_scan`/`spin_once`/`set_scan_callback`/
+   `detected_model`/`is_open`/`get_driver_info`, và cập nhật
+   `get_diagnostics()` + `mark_scan_decoded()` mỗi vòng quét.
+3. Export hai entry point:
+
+   ```cpp
+   XLIDAR_PLUGIN_EXPORT void get_driver_info(xlidar::DriverInfo* out) { *out = kDriverInfo; }
+   XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface*
+   create_driver_instance(const xlidar::DeviceConfig* cfg) { return new MyDriver(...); }
+   ```
+
+4. Chọn `driver_id` duy nhất, mô tả `description` rõ driver phụ trách nhóm
+   thiết bị nào; dùng `apply_device_config()` trong `plugins/common` để tôn
+   trọng các cửa sổ góc/dải đo chung.
+
+Plugin build với `-fvisibility=hidden` — chỉ hai entry point lộ ra ngoài.
+Manager và plugin phải build cùng toolchain (chúng trao đổi kiểu C++).

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

@@ -0,0 +1,251 @@
+# 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 `xlidar::Diagnostics` / `LidarDriverInterface::get_diagnostics()`
+(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
+tin chẩn đoán được **nhúng trong header của chính gói dữ liệu quét**, nên "lấy
+data diagnosis" = decode header của stream đang chạy, không tốn thêm băng thông
+hay round-trip nào.
+
+## 1. Family A (Frame ID `0xFAF0` — VB, VF, LR-1F)
+
+Header 20 byte, thông tin chẩn đoán nằm ở các offset sau:
+
+| Offset | Kích thước | Trường | Ý nghĩa |
+|---|---|---|---|
+| `[4]` | u8 | `distance_scale` | mm/count — gián tiếp cho biết chế độ đo |
+| `[5]` | u8 | **`error_status`** | Byte lỗi thiết bị, xem bảng bit bên dưới |
+| `[10-11]` | u16 LE | `rotation_raw` | Tốc độ quay motor (raw, **đơn vị chưa xác minh** — nghi là RPM hoặc Hz×100) |
+| `[12-15]` | u32 LE | `timestamp` | Đồng hồ thiết bị (ms) — dùng phát hiện thiết bị treo/reset |
+
+### Bit map của `error_status` (byte `[5]`)
+
+| Bit | Mask | Tên | Ý nghĩa khi = 1 |
+|---|---|---|---|
+| 0 | `0x01` | Monitor | Khối giám sát/motor bất thường (motor không đạt tốc độ, mất đồng bộ encoder) |
+| 1 | `0x02` | Voltage | Điện áp nguồn ngoài dải cho phép |
+| 2 | `0x04` | Temperature | Nhiệt độ bên trong bất thường (quá nóng/quá lạnh) |
+| 3-7 | `0xF8` | Reserved | Chưa định nghĩa trong tài liệu OLEI; driver vẫn coi ≠0 là fault và log dạng `reserved(0xXX)` |
+
+Ghi chú thực nghiệm:
+
+- Trên thiết bị chạy bình thường byte này luôn `0x00`; chưa tái tạo được
+  fault thật trên phần cứng (chưa thử hạ áp/che gương), nên ý nghĩa bit lấy
+  theo tài liệu giao thức OLEI, **chưa verify từng bit bằng lỗi thật**.
+- `error_status` lặp lại trong *mỗi packet* (~22°/packet), nhưng driver chỉ
+  chốt giá trị theo **vòng quét** (lần `flush_scan()` gần nhất) — đủ nhanh
+  (10-20 Hz) và nhất quán với `ScanResult`.
+
+## 2. Family B (Frame ID `0xFEF0` — LR-1BS5, LR-1FMI)
+
+Header 40 byte mang **tên model ASCII** tại `[7-16]` và `distance_scale` tại
+`[6]`, nhưng **không có byte lỗi, không có timestamp**. Đã dò toàn bộ 40 byte
+header trên stream thật của LR-1FMI: các byte còn lại là hằng số/counter,
+không thấy trường nào đổi giá trị khi thiết bị hoạt động — kết luận Family B
+**không phát dữ liệu chẩn đoán trên wire**.
+
+Chẩn đoán khả dụng duy nhất với Family B là **gián tiếp**:
+
+- Mất gói / `ErrorCode::Timeout` từ `recv_scan()` → thiết bị ngắt kết nối
+  hoặc treo.
+- Số điểm mỗi vòng tụt bất thường (LR-1FMI chuẩn ~2400 điểm/vòng) → nghi
+  bẩn kính / lỗi quang học.
+
+`get_diagnostics()` với Family B trả về `valid = true` sau scan đầu tiên nhưng
+`error_status = 0` và mọi trường optional là `nullopt` — nghĩa là "không có
+thông tin", **không** đồng nghĩa "thiết bị khỏe".
+
+## 3. Family C / Protocol V3 (Magic `0xFEAC` — GS1-5)
+
+Header 48 byte, giàu thông tin trạng thái nhất (GS1-5 là dòng có field
+an toàn kiểu safety-scanner). Các trường chẩn đoán (port từ driver C#
+`OleiGS15Driver.cs`, **chưa verify trên phần cứng thật**):
+
+| Offset | Kích thước | Trường | Ý nghĩa (theo driver C#) |
+|---|---|---|---|
+| `[24-25]` | u16 LE | `scan_frequency_raw` | Tần số quét raw (nghi Hz×100) |
+| `[28-29]` | u16 LE | `input_status` | Trạng thái các chân input số |
+| `[30-31]` | u16 LE | `output_status` | Trạng thái các chân output số (OSSD?) |
+| `[32-35]` | u32 LE | `field_status` | Trạng thái các vùng field an toàn (bit nào = vùng nào chưa rõ) |
+| `[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::raw` (key `"olei.scan_frequency"`, `"olei.input_status"`,
+`"olei.output_status"`, `"olei.field_status"`, `"olei.status_flags"`,
+`"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`)
+
+Khác OLEI, TiM có **hai đường** lấy chẩn đoán:
+
+1. **Nhúng trong stream** (driver dùng đường này): telegram `sSN LMDscandata`
+   mang cặp **Device Status** ngay sau SerialNumber. Theo SICK Telegram
+   Listing (8014631):
+
+   | Cặp giá trị | Ý nghĩa |
+   |---|---|
+   | `0 0` | OK |
+   | `0 1` | Error — thiết bị lỗi, dữ liệu không tin được |
+   | `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`);
+   `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 để
+   xác nhận giá trị 2/4.
+
+2. **Query chủ động qua SOPAS** (chưa implement): `sRN SCdevicestate`
+   (0=busy, 1=ready, 2=error), `sRN LCMstate` (mức nhiễm bẩn),
+   `sRN DItype`/`sRN ODoprh` (giờ vận hành). Cần cơ chế request/response
+   xen giữa stream — xem mục Hướng mở rộng.
+
+## 5. SICK nanoScan3 (UDP safety-data — block General System State)
+
+Packet UDP của nanoScan3 gồm nhiều block, header trỏ tới từng block bằng cặp
+offset/size. Block **General System State** (offset tại header `[32]`, size
+`[34]`) là block trạng thái an toàn; **byte 0** của block (layout theo
+`sick_safetyscanners`, **chưa verify phần cứng**):
+
+| Bit | Mask | Ý nghĩa khi = 1 |
+|---|---|---|
+| 0 | `0x01` | Run mode active (đang chạy bình thường) |
+| 1 | `0x02` | Standby mode |
+| 2 | `0x04` | Contamination warning — kính bẩn nhẹ |
+| 3 | `0x08` | Contamination error — kính bẩn nặng, vùng an toàn không tin được |
+| 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_diagnostics()`
+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. 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:
+
+| Tín hiệu | API | Ý nghĩa |
+|---|---|---|
+| Không mở được socket | `open()` → `ErrorCode` | Lỗi cấu hình host (port bận, IP sai...) |
+| Không có gói trong `timeout_ms` | `recv_scan()` = false + `last_error() == Timeout` | Đứt cáp, thiết bị mất nguồn, sai port |
+| Lỗi recv giữa chừng | `last_error() == DeviceDisconnected` | Socket lỗi cứng |
+| `timestamp_ms` nhảy lùi | so sánh giữa 2 scan (Family A) | Thiết bị vừa reset/reboot |
+
+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`.
+
+## 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 `LidarDriverInterface`:
+
+```cpp
+lidar->open();
+if (!lidar->wait_ready(5000)) {          // bơm recv_scan() tới khi ready
+    // timeout: xem last_error() (Timeout, DeviceDisconnected...)
+}
+// ... vòng lặp chính đang bơm recv_scan()/spin_once() ...
+if (!lidar->is_ready()) { /* mất dữ liệu hoặc thiết bị báo fault */ }
+```
+
+- `is_ready(max_age_ms = 3000)` = `is_open()` **và** `get_diagnostics().healthy()`
+  **và** scan decode gần nhất chưa quá `max_age_ms` (truyền `0` để bỏ kiểm tra
+  tuổi). Kiểm tra tuổi giúp phát hiện cảm biến chết giữa chừng — socket UDP
+  vẫn "mở" và snapshot diagnostics vẫn "khỏe" dù thiết bị đã ngừng phát.
+  Diagnostics chỉ được cập nhật bởi `recv_scan()`/`spin_once()`, nên phải có
+  vòng lặp đang bơm dữ liệu thì `is_ready()` mới có nghĩa.
+- `wait_ready(timeout_ms = 5000)` = bơm `recv_scan()` (bỏ qua dữ liệu, không
+  gọi callback) cho tới khi `is_ready()` hoặc hết giờ — dùng lúc khởi động,
+  trước khi giao quyền bơm cho vòng lặp chính.
+- `is_ready()` **không** tính `has_warning()` (kính bẩn nhẹ vẫn đo được →
+  vẫn ready); app muốn chặt hơn thì tự kiểm tra thêm
+  `!get_diagnostics().has_warning()`.
+
+## 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 "lidar_manager.hpp"
+
+xlidar::ScanResult r;
+if (lidar->recv_scan(r, 1000)) {
+    xlidar::Diagnostics d = lidar->get_diagnostics();
+
+    if (!d.valid) {
+        // chưa có scan nào được decode
+    }
+    for (const xlidar::DiagnosticIssue& issue : d.issues) {
+        printf("[%s] %s — %s\n", xlidar::to_string(issue.severity),
+               issue.code.c_str(), issue.detail.c_str());
+    }
+    if (d.has_fault())   { /* dừng tin dữ liệu */ }
+    if (d.has_warning()) { /* lên lịch bảo trì */ }
+
+    // Giá trị thô của hãng (chỉ có khi wire mang nó), VD Family C raw:
+    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
+}
+```
+
+- `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: optics_dirty` / `FAULT: voltage temperature | WARN: optics_dirty`.
+- `to_json(Diagnostics)` — chuỗi JSON đầy đủ (`valid`, `model`, `firmware`,
+  `healthy`, `issues[]`, `raw{}`) cho host nào muốn nhận string thay struct.
+- `has_fault()` / `has_warning()` quét `issues` theo severity;
+  `healthy()` = `valid && !has_fault()`.
+
+## 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ế
+  request/response xen giữa stream `LMDscandata`.
+- **Family A bit 3-7**: cần bảng tra từ OLEI hoặc thử nghiệm gây lỗi có chủ
+  đích (hạ áp nguồn, chặn tản nhiệt) để xác minh.
+- **GS1-5**: cần thiết bị thật để xác minh toàn bộ Family C.
+- **TiM pollution 2/4**: cần thử che/bôi bẩn kính TiM781S thật để xác nhận.

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/example.cpp

@@ -1,37 +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";
 
-    if (!drv.open()) {
+    xlidar::LidarManager manager(plugins_dir);
-        fprintf(stderr, "Không mở được socket\n");
+    manager.load_all_plugins();
+
+    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;
+    }
+
+    // Wait until the sensor is usable: at least one complete scan decoded
+    // and the device reports no fault (motor/voltage/pollution/...).
+    if (!lidar->wait_ready(5000)) {
+        fprintf(stderr, "sensor not ready: %s (diag: %s)\n",
+                xlidar::to_string(lidar->last_error()),
+                xlidar::to_string(lidar->get_diagnostics()).c_str());
+        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());
 
-        for (size_t j = 0; j < 20 && j < scan.ranges.size(); ++j) {
+        // Device self-diagnostics from the newest scan.
+        xlidar::Diagnostics diag = lidar->get_diagnostics();
+        printf("  diag: %s\n", xlidar::to_string(diag).c_str());
+
+        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,31 +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);
-    if (!lidar->open()) {
+    if (!lidar) return;
-        fprintf(stderr, "[%s] khong mo duoc %s %s:%u\n",
+
-                cfg.name.c_str(), cfg.brand.c_str(), cfg.ip.c_str(), cfg.port);
+    xlidar::ErrorCode err = lidar->open();
+    if (err != xlidar::ErrorCode::Ok) {
+        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
@@ -46,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,37 +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);
-
-    if (!drv.open()) {
-        fprintf(stderr, "Không mở được UDP socket cho nanoScan3\n");
-        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,37 +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);
-
-    if (!drv.open()) {
-        fprintf(stderr, "Không kết nối được TCP tới lidar SICK\n");
-        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,46 +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);
-    if (!drv.open()) {
-        fprintf(stderr, "[%s] Khong mo duoc socket tren %s:%u (interface khong ton tai?)\n",
-                tag, local_ip.c_str(), port);
-        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,479 @@
+#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.
+//
+// ANGLE CONVENTION CONTRACT (ROS REP-103, right-handed): 0 = device front,
+// positive = LEFT, increasing counter-clockwise viewed from the top. Every
+// driver must NORMALIZE its device's native scale into this frame — three
+// independent, stackable device quirks to absorb:
+//   1. shifted 0° reference        -> ModelConfig::angle_offset_deg
+//      (OLEI LR-1F/1FMI/1BS5: 0° at the rear; SICK TiM: front at 90°);
+//   2. CLOCKWISE angle scale       -> mirror the raw angle (θ -> −θ) BEFORE
+//      the offset (all Slamtec RPLIDARs per the Interface Protocol spec;
+//      OLEI LR-1BS5, field-verified). Decoding a CW scale as CCW mirrors the
+//      whole world left/right — undetectable with a single lidar (the SLAM
+//      map is self-consistently mirrored and heading checks still pass), so
+//      verify handedness against the real room, not just against motion;
+//   3. upside-down mounting        -> the physical flip reverses the
+//      apparent rotation, one more angle negation (the driver's `inverted`).
+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,60 +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;    // OLEI only
-    std::string brand     = "OLEI";   // "OLEI" or "SICK"
-
-    // 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.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); 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/lidar.hpp

@@ -1,157 +0,0 @@
-#pragma once
-#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;
-};
-
-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;
-
-    virtual bool open() = 0;
-    virtual void close() = 0;
-    // Block until one full scan; false on error/timeout. 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;
-    virtual void set_scan_callback(ScanCallback cb) = 0;
-    virtual bool spin_once() = 0;
-    virtual const char* detected_model() const = 0;
-};
-
-// 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;
-
-    bool 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;
-
-    // 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(); }
-
-private:
-    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_;
-
-    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,5 +0,0 @@
-#pragma once
-// One-include convenience header for the whole public API.
-#include "lidarlib/lidar.hpp"
-#include "lidarlib/sick_lidar.hpp"
-#include "lidarlib/config.hpp"

include/lidarlib/sick_lidar.hpp

@@ -1,100 +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;
-
-    explicit SickDriver(const ModelConfig& cfg,
-                         const std::string& ip,
-                         uint16_t            port = 2111);
-    ~SickDriver();
-
-    SickDriver(const SickDriver&)            = delete;
-    SickDriver& operator=(const SickDriver&) = delete;
-
-    // Connect + send "sEN LMDscandata 1" to start continuous scan output.
-    bool 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;
-
-    // No model string on the wire — returns the configured name.
-    const char* detected_model() const override { return detected_model_name_.c_str(); }
-
-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_;
-    int          sock_fd_ = -1;
-    ScanCallback cb_;
-
-    // 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.
-    explicit NanoScanDriver(const ModelConfig& cfg,
-                            const std::string& ip   = "0.0.0.0",
-                            uint16_t            port = 6060);
-    ~NanoScanDriver();
-
-    NanoScanDriver(const NanoScanDriver&)            = delete;
-    NanoScanDriver& operator=(const NanoScanDriver&) = delete;
-
-    bool 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;
-
-    // No model string on the wire — returns the configured name.
-    const char* detected_model() const override { return detected_model_name_.c_str(); }
-
-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_;
-    int          sock_fd_ = -1;
-    ScanCallback cb_;
-
-    // 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

@@ -0,0 +1,402 @@
+// ESPE LGA60 — "HISN" range frames + "WSimu" area frames over TCP/UDP.
+#include "espe_driver.hpp"
+#include "plugin_helpers.hpp"
+
+#include <algorithm>
+#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 {
+// "RAuto" + fixed tail — puts the device into continuous measurement output.
+constexpr uint8_t kStartCapture[8] = {0x52, 0x41, 0x75, 0x74, 0x6F, 0x01, 0x87, 0x80};
+
+constexpr char kRangeMagic[4] = {'H', 'I', 'S', 'N'};
+constexpr char kAreaMagic[5]  = {'W', 'S', 'i', 'm', 'u'};
+
+constexpr size_t   kRangeHeaderSize = 16;    // magic + 6 big-endian u16 fields
+constexpr size_t   kAreaFrameSize   = 13;    // magic + 4 status bytes + err u16 + crc u16
+constexpr uint16_t kMaxDistanceMm   = 50000; // wire sentinel: beyond = no return
+constexpr uint16_t kMaxIntensity    = 30000;
+constexpr uint32_t kMaxPointsPerRev = 12800; // 320° at the finest 0.025° step
+constexpr int      kConnectTimeoutMs = 2000;
+
+// The head measures over a fixed window of every turn — from 20° to 340° in
+// device angles, 0° at the rear — and is blind over the remaining 40°. Both
+// the revolution's size and every packet's position are anchored to that
+// window (the vendor ROS driver hard-codes the same two numbers). Consistent
+// with MODEL_ESPE_LGA60's -160…160 preset once angle_offset_deg (-180) is
+// applied.
+constexpr uint16_t kSweepStartDeg = 20;
+constexpr uint16_t kSweepEndDeg   = 340;
+constexpr float    kSweepSpanDeg  = static_cast<float>(kSweepEndDeg - kSweepStartDeg);
+constexpr float    kFullTurnDeg   = 360.f;
+
+// Rotation periods above this are stalls/reconnects, not a spin rate (the
+// device runs at 10 or 20 Hz): don't publish timing derived from them.
+constexpr float kMaxRevPeriodS = 1.f;
+
+// A point no packet ever delivered — "invalid", distinct from the infinity
+// that means the device looked and got no return.
+const float kMissingPoint = std::numeric_limits<float>::quiet_NaN();
+
+uint16_t be16(const uint8_t* p) {
+    return static_cast<uint16_t>((p[0] << 8) | p[1]);
+}
+} // namespace
+
+EspeDriver::EspeDriver(const ModelConfig& cfg, const std::string& ip,
+                       uint16_t port, bool use_udp, bool inverted)
+    : cfg_(cfg), detected_model_name_(cfg.name ? cfg.name : ""), ip_(ip),
+      port_(port), use_udp_(use_udp), inverted_(inverted) {}
+
+EspeDriver::~EspeDriver() { close(); }
+
+ErrorCode EspeDriver::open() {
+    if (is_open()) return set_error(ErrorCode::AlreadyOpen);
+
+    sockaddr_in addr{};
+    addr.sin_family = AF_INET;
+    addr.sin_port   = htons(port_);
+    if (::inet_pton(AF_INET, ip_.c_str(), &addr.sin_addr) != 1)
+        return set_error(ErrorCode::InvalidAddress);
+
+    sock_fd_ = ::socket(AF_INET, use_udp_ ? SOCK_DGRAM : SOCK_STREAM, 0);
+    if (sock_fd_ < 0) return set_error(ErrorCode::SocketError);
+
+    ErrorCode conn_err = ErrorCode::Ok;
+    if (use_udp_) {
+        // connect() on UDP just fixes the peer; replies come to our port.
+        if (::connect(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0)
+            conn_err = ErrorCode::ConnectionFailed;
+    } else {
+        conn_err = connect_tcp_with_timeout(sock_fd_, addr, kConnectTimeoutMs);
+    }
+
+    if (conn_err != ErrorCode::Ok) {
+        ::close(sock_fd_);
+        sock_fd_ = -1;
+        return set_error(conn_err);
+    }
+
+    recv_buf_.clear();
+    pending_ranges_.clear();
+    pending_intensities_.clear();
+    angle_inc_deg_  = 0.f;
+    points_total_   = 0;
+    have_last_rev_  = false;
+    scan_ready_     = false;
+    espe_error_status_.reset();
+    latest_diag_  = Diagnostics{};
+
+    // Device is passive until told to stream.
+    ssize_t n = ::send(sock_fd_, kStartCapture, sizeof(kStartCapture), 0);
+    if (n != static_cast<ssize_t>(sizeof(kStartCapture))) {
+        close();
+        return set_error(ErrorCode::HandshakeFailed);
+    }
+    return set_error(ErrorCode::Ok);
+}
+
+void EspeDriver::close() {
+    if (sock_fd_ >= 0) {
+        ::close(sock_fd_);
+        sock_fd_ = -1;
+    }
+}
+
+bool EspeDriver::fill_buffer(int timeout_ms) {
+    if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }
+
+    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 false;
+        }
+    }
+
+    char buf[4096];
+    ssize_t n = ::recv(sock_fd_, buf, sizeof(buf), 0);
+    if (n <= 0) { set_error(ErrorCode::DeviceDisconnected); return false; }
+    recv_buf_.append(buf, static_cast<size_t>(n));
+    return true;
+}
+
+// Consume complete frames from recv_buf_; returns true once a full revolution
+// has been assembled (ready_result_/scan_ready_ set by finish_scan()).
+bool EspeDriver::parse_buffer() {
+    for (;;) {
+        size_t range_pos = recv_buf_.find(kRangeMagic, 0, sizeof(kRangeMagic));
+        size_t area_pos  = recv_buf_.find(kAreaMagic,  0, sizeof(kAreaMagic));
+        size_t pos = std::min(range_pos, area_pos);
+        if (pos == std::string::npos) {
+            // No magic in sight: keep only a possible partial magic at the tail.
+            if (recv_buf_.size() > sizeof(kAreaMagic) - 1)
+                recv_buf_.erase(0, recv_buf_.size() - (sizeof(kAreaMagic) - 1));
+            return scan_ready_;
+        }
+        if (pos > 0) recv_buf_.erase(0, pos);
+
+        const uint8_t* d = reinterpret_cast<const uint8_t*>(recv_buf_.data());
+
+        if (area_pos < range_pos) {
+            if (recv_buf_.size() < kAreaFrameSize) return scan_ready_;
+            // Zone/obstacle frame — only sent when the host polls areas, but
+            // it carries the device fault word, so latch it if it appears.
+            // Byte order unverified on hardware: the protocol is mixed-endian
+            // (header fields big-endian, point payload little-endian) and no
+            // spec covers this field; little-endian assumed like the payload.
+            espe_error_status_ = le16(d + 9);
+            recv_buf_.erase(0, kAreaFrameSize);
+            continue;
+        }
+
+        if (recv_buf_.size() < kRangeHeaderSize) return scan_ready_;
+        uint16_t data_size    = be16(d + 8);
+        uint16_t measure_size = be16(d + 12);
+        if (measure_size == 0 || measure_size > kMaxPointsPerRev) {
+            recv_buf_.erase(0, sizeof(kRangeMagic));  // bogus header — resync
+            continue;
+        }
+        // Both counters describe this packet; the clamp is the vendor's.
+        if (data_size > measure_size) data_size = measure_size;
+
+        size_t frame_size = kRangeHeaderSize + static_cast<size_t>(data_size) * 4;
+        if (recv_buf_.size() < frame_size) return scan_ready_;
+
+        handle_range_frame(d, data_size);
+        recv_buf_.erase(0, frame_size);
+        // Stop as soon as a revolution completes — draining further frames
+        // could finish a second revolution and overwrite ready_result_ before
+        // the caller consumes it. Leftover bytes wait for the next call.
+        if (scan_ready_) return true;
+    }
+}
+
+// Latch the angular step and size the revolution around it. Kept stable once
+// latched: the wire angles are whole degrees, so a step re-derived from a
+// mid-sweep packet jitters, and re-sizing would drop the sweep in flight.
+void EspeDriver::set_resolution(float inc_deg) {
+    if (!(inc_deg > 0.f)) return;
+    const long total = std::lround(kSweepSpanDeg / inc_deg);
+    if (total < 2 || total > static_cast<long>(kMaxPointsPerRev)) return;  // implausible step
+    if (angle_inc_deg_ > 0.f && static_cast<uint32_t>(total) == points_total_) return;
+
+    angle_inc_deg_ = inc_deg;
+    points_total_  = static_cast<uint32_t>(total);
+    begin_revolution();
+}
+
+void EspeDriver::begin_revolution() {
+    if (points_total_ == 0) return;
+    pending_ranges_.assign(points_total_, kMissingPoint);
+    pending_intensities_.assign(points_total_, 0.f);
+}
+
+// Range frame: "HISN", then big-endian u16 start_angle, end_angle (the
+// angular window THIS packet covers, whole degrees), data_size (points in
+// this packet's payload), data_position and measure_size (the vendor's
+// "position"/"count" of the current packet's points), time; then data_size ×
+// 4 B little-endian (u16 distance mm, u16 intensity).
+//
+// A packet is a slice of the sweep, not a revolution: the device splits every
+// 20°→340° sweep into several of them, the first opening at 20° and the last
+// closing at 340°. So the revolution holds 320°/step points, NOT measure_size
+// — reading measure_size as the revolution size (as this driver first did)
+// emits one scan per packet, each covering only that packet's few degrees.
+void EspeDriver::handle_range_frame(const uint8_t* frame, uint16_t data_size) {
+    const uint16_t start_angle   = be16(frame + 4);
+    const uint16_t end_angle     = be16(frame + 6);
+    const uint16_t data_position = be16(frame + 10);
+    const uint16_t measure_size  = be16(frame + 12);
+    // frame + 14 is a 16-bit device counter the vendor header marks as "time
+    // flag (not enabled)"; see finish_scan() for why it is not a timestamp.
+
+    // Step = this packet's angular span / its point count. Taken from the
+    // packet that opens a sweep — the one the vendor driver trusts — or from
+    // whatever arrives first while nothing is latched yet.
+    if (start_angle == kSweepStartDeg || angle_inc_deg_ <= 0.f) {
+        const float span = static_cast<float>(end_angle) - static_cast<float>(start_angle);
+        if (span > 0.f && measure_size > 0)
+            set_resolution(span / static_cast<float>(measure_size));
+    }
+    if (angle_inc_deg_ <= 0.f) return;  // step still unknown — nowhere to put the points
+
+    // First packet of a sweep: drop anything a lost closing packet left behind.
+    if (start_angle == kSweepStartDeg && data_position <= data_size) begin_revolution();
+
+    // Index of this packet's first point within the sweep, verbatim from the
+    // vendor driver: the angular offset from 20°, plus what the header's own
+    // counters carry. Exactly one of the two terms moves, whichever way the
+    // firmware numbers its packets — either start_angle walks the sweep while
+    // data_position stays at this packet's own count, or start_angle stays at
+    // 20° while data_position accumulates — so the sum is the packet's true
+    // start index in both cases.
+    const int32_t begin =
+        static_cast<int32_t>(std::lround(
+            (static_cast<float>(start_angle) - static_cast<float>(kSweepStartDeg)) / angle_inc_deg_))
+        + static_cast<int32_t>(data_position) - static_cast<int32_t>(data_size);
+
+    // Integer wire angles make 320°/step land a point or two short of what the
+    // device actually streams; grow rather than clip the tail (the vendor
+    // driver does the same).
+    const size_t needed = static_cast<size_t>(begin > 0 ? begin : 0) + data_size;
+    if (needed > pending_ranges_.size() && needed <= kMaxPointsPerRev) {
+        pending_ranges_.resize(needed, kMissingPoint);
+        pending_intensities_.resize(needed, 0.f);
+        points_total_ = static_cast<uint32_t>(needed);
+    }
+
+    const uint8_t* p = frame + kRangeHeaderSize;
+    for (uint16_t i = 0; i < data_size; ++i, p += 4) {
+        const int32_t idx = begin + i;
+        if (idx < 0 || idx >= static_cast<int32_t>(pending_ranges_.size())) continue;
+        const uint16_t dist  = le16(p + 0);
+        const uint16_t inten = le16(p + 2);
+        pending_ranges_[idx] = (dist > kMaxDistanceMm)
+            ? std::numeric_limits<float>::infinity()
+            : static_cast<float>(dist) * 1e-3f;  // mm -> m
+        // Wire intensity is 0..30000 — rescale to the 0-255 LaserScan contract.
+        pending_intensities_[idx] =
+            static_cast<float>(inten > kMaxIntensity ? kMaxIntensity : inten)
+            * (255.f / kMaxIntensity);
+    }
+
+    // The packet that closes the sweep at 340°, with its point counter full,
+    // ends the revolution — the vendor driver's condition unchanged. It holds
+    // whichever way the firmware numbers packets: per-packet counters make
+    // data_position == measure_size true on every packet (so the 340° edge
+    // decides), cumulative ones make it true only on the sweep's last packet.
+    if (end_angle == kSweepEndDeg && data_position == measure_size) finish_scan();
+}
+
+void EspeDriver::finish_scan() {
+    if (pending_ranges_.size() < 2 || angle_inc_deg_ <= 0.f) return;
+
+    LaserScan& scan = ready_result_.scan;
+    scan = LaserScan{};
+    // The header's 16-bit "time" field is a device counter of unverified unit
+    // (the vendor header calls it "not enabled" and its ROS driver never
+    // stamps a scan with it), while this field is contracted to be a device
+    // clock in ms — leave it at 0 and report timing from the rotation below.
+    scan.timestamp_ms = 0;
+    scan.ranges      = std::move(pending_ranges_);
+    scan.intensities = std::move(pending_intensities_);
+    scan.angle_min       = (static_cast<float>(kSweepStartDeg) + cfg_.angle_offset_deg) * kDeg2Rad;
+    scan.angle_increment = angle_inc_deg_ * kDeg2Rad;
+    scan.angle_max       = scan.angle_min +
+        scan.angle_increment * static_cast<float>(scan.ranges.size() - 1);
+    scan.range_min = cfg_.range_min_m;
+    scan.range_max = cfg_.range_max_m;
+
+    // Timing: revolutions complete one rotation period apart, but the points
+    // in one only span the 320° the head measures — the remaining 40° is dead
+    // time before the next sweep starts. scan_time is what a consumer
+    // subtracts from the arrival time to date the FIRST point, so it must be
+    // the sweep, not the period (the vendor ROS driver stamps with the same
+    // 320/360 factor). Consequence: a spin rate read back as 1/scan_time is
+    // 360/320 higher than the mechanical one. The first revolution has
+    // nothing to measure against — leave the fields at 0 and let the consumer
+    // fall back.
+    const auto now = std::chrono::steady_clock::now();
+    if (have_last_rev_) {
+        const float period = std::chrono::duration<float>(now - last_rev_end_).count();
+        if (period > 0.f && period < kMaxRevPeriodS) {
+            scan.scan_time      = period * (kSweepSpanDeg / kFullTurnDeg);
+            scan.time_increment = scan.scan_time / static_cast<float>(scan.ranges.size() - 1);
+        }
+    }
+    last_rev_end_  = now;
+    have_last_rev_ = true;
+
+    finalize_scan(scan, cfg_, inverted_);
+
+    ExtraInfo& info = ready_result_.info;
+    info = ExtraInfo{};
+    info.detected_model    = cfg_.name;
+    info.espe_error_status = espe_error_status_;
+
+    latest_diag_ = decode_diagnostics(info);
+    mark_scan_decoded();
+
+    begin_revolution();  // the vectors above were moved out — restore them
+    scan_ready_ = true;
+}
+
+bool EspeDriver::recv_scan(ScanResult& out, int timeout_ms) {
+    for (;;) {
+        if (parse_buffer()) {
+            scan_ready_ = false;
+            out = std::move(ready_result_);
+            set_error(ErrorCode::Ok);
+            return true;
+        }
+        if (!fill_buffer(timeout_ms)) return false;
+    }
+}
+
+bool EspeDriver::spin_once() {
+    if (!parse_buffer()) {
+        if (!fill_buffer(0)) return false;
+        parse_buffer();
+    }
+    if (scan_ready_) {
+        scan_ready_ = false;
+        if (cb_) cb_(ready_result_);
+    }
+    return true;
+}
+
+// ── 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

@@ -0,0 +1,101 @@
+// ESPE LGA60 laser scanner over TCP or UDP — plugin-private header.
+#pragma once
+#include "lidar_interface.hpp"
+
+#include <chrono>
+#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;
+// the wire caps distance at 50000 mm.
+inline constexpr ModelConfig MODEL_ESPE_LGA60 { "ESPE-LGA60", -160.f, 160.f, 0.05f, 50.f, -180.f };
+
+// ESPE LGA60 over TCP (default port 8080) or UDP, ported from the vendor's
+// 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.
+//
+// The device streams each 20°→340° sweep as several packets, so a scan is
+// assembled across packets and only completes when the sweep closes at 340°
+// (see handle_range_frame). Rotation timing is measured host-side: the wire
+// carries no usable clock.
+class EspeDriver : public LidarDriverInterface {
+public:
+    // 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.
+    explicit EspeDriver(const ModelConfig& cfg,
+                        const std::string& ip,
+                        uint16_t            port     = 8080,
+                        bool                use_udp  = false,
+                        bool                inverted = false);
+    ~EspeDriver();
+
+    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;
+    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:
+    bool fill_buffer(int timeout_ms);   // one recv() into recv_buf_
+    bool parse_buffer();                 // consume frames; true when a scan completed
+    void handle_range_frame(const uint8_t* frame, uint16_t data_size);
+    void set_resolution(float inc_deg);  // latch the step + (re)size the revolution
+    void begin_revolution();             // blank the buffer for a fresh sweep
+    void finish_scan();
+
+    ModelConfig  cfg_;
+    std::string  detected_model_name_;  // owned copy of cfg_.name (stable lifetime)
+    std::string  ip_;
+    uint16_t     port_;
+    bool         use_udp_  = false;
+    bool         inverted_ = false;
+    int          sock_fd_ = -1;
+    ScanCallback cb_;
+
+    // Stream bytes carried across frame boundaries; per-instance.
+    std::string recv_buf_;
+
+    // Per-revolution accumulation. A revolution is the device's fixed
+    // 20°→340° sweep, so it holds 320°/angle_inc_deg_ points — several
+    // packets' worth. The header's point counters describe one PACKET and say
+    // nothing about the revolution's size.
+    std::vector<float> pending_ranges_;
+    std::vector<float> pending_intensities_;
+    float    angle_inc_deg_ = 0.f;  // angular step; 0 = not latched yet
+    uint32_t points_total_  = 0;    // points per revolution = 320° / step
+
+    // Rotation period, measured host-side between completed revolutions: the
+    // wire carries no usable clock (see finish_scan()).
+    std::chrono::steady_clock::time_point last_rev_end_{};
+    bool                                  have_last_rev_ = false;
+
+    // Latched from the newest "WSimu" area frame, if the device sends any.
+    std::optional<uint16_t> espe_error_status_;
+
+    // Snapshot for get_diagnostics(); refreshed by finish_scan().
+    Diagnostics latest_diag_;
+
+    ScanResult ready_result_;
+    bool       scan_ready_ = false;
+};
+
+} // 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,15 +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) {
@@ -38,17 +40,56 @@ 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)
 
-Driver::Driver(const ModelConfig& cfg, const std::string& ip, uint16_t port, bool inverted)
+// Models whose angle scale runs CLOCKWISE (left-handed): decoding them as CCW
+// mirrors the world left/right. Undetectable with a single lidar (the SLAM map
+// is self-consistently mirrored, and heading-vs-motion checks pass because a
+// mirror about x preserves "ahead") — it only surfaces when a second,
+// right-handed lidar disagrees, or when the map is compared against the real
+// room.
+//
+// Evidence:
+//  - LR-1BS5: field-verified 2026-07-23 (rotation-only decode produced a
+//    left-right mirrored map versus the actual room; mirrored decode matches).
+//  - The Family B azimuth protocol itself is clockwise per DF Automation's
+//    production ROS driver (github.com/dfautomation/ole2d,
+//    src/olelidar/src/decoder.cpp: walks the device array backwards with the
+//    comment "reverse, laserscan is anticlockwise"; its packet layout —
+//    azimuth x0.01 deg, invalid sentinel 0xFF00 — is exactly our Family B).
+//    LR-1FMI speaks the same Family B protocol, so it is listed too.
+//  - Family A (VB/VF/LR-1F) and Family C (GS1-5) units are unverified; they
+//    stay CCW until checked against a real room.
+static bool model_angles_clockwise(const char* name) {
+    return std::strcmp(name, "LR-1BS5") == 0 || std::strcmp(name, "LR-1FMI") == 0;
+}
+
+OleiDriver::OleiDriver(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);
+    model_mirror_ = model_angles_clockwise(cfg.name);
 }
 
-Driver::~Driver() { close(); }
+// Device angle -> our CCW convention: clockwise models get their raw angle
+// negated BEFORE the model's 0°-reference offset is added (LR-1BS5: 0° at the
+// rear AND clockwise, so out = 180 − raw).
+float OleiDriver::device_deg(float raw_deg) const {
+    return (model_mirror_ ? -raw_deg : raw_deg) + cfg_.angle_offset_deg;
+}
+
+OleiDriver::~OleiDriver() { close(); }
+
+ErrorCode OleiDriver::open() {
+    if (is_open()) return set_error(ErrorCode::AlreadyOpen);
+
+    sockaddr_in addr{};
+    addr.sin_family = AF_INET;
+    addr.sin_port   = htons(port_);
+    if (::inet_pton(AF_INET, ip_.c_str(), &addr.sin_addr) != 1)
+        return set_error(ErrorCode::InvalidAddress);
 
-bool Driver::open() {
     sock_fd_ = ::socket(AF_INET, SOCK_DGRAM, 0);
-    if (sock_fd_ < 0) return false;
+    if (sock_fd_ < 0) return set_error(ErrorCode::SocketError);
 
     int reuse = 1;
     ::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));
@@ -56,30 +97,38 @@ bool Driver::open() {
     ::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEPORT, &reuse, sizeof(reuse));
 #endif
 
-    sockaddr_in addr{};
-    addr.sin_family      = AF_INET;
-    addr.sin_port        = htons(port_);
-    addr.sin_addr.s_addr = inet_addr(ip_.c_str());
-
     if (::bind(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0) {
+        int err = errno;
         ::close(sock_fd_);
         sock_fd_ = -1;
-        return false;
+        return set_error((err == EADDRINUSE || err == EACCES) ? ErrorCode::PortInUse
+                                                              : ErrorCode::BindFailed);
     }
+
+    // Reset per-revolution state so a close()/open() cycle starts clean.
+    pending_angle_deg_.clear();
+    pending_dist_m_.clear();
+    pending_intensity_.clear();
+    pending_info_ = ExtraInfo{};
+    latest_diag_  = Diagnostics{};
+    last_angle_   = -1.f;
+    scan_ready_   = false;
+
     pending_angle_deg_.reserve(2048);
     pending_dist_m_.reserve(2048);
     pending_intensity_.reserve(2048);
-    return true;
+    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;
 
     while (!scan_ready_) {
@@ -87,22 +136,36 @@ bool Driver::recv_scan(ScanResult& out, int timeout_ms) {
             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) return false;
+            if (r <= 0) {
+                set_error(r == 0 ? ErrorCode::Timeout : ErrorCode::DeviceDisconnected);
+                return false;
+            }
         }
-        if (!spin_once()) return false;
+        if (!poll_packet()) return false;
     }
     out = std::move(ready_result_);
+    set_error(ErrorCode::Ok);
     return true;
 }
 
-bool Driver::spin_once() {
+bool OleiDriver::spin_once() {
+    if (!poll_packet()) return false;
+    if (scan_ready_) {
+        scan_ready_ = false;
+        if (cb_) cb_(ready_result_);
+    }
+    return true;
+}
+
+bool OleiDriver::poll_packet() {
+    if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }
     uint8_t* buf = recv_buf_;
     sockaddr_in from{};
     socklen_t   fromlen = sizeof(from);
 
     ssize_t n = ::recvfrom(sock_fd_, buf, sizeof(recv_buf_), 0,
                             reinterpret_cast<sockaddr*>(&from), &fromlen);
-    if (n < 0) return false;
+    if (n < 0) { set_error(ErrorCode::DeviceDisconnected); return false; }
 
     // A/C carry the frame id at [0-1]; B has a 0x010F preamble, real id at [2-3].
     if (n < 4) return true;
@@ -117,7 +180,7 @@ bool Driver::spin_once() {
 }
 
 // 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();
@@ -129,7 +192,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();
@@ -147,25 +210,27 @@ 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_;
     info.detected_model = detected_model_name_;
     info.error_status    = pending_err_;
 
+    latest_diag_ = decode_diagnostics(info);
+    latest_diag_.device_timestamp_ms = scan.timestamp_ms;
+    mark_scan_decoded();
+
     pending_angle_deg_.clear();
     pending_dist_m_.clear();
     pending_intensity_.clear();
     pending_info_ = ExtraInfo{};
     scan_ready_   = true;
-
-    if (cb_) cb_(ready_result_);
 }
 
 // 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;
 
@@ -203,7 +268,7 @@ bool Driver::parse_family_a(const uint8_t* buf, int len) {
         uint8_t  intensity = blk[2];
 
         float frac  = (num_pts > 1) ? static_cast<float>(i) / (num_pts - 1) : 0.f;
-        float angle = to_signed_deg(ang_start + frac * (ang_end - ang_start) + cfg_.angle_offset_deg);
+        float angle = to_signed_deg(device_deg(ang_start + frac * (ang_end - ang_start)));
         angle       = maybe_invert(angle, inverted_);
 
         if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue;
@@ -217,7 +282,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;
 
@@ -250,6 +315,7 @@ bool Driver::parse_family_b(const uint8_t* buf, int len) {
                     cfg_.range_min_m      = entry.cfg->range_min_m;
                     cfg_.range_max_m      = entry.cfg->range_max_m;
                     cfg_.angle_offset_deg = entry.cfg->angle_offset_deg;
+                    model_mirror_         = model_angles_clockwise(entry.cfg->name);
                     break;
                 }
             }
@@ -273,7 +339,7 @@ bool Driver::parse_family_b(const uint8_t* buf, int len) {
         }
         last_angle_ = dev_deg;
 
-        float    angle     = maybe_invert(to_signed_deg(angle_raw * 0.01f + cfg_.angle_offset_deg), inverted_);
+        float    angle     = maybe_invert(to_signed_deg(device_deg(angle_raw * 0.01f)), inverted_);
         float    dist_m    = le16(blk + 2) * scale_mm * 0.001f;
         uint8_t  intensity = static_cast<uint8_t>(le16(blk + 4) >> 2);   // 10-bit → 8-bit
 
@@ -288,7 +354,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;
 
@@ -362,7 +428,8 @@ bool Driver::parse_family_c(const uint8_t* buf, int len) {
             range_mm = le16(blk + 2);
         }
 
-        float angle = to_signed_deg(static_cast<float>(first_index + i) * angle_inc - 180.f + cfg_.angle_offset_deg);
+        float angle = to_signed_deg(
+            device_deg(static_cast<float>(first_index + i) * angle_inc - 180.f));
         angle       = maybe_invert(angle, inverted_);
         if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue;
 
@@ -374,4 +441,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,96 @@
+// 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, 180.f };  // 2D 360° (Family B); device 0° = rear AND the angle scale runs CLOCKWISE — the driver mirrors it (out = 180 − raw, see model_angles_clockwise). Field-verified on OLELR-1BS5: offset 0 flipped the SLAM heading, rotation-only 180 then left-right mirrored the whole map.
+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();
+    // Raw device angle -> our CCW convention (negated for clockwise-scale
+    // models, then the model's 0°-reference offset).
+    float device_deg(float raw_deg) const;
+
+    ModelConfig  cfg_;
+    std::string  ip_;
+    uint16_t     port_;
+    bool         inverted_  = false;
+    // Model's angle scale runs clockwise (e.g. LR-1BS5): device_deg() negates
+    // the raw angle so the output is right-handed. Set from the preset in the
+    // ctor and again when AUTO locks onto a detected model.
+    bool         model_mirror_ = 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,12 @@
+# Compiles the vendored Slamtec SDK (third_party/rplidar_sdk) straight into
+# the plugin .so — no external dependency.
+set(RPLIDAR_SDK_DIR ${CMAKE_SOURCE_DIR}/third_party/rplidar_sdk)
+
+file(GLOB_RECURSE RPLIDAR_SDK_SOURCES CONFIGURE_DEPENDS
+    ${RPLIDAR_SDK_DIR}/src/*.cpp)
+
+xlidar_add_plugin(driver_rplidar rplidar_driver.cpp ${RPLIDAR_SDK_SOURCES})
+target_include_directories(driver_rplidar SYSTEM PRIVATE
+    ${RPLIDAR_SDK_DIR}/include
+    ${RPLIDAR_SDK_DIR}/src
+)

plugins/driver_rplidar/rplidar_driver.cpp

@@ -0,0 +1,343 @@
+// Slamtec RPLIDAR over serial (C1 defaults), built on the vendored SDK
+// (third_party/rplidar_sdk, sl_lidar.h). Scan math: angle/distance decoding,
+// inversion, FOV window, invalid points as NaN.
+//
+// Unlike the network drivers, angles arrive in the DEVICE frame [0, 2π),
+// 0 = ahead, ascending (the SDK's ascendScanData order) — but Slamtec's
+// angle scale runs CLOCKWISE viewed from the top, so decode mirrors it into
+// our right-handed convention (see the handedness comment in recv_scan).
+#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{};
+        // Handedness: Slamtec devices count their angle CLOCKWISE viewed from
+        // the top (Interface Protocol spec; the official rplidar_ros node
+        // mirrors the angles for exactly this reason). Our output convention
+        // is right-handed (CCW, + = left), so:
+        //   - right-side-up (inverted_ == false): mirror (angle' = 2π − raw)
+        //     and walk the nodes backwards to keep ascending order;
+        //   - upside-down (inverted_ == true): the flip makes the rotation
+        //     appear CCW from above, so the raw ascending angles are already
+        //     right-handed — use them as-is.
+        // Field-verified (2026-07-23): a right-side-up C1 decoded as CCW
+        // produced a left-right mirrored sweep against a verified
+        // right-handed reference lidar; invisible with the C1 alone because
+        // a single-sensor SLAM map is self-consistently mirrored.
+        if (inverted_) {
+            scan.angle_min = angle_first;
+            scan.angle_max = angle_last;
+        } else {
+            scan.angle_min = kTwoPi - angle_last;
+            scan.angle_max = kTwoPi - angle_first;
+        }
+        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) {
+            // Mirrored branches walk backwards (see the handedness comment
+            // above): backwards for a right-side-up unit, forwards when the
+            // physical flip already reversed the apparent rotation.
+            const std::size_t node_index = inverted_ ? i : count - 1 - 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

@@ -0,0 +1,322 @@
+// SICK TiM 5xx/7xx — SOPAS/CoLa-A ASCII telegrams over TCP ("sSN/sRA
+// LMDscandata" parsing).
+#include "sick_tim_driver.hpp"
+#include "plugin_helpers.hpp"
+
+#include <cctype>
+#include <cerrno>
+#include <cstdlib>
+#include <cstring>
+#include <vector>
+#include <arpa/inet.h>
+#include <netinet/in.h>
+#include <sys/select.h>
+#include <sys/socket.h>
+#include <unistd.h>
+
+namespace xlidar {
+
+namespace {
+constexpr char  kStx              = 0x02;
+constexpr char  kEtx              = 0x03;
+constexpr int   kConnectTimeoutMs = 2000;
+
+uint32_t hex_to_u32(const std::string& tok) {
+    return static_cast<uint32_t>(std::strtoul(tok.c_str(), nullptr, 16));
+}
+int32_t hex_to_i32(const std::string& tok) {
+    // SICK encodes signed fields as plain hex of the 2's-complement bits.
+    return static_cast<int32_t>(hex_to_u32(tok));
+}
+
+std::vector<std::string> tokenize(const std::string& s) {
+    std::vector<std::string> out;
+    size_t i = 0, n = s.size();
+    while (i < n) {
+        while (i < n && std::isspace(static_cast<unsigned char>(s[i]))) ++i;
+        size_t start = i;
+        while (i < n && !std::isspace(static_cast<unsigned char>(s[i]))) ++i;
+        if (i > start) out.push_back(s.substr(start, i - start));
+    }
+    return out;
+}
+} // namespace
+
+SickTimDriver::SickTimDriver(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) {}
+
+SickTimDriver::~SickTimDriver() { close(); }
+
+ErrorCode SickTimDriver::open() {
+    if (is_open()) return set_error(ErrorCode::AlreadyOpen);
+
+    sockaddr_in addr{};
+    addr.sin_family = AF_INET;
+    addr.sin_port   = htons(port_);
+    if (::inet_pton(AF_INET, ip_.c_str(), &addr.sin_addr) != 1)
+        return set_error(ErrorCode::InvalidAddress);
+
+    sock_fd_ = ::socket(AF_INET, SOCK_STREAM, 0);
+    if (sock_fd_ < 0) return set_error(ErrorCode::SocketError);
+
+    ErrorCode conn_err = connect_tcp_with_timeout(sock_fd_, addr, kConnectTimeoutMs);
+    if (conn_err != ErrorCode::Ok) {
+        ::close(sock_fd_);
+        sock_fd_ = -1;
+        return set_error(conn_err);
+    }
+
+    recv_buf_.clear();
+    latest_diag_ = Diagnostics{};
+
+    // Device is passive until told to stream.
+    if (!send_telegram("sEN LMDscandata 1")) {
+        close();
+        return set_error(ErrorCode::HandshakeFailed);
+    }
+    return set_error(ErrorCode::Ok);
+}
+
+void SickTimDriver::close() {
+    if (sock_fd_ >= 0) {
+        send_telegram("sEN LMDscandata 0");  // best-effort
+        ::close(sock_fd_);
+        sock_fd_ = -1;
+    }
+}
+
+bool SickTimDriver::send_telegram(const std::string& body) {
+    if (sock_fd_ < 0) return false;
+    std::string framed;
+    framed.reserve(body.size() + 2);
+    framed.push_back(kStx);
+    framed += body;
+    framed.push_back(kEtx);
+
+    size_t sent = 0;
+    while (sent < framed.size()) {
+        ssize_t n = ::send(sock_fd_, framed.data() + sent, framed.size() - sent, 0);
+        if (n <= 0) return false;
+        sent += static_cast<size_t>(n);
+    }
+    return true;
+}
+
+// CoLa-A has no length prefix, so ETX is the only frame boundary; recv_buf_
+// carries leftover bytes across calls.
+bool SickTimDriver::read_telegram(std::string& out, int timeout_ms) {
+    if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }
+
+    for (;;) {
+        size_t etx_pos = recv_buf_.find(kEtx);
+        if (etx_pos != std::string::npos) {
+            size_t stx_pos = recv_buf_.find(kStx);
+            if (stx_pos == std::string::npos || stx_pos > etx_pos) {
+                recv_buf_.erase(0, etx_pos + 1);
+                continue;
+            }
+            out = recv_buf_.substr(stx_pos + 1, etx_pos - stx_pos - 1);
+            recv_buf_.erase(0, etx_pos + 1);
+            return true;
+        }
+
+        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 false;
+            }
+        }
+
+        char buf[4096];
+        ssize_t n = ::recv(sock_fd_, buf, sizeof(buf), 0);
+        if (n <= 0) { set_error(ErrorCode::DeviceDisconnected); return false; }
+        recv_buf_.append(buf, static_cast<size_t>(n));
+    }
+}
+
+bool SickTimDriver::recv_scan(ScanResult& out, int timeout_ms) {
+    for (;;) {
+        std::string telegram;
+        if (!read_telegram(telegram, timeout_ms)) return false;
+        if (parse_lmdscandata(telegram, out)) { set_error(ErrorCode::Ok); return true; }
+        // Non-scan telegram (e.g. an ack) — keep waiting.
+    }
+}
+
+bool SickTimDriver::spin_once() {
+    std::string telegram;
+    if (!read_telegram(telegram, 0)) return false;
+
+    ScanResult result;
+    if (!parse_lmdscandata(telegram, result)) return true;
+    if (cb_) cb_(result);
+    return true;
+}
+
+// CoLa-A "sSN/sRA LMDscandata": space-separated ASCII hex tokens, field order
+// per SICK's Telegram Listing. "DIST1" → ranges, "RSSI1" → intensities.
+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;
+    if (tok[1] != "LMDscandata") return false;
+
+    size_t i = 2;
+    auto next = [&]() -> std::string { return (i < tok.size()) ? tok[i++] : std::string(); };
+
+    hex_to_u32(next());  // VersionNumber
+    hex_to_u32(next());  // DeviceNumber
+    hex_to_u32(next());  // SerialNumber
+    uint32_t status0              = hex_to_u32(next());
+    uint32_t status1              = hex_to_u32(next());
+    hex_to_u32(next());  // TelegramCounter
+    hex_to_u32(next());  // ScanCounter
+    hex_to_u32(next());  // TimeSinceStartup
+    uint32_t time_of_transmission = hex_to_u32(next());
+    uint32_t in0  = hex_to_u32(next());
+    uint32_t in1  = hex_to_u32(next());
+    uint32_t out0 = hex_to_u32(next());
+    uint32_t out1 = hex_to_u32(next());
+    next();  // Reserved
+    uint32_t scanning_frequency = hex_to_u32(next());
+    hex_to_u32(next());  // MeasurementFrequency
+
+    uint32_t num_encoders = hex_to_u32(next());
+    for (uint32_t e = 0; e < num_encoders; ++e) {
+        next();  // EncoderPosition
+        next();  // EncoderSpeed
+    }
+
+    LaserScan& scan = out.scan;
+    scan.ranges.clear();
+    scan.intensities.clear();
+    float angle_min_deg = 0.f, angle_inc_deg = 0.f;
+    bool  got_dist       = false;
+
+    // 16-bit and 8-bit channel blocks share the same ASCII layout.
+    auto parse_channel_block = [&]() {
+        std::string content    = next();
+        uint32_t    scale_bits = hex_to_u32(next());
+        hex_to_u32(next());                            // ScalingOffset
+        int32_t     start_angle = hex_to_i32(next());  // 1/10000 deg
+        int32_t     step_width  = hex_to_i32(next());  // 1/10000 deg
+        uint32_t    num_data    = hex_to_u32(next());
+
+        float scale = bits_to_float(scale_bits);
+        if (scale == 0.f) scale = 1.f;
+
+        bool is_dist = content.rfind("DIST", 0) == 0;
+        bool is_rssi = content.rfind("RSSI", 0) == 0;
+
+        if (is_dist) {
+            angle_min_deg = static_cast<float>(start_angle) * 0.0001f + cfg_.angle_offset_deg;
+            angle_inc_deg = static_cast<float>(step_width)  * 0.0001f;
+            scan.ranges.assign(num_data, 0.f);
+        } else if (is_rssi && scan.intensities.empty()) {
+            scan.intensities.assign(num_data, 0.f);
+        }
+
+        for (uint32_t d = 0; d < num_data; ++d) {
+            uint32_t raw = hex_to_u32(next());
+            if (is_dist) {
+                scan.ranges[d] = static_cast<float>(raw) * scale * 0.001f;  // mm -> m
+                got_dist       = true;
+            } else if (is_rssi && d < scan.intensities.size()) {
+                // Clamp to the 0-255 LaserScan contract (16-bit RSSI can exceed it).
+                float v = static_cast<float>(raw) * scale;
+                scan.intensities[d] = v > 255.f ? 255.f : v;
+            }
+        }
+    };
+
+    uint32_t num_16bit_channels = hex_to_u32(next());
+    for (uint32_t c = 0; c < num_16bit_channels; ++c) parse_channel_block();
+
+    uint32_t num_8bit_channels = hex_to_u32(next());
+    for (uint32_t c = 0; c < num_8bit_channels; ++c) parse_channel_block();
+
+    if (!got_dist || scan.ranges.empty()) return false;
+
+    scan.timestamp_ms    = time_of_transmission;
+    scan.angle_min       = angle_min_deg * kDeg2Rad;
+    scan.angle_increment = angle_inc_deg * kDeg2Rad;
+    scan.angle_max        = scan.angle_min +
+        scan.angle_increment * static_cast<float>(scan.ranges.size() - 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;
+    if (scan.intensities.size() != scan.ranges.size())
+        scan.intensities.assign(scan.ranges.size(), 0.f);
+
+    finalize_scan(scan, cfg_, inverted_);
+
+    ExtraInfo& info = out.info;
+    info = ExtraInfo{};
+    info.detected_model     = cfg_.name;
+    info.sick_device_status = static_cast<uint16_t>(((status0 & 0xFF) << 8) | (status1 & 0xFF));
+    info.status_flags       = (status0 << 8) | status1;
+    info.scan_frequency_raw = static_cast<uint16_t>(scanning_frequency);
+    info.input_status        = static_cast<uint16_t>((in0 << 8) | in1);
+    info.output_status        = static_cast<uint16_t>((out0 << 8) | out1);
+
+    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       = "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;
+}();
+
+const ModelConfig* model_by_name(const std::string& name) {
+    static constexpr const ModelConfig* kModels[] = {
+        &MODEL_SICK_TIM5XX, &MODEL_SICK_TIM571, &MODEL_SICK_TIM7XX,
+    };
+    for (const ModelConfig* m : kModels)
+        if (name == m->name) return m;
+    return nullptr;
+}
+
+} // namespace
+
+DriverInfo SickTimDriver::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_SICK_TIM571;  // brand default
+    const uint16_t port = cfg->port ? cfg->port : 2111;
+    return new SickTimDriver(apply_device_config(*preset, *cfg), cfg->ip, port, cfg->inverted);
+}

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,42 +0,0 @@
-// Internal helpers shared by the driver TUs — not part of the public API.
-#pragma once
-#include "lidarlib/lidar.hpp"
-#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;
-}
-
-// 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,161 +0,0 @@
-#include "lidarlib/config.hpp"
-#include "lidarlib/sick_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" },
-};
-
-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("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.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"};
-    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 string "SICK" is OLEI (keeps brand-less configs working).
-    const bool is_sick = (cfg.brand == "SICK");
-
-    const ModelConfig* model = model_by_name_for_brand(cfg.model, is_sick ? "SICK" : "OLEI");
-    if (!model) model = is_sick ? &MODEL_SICK_TIM571 : &MODEL_AUTO;
-
-    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);
-        return std::make_unique<SickDriver>(mc, cfg.ip, cfg.port);
-    }
-    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/sick_lidar.cpp

@@ -1,466 +0,0 @@
-#include "lidarlib/sick_lidar.hpp"
-#include "lidar_bytes.hpp"
-
-#include <cctype>
-#include <cerrno>
-#include <cmath>
-#include <cstdlib>
-#include <cstring>
-#include <limits>
-#include <vector>
-#include <fcntl.h>
-#include <sys/socket.h>
-#include <arpa/inet.h>
-#include <unistd.h>
-#include <sys/select.h>
-#include <netinet/in.h>
-#include <netinet/tcp.h>
-
-namespace lidarlib {
-
-namespace {
-constexpr char  kStx              = 0x02;
-constexpr char  kEtx              = 0x03;
-constexpr int   kConnectTimeoutMs = 2000;
-
-uint32_t hex_to_u32(const std::string& tok) {
-    return static_cast<uint32_t>(std::strtoul(tok.c_str(), nullptr, 16));
-}
-int32_t hex_to_i32(const std::string& tok) {
-    // SICK encodes signed fields as plain hex of the 2's-complement bits.
-    return static_cast<int32_t>(hex_to_u32(tok));
-}
-
-std::vector<std::string> tokenize(const std::string& s) {
-    std::vector<std::string> out;
-    size_t i = 0, n = s.size();
-    while (i < n) {
-        while (i < n && std::isspace(static_cast<unsigned char>(s[i]))) ++i;
-        size_t start = i;
-        while (i < n && !std::isspace(static_cast<unsigned char>(s[i]))) ++i;
-        if (i > start) out.push_back(s.substr(start, i - start));
-    }
-    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)
-    : cfg_(cfg), detected_model_name_(cfg.name ? cfg.name : ""), ip_(ip), port_(port) {}
-
-SickDriver::~SickDriver() { close(); }
-
-bool SickDriver::open() {
-    sock_fd_ = ::socket(AF_INET, SOCK_STREAM, 0);
-    if (sock_fd_ < 0) return false;
-
-    sockaddr_in addr{};
-    addr.sin_family      = AF_INET;
-    addr.sin_port        = htons(port_);
-    addr.sin_addr.s_addr = inet_addr(ip_.c_str());
-
-    // Non-blocking connect with a bounded timeout — a blocking connect() to an
-    // unreachable device would stall for the OS default (~2 min on Linux).
-    int flags = ::fcntl(sock_fd_, F_GETFL, 0);
-    ::fcntl(sock_fd_, F_SETFL, flags | O_NONBLOCK);
-
-    int rc = ::connect(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr));
-    if (rc < 0 && errno == EINPROGRESS) {
-        fd_set wfds; FD_ZERO(&wfds); FD_SET(sock_fd_, &wfds);
-        timeval tv{ kConnectTimeoutMs / 1000, (kConnectTimeoutMs % 1000) * 1000 };
-        rc = ::select(sock_fd_ + 1, nullptr, &wfds, nullptr, &tv);
-        if (rc > 0) {
-            int err = 0; socklen_t errlen = sizeof(err);
-            ::getsockopt(sock_fd_, SOL_SOCKET, SO_ERROR, &err, &errlen);
-            rc = (err == 0) ? 0 : -1;
-        } else {
-            rc = -1;
-        }
-    }
-    ::fcntl(sock_fd_, F_SETFL, flags);
-
-    if (rc < 0) {
-        ::close(sock_fd_);
-        sock_fd_ = -1;
-        return false;
-    }
-
-    int nodelay = 1;
-    ::setsockopt(sock_fd_, IPPROTO_TCP, TCP_NODELAY, &nodelay, sizeof(nodelay));
-
-    recv_buf_.clear();
-
-    // Device is passive until told to stream.
-    if (!send_telegram("sEN LMDscandata 1")) {
-        close();
-        return false;
-    }
-    return true;
-}
-
-void SickDriver::close() {
-    if (sock_fd_ >= 0) {
-        send_telegram("sEN LMDscandata 0");  // best-effort
-        ::close(sock_fd_);
-        sock_fd_ = -1;
-    }
-}
-
-bool SickDriver::send_telegram(const std::string& body) {
-    if (sock_fd_ < 0) return false;
-    std::string framed;
-    framed.reserve(body.size() + 2);
-    framed.push_back(kStx);
-    framed += body;
-    framed.push_back(kEtx);
-
-    size_t sent = 0;
-    while (sent < framed.size()) {
-        ssize_t n = ::send(sock_fd_, framed.data() + sent, framed.size() - sent, 0);
-        if (n <= 0) return false;
-        sent += static_cast<size_t>(n);
-    }
-    return true;
-}
-
-// 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) {
-    if (sock_fd_ < 0) return false;
-
-    for (;;) {
-        size_t etx_pos = recv_buf_.find(kEtx);
-        if (etx_pos != std::string::npos) {
-            size_t stx_pos = recv_buf_.find(kStx);
-            if (stx_pos == std::string::npos || stx_pos > etx_pos) {
-                recv_buf_.erase(0, etx_pos + 1);
-                continue;
-            }
-            out = recv_buf_.substr(stx_pos + 1, etx_pos - stx_pos - 1);
-            recv_buf_.erase(0, etx_pos + 1);
-            return true;
-        }
-
-        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) return false;
-        }
-
-        char buf[4096];
-        ssize_t n = ::recv(sock_fd_, buf, sizeof(buf), 0);
-        if (n <= 0) return false;
-        recv_buf_.append(buf, static_cast<size_t>(n));
-    }
-}
-
-bool SickDriver::recv_scan(ScanResult& out, int timeout_ms) {
-    for (;;) {
-        std::string telegram;
-        if (!read_telegram(telegram, timeout_ms)) return false;
-        if (parse_lmdscandata(telegram, out)) return true;
-        // Non-scan telegram (e.g. an ack) — keep waiting.
-    }
-}
-
-bool SickDriver::spin_once() {
-    std::string telegram;
-    if (!read_telegram(telegram, 0)) return false;
-
-    ScanResult result;
-    if (!parse_lmdscandata(telegram, result)) return true;
-    if (cb_) cb_(result);
-    return true;
-}
-
-// 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) {
-    std::vector<std::string> tok = tokenize(telegram);
-    if (tok.size() < 20) return false;
-    if (tok[0] != "sSN" && tok[0] != "sRA") return false;
-    if (tok[1] != "LMDscandata") return false;
-
-    size_t i = 2;
-    auto next = [&]() -> std::string { return (i < tok.size()) ? tok[i++] : std::string(); };
-
-    hex_to_u32(next());  // VersionNumber
-    hex_to_u32(next());  // DeviceNumber
-    hex_to_u32(next());  // SerialNumber
-    uint32_t status0              = hex_to_u32(next());
-    uint32_t status1              = hex_to_u32(next());
-    hex_to_u32(next());  // TelegramCounter
-    hex_to_u32(next());  // ScanCounter
-    hex_to_u32(next());  // TimeSinceStartup
-    uint32_t time_of_transmission = hex_to_u32(next());
-    uint32_t in0  = hex_to_u32(next());
-    uint32_t in1  = hex_to_u32(next());
-    uint32_t out0 = hex_to_u32(next());
-    uint32_t out1 = hex_to_u32(next());
-    next();  // Reserved
-    uint32_t scanning_frequency = hex_to_u32(next());
-    hex_to_u32(next());  // MeasurementFrequency
-
-    uint32_t num_encoders = hex_to_u32(next());
-    for (uint32_t e = 0; e < num_encoders; ++e) {
-        next();  // EncoderPosition
-        next();  // EncoderSpeed
-    }
-
-    LaserScan& scan = out.scan;
-    scan.ranges.clear();
-    scan.intensities.clear();
-    float angle_min_deg = 0.f, angle_inc_deg = 0.f;
-    bool  got_dist       = false;
-
-    // 16-bit and 8-bit channel blocks share the same ASCII layout.
-    auto parse_channel_block = [&]() {
-        std::string content    = next();
-        uint32_t    scale_bits = hex_to_u32(next());
-        hex_to_u32(next());                            // ScalingOffset
-        int32_t     start_angle = hex_to_i32(next());  // 1/10000 deg
-        int32_t     step_width  = hex_to_i32(next());  // 1/10000 deg
-        uint32_t    num_data    = hex_to_u32(next());
-
-        float scale = bits_to_float(scale_bits);
-        if (scale == 0.f) scale = 1.f;
-
-        bool is_dist = content.rfind("DIST", 0) == 0;
-        bool is_rssi = content.rfind("RSSI", 0) == 0;
-
-        if (is_dist) {
-            angle_min_deg = static_cast<float>(start_angle) * 0.0001f + cfg_.angle_offset_deg;
-            angle_inc_deg = static_cast<float>(step_width)  * 0.0001f;
-            scan.ranges.assign(num_data, 0.f);
-        } else if (is_rssi && scan.intensities.empty()) {
-            scan.intensities.assign(num_data, 0.f);
-        }
-
-        for (uint32_t d = 0; d < num_data; ++d) {
-            uint32_t raw = hex_to_u32(next());
-            if (is_dist) {
-                scan.ranges[d] = static_cast<float>(raw) * scale * 0.001f;  // mm -> m
-                got_dist       = true;
-            } else if (is_rssi && d < scan.intensities.size()) {
-                scan.intensities[d] = static_cast<float>(raw) * scale;
-            }
-        }
-    };
-
-    uint32_t num_16bit_channels = hex_to_u32(next());
-    for (uint32_t c = 0; c < num_16bit_channels; ++c) parse_channel_block();
-
-    uint32_t num_8bit_channels = hex_to_u32(next());
-    for (uint32_t c = 0; c < num_8bit_channels; ++c) parse_channel_block();
-
-    if (!got_dist || scan.ranges.empty()) return false;
-
-    scan.timestamp_ms    = time_of_transmission;
-    scan.angle_min       = angle_min_deg * kDeg2Rad;
-    scan.angle_increment = angle_inc_deg * kDeg2Rad;
-    scan.angle_max        = scan.angle_min +
-        scan.angle_increment * static_cast<float>(scan.ranges.size() - 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;
-    if (scan.intensities.size() != scan.ranges.size())
-        scan.intensities.assign(scan.ranges.size(), 0.f);
-
-    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;
-    info.error_status      = static_cast<uint8_t>(status0 & 0xFF);
-    info.status_flags       = (status0 << 8) | status1;
-    info.scan_frequency_raw = static_cast<uint16_t>(scanning_frequency);
-    info.input_status        = static_cast<uint16_t>((in0 << 8) | in1);
-    info.output_status        = static_cast<uint16_t>((out0 << 8) | out1);
-
-    return true;
-}
-
-// ── NanoScanDriver — SICK nanoScan3/microScan3 safety-scanner UDP output ────
-
-NanoScanDriver::NanoScanDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port)
-    : cfg_(cfg), detected_model_name_(cfg.name ? cfg.name : ""), ip_(ip), port_(port),
-      recv_buf_(kNanoRecvBufSize) {}
-
-NanoScanDriver::~NanoScanDriver() { close(); }
-
-bool NanoScanDriver::open() {
-    sock_fd_ = ::socket(AF_INET, SOCK_DGRAM, 0);
-    if (sock_fd_ < 0) return false;
-
-    int reuse = 1;
-    ::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));
-
-    sockaddr_in addr{};
-    addr.sin_family      = AF_INET;
-    addr.sin_port        = htons(port_);
-    addr.sin_addr.s_addr = (ip_ == "0.0.0.0" || ip_.empty()) ? INADDR_ANY
-                                                              : inet_addr(ip_.c_str());
-
-    if (::bind(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0) {
-        ::close(sock_fd_);
-        sock_fd_ = -1;
-        return false;
-    }
-    return true;
-}
-
-void NanoScanDriver::close() {
-    if (sock_fd_ >= 0) {
-        ::close(sock_fd_);
-        sock_fd_ = -1;
-    }
-}
-
-int NanoScanDriver::recv_datagram(int timeout_ms) {
-    if (sock_fd_ < 0) 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) return -1;
-    }
-
-    ssize_t n = ::recv(sock_fd_, recv_buf_.data(), recv_buf_.size(), 0);
-    return (n <= 0) ? -1 : 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;
-    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)) 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);
-            assembling = true;
-        }
-
-        if (static_cast<uint64_t>(foff) + pl_len <= total) {
-            std::memcpy(tele.data() + foff, pl, pl_len);
-            got += pl_len;
-        }
-
-        if (got >= total) {
-            assembling = false;
-            if (parse_packet(tele.data(), static_cast<int>(total), out)) 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 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 (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;
-
-    return true;
-}
-
-} // namespace lidarlib

third_party/rplidar_sdk/LICENSE

@@ -0,0 +1,23 @@
+Copyright (c) 2009 - 2014 RoboPeak Team (http://www.robopeak.com)
+Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. (http://www.slamtec.com)
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

third_party/rplidar_sdk/include/rplidar.h

@@ -0,0 +1,45 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+
+#include <vector>
+#include "hal/types.h"
+#include "rplidar_protocol.h"
+#include "rplidar_cmd.h"
+
+#include "rplidar_driver.h"
+
+#define RPLIDAR_SDK_VERSION "2.0.0"
+#define SLAMTEC_LIDAR_SDK_VERSION  SL_LIDAR_SDK_VERSION

third_party/rplidar_sdk/include/rplidar_cmd.h

@@ -0,0 +1,215 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014-2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+#include "sl_lidar_cmd.h"
+#include "rplidar_protocol.h"
+
+// Commands
+//-----------------------------------------
+
+#define RPLIDAR_AUTOBAUD_MAGICBYTE     SL_LIDAR_AUTOBAUD_MAGICBYTE
+
+// Commands without payload and response
+#define RPLIDAR_CMD_STOP               SL_LIDAR_CMD_STOP
+#define RPLIDAR_CMD_SCAN               SL_LIDAR_CMD_SCAN
+#define RPLIDAR_CMD_FORCE_SCAN         SL_LIDAR_CMD_FORCE_SCAN
+#define RPLIDAR_CMD_RESET              SL_LIDAR_CMD_RESET
+
+
+// Commands without payload but have response
+#define RPLIDAR_CMD_GET_DEVICE_INFO          SL_LIDAR_CMD_GET_DEVICE_INFO
+#define RPLIDAR_CMD_GET_DEVICE_HEALTH        SL_LIDAR_CMD_GET_DEVICE_HEALTH
+
+#define RPLIDAR_CMD_GET_SAMPLERATE           SL_LIDAR_CMD_GET_SAMPLERATE //added in fw 1.17
+
+#define RPLIDAR_CMD_HQ_MOTOR_SPEED_CTRL      SL_LIDAR_CMD_HQ_MOTOR_SPEED_CTRL
+
+// Commands with payload but no response
+#define RPLIDAR_CMD_NEW_BAUDRATE_CONFIRM     SL_LIDAR_CMD_NEW_BAUDRATE_CONFIRM //added in fw 1.30
+
+// Commands with payload and have response
+#define RPLIDAR_CMD_EXPRESS_SCAN            SL_LIDAR_CMD_EXPRESS_SCAN //added in fw 1.17
+#define RPLIDAR_CMD_HQ_SCAN                 SL_LIDAR_CMD_HQ_SCAN //added in fw 1.24
+#define RPLIDAR_CMD_GET_LIDAR_CONF          SL_LIDAR_CMD_GET_LIDAR_CONF //added in fw 1.24
+#define RPLIDAR_CMD_SET_LIDAR_CONF          SL_LIDAR_CMD_SET_LIDAR_CONF //added in fw 1.24
+//add for A2 to set RPLIDAR motor pwm when using accessory board
+#define RPLIDAR_CMD_SET_MOTOR_PWM           SL_LIDAR_CMD_SET_MOTOR_PWM
+#define RPLIDAR_CMD_GET_ACC_BOARD_FLAG      SL_LIDAR_CMD_GET_ACC_BOARD_FLAG
+
+#if defined(_WIN32)
+#pragma pack(1)
+#endif
+
+
+// Payloads
+// ------------------------------------------
+#define RPLIDAR_EXPRESS_SCAN_MODE_NORMAL      SL_LIDAR_EXPRESS_SCAN_MODE_NORMAL 
+#define RPLIDAR_EXPRESS_SCAN_MODE_FIXANGLE    SL_LIDAR_EXPRESS_SCAN_MODE_FIXANGLE  // won't been supported but keep to prevent build fail
+//for express working flag(extending express scan protocol)
+#define RPLIDAR_EXPRESS_SCAN_FLAG_BOOST                 SL_LIDAR_EXPRESS_SCAN_FLAG_BOOST 
+#define RPLIDAR_EXPRESS_SCAN_FLAG_SUNLIGHT_REJECTION    SL_LIDAR_EXPRESS_SCAN_FLAG_SUNLIGHT_REJECTION
+
+//for ultra express working flag
+#define RPLIDAR_ULTRAEXPRESS_SCAN_FLAG_STD                 SL_LIDAR_ULTRAEXPRESS_SCAN_FLAG_STD 
+#define RPLIDAR_ULTRAEXPRESS_SCAN_FLAG_HIGH_SENSITIVITY    SL_LIDAR_ULTRAEXPRESS_SCAN_FLAG_HIGH_SENSITIVITY
+
+#define RPLIDAR_HQ_SCAN_FLAG_CCW            (0x1<<0)
+#define RPLIDAR_HQ_SCAN_FLAG_RAW_ENCODER    (0x1<<1)
+#define RPLIDAR_HQ_SCAN_FLAG_RAW_DISTANCE   (0x1<<2)
+
+typedef sl_lidar_payload_express_scan_t   rplidar_payload_express_scan_t;
+typedef sl_lidar_payload_hq_scan_t        rplidar_payload_hq_scan_t;
+typedef sl_lidar_payload_get_scan_conf_t  rplidar_payload_get_scan_conf_t;
+typedef sl_lidar_payload_motor_pwm_t      rplidar_payload_motor_pwm_t;
+typedef sl_lidar_payload_acc_board_flag_t rplidar_payload_acc_board_flag_t;
+typedef sl_lidar_payload_set_scan_conf_t  rplidar_payload_set_scan_conf_t;
+typedef sl_lidar_payload_new_bps_confirmation_t  rplidar_payload_new_bps_confirmation_t;
+
+// Response
+// ------------------------------------------
+#define RPLIDAR_ANS_TYPE_DEVINFO                                 SL_LIDAR_ANS_TYPE_DEVINFO
+#define RPLIDAR_ANS_TYPE_DEVHEALTH                               SL_LIDAR_ANS_TYPE_DEVHEALTH
+#define RPLIDAR_ANS_TYPE_MEASUREMENT                             SL_LIDAR_ANS_TYPE_MEASUREMENT
+// Added in FW ver 1.17
+#define RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED                    SL_LIDAR_ANS_TYPE_MEASUREMENT_CAPSULED
+#define RPLIDAR_ANS_TYPE_MEASUREMENT_HQ                          SL_LIDAR_ANS_TYPE_MEASUREMENT_HQ
+// Added in FW ver 1.17
+#define RPLIDAR_ANS_TYPE_SAMPLE_RATE                             SL_LIDAR_ANS_TYPE_SAMPLE_RATE
+//added in FW ver 1.23alpha
+#define RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA              SL_LIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA
+//added in FW ver 1.24
+#define RPLIDAR_ANS_TYPE_GET_LIDAR_CONF                          SL_LIDAR_ANS_TYPE_GET_LIDAR_CONF
+#define RPLIDAR_ANS_TYPE_SET_LIDAR_CONF                          SL_LIDAR_ANS_TYPE_SET_LIDAR_CONF
+#define RPLIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED              SL_LIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED
+#define RPLIDAR_ANS_TYPE_MEASUREMENT_ULTRA_DENSE_CAPSULED        SL_LIDAR_ANS_TYPE_MEASUREMENT_ULTRA_DENSE_CAPSULED
+
+#define RPLIDAR_ANS_TYPE_ACC_BOARD_FLAG                          SL_LIDAR_ANS_TYPE_ACC_BOARD_FLAG
+
+#define RPLIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK      SL_LIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK
+
+typedef sl_lidar_response_acc_board_flag_t rplidar_response_acc_board_flag_t;
+
+
+#define RPLIDAR_STATUS_OK                 SL_LIDAR_STATUS_OK
+#define RPLIDAR_STATUS_WARNING            SL_LIDAR_STATUS_WARNING
+#define RPLIDAR_STATUS_ERROR              SL_LIDAR_STATUS_ERROR
+
+#define RPLIDAR_RESP_MEASUREMENT_SYNCBIT        SL_LIDAR_RESP_MEASUREMENT_SYNCBIT
+#define RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT  SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT
+#define RPLIDAR_RESP_HQ_FLAG_SYNCBIT            SL_LIDAR_RESP_HQ_FLAG_SYNCBIT
+#define RPLIDAR_RESP_MEASUREMENT_CHECKBIT       SL_LIDAR_RESP_MEASUREMENT_CHECKBIT
+#define RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT    SL_LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT
+
+typedef sl_lidar_response_sample_rate_t rplidar_response_sample_rate_t;
+typedef sl_lidar_response_measurement_node_t rplidar_response_measurement_node_t;
+
+//[distance_sync flags]
+#define RPLIDAR_RESP_MEASUREMENT_EXP_ANGLE_MASK           SL_LIDAR_RESP_MEASUREMENT_EXP_ANGLE_MASK
+#define RPLIDAR_RESP_MEASUREMENT_EXP_DISTANCE_MASK        SL_LIDAR_RESP_MEASUREMENT_EXP_DISTANCE_MASK
+
+typedef sl_lidar_response_cabin_nodes_t rplidar_response_cabin_nodes_t;
+
+
+#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1               SL_LIDAR_RESP_MEASUREMENT_EXP_SYNC_1
+#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2               SL_LIDAR_RESP_MEASUREMENT_EXP_SYNC_2
+#define RPLIDAR_RESP_MEASUREMENT_HQ_SYNC                  SL_LIDAR_RESP_MEASUREMENT_HQ_SYNC
+#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT              SL_LIDAR_RESP_MEASUREMENT_EXP_SYNCBIT
+
+
+typedef sl_lidar_response_capsule_measurement_nodes_t         rplidar_response_capsule_measurement_nodes_t;
+typedef sl_lidar_response_dense_cabin_nodes_t                 rplidar_response_dense_cabin_nodes_t;
+typedef sl_lidar_response_dense_capsule_measurement_nodes_t   rplidar_response_dense_capsule_measurement_nodes_t;
+typedef sl_lidar_response_ultra_dense_capsule_measurement_nodes_t rplidar_response_ultra_dense_capsule_measurement_nodes_t;
+// ext1 : x2 boost mode
+
+#define RPLIDAR_RESP_MEASUREMENT_EXP_ULTRA_MAJOR_BITS     SL_LIDAR_RESP_MEASUREMENT_EXP_ULTRA_MAJOR_BITS
+#define RPLIDAR_RESP_MEASUREMENT_EXP_ULTRA_PREDICT_BITS   SL_LIDAR_RESP_MEASUREMENT_EXP_ULTRA_PREDICT_BITS
+
+typedef sl_lidar_response_ultra_cabin_nodes_t                      rplidar_response_ultra_cabin_nodes_t;
+typedef sl_lidar_response_ultra_capsule_measurement_nodes_t        rplidar_response_ultra_capsule_measurement_nodes_t;
+typedef sl_lidar_response_measurement_node_hq_t                    rplidar_response_measurement_node_hq_t;
+typedef sl_lidar_response_hq_capsule_measurement_nodes_t           rplidar_response_hq_capsule_measurement_nodes_t;
+
+
+#   define RPLIDAR_CONF_SCAN_COMMAND_STD            SL_LIDAR_CONF_SCAN_COMMAND_STD
+#   define RPLIDAR_CONF_SCAN_COMMAND_EXPRESS        SL_LIDAR_CONF_SCAN_COMMAND_EXPRESS
+#   define RPLIDAR_CONF_SCAN_COMMAND_HQ             SL_LIDAR_CONF_SCAN_COMMAND_HQ
+#   define RPLIDAR_CONF_SCAN_COMMAND_BOOST          SL_LIDAR_CONF_SCAN_COMMAND_BOOST
+#   define RPLIDAR_CONF_SCAN_COMMAND_STABILITY      SL_LIDAR_CONF_SCAN_COMMAND_STABILITY
+#   define RPLIDAR_CONF_SCAN_COMMAND_SENSITIVITY    SL_LIDAR_CONF_SCAN_COMMAND_SENSITIVITY
+
+#define RPLIDAR_CONF_ANGLE_RANGE                    SL_LIDAR_CONF_ANGLE_RANGE
+#define RPLIDAR_CONF_DESIRED_ROT_FREQ               SL_LIDAR_CONF_DESIRED_ROT_FREQ
+#define RPLIDAR_CONF_SCAN_COMMAND_BITMAP            SL_LIDAR_CONF_SCAN_COMMAND_BITMAP
+#define RPLIDAR_CONF_MIN_ROT_FREQ                   SL_LIDAR_CONF_MIN_ROT_FREQ
+#define RPLIDAR_CONF_MAX_ROT_FREQ                   SL_LIDAR_CONF_MAX_ROT_FREQ
+#define RPLIDAR_CONF_MAX_DISTANCE                   SL_LIDAR_CONF_MAX_DISTANCE
+        
+#define RPLIDAR_CONF_SCAN_MODE_COUNT                SL_LIDAR_CONF_SCAN_MODE_COUNT
+#define RPLIDAR_CONF_SCAN_MODE_US_PER_SAMPLE        SL_LIDAR_CONF_SCAN_MODE_US_PER_SAMPLE
+#define RPLIDAR_CONF_SCAN_MODE_MAX_DISTANCE         SL_LIDAR_CONF_SCAN_MODE_MAX_DISTANCE
+#define RPLIDAR_CONF_SCAN_MODE_ANS_TYPE             SL_LIDAR_CONF_SCAN_MODE_ANS_TYPE
+#define RPLIDAR_CONF_SCAN_MODE_TYPICAL              SL_LIDAR_CONF_SCAN_MODE_TYPICAL
+#define RPLIDAR_CONF_SCAN_MODE_NAME                 SL_LIDAR_CONF_SCAN_MODE_NAME
+#define RPLIDAR_EXPRESS_SCAN_STABILITY_BITMAP                 SL_LIDAR_EXPRESS_SCAN_STABILITY_BITMAP
+#define RPLIDAR_EXPRESS_SCAN_SENSITIVITY_BITMAP               SL_LIDAR_EXPRESS_SCAN_SENSITIVITY_BITMAP
+#define RPLIDAR_CONF_LIDAR_STATIC_IP_ADDR           SL_LIDAR_CONF_LIDAR_STATIC_IP_ADDR
+#define RPLIDAR_CONF_LIDAR_MAC_ADDR                 SL_LIDAR_CONF_LIDAR_MAC_ADDR
+
+#define RPLIDAR_CONF_DETECTED_SERIAL_BPS            SL_LIDAR_CONF_DETECTED_SERIAL_BPS
+
+typedef sl_lidar_response_get_lidar_conf_t       rplidar_response_get_lidar_conf_t;
+typedef sl_lidar_response_set_lidar_conf_t       rplidar_response_set_lidar_conf_t;
+typedef sl_lidar_response_device_info_t          rplidar_response_device_info_t;
+typedef sl_lidar_response_device_health_t        rplidar_response_device_health_t;
+typedef sl_lidar_ip_conf_t                       rplidar_ip_conf_t;
+typedef sl_lidar_response_device_macaddr_info_t  rplidar_response_device_macaddr_info_t;
+
+// Definition of the variable bit scale encoding mechanism
+#define RPLIDAR_VARBITSCALE_X2_SRC_BIT  SL_LIDAR_VARBITSCALE_X2_SRC_BIT
+#define RPLIDAR_VARBITSCALE_X4_SRC_BIT  SL_LIDAR_VARBITSCALE_X4_SRC_BIT
+#define RPLIDAR_VARBITSCALE_X8_SRC_BIT  SL_LIDAR_VARBITSCALE_X8_SRC_BIT
+#define RPLIDAR_VARBITSCALE_X16_SRC_BIT SL_LIDAR_VARBITSCALE_X16_SRC_BIT
+
+#define RPLIDAR_VARBITSCALE_X2_DEST_VAL SL_LIDAR_VARBITSCALE_X2_DEST_VAL
+#define RPLIDAR_VARBITSCALE_X4_DEST_VAL SL_LIDAR_VARBITSCALE_X4_DEST_VAL
+#define RPLIDAR_VARBITSCALE_X8_DEST_VAL SL_LIDAR_VARBITSCALE_X8_DEST_VAL
+#define RPLIDAR_VARBITSCALE_X16_DEST_VAL SL_LIDAR_VARBITSCALE_X16_DEST_VAL
+
+#define RPLIDAR_VARBITSCALE_GET_SRC_MAX_VAL_BY_BITS(_BITS_)   SL_LIDAR_VARBITSCALE_GET_SRC_MAX_VAL_BY_BITS(_BITS_)
+
+#if defined(_WIN32)
+#pragma pack()
+#endif

third_party/rplidar_sdk/include/rplidar_driver.h

@@ -0,0 +1,247 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+#include "sl_lidar_driver.h"
+
+#ifndef __cplusplus
+#error "The RPlidar SDK requires a C++ compiler to be built"
+#endif
+
+
+namespace rp { namespace standalone{ namespace rplidar {
+    using namespace sl;
+    typedef LidarScanMode RplidarScanMode;
+
+enum {
+   DRIVER_TYPE_SERIALPORT = 0x0,
+   DRIVER_TYPE_TCP = 0x1,
+   DRIVER_TYPE_UDP = 0x2,
+};
+
+class RPlidarDriver {
+public:
+    enum {
+        DEFAULT_TIMEOUT = 2000, //2000 ms
+    };
+
+    enum {
+        MAX_SCAN_NODES = 8192,
+    };
+
+    enum {
+        LEGACY_SAMPLE_DURATION = 476,
+    };
+
+public:
+    /// Create an RPLIDAR Driver Instance
+    /// This interface should be invoked first before any other operations
+    ///
+    /// \param drivertype the connection type used by the driver. 
+    static RPlidarDriver * CreateDriver(_u32 drivertype = CHANNEL_TYPE_SERIALPORT);
+    
+
+    RPlidarDriver(sl_u32 channelType);
+
+    /// Dispose the RPLIDAR Driver Instance specified by the drv parameter
+    /// Applications should invoke this interface when the driver instance is no longer used in order to free memory
+    static void DisposeDriver(RPlidarDriver * drv);
+
+    /// Open the specified serial port and connect to a target RPLIDAR device
+    ///
+    /// \param port_path     the device path of the serial port 
+    ///        e.g. on Windows, it may be com3 or \\.\com10 
+    ///             on Unix-Like OS, it may be /dev/ttyS1, /dev/ttyUSB2, etc
+    ///
+    /// \param baudrate      the baudrate used
+    ///        For most RPLIDAR models, the baudrate should be set to 115200
+    ///
+    /// \param flag          other flags
+    ///        Reserved for future use, always set to Zero
+    u_result connect(const char *path, _u32 portOrBaud, _u32 flag = 0);
+    
+    /// Disconnect with the RPLIDAR and close the serial port
+    void disconnect();
+
+    /// Returns TRUE when the connection has been established
+    bool isConnected(); 
+
+    /// Ask the RPLIDAR core system to reset it self
+    /// The host system can use the Reset operation to help RPLIDAR escape the self-protection mode.
+    ///
+    ///  \param timeout       The operation timeout value (in millisecond) for the serial port communication                     
+    u_result reset(_u32 timeout = DEFAULT_TIMEOUT);
+
+    u_result clearNetSerialRxCache() {
+        return RESULT_OK;
+    }
+    // FW1.24
+    /// Get all scan modes that supported by lidar
+    u_result getAllSupportedScanModes(std::vector<RplidarScanMode>& outModes, _u32 timeoutInMs = DEFAULT_TIMEOUT);
+   
+    /// Get typical scan mode of lidar
+    u_result getTypicalScanMode(_u16& outMode, _u32 timeoutInMs = DEFAULT_TIMEOUT);
+
+    /// Start scan
+    ///
+    /// \param force            Force the core system to output scan data regardless whether the scanning motor is rotating or not.
+    /// \param useTypicalScan   Use lidar's typical scan mode or use the compatibility mode (2k sps)
+    /// \param options          Scan options (please use 0)
+    /// \param outUsedScanMode  The scan mode selected by lidar
+    u_result startScan(bool force, bool useTypicalScan, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL);
+
+    /// Start scan in specific mode
+    ///
+    /// \param force            Force the core system to output scan data regardless whether the scanning motor is rotating or not.
+    /// \param scanMode         The scan mode id (use getAllSupportedScanModes to get supported modes)
+    /// \param options          Scan options (please use 0)
+    /// \param outUsedScanMode  The scan mode selected by lidar
+    u_result startScanExpress(bool force, _u16 scanMode, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL, _u32 timeout = DEFAULT_TIMEOUT);
+
+    /// Retrieve the health status of the RPLIDAR
+    /// The host system can use this operation to check whether RPLIDAR is in the self-protection mode.
+    ///
+    /// \param health        The health status info returned from the RPLIDAR
+    ///
+    /// \param timeout       The operation timeout value (in millisecond) for the serial port communication     
+    u_result getHealth(rplidar_response_device_health_t & health, _u32 timeout = DEFAULT_TIMEOUT);
+
+    /// Get the device information of the RPLIDAR include the serial number, firmware version, device model etc.
+    /// 
+    /// \param info          The device information returned from the RPLIDAR
+    /// \param timeout       The operation timeout value (in millisecond) for the serial port communication  
+    u_result getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout = DEFAULT_TIMEOUT);
+
+    /// Set the RPLIDAR's motor pwm when using accessory board, currently valid for A2 only.
+    /// 
+    /// \param pwm           The motor pwm value would like to set 
+    u_result setMotorPWM(_u16 pwm);
+
+    /// Start RPLIDAR's motor when using accessory board
+    u_result startMotor();
+
+    /// Stop RPLIDAR's motor when using accessory board
+    u_result stopMotor();
+
+    /// Check whether the device support motor control.
+    /// Note: this API will disable grab.
+    /// 
+    /// \param support       Return the result.
+    /// \param timeout       The operation timeout value (in millisecond) for the serial port communication. 
+    u_result checkMotorCtrlSupport(bool & support, _u32 timeout = DEFAULT_TIMEOUT);
+
+	///Set LPX and S2E series lidar's static IP address
+	///
+	/// \param conf             Network parameter that LPX series lidar owned
+	/// \param timeout          The operation timeout value (in millisecond) for the ethernet udp communication
+	u_result  setLidarIpConf(const rplidar_ip_conf_t& conf, _u32 timeout = DEFAULT_TIMEOUT);
+
+    ///Get LPX and S2E series lidar's static IP address
+    ///
+    /// \param conf             Network parameter that LPX series lidar owned
+    /// \param timeout          The operation timeout value (in millisecond) for the ethernet udp communication
+    u_result  getLidarIpConf(rplidar_ip_conf_t& conf, _u32 timeout = DEFAULT_TIMEOUT);
+
+	///Get LPX and S2E series lidar's MAC address
+	///
+	/// \param macAddrArray         The device MAC information returned from the LPX series lidar
+	u_result getDeviceMacAddr(_u8* macAddrArray, _u32 timeoutInMs = DEFAULT_TIMEOUT);
+
+    /// Ask the RPLIDAR core system to stop the current scan operation and enter idle state. The background thread will be terminated
+    ///
+    /// \param timeout       The operation timeout value (in millisecond) for the serial port communication 
+    u_result stop(_u32 timeout = DEFAULT_TIMEOUT);
+
+    /// Wait and grab a complete 0-360 degree scan data previously received. 
+    /// The grabbed scan data returned by this interface always has the following charactistics:
+    ///
+    /// 1) The first node of the grabbed data array (nodebuffer[0]) must be the first sample of a scan, i.e. the start_bit == 1
+    /// 2) All data nodes are belong to exactly ONE complete 360-degrees's scan
+    /// 3) Note, the angle data in one scan may not be ascending. You can use API ascendScanData to reorder the nodebuffer.
+    ///
+    /// \param nodebuffer     Buffer provided by the caller application to store the scan data
+    ///
+    /// \param count          The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t).
+    ///                       Once the interface returns, this parameter will store the actual received data count.
+    ///
+    /// \param timeout        Max duration allowed to wait for a complete scan data, nothing will be stored to the nodebuffer if a complete 360-degrees' scan data cannot to be ready timely.
+    ///
+    /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that no complete 360-degrees' scan can be retrieved withing the given timeout duration. 
+    ///
+    /// \The caller application can set the timeout value to Zero(0) to make this interface always returns immediately to achieve non-block operation.
+    u_result grabScanDataHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT);
+
+    /// Ascending the scan data according to the angle value in the scan.
+    ///
+    /// \param nodebuffer     Buffer provided by the caller application to do the reorder. Should be retrived from the grabScanData
+    ///
+    /// \param count          The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t).
+    ///                       Once the interface returns, this parameter will store the actual received data count.
+    /// The interface will return RESULT_OPERATION_FAIL when all the scan data is invalid. 
+    u_result ascendScanData(rplidar_response_measurement_node_hq_t * nodebuffer, size_t count);
+
+    /// Return received scan points even if it's not complete scan
+    ///
+    /// \param nodebuffer     Buffer provided by the caller application to store the scan data
+    ///
+    /// \param count          Once the interface returns, this parameter will store the actual received data count.
+    ///
+    /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call. 
+    u_result getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count);
+
+    /// Return received scan points even if it's not complete scan
+    ///
+    /// \param nodebuffer     Buffer provided by the caller application to store the scan data
+    ///
+    /// \param count          Once the interface returns, this parameter will store the actual received data count.
+    ///
+    /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call. 
+    u_result getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count);
+
+
+    virtual ~RPlidarDriver();
+protected:
+    RPlidarDriver();
+
+private:
+    sl_u32 _channelType;
+    IChannel* _channel;
+    ILidarDriver* _lidarDrv;
+    
+};
+
+
+
+
+}}}

third_party/rplidar_sdk/include/rplidar_protocol.h

@@ -0,0 +1,61 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+#include "sl_lidar_protocol.h"
+// RP-Lidar Input Packets
+
+#define RPLIDAR_CMD_SYNC_BYTE        SL_LIDAR_CMD_SYNC_BYTE
+#define RPLIDAR_CMDFLAG_HAS_PAYLOAD  SL_LIDAR_CMDFLAG_HAS_PAYLOAD
+
+
+#define RPLIDAR_ANS_SYNC_BYTE1       SL_LIDAR_ANS_SYNC_BYTE1
+#define RPLIDAR_ANS_SYNC_BYTE2       SL_LIDAR_ANS_SYNC_BYTE2
+
+#define RPLIDAR_ANS_PKTFLAG_LOOP     SL_LIDAR_ANS_PKTFLAG_LOOP
+
+#define RPLIDAR_ANS_HEADER_SIZE_MASK        SL_LIDAR_ANS_HEADER_SIZE_MASK
+#define RPLIDAR_ANS_HEADER_SUBTYPE_SHIFT    SL_LIDAR_ANS_HEADER_SUBTYPE_SHIFT
+
+#if defined(_WIN32)
+#pragma pack(1)
+#endif
+
+typedef sl_lidar_cmd_packet_t rplidar_cmd_packet_t;
+typedef sl_lidar_ans_header_t rplidar_ans_header_t;
+
+
+#if defined(_WIN32)
+#pragma pack()
+#endif

third_party/rplidar_sdk/include/rptypes.h

@@ -0,0 +1,116 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+
+
+#ifdef _WIN32
+
+//fake stdint.h for VC only
+
+typedef signed   char     int8_t;
+typedef unsigned char     uint8_t;
+
+typedef __int16           int16_t;
+typedef unsigned __int16  uint16_t;
+
+typedef __int32           int32_t;
+typedef unsigned __int32  uint32_t;
+
+typedef __int64           int64_t;
+typedef unsigned __int64  uint64_t;
+
+#else
+
+#include <stdint.h>
+
+#endif
+
+
+//based on stdint.h
+typedef int8_t         _s8;
+typedef uint8_t        _u8;
+
+typedef int16_t        _s16;
+typedef uint16_t       _u16;
+
+typedef int32_t        _s32;
+typedef uint32_t       _u32;
+
+typedef int64_t        _s64;
+typedef uint64_t       _u64;
+
+#define __small_endian
+
+#ifndef __GNUC__
+#define __attribute__(x)
+#endif
+
+
+// The _word_size_t uses actual data bus width of the current CPU
+#ifdef _AVR_
+typedef _u8            _word_size_t;
+#define THREAD_PROC    
+#elif defined (WIN64)
+typedef _u64           _word_size_t;
+#define THREAD_PROC    __stdcall
+#elif defined (WIN32)
+typedef _u32           _word_size_t;
+#define THREAD_PROC    __stdcall
+#elif defined (__GNUC__)
+typedef unsigned long  _word_size_t;
+#define THREAD_PROC   
+#elif defined (__ICCARM__)
+typedef _u32            _word_size_t;
+#define THREAD_PROC  
+#endif
+
+
+typedef uint32_t u_result;
+
+#define RESULT_OK              0
+#define RESULT_FAIL_BIT        0x80000000
+#define RESULT_ALREADY_DONE    0x20
+#define RESULT_INVALID_DATA    (0x8000 | RESULT_FAIL_BIT)
+#define RESULT_OPERATION_FAIL  (0x8001 | RESULT_FAIL_BIT)
+#define RESULT_OPERATION_TIMEOUT  (0x8002 | RESULT_FAIL_BIT)
+#define RESULT_OPERATION_STOP    (0x8003 | RESULT_FAIL_BIT)
+#define RESULT_OPERATION_NOT_SUPPORT    (0x8004 | RESULT_FAIL_BIT)
+#define RESULT_FORMAT_NOT_SUPPORT    (0x8005 | RESULT_FAIL_BIT)
+#define RESULT_INSUFFICIENT_MEMORY   (0x8006 | RESULT_FAIL_BIT)
+
+#define IS_OK(x)    ( ((x) & RESULT_FAIL_BIT) == 0 )
+#define IS_FAIL(x)  ( ((x) & RESULT_FAIL_BIT) )
+
+typedef _word_size_t (THREAD_PROC * thread_proc_t ) ( void * );

third_party/rplidar_sdk/include/sl_crc.h

@@ -0,0 +1,43 @@
+/*
+* Slamtec LIDAR SDK
+*
+* sl_crc.h
+*
+* Copyright (c) 2020 Shanghai Slamtec Co., Ltd.
+*/
+
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+
+#include "sl_lidar_cmd.h"
+
+namespace sl {namespace crc32 {
+    sl_u32 bitrev(sl_u32 input, sl_u16 bw);//reflect
+    void init(sl_u32 poly); // table init
+    sl_u32 cal(sl_u32 crc, void* input, sl_u16 len);
+    sl_result getResult(sl_u8 *ptr, sl_u32 len);
+}}

third_party/rplidar_sdk/include/sl_lidar.h

@@ -0,0 +1,47 @@
+/*
+* Slamtec LIDAR SDK
+*
+* sl_lidar.h
+*
+* Copyright (c) 2020 Shanghai Slamtec Co., Ltd.
+*/
+
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+
+#include "sl_lidar_driver.h"
+
+#define SL_LIDAR_SDK_VERSION_MAJOR  2
+#define SL_LIDAR_SDK_VERSION_MINOR  1
+#define SL_LIDAR_SDK_VERSION_PATCH  0
+#define SL_LIDAR_SDK_VERSION_SEQ    ((SL_LIDAR_SDK_VERSION_MAJOR << 16) | (SL_LIDAR_SDK_VERSION_MINOR << 8) | SL_LIDAR_SDK_VERSION_PATCH)
+
+
+#define SL_LIDAR_SDK_VERSION_MK_STR_INDIR(x)  #x
+#define SL_LIDAR_SDK_VERSION_MK_STR(x)        SL_LIDAR_SDK_VERSION_MK_STR_INDIR(x)
+
+#define SL_LIDAR_SDK_VERSION        (SL_LIDAR_SDK_VERSION_MK_STR(SL_LIDAR_SDK_VERSION_MAJOR) "." SL_LIDAR_SDK_VERSION_MK_STR(SL_LIDAR_SDK_VERSION_MINOR) "." SL_LIDAR_SDK_VERSION_MK_STR(SL_LIDAR_SDK_VERSION_PATCH))

third_party/rplidar_sdk/include/sl_lidar_cmd.h

@@ -0,0 +1,388 @@
+/*
+* Slamtec LIDAR SDK
+*
+* sl_lidar_cmd.h
+*
+* Copyright (c) 2020 Shanghai Slamtec Co., Ltd.
+*/
+
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4200)
+#endif
+
+#include "sl_lidar_protocol.h"
+
+ // Commands
+ //-----------------------------------------
+
+
+#define SL_LIDAR_AUTOBAUD_MAGICBYTE         0x41
+
+ // Commands without payload and response
+#define SL_LIDAR_CMD_STOP                   0x25
+#define SL_LIDAR_CMD_SCAN                   0x20
+#define SL_LIDAR_CMD_FORCE_SCAN             0x21
+#define SL_LIDAR_CMD_RESET                  0x40
+
+// Commands with payload but no response
+#define SL_LIDAR_CMD_NEW_BAUDRATE_CONFIRM   0x90 //added in fw 1.30
+
+// Commands without payload but have response
+#define SL_LIDAR_CMD_GET_DEVICE_INFO        0x50
+#define SL_LIDAR_CMD_GET_DEVICE_HEALTH      0x52
+
+#define SL_LIDAR_CMD_GET_SAMPLERATE         0x59 //added in fw 1.17
+
+#define SL_LIDAR_CMD_HQ_MOTOR_SPEED_CTRL    0xA8
+
+
+// Commands with payload and have response
+#define SL_LIDAR_CMD_EXPRESS_SCAN           0x82 //added in fw 1.17
+#define SL_LIDAR_CMD_HQ_SCAN                0x83 //added in fw 1.24
+#define SL_LIDAR_CMD_GET_LIDAR_CONF         0x84 //added in fw 1.24
+#define SL_LIDAR_CMD_SET_LIDAR_CONF         0x85 //added in fw 1.24
+//add for A2 to set RPLIDAR motor pwm when using accessory board
+#define SL_LIDAR_CMD_SET_MOTOR_PWM          0xF0
+#define SL_LIDAR_CMD_GET_ACC_BOARD_FLAG     0xFF
+
+#if defined(_WIN32)
+#pragma pack(1)
+#endif
+
+
+// Payloads
+// ------------------------------------------
+#define SL_LIDAR_EXPRESS_SCAN_MODE_NORMAL      0 
+#define SL_LIDAR_EXPRESS_SCAN_MODE_FIXANGLE    0  // won't been supported but keep to prevent build fail
+//for express working flag(extending express scan protocol)
+#define SL_LIDAR_EXPRESS_SCAN_FLAG_BOOST                 0x0001 
+#define SL_LIDAR_EXPRESS_SCAN_FLAG_SUNLIGHT_REJECTION    0x0002
+
+//for ultra express working flag
+#define SL_LIDAR_ULTRAEXPRESS_SCAN_FLAG_STD                 0x0001 
+#define SL_LIDAR_ULTRAEXPRESS_SCAN_FLAG_HIGH_SENSITIVITY    0x0002
+
+typedef struct _sl_lidar_payload_express_scan_t
+{
+    sl_u8   working_mode;
+    sl_u16  working_flags;
+    sl_u16  param;
+} __attribute__((packed)) sl_lidar_payload_express_scan_t;
+
+typedef struct _sl_lidar_payload_hq_scan_t
+{
+    sl_u8  flag;
+    sl_u8   reserved[32];
+} __attribute__((packed)) sl_lidar_payload_hq_scan_t;
+
+typedef struct _sl_lidar_payload_get_scan_conf_t
+{
+    sl_u32  type;
+} __attribute__((packed)) sl_lidar_payload_get_scan_conf_t;
+
+typedef struct _sl_payload_set_scan_conf_t {
+    sl_u32  type;
+} __attribute__((packed)) sl_lidar_payload_set_scan_conf_t;
+
+
+#define DEFAULT_MOTOR_SPEED         (0xFFFFu)
+
+typedef struct _sl_lidar_payload_motor_pwm_t
+{
+    sl_u16 pwm_value;
+} __attribute__((packed)) sl_lidar_payload_motor_pwm_t;
+
+typedef struct _sl_lidar_payload_acc_board_flag_t
+{
+    sl_u32 reserved;
+} __attribute__((packed)) sl_lidar_payload_acc_board_flag_t;
+
+typedef struct _sl_lidar_payload_hq_spd_ctrl_t {
+    sl_u16  rpm;
+} __attribute__((packed))sl_lidar_payload_hq_spd_ctrl_t;
+
+
+typedef struct _sl_lidar_payload_new_bps_confirmation_t {
+    sl_u16   flag; // reserved, must be 0x5F5F
+    sl_u32  required_bps;
+    sl_u16  param;
+} __attribute__((packed)) sl_lidar_payload_new_bps_confirmation_t;
+
+// Response
+// ------------------------------------------
+#define SL_LIDAR_ANS_TYPE_DEVINFO          0x4
+#define SL_LIDAR_ANS_TYPE_DEVHEALTH        0x6
+
+#define SL_LIDAR_ANS_TYPE_MEASUREMENT                       0x81
+// Added in FW ver 1.17
+#define SL_LIDAR_ANS_TYPE_MEASUREMENT_CAPSULED              0x82
+#define SL_LIDAR_ANS_TYPE_MEASUREMENT_HQ                    0x83
+//added in FW ver 1.23alpha
+#define SL_LIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA        0x84
+#define SL_LIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED        0x85
+#define SL_LIDAR_ANS_TYPE_MEASUREMENT_ULTRA_DENSE_CAPSULED  0x86
+
+
+// Added in FW ver 1.17
+#define SL_LIDAR_ANS_TYPE_SAMPLE_RATE      0x15
+
+//added in FW ver 1.24
+#define SL_LIDAR_ANS_TYPE_GET_LIDAR_CONF     0x20
+#define SL_LIDAR_ANS_TYPE_SET_LIDAR_CONF     0x21
+
+
+#define SL_LIDAR_ANS_TYPE_ACC_BOARD_FLAG   0xFF
+
+#define SL_LIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK      (0x1)
+typedef struct _sl_lidar_response_acc_board_flag_t
+{
+    sl_u32 support_flag;
+} __attribute__((packed)) sl_lidar_response_acc_board_flag_t;
+
+
+#define SL_LIDAR_STATUS_OK                 0x0
+#define SL_LIDAR_STATUS_WARNING            0x1
+#define SL_LIDAR_STATUS_ERROR              0x2
+
+#define SL_LIDAR_RESP_MEASUREMENT_SYNCBIT        (0x1<<0)
+#define SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT  2
+
+#define SL_LIDAR_RESP_HQ_FLAG_SYNCBIT               (0x1<<0)
+
+#define SL_LIDAR_RESP_MEASUREMENT_CHECKBIT       (0x1<<0)
+#define SL_LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT    1
+
+typedef struct _sl_lidar_response_sample_rate_t
+{
+    sl_u16  std_sample_duration_us;
+    sl_u16  express_sample_duration_us;
+} __attribute__((packed)) sl_lidar_response_sample_rate_t;
+
+typedef struct _sl_lidar_response_measurement_node_t
+{
+    sl_u8    sync_quality;      // syncbit:1;syncbit_inverse:1;quality:6;
+    sl_u16   angle_q6_checkbit; // check_bit:1;angle_q6:15;
+    sl_u16   distance_q2;
+} __attribute__((packed)) sl_lidar_response_measurement_node_t;
+
+//[distance_sync flags]
+#define SL_LIDAR_RESP_MEASUREMENT_EXP_ANGLE_MASK           (0x3)
+#define SL_LIDAR_RESP_MEASUREMENT_EXP_DISTANCE_MASK        (0xFC)
+
+typedef struct _sl_lidar_response_cabin_nodes_t
+{
+    sl_u16   distance_angle_1; // see [distance_sync flags]
+    sl_u16   distance_angle_2; // see [distance_sync flags]
+    sl_u8    offset_angles_q3;
+} __attribute__((packed)) sl_lidar_response_cabin_nodes_t;
+
+
+#define SL_LIDAR_RESP_MEASUREMENT_EXP_SYNC_1               0xA
+#define SL_LIDAR_RESP_MEASUREMENT_EXP_SYNC_2               0x5
+
+#define SL_LIDAR_RESP_MEASUREMENT_HQ_SYNC                  0xA5
+
+#define SL_LIDAR_RESP_MEASUREMENT_EXP_SYNCBIT              (0x1<<15)
+
+typedef struct _sl_lidar_response_capsule_measurement_nodes_t
+{
+    sl_u8                             s_checksum_1; // see [s_checksum_1]
+    sl_u8                             s_checksum_2; // see [s_checksum_1]
+    sl_u16                            start_angle_sync_q6;
+    sl_lidar_response_cabin_nodes_t  cabins[16];
+} __attribute__((packed)) sl_lidar_response_capsule_measurement_nodes_t;
+
+typedef struct _sl_lidar_response_dense_cabin_nodes_t
+{
+    sl_u16   distance;
+} __attribute__((packed)) sl_lidar_response_dense_cabin_nodes_t;
+
+typedef struct _sl_lidar_response_dense_capsule_measurement_nodes_t
+{
+    sl_u8                             s_checksum_1; // see [s_checksum_1]
+    sl_u8                             s_checksum_2; // see [s_checksum_1]
+    sl_u16                            start_angle_sync_q6;
+    sl_lidar_response_dense_cabin_nodes_t  cabins[40];
+} __attribute__((packed)) sl_lidar_response_dense_capsule_measurement_nodes_t;
+
+
+typedef struct _sl_lidar_response_ultra_dense_cabin_nodes_t {
+    sl_u16  qualityl_distance_scale[2];
+    sl_u8   qualityh_array;
+} __attribute__((packed)) sl_lidar_response_ultra_dense_cabin_nodes_t;
+
+typedef struct _sl_lidar_response_ultra_dense_capsule_measurement_nodes_t {
+    sl_u8                             s_checksum_1; // see [s_checksum_1]
+    sl_u8                             s_checksum_2; // see [s_checksum_1]
+    sl_u32                            time_stamp;
+    sl_u16                            dev_status;
+    sl_u16                            start_angle_sync_q6;
+    sl_lidar_response_ultra_dense_cabin_nodes_t  cabins[32];
+} __attribute__((packed)) sl_lidar_response_ultra_dense_capsule_measurement_nodes_t;
+
+
+// ext1 : x2 boost mode
+
+#define SL_LIDAR_RESP_MEASUREMENT_EXP_ULTRA_MAJOR_BITS     12
+#define SL_LIDAR_RESP_MEASUREMENT_EXP_ULTRA_PREDICT_BITS   10
+
+typedef struct _sl_lidar_response_ultra_cabin_nodes_t
+{
+    // 31                                              0
+    // | predict2 10bit | predict1 10bit | major 12bit |
+    sl_u32 combined_x3;
+} __attribute__((packed)) sl_lidar_response_ultra_cabin_nodes_t;
+
+typedef struct _sl_lidar_response_ultra_capsule_measurement_nodes_t
+{
+    sl_u8                             s_checksum_1; // see [s_checksum_1]
+    sl_u8                             s_checksum_2; // see [s_checksum_1]
+    sl_u16                            start_angle_sync_q6;
+    sl_lidar_response_ultra_cabin_nodes_t  ultra_cabins[32];
+} __attribute__((packed)) sl_lidar_response_ultra_capsule_measurement_nodes_t;
+
+typedef struct sl_lidar_response_measurement_node_hq_t
+{
+    sl_u16   angle_z_q14;
+    sl_u32   dist_mm_q2;
+    sl_u8    quality;
+    sl_u8    flag;
+} __attribute__((packed)) sl_lidar_response_measurement_node_hq_t;
+
+typedef struct _sl_lidar_response_hq_capsule_measurement_nodes_t
+{
+    sl_u8 sync_byte;
+    sl_u64 time_stamp;
+    sl_lidar_response_measurement_node_hq_t node_hq[96];
+    sl_u32  crc32;
+}__attribute__((packed)) sl_lidar_response_hq_capsule_measurement_nodes_t;
+
+
+#   define SL_LIDAR_CONF_SCAN_COMMAND_STD            0
+#   define SL_LIDAR_CONF_SCAN_COMMAND_EXPRESS        1
+#   define SL_LIDAR_CONF_SCAN_COMMAND_HQ             2
+#   define SL_LIDAR_CONF_SCAN_COMMAND_BOOST          3
+#   define SL_LIDAR_CONF_SCAN_COMMAND_STABILITY      4
+#   define SL_LIDAR_CONF_SCAN_COMMAND_SENSITIVITY    5
+
+#define SL_LIDAR_CONF_ANGLE_RANGE                    0x00000000
+#define SL_LIDAR_CONF_DESIRED_ROT_FREQ               0x00000001
+#define SL_LIDAR_CONF_SCAN_COMMAND_BITMAP            0x00000002
+#define SL_LIDAR_CONF_MIN_ROT_FREQ                   0x00000004
+#define SL_LIDAR_CONF_MAX_ROT_FREQ                   0x00000005
+#define SL_LIDAR_CONF_MAX_DISTANCE                   0x00000060
+
+#define SL_LIDAR_CONF_SCAN_MODE_COUNT                0x00000070
+#define SL_LIDAR_CONF_SCAN_MODE_US_PER_SAMPLE        0x00000071
+#define SL_LIDAR_CONF_SCAN_MODE_MAX_DISTANCE         0x00000074
+#define SL_LIDAR_CONF_SCAN_MODE_ANS_TYPE             0x00000075
+#define SL_LIDAR_CONF_LIDAR_MAC_ADDR                 0x00000079
+#define SL_LIDAR_CONF_SCAN_MODE_TYPICAL              0x0000007C
+#define SL_LIDAR_CONF_SCAN_MODE_NAME                 0x0000007F
+
+
+#define SL_LIDAR_CONF_MODEL_REVISION_ID              0x00000080
+#define SL_LIDAR_CONF_MODEL_NAME_ALIAS               0x00000081
+
+#define SL_LIDAR_CONF_DETECTED_SERIAL_BPS            0x000000A1
+
+#define SL_LIDAR_CONF_LIDAR_STATIC_IP_ADDR           0x0001CCC0
+#define SL_LIDAR_EXPRESS_SCAN_STABILITY_BITMAP                 4
+#define SL_LIDAR_EXPRESS_SCAN_SENSITIVITY_BITMAP               5
+
+typedef struct _sl_lidar_response_get_lidar_conf
+{
+    sl_u32 type;
+    sl_u8  payload[0];
+}__attribute__((packed)) sl_lidar_response_get_lidar_conf_t;
+
+typedef struct _sl_lidar_response_set_lidar_conf
+{
+    sl_u32 type;
+    sl_u32 result;
+}__attribute__((packed)) sl_lidar_response_set_lidar_conf_t;
+
+
+typedef struct _sl_lidar_response_device_info_t
+{
+    sl_u8   model;
+    sl_u16  firmware_version;
+    sl_u8   hardware_version;
+    sl_u8   serialnum[16];
+} __attribute__((packed)) sl_lidar_response_device_info_t;
+
+typedef struct _sl_lidar_response_device_health_t
+{
+    sl_u8   status;
+    sl_u16  error_code;
+} __attribute__((packed)) sl_lidar_response_device_health_t;
+
+typedef struct _sl_lidar_ip_conf_t {
+    sl_u8 ip_addr[4];
+    sl_u8 net_mask[4];
+    sl_u8 gw[4];
+}__attribute__((packed)) sl_lidar_ip_conf_t;
+
+typedef struct _sl_lidar_response_device_macaddr_info_t {
+    sl_u8   macaddr[6];
+} __attribute__((packed)) sl_lidar_response_device_macaddr_info_t;
+
+typedef struct  _sl_lidar_response_desired_rot_speed_t{
+    sl_u16 rpm;
+    sl_u16 pwm_ref;
+}__attribute__((packed)) sl_lidar_response_desired_rot_speed_t;
+
+// Definition of the variable bit scale encoding mechanism
+#define SL_LIDAR_VARBITSCALE_X2_SRC_BIT  9
+#define SL_LIDAR_VARBITSCALE_X4_SRC_BIT  11
+#define SL_LIDAR_VARBITSCALE_X8_SRC_BIT  12
+#define SL_LIDAR_VARBITSCALE_X16_SRC_BIT 14
+
+#define SL_LIDAR_VARBITSCALE_X2_DEST_VAL 512
+#define SL_LIDAR_VARBITSCALE_X4_DEST_VAL 1280
+#define SL_LIDAR_VARBITSCALE_X8_DEST_VAL 1792
+#define SL_LIDAR_VARBITSCALE_X16_DEST_VAL 3328
+
+#define SL_LIDAR_VARBITSCALE_GET_SRC_MAX_VAL_BY_BITS(_BITS_) \
+    (  (((0x1<<(_BITS_)) - SL_LIDAR_VARBITSCALE_X16_DEST_VAL)<<4) + \
+       ((SL_LIDAR_VARBITSCALE_X16_DEST_VAL - SL_LIDAR_VARBITSCALE_X8_DEST_VAL)<<3) + \
+       ((SL_LIDAR_VARBITSCALE_X8_DEST_VAL - SL_LIDAR_VARBITSCALE_X4_DEST_VAL)<<2) + \
+       ((SL_LIDAR_VARBITSCALE_X4_DEST_VAL - SL_LIDAR_VARBITSCALE_X2_DEST_VAL)<<1) + \
+       SL_LIDAR_VARBITSCALE_X2_DEST_VAL - 1)
+
+
+#if defined(_WIN32)
+#pragma pack()
+#endif
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif

third_party/rplidar_sdk/include/sl_lidar_driver.h

@@ -0,0 +1,569 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+#pragma once
+
+#ifndef __cplusplus
+#error "The Slamtec LIDAR SDK requires a C++ compiler to be built"
+#endif
+
+#include <vector>
+#include <map>
+#include <string>
+
+#ifndef DEPRECATED
+    #ifdef __GNUC__
+        #define DEPRECATED(func) func __attribute__ ((deprecated))
+    #elif defined(_MSC_VER)
+        #define DEPRECATED(func) __declspec(deprecated) func
+    #else
+        #pragma message("WARNING: You need to implement DEPRECATED for this compiler")
+        #define DEPRECATED(func) func
+    #endif
+#endif
+
+
+#include "sl_lidar_cmd.h"
+
+#include <string>
+
+namespace sl {
+
+#ifdef DEPRECATED
+#define DEPRECATED_WARN(fn, replacement) do { \
+        static bool __shown__ = false; \
+        if (!__shown__) { \
+            printDeprecationWarn(fn, replacement); \
+            __shown__ = true; \
+        } \
+    } while (0)
+#endif
+
+    /**
+    * Lidar scan mode
+    */
+    struct LidarScanMode
+    {
+        // Mode id
+        sl_u16  id;
+
+        // Time cost for one measurement (in microseconds)
+        float   us_per_sample;
+
+        // Max distance in this scan mode (in meters)
+        float   max_distance;
+
+        // The answer command code for this scan mode
+        sl_u8   ans_type;
+
+        // The name of scan mode (padding with 0 if less than 64 characters)
+        char    scan_mode[64];
+    };
+
+    template <typename T>
+    struct Result
+    {
+        sl_result err;
+        T value;
+        Result(const T& value)
+            : err(SL_RESULT_OK)
+            , value(value)
+        {
+        }
+
+        Result(sl_result err)
+            : err(err)
+            , value()
+        {
+        }
+
+        operator sl_result() const
+        {
+            return err;
+        }
+
+        operator bool() const
+        {
+            return SL_IS_OK(err);
+        }
+
+        T& operator* ()
+        {
+            return value;
+        }
+
+        T* operator-> ()
+        {
+            return &value;
+        }
+    };
+
+    enum LIDARTechnologyType {
+        LIDAR_TECHNOLOGY_UNKNOWN = 0,
+        LIDAR_TECHNOLOGY_TRIANGULATION = 1,
+        LIDAR_TECHNOLOGY_DTOF = 2,
+        LIDAR_TECHNOLOGY_ETOF = 3,
+        LIDAR_TECHNOLOGY_FMCW = 4,
+    };
+
+    enum LIDARMajorType {
+        LIDAR_MAJOR_TYPE_UNKNOWN = 0,
+        LIDAR_MAJOR_TYPE_A_SERIES = 1,
+        LIDAR_MAJOR_TYPE_S_SERIES = 2,
+        LIDAR_MAJOR_TYPE_T_SERIES = 3,
+        LIDAR_MAJOR_TYPE_M_SERIES = 4,
+        LIDAR_MAJOR_TYPE_C_SERIES = 6,
+    };
+
+    enum LIDARInterfaceType {
+        LIDAR_INTERFACE_UART = 0,
+        LIDAR_INTERFACE_ETHERNET = 1,
+        LIDAR_INTERFACE_USB = 2,
+        LIDAR_INTERFACE_CANBUS = 5,
+
+
+        LIDAR_INTERFACE_UNKNOWN = 0xFFFF,
+    };
+
+    struct SlamtecLidarTimingDesc {
+
+        sl_u32  sample_duration_uS;
+        sl_u32  native_baudrate;
+        
+        sl_u32  linkage_delay_uS;
+
+        LIDARInterfaceType native_interface_type;
+
+        bool    native_timestamp_support;
+    };
+
+    /**
+    * Abstract interface of communication channel
+    */
+    class IChannel
+    {
+    public:
+        virtual ~IChannel() {}
+
+    public:
+        /**
+        * Open communication channel (return true if succeed)
+        */
+        virtual bool open() = 0;
+
+        /**
+        * Close communication channel
+        */
+        virtual void close() = 0;
+
+        /**
+        * Flush all written data to remote endpoint
+        */
+        virtual void flush() = 0;
+
+        /**
+        * Wait for some data
+        * \param size Bytes to wait
+        * \param timeoutInMs Wait timeout (in microseconds, -1 for forever)
+        * \param actualReady [out] actual ready bytes
+        * \return true for data ready
+        */
+        virtual bool waitForData(size_t size, sl_u32 timeoutInMs = -1, size_t* actualReady = nullptr) = 0;
+
+
+        /**
+        * Wait for some data
+        * \param size_hint Byte count may available to retrieve without beening blocked 
+        * \param timeoutInMs Wait timeout (in microseconds, -1 for forever)
+        * \return RESULT_OK if there is data available for receiving
+        *         RESULT_OPERATION_TIMEOUT if the given timeout duration is exceed
+        *         RESULT_OPERATION_FAIL if there is something wrong with the channel
+        */
+        virtual sl_result waitForDataExt(size_t& size_hint, sl_u32 timeoutInMs = 1000) = 0;
+
+
+        /**
+        * Send data to remote endpoint
+        * \param data The data buffer
+        * \param size The size of data buffer (in bytes)
+        * \return Bytes written (negative for write failure)
+        */
+        virtual int write(const void* data, size_t size) = 0;
+
+        /**
+        * Read data from the chanel
+        * \param buffer The buffer to receive data
+        * \param size The size of the read buffer
+        * \return Bytes read (negative for read failure)
+        */
+        virtual int read(void* buffer, size_t size) = 0;
+
+        /**
+        * Clear read cache
+        */
+        virtual void clearReadCache() = 0;
+
+        virtual int getChannelType() = 0;
+
+    private:
+
+    };
+
+    /**
+    * Abstract interface of serial port channel
+    */
+    class ISerialPortChannel : public IChannel
+    {
+    public:
+        virtual ~ISerialPortChannel() {}
+
+    public:
+        virtual void setDTR(bool dtr) = 0;
+    };
+
+    /**
+    * Create a serial channel
+    * \param device Serial port device
+    *                   e.g. on Windows, it may be com3 or \\.\com10
+    *                   on Unix-Like OS, it may be /dev/ttyS1, /dev/ttyUSB2, etc
+    * \param baudrate Baudrate
+    *                   Please refer to the datasheet for the baudrate (maybe 115200 or 256000)
+    */
+    Result<IChannel*> createSerialPortChannel(const std::string& device, int baudrate);
+
+    /**
+    * Create a TCP channel
+    * \param ip IP address of the device
+    * \param port TCP port
+    */
+    Result<IChannel*> createTcpChannel(const std::string& ip, int port);
+
+    /**
+    * Create a UDP channel
+    * \param ip IP address of the device
+    * \param port UDP port
+    */
+    Result<IChannel*> createUdpChannel(const std::string& ip, int port);
+
+    enum MotorCtrlSupport
+    {
+        MotorCtrlSupportNone = 0,
+        MotorCtrlSupportPwm = 1,
+        MotorCtrlSupportRpm = 2,
+    };
+
+    enum ChannelType{
+        CHANNEL_TYPE_SERIALPORT = 0x0,
+        CHANNEL_TYPE_TCP = 0x1,
+        CHANNEL_TYPE_UDP = 0x2,
+    };
+
+        /**
+    * Lidar motor info
+    */
+    struct LidarMotorInfo
+    {
+        MotorCtrlSupport motorCtrlSupport;
+
+        // Desire speed
+        sl_u16 desired_speed;
+
+        // Max speed 
+        sl_u16 max_speed;
+
+        // Min speed
+        sl_u16 min_speed;
+    };
+
+    class ILidarDriver
+    {
+    public:
+        virtual ~ILidarDriver() {}
+
+    public:
+        /**
+        * Connect to LIDAR via channel
+        * \param channel The communication channel
+        *                    Note: you should manage the lifecycle of the channel object, make sure it is alive during lidar driver's lifecycle
+        */
+        virtual sl_result connect(IChannel* channel) = 0;
+
+        /**
+        * Disconnect from the LIDAR
+        */
+        virtual void disconnect() = 0;
+        
+        /**
+        * Check if the connection is established
+        */
+        virtual bool isConnected() = 0;
+
+    public:
+        enum
+        {
+            DEFAULT_TIMEOUT = 2000
+        };
+
+    public:
+        /// Ask the LIDAR core system to reset it self
+        /// The host system can use the Reset operation to help LIDAR escape the self-protection mode.
+        ///
+        ///  \param timeout       The operation timeout value (in millisecond)
+        virtual sl_result reset(sl_u32 timeoutInMs = DEFAULT_TIMEOUT) = 0;
+
+        /// Get all scan modes that supported by lidar
+        virtual sl_result getAllSupportedScanModes(std::vector<LidarScanMode>& outModes, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) = 0;
+
+        /// Get typical scan mode of lidar
+        virtual sl_result getTypicalScanMode(sl_u16& outMode, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) = 0;
+
+        /// Start scan
+        ///
+        /// \param force            Force the core system to output scan data regardless whether the scanning motor is rotating or not.
+        /// \param useTypicalScan   Use lidar's typical scan mode or use the compatibility mode (2k sps)
+        /// \param options          Scan options (please use 0)
+        /// \param outUsedScanMode  The scan mode selected by lidar
+        virtual sl_result startScan(bool force, bool useTypicalScan, sl_u32 options = 0, LidarScanMode* outUsedScanMode = nullptr) = 0;
+
+        /// Start scan in specific mode
+        ///
+        /// \param force            Force the core system to output scan data regardless whether the scanning motor is rotating or not.
+        /// \param scanMode         The scan mode id (use getAllSupportedScanModes to get supported modes)
+        /// \param options          Scan options (please use 0)
+        /// \param outUsedScanMode  The scan mode selected by lidar
+        virtual sl_result startScanExpress(bool force, sl_u16 scanMode, sl_u32 options = 0, LidarScanMode* outUsedScanMode = nullptr, sl_u32 timeout = DEFAULT_TIMEOUT) = 0;
+
+        /// Retrieve the health status of the RPLIDAR
+        /// The host system can use this operation to check whether RPLIDAR is in the self-protection mode.
+        ///
+        /// \param health        The health status info returned from the RPLIDAR
+        ///
+        /// \param timeout       The operation timeout value (in millisecond) for the serial port communication     
+        virtual sl_result getHealth(sl_lidar_response_device_health_t& health, sl_u32 timeout = DEFAULT_TIMEOUT) = 0;
+
+        /// Get the device information of the RPLIDAR include the serial number, firmware version, device model etc.
+        /// 
+        /// \param info          The device information returned from the RPLIDAR
+        /// \param timeout       The operation timeout value (in millisecond) for the serial port communication  
+        virtual sl_result getDeviceInfo(sl_lidar_response_device_info_t& info, sl_u32 timeout = DEFAULT_TIMEOUT) = 0;
+
+        /// Check whether the device support motor control
+        /// Note: this API will disable grab.
+        /// 
+        /// \param motorCtrlSupport Return the result.
+        /// \param timeout          The operation timeout value (in millisecond) for the serial port communication. 
+        virtual sl_result checkMotorCtrlSupport(MotorCtrlSupport& motorCtrlSupport, sl_u32 timeout = DEFAULT_TIMEOUT) = 0;
+
+        /// Calculate LIDAR's current scanning frequency from the given scan data
+        /// Please refer to the application note doc for details
+        /// Remark: the calcuation will be incorrect if the specified scan data doesn't contains enough data
+        ///
+        /// \param scanMode      Lidar's current scan mode
+        /// \param nodes         Current scan's measurements
+        /// \param count         The number of sample nodes inside the given buffer
+        virtual sl_result getFrequency(const LidarScanMode& scanMode, const sl_lidar_response_measurement_node_hq_t* nodes, size_t count, float& frequency) = 0;
+
+		///Set LPX and S2E series lidar's static IP address
+		///
+		/// \param conf             Network parameter that LPX series lidar owned
+		/// \param timeout          The operation timeout value (in millisecond) for the ethernet udp communication
+		virtual sl_result setLidarIpConf(const sl_lidar_ip_conf_t& conf, sl_u32 timeout = DEFAULT_TIMEOUT) = 0;
+       
+        ///Get LPX and S2E series lidar's static IP address
+        ///
+        /// \param conf             Network parameter that LPX series lidar owned
+        /// \param timeout          The operation timeout value (in millisecond) for the ethernet udp communication
+        virtual sl_result getLidarIpConf( sl_lidar_ip_conf_t& conf, sl_u32 timeout = DEFAULT_TIMEOUT) = 0;
+  // 
+		/////Get LPX series lidar's MAC address
+		///
+		/// \param macAddrArray         The device MAC information returned from the LPX series lidar
+        ///                             Notice: the macAddrArray must point to a valid buffer with at least 6 bytes length
+        ///                                     Otherwise, buffer overwrite will occur
+		virtual sl_result getDeviceMacAddr(sl_u8* macAddrArray, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) = 0;
+
+        /// Ask the LIDAR core system to stop the current scan operation and enter idle state. The background thread will be terminated
+        ///
+        /// \param timeout       The operation timeout value (in millisecond) for the serial port communication 
+        virtual sl_result stop(sl_u32 timeout = DEFAULT_TIMEOUT) = 0;
+
+        /// Wait and grab a complete 0-360 degree scan data previously received. 
+        /// The grabbed scan data returned by this interface always has the following charactistics:
+        ///
+        /// 1) The first node of the grabbed data array (nodebuffer[0]) must be the first sample of a scan, i.e. the start_bit == 1
+        /// 2) All data nodes are belong to exactly ONE complete 360-degrees's scan
+        /// 3) Note, the angle data in one scan may not be ascending. You can use API ascendScanData to reorder the nodebuffer.
+        ///
+        /// \param nodebuffer     Buffer provided by the caller application to store the scan data
+        ///
+        /// \param count          The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t).
+        ///                       Once the interface returns, this parameter will store the actual received data count.
+        ///
+        /// \param timeout        Max duration allowed to wait for a complete scan data, nothing will be stored to the nodebuffer if a complete 360-degrees' scan data cannot to be ready timely.
+        ///
+        /// The interface will return SL_RESULT_OPERATION_TIMEOUT to indicate that no complete 360-degrees' scan can be retrieved withing the given timeout duration. 
+        ///
+        /// \The caller application can set the timeout value to Zero(0) to make this interface always returns immediately to achieve non-block operation.
+        virtual sl_result grabScanDataHq(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t& count, sl_u32 timeout = DEFAULT_TIMEOUT) = 0;
+
+
+
+        /// Wait and grab a complete 0-360 degree scan data previously received with timestamp support.
+        /// 
+        /// The returned timestamp belongs to the first data point of the scan data (begining of the scan).
+        /// Its value is represented based on the current machine's time domain with the unit of microseconds (uS).
+        /// 
+        /// If the currently connected LIDAR supports hardware timestamp mechanism, this timestamp will use 
+        /// the actual data emitted by the LIDAR device and remap it to the current machine's time domain. 
+        /// 
+        /// For other models that do not support hardware timestamps, this data will be deducted through estimation, 
+        /// and there may be a slight deviation from the actual situation. 
+        ///
+        /// The grabbed scan data returned by this interface always has the following charactistics:
+        ///
+        /// 1) The first node of the grabbed data array (nodebuffer[0]) must be the first sample of a scan, i.e. the start_bit == 1
+        /// 2) All data nodes are belong to exactly ONE complete 360-degrees's scan
+        /// 3) Note, the angle data in one scan may not be ascending. You can use API ascendScanData to reorder the nodebuffer.
+        ///
+        /// \param nodebuffer     Buffer provided by the caller application to store the scan data
+        ///
+        /// \param count          The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t).
+        ///                       Once the interface returns, this parameter will store the actual received data count.
+        ///
+        /// \param timestamp_uS   The reference used to store the timestamp value.
+        /// \param timeout        Max duration allowed to wait for a complete scan data, nothing will be stored to the nodebuffer if a complete 360-degrees' scan data cannot to be ready timely.
+        ///
+        /// The interface will return SL_RESULT_OPERATION_TIMEOUT to indicate that no complete 360-degrees' scan can be retrieved withing the given timeout duration. 
+        ///
+        /// \The caller application can set the timeout value to Zero(0) to make this interface always returns immediately to achieve non-block operation.
+        virtual sl_result grabScanDataHqWithTimeStamp(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t& count, sl_u64 & timestamp_uS, sl_u32 timeout = DEFAULT_TIMEOUT) = 0;
+
+
+        /// Ascending the scan data according to the angle value in the scan.
+        ///
+        /// \param nodebuffer     Buffer provided by the caller application to do the reorder. Should be retrived from the grabScanData
+        ///
+        /// \param count          The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t).
+        ///                       Once the interface returns, this parameter will store the actual received data count.
+        /// The interface will return SL_RESULT_OPERATION_FAIL when all the scan data is invalid. 
+        virtual sl_result ascendScanData(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t count) = 0;
+
+        /// Return received scan points even if it's not complete scan
+        ///
+        /// \param nodebuffer     Buffer provided by the caller application to store the scan data
+        ///
+        /// \param count          Once the interface returns, this parameter will store the actual received data count.
+        ///
+        /// The interface will return SL_RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call. 
+        virtual sl_result getScanDataWithIntervalHq(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t& count) = 0;
+        /// Set lidar motor speed
+        /// The host system can use this operation to set lidar motor speed.
+        ///
+        /// \param speed        The speed value set to lidar
+        ///
+        ///Note: The function will stop scan if speed is DEFAULT_MOTOR_SPEED.
+        virtual sl_result setMotorSpeed(sl_u16 speed = DEFAULT_MOTOR_SPEED) = 0;
+        
+        /// Get the motor information of the RPLIDAR include the max speed, min speed, desired speed.
+        /// 
+        /// \param motorInfo          The motor information returned from the RPLIDAR
+        virtual sl_result getMotorInfo(LidarMotorInfo &motorInfo, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) = 0;
+    
+
+        /// Ask the LIDAR to use a new baudrate for serial communication
+        /// The target LIDAR system must support such feature to work.
+        /// This function does NOT check whether the target LIDAR works with the requiredBaudRate or not.
+        /// In order to verifiy the result, use getDeviceInfo or other getXXXX functions instead.
+        /// 
+        /// \param requiredBaudRate   The new baudrate required to be used. It MUST matches with the baudrate of the binded channel.
+        /// \param baudRateDetected   The actual baudrate detected by the LIDAR system
+        virtual sl_result negotiateSerialBaudRate(sl_u32 requiredBaudRate, sl_u32* baudRateDetected = NULL) = 0;
+
+
+
+        /// Get the technology of the LIDAR's measurement system
+        /// 
+        /// 
+        /// \param devInfo   The device info used to deduct the result
+        ///                  If NULL is specified, a driver cached version of the connected LIDAR will be used
+        virtual LIDARTechnologyType getLIDARTechnologyType(const sl_lidar_response_device_info_t* devInfo = nullptr) = 0;
+        
+        
+        /// Get the Major Type (Series Info) of the LIDAR
+        /// 
+        /// 
+        /// \param devInfo   The device info used to deduct the result
+        ///                  If NULL is specified, a driver cached version of the connected LIDAR will be used
+        virtual LIDARMajorType getLIDARMajorType(const sl_lidar_response_device_info_t* devInfo = nullptr) = 0;
+
+
+        /// Get the Model Name of the LIDAR
+        /// The result will be somthing like: "A1M8" or "S1M1" or "A3M1-R1"
+        /// 
+        /// \param out_description   The output string that contains the generated model name
+        ///                          
+        /// \param fetchAliasName    If set to true, a communication will be taken to ask if there is any Alias name availabe
+        /// \param devInfo           The device info used to deduct the result
+        ///                          If NULL is specified, a driver cached version of the connected LIDAR will be used
+        /// \param timeout           The timeout value used by potential data communication
+        virtual sl_result getModelNameDescriptionString(std::string& out_description, bool fetchAliasName = true, const sl_lidar_response_device_info_t* devInfo = nullptr, sl_u32 timeout = DEFAULT_TIMEOUT) = 0;
+
+};
+
+    /**
+    * Create a LIDAR driver instance
+    *
+    * Example
+    * Result<ISerialChannel*> channel = createSerialPortChannel("/dev/ttyUSB0", 115200);
+    * assert((bool)channel);
+    * assert(*channel);
+    *
+    * auto lidar = createLidarDriver();
+    * assert((bool)lidar);
+    * assert(*lidar);
+    *
+    * auto res = (*lidar)->connect(*channel);
+    * assert(SL_IS_OK(res));
+    *
+    * sl_lidar_response_device_info_t deviceInfo;
+    * res = (*lidar)->getDeviceInfo(deviceInfo);
+    * assert(SL_IS_OK(res));
+    *
+    * printf("Model: %d, Firmware Version: %d.%d, Hardware Version: %d\n",
+    *        deviceInfo.model,
+    *        deviceInfo.firmware_version >> 8, deviceInfo.firmware_version & 0xffu,
+    *        deviceInfo.hardware_version);
+    *
+    * delete *lidar;
+    * delete *channel;
+    */
+    Result<ILidarDriver*> createLidarDriver();
+}

third_party/rplidar_sdk/include/sl_lidar_driver_impl.h

@@ -0,0 +1,157 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+#pragma once
+#include "sl_lidar_driver.h"
+
+namespace sl {
+	class SL_LidarDriver :public ILidarDriver
+	{
+		public:
+			enum {
+				LEGACY_SAMPLE_DURATION = 476,
+			};
+
+			enum
+			{
+					NORMAL_CAPSULE = 0,
+					DENSE_CAPSULE = 1,
+			};
+
+			enum {
+				A2A3_LIDAR_MINUM_MAJOR_ID  = 2,
+				TOF_LIDAR_MINUM_MAJOR_ID = 6,
+			};
+		public:
+			SL_LidarDriver()
+				:_channel(NULL)
+				, _isConnected(false)
+				, _isScanning(false)
+				, _isSupportingMotorCtrl(MotorCtrlSupportNone)
+				, _cached_sampleduration_std(LEGACY_SAMPLE_DURATION)
+				,_cached_sampleduration_express(LEGACY_SAMPLE_DURATION)
+				, _cached_scan_node_hq_count(0)
+				, _cached_scan_node_hq_count_for_interval_retrieve(0)
+			{}
+
+			sl_result connect(IChannel* channel);
+			void disconnect();
+			bool isConnected();
+			sl_result reset(sl_u32 timeoutInMs = DEFAULT_TIMEOUT);
+			sl_result getAllSupportedScanModes(std::vector<LidarScanMode>& outModes, sl_u32 timeoutInMs = DEFAULT_TIMEOUT);
+			sl_result getTypicalScanMode(sl_u16& outMode, sl_u32 timeoutInMs = DEFAULT_TIMEOUT);
+			sl_result startScan(bool force, bool useTypicalScan, sl_u32 options = 0, LidarScanMode* outUsedScanMode = nullptr);
+			sl_result startScanNormal(bool force, sl_u32 timeout = DEFAULT_TIMEOUT);
+			sl_result startScanExpress(bool force, sl_u16 scanMode, sl_u32 options = 0, LidarScanMode* outUsedScanMode = nullptr, sl_u32 timeout = DEFAULT_TIMEOUT);
+			sl_result stop(sl_u32 timeout = DEFAULT_TIMEOUT);
+			DEPRECATED(sl_result grabScanData(sl_lidar_response_measurement_node_t * nodebuffer, size_t& count, sl_u32 timeout = DEFAULT_TIMEOUT));
+			sl_result grabScanDataHq(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t& count, sl_u32 timeout = DEFAULT_TIMEOUT);
+			sl_result getDeviceInfo(sl_lidar_response_device_info_t& info, sl_u32 timeout = DEFAULT_TIMEOUT);
+			sl_result checkMotorCtrlSupport(MotorCtrlSupport & support, sl_u32 timeout = DEFAULT_TIMEOUT);
+			sl_result getFrequency(const LidarScanMode& scanMode, const sl_lidar_response_measurement_node_hq_t* nodes, size_t count, float& frequency);
+			sl_result setLidarIpConf(const sl_lidar_ip_conf_t& conf, sl_u32 timeout);
+			sl_result getLidarIpConf(sl_lidar_ip_conf_t& conf, sl_u32 timeout);
+			sl_result getHealth(sl_lidar_response_device_health_t& health, sl_u32 timeout = DEFAULT_TIMEOUT);
+			sl_result getDeviceMacAddr(sl_u8* macAddrArray, sl_u32 timeoutInMs);
+			sl_result ascendScanData(sl_lidar_response_measurement_node_t * nodebuffer, size_t count);
+			sl_result ascendScanData(sl_lidar_response_measurement_node_hq_t * nodebuffer, size_t count);
+			sl_result getScanDataWithIntervalHq(sl_lidar_response_measurement_node_hq_t * nodebuffer, size_t & count);
+			sl_result setMotorSpeed(sl_u16 speed = DEFAULT_MOTOR_PWM);//
+			sl_result negotiateSerialBaudRate(sl_u32 requiredBaudRate, sl_u32* baudRateDetected = NULL);
+	
+	protected:
+			sl_result startMotor();
+			sl_result checkSupportConfigCommands(bool& outSupport, sl_u32 timeoutInMs = DEFAULT_TIMEOUT);
+			sl_result getScanModeCount(sl_u16& modeCount, sl_u32 timeoutInMs = DEFAULT_TIMEOUT);
+			sl_result setLidarConf(sl_u32 type, const void* payload, size_t payloadSize, sl_u32 timeout);
+			sl_result getLidarConf(sl_u32 type, std::vector<sl_u8> &outputBuf, const std::vector<sl_u8> &reserve = std::vector<sl_u8>(), sl_u32 timeout = DEFAULT_TIMEOUT);
+			sl_result getLidarSampleDuration(float& sampleDurationRes, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT);
+			sl_result getMaxDistance(float &maxDistance, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT);
+			sl_result getScanModeAnsType(sl_u8 &ansType, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT);
+			sl_result getScanModeName(char* modeName, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT);
+			//DEPRECATED(sl_result getSampleDuration_uS(sl_lidar_response_sample_rate_t & rateInfo, sl_u32 timeout = DEFAULT_TIMEOUT));
+			//DEPRECATED (sl_result checkExpressScanSupported(bool & support, sl_u32 timeout = DEFAULT_TIMEOUT));
+			//DEPRECATED(sl_result getFrequency(bool inExpressMode, size_t count, float & frequency, bool & is4kmode));
+		private:
+			sl_result  _sendCommand(sl_u16 cmd, const void * payload = NULL, size_t payloadsize = 0 );
+			sl_result _waitResponseHeader(sl_lidar_ans_header_t * header, sl_u32 timeout = DEFAULT_TIMEOUT);
+			template <typename T>
+			sl_result _waitResponse(T &payload ,sl_u8 ansType, sl_u32 timeout = DEFAULT_TIMEOUT);
+			void _disableDataGrabbing();
+			sl_result _waitNode(sl_lidar_response_measurement_node_t * node, sl_u32 timeout = DEFAULT_TIMEOUT);
+			sl_result _waitScanData(sl_lidar_response_measurement_node_t * nodebuffer, size_t & count, sl_u32 timeout = DEFAULT_TIMEOUT);
+			sl_result _cacheScanData();
+			void _ultraCapsuleToNormal(const sl_lidar_response_ultra_capsule_measurement_nodes_t & capsule, sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount);
+			sl_result _waitCapsuledNode(sl_lidar_response_capsule_measurement_nodes_t & node, sl_u32 timeout = DEFAULT_TIMEOUT);
+			void _capsuleToNormal(const sl_lidar_response_capsule_measurement_nodes_t & capsule, sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount);
+			void _dense_capsuleToNormal(const sl_lidar_response_capsule_measurement_nodes_t & capsule, sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount);
+			sl_result _cacheCapsuledScanData();
+
+			void _ultra_dense_capsuleToNormal(const sl_lidar_response_ultra_dense_capsule_measurement_nodes_t& capslue, sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t& nodeCount);
+			sl_result _waitUltraDenseCapsuledNode(sl_lidar_response_ultra_dense_capsule_measurement_nodes_t& node, sl_u32 timeout = DEFAULT_TIMEOUT);
+			sl_result _cacheUltraDenseCapsuledScanData();
+			
+
+			sl_result _waitHqNode(sl_lidar_response_hq_capsule_measurement_nodes_t & node, sl_u32 timeout = DEFAULT_TIMEOUT);
+			void _HqToNormal(const sl_lidar_response_hq_capsule_measurement_nodes_t & node_hq, sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount);
+			sl_result _cacheHqScanData();
+			sl_result _waitUltraCapsuledNode(sl_lidar_response_ultra_capsule_measurement_nodes_t & node, sl_u32 timeout = DEFAULT_TIMEOUT);
+			sl_result _cacheUltraCapsuledScanData();
+			sl_result _clearRxDataCache();
+
+		private:
+			IChannel *_channel;
+			bool _isConnected;
+			bool _isScanning;
+			MotorCtrlSupport _isSupportingMotorCtrl;
+
+			rp::hal::Locker         _lock;
+			rp::hal::Event          _dataEvt;
+			rp::hal::Thread         _cachethread;
+			sl_u16                    _cached_sampleduration_std;
+			sl_u16                    _cached_sampleduration_express;
+
+			sl_lidar_response_measurement_node_hq_t   _cached_scan_node_hq_buf[8192];
+			size_t                                   _cached_scan_node_hq_count;
+			sl_u8                                    _cached_capsule_flag;
+
+			sl_lidar_response_measurement_node_hq_t   _cached_scan_node_hq_buf_for_interval_retrieve[8192];
+			size_t                                   _cached_scan_node_hq_count_for_interval_retrieve;
+
+			sl_lidar_response_capsule_measurement_nodes_t       _cached_previous_capsuledata;
+			sl_lidar_response_dense_capsule_measurement_nodes_t _cached_previous_dense_capsuledata;
+			sl_lidar_response_ultra_capsule_measurement_nodes_t _cached_previous_ultracapsuledata;
+			sl_lidar_response_hq_capsule_measurement_nodes_t _cached_previous_Hqdata;
+			bool                                         _is_previous_capsuledataRdy;
+			bool                                         _is_previous_HqdataRdy;
+	};
+
+}

third_party/rplidar_sdk/include/sl_lidar_protocol.h

@@ -0,0 +1,85 @@
+/*
+* Slamtec LIDAR SDK
+*
+* sl_lidar_protocol.h
+*
+* Copyright (c) 2020 Shanghai Slamtec Co., Ltd.
+*/
+
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4200)
+#endif
+
+#include "sl_types.h"
+
+#define SL_LIDAR_CMD_SYNC_BYTE              0xA5
+#define SL_LIDAR_CMDFLAG_HAS_PAYLOAD        0x80
+
+#define SL_LIDAR_ANS_SYNC_BYTE1             0xA5
+#define SL_LIDAR_ANS_SYNC_BYTE2             0x5A
+
+#define SL_LIDAR_ANS_PKTFLAG_LOOP           0x1
+
+#define SL_LIDAR_ANS_HEADER_SIZE_MASK       0x3FFFFFFF
+#define SL_LIDAR_ANS_HEADER_SUBTYPE_SHIFT   (30)
+
+#if defined(_WIN32)
+#pragma pack(1)
+#endif
+
+
+
+typedef struct sl_lidar_cmd_packet_t
+{
+    sl_u8 syncByte; //must be SL_LIDAR_CMD_SYNC_BYTE
+    sl_u8 cmd_flag;
+    sl_u8 size;
+    sl_u8 data[0];
+} __attribute__((packed)) sl_lidar_cmd_packet_t;
+
+
+typedef struct sl_lidar_ans_header_t
+{
+    sl_u8  syncByte1; // must be SL_LIDAR_ANS_SYNC_BYTE1
+    sl_u8  syncByte2; // must be SL_LIDAR_ANS_SYNC_BYTE2
+    sl_u32 size_q30_subtype; // see _u32 size:30; _u32 subType:2;
+    sl_u8  type;
+} __attribute__((packed)) sl_lidar_ans_header_t;
+
+#if defined(_WIN32)
+#pragma pack()
+#endif
+
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif

third_party/rplidar_sdk/include/sl_types.h

@@ -0,0 +1,83 @@
+/*
+* Slamtec LIDAR SDK
+*
+* sl_types.h
+*
+* Copyright (c) 2020 Shanghai Slamtec Co., Ltd. 
+*/
+
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+
+#ifdef __cplusplus
+#include <cstdint>
+
+#define SL_DEFINE_TYPE(IntType, NewType)    typedef std::IntType NewType
+#else
+#include <stdint.h>
+
+#define SL_DEFINE_TYPE(IntType, NewType)    typedef IntType NewType
+#endif
+
+#define SL_DEFINE_INT_TYPE(Bits) \
+    SL_DEFINE_TYPE(int ## Bits ## _t, sl_s ## Bits); \
+    SL_DEFINE_TYPE(uint ## Bits ## _t, sl_u ## Bits); \
+
+SL_DEFINE_INT_TYPE(8)
+SL_DEFINE_INT_TYPE(16)
+SL_DEFINE_INT_TYPE(32)
+SL_DEFINE_INT_TYPE(64)
+
+#if !defined(__GNUC__) && !defined(__attribute__)
+#   define __attribute__(x)
+#endif
+
+#ifdef WIN64
+typedef sl_u64          sl_word_size_t;
+#elif defined(WIN32)
+typedef sl_u32          sl_word_size_t;
+#elif defined(__GNUC__)
+typedef unsigned long   sl_word_size_t;
+#elif defined(__ICCARM__)
+typedef sl_u32          sl_word_size_t;
+#endif
+
+typedef uint32_t sl_result;
+
+#define SL_RESULT_OK                     (sl_result)0
+#define SL_RESULT_FAIL_BIT               (sl_result)0x80000000
+#define SL_RESULT_ALREADY_DONE           (sl_result)0x20
+#define SL_RESULT_INVALID_DATA           (sl_result)(0x8000 | SL_RESULT_FAIL_BIT)
+#define SL_RESULT_OPERATION_FAIL         (sl_result)(0x8001 | SL_RESULT_FAIL_BIT)
+#define SL_RESULT_OPERATION_TIMEOUT      (sl_result)(0x8002 | SL_RESULT_FAIL_BIT)
+#define SL_RESULT_OPERATION_STOP         (sl_result)(0x8003 | SL_RESULT_FAIL_BIT)
+#define SL_RESULT_OPERATION_NOT_SUPPORT  (sl_result)(0x8004 | SL_RESULT_FAIL_BIT)
+#define SL_RESULT_FORMAT_NOT_SUPPORT     (sl_result)(0x8005 | SL_RESULT_FAIL_BIT)
+#define SL_RESULT_INSUFFICIENT_MEMORY    (sl_result)(0x8006 | SL_RESULT_FAIL_BIT)
+
+#define SL_IS_OK(x)    ( ((x) & SL_RESULT_FAIL_BIT) == 0 )
+#define SL_IS_FAIL(x)  ( ((x) & SL_RESULT_FAIL_BIT) )

third_party/rplidar_sdk/src/arch/linux/arch_linux.h

@@ -0,0 +1,64 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+
+// libc dep
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include <assert.h>
+#include <math.h>
+#include <time.h>
+#include <stdarg.h>
+
+// libc++ dep
+#include <iostream>
+#include <string>
+
+// linux specific
+#include <unistd.h>
+#include <errno.h>
+#include <pthread.h>
+#include <sys/time.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <sys/select.h>
+#include <time.h>
+
+#include "timer.h"
+

third_party/rplidar_sdk/src/arch/linux/net_serial.cpp

@@ -0,0 +1,475 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "arch/linux/arch_linux.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <assert.h>
+// linux specific
+
+#include <errno.h>
+#include <fcntl.h>
+
+#include <time.h>
+#include "hal/types.h"
+#include "arch/linux/net_serial.h"
+#include <sys/select.h>
+
+#include <algorithm>
+//__GNUC__
+#if defined(__GNUC__)
+// for Linux extension
+#include <asm/ioctls.h>
+#include <asm/termbits.h>
+#include <sys/ioctl.h>
+extern "C" int tcflush(int fildes, int queue_selector);
+#else
+// for other standard UNIX
+#include <termios.h>
+#include <sys/ioctl.h>
+
+#endif
+
+
+namespace rp{ namespace arch{ namespace net{
+
+raw_serial::raw_serial()
+    : rp::hal::serial_rxtx()
+    , _baudrate(0)
+    , _flags(0)
+    , serial_fd(-1)
+{
+    _init();
+}
+
+raw_serial::~raw_serial()
+{
+    close();
+
+}
+
+bool raw_serial::open()
+{
+    return open(_portName, _baudrate, _flags);
+}
+
+bool raw_serial::bind(const char * portname, uint32_t baudrate, uint32_t flags)
+{   
+    strncpy(_portName, portname, sizeof(_portName));
+    _baudrate = baudrate;
+    _flags    = flags;
+    return true;
+}
+
+bool raw_serial::open(const char * portname, uint32_t baudrate, uint32_t flags)
+{
+    if (isOpened()) close();
+    
+    serial_fd = ::open(portname, O_RDWR | O_NOCTTY | O_NDELAY);
+
+    if (serial_fd == -1) return false;
+
+    
+
+#if !defined(__GNUC__)
+    // for standard UNIX
+    struct termios options, oldopt;
+    tcgetattr(serial_fd, &oldopt);
+    bzero(&options,sizeof(struct termios));
+
+    // enable rx and tx
+    options.c_cflag |= (CLOCAL | CREAD);
+
+    _u32 termbaud = getTermBaudBitmap(baudrate);
+
+    if (termbaud == (_u32)-1) {
+        close();
+        return false;
+    }
+    cfsetispeed(&options, termbaud);
+    cfsetospeed(&options, termbaud);
+
+    options.c_cflag &= ~PARENB; //no checkbit
+    options.c_cflag &= ~CSTOPB; //1bit stop bit
+    options.c_cflag &= ~CRTSCTS; //no flow control
+
+    options.c_cflag &= ~CSIZE;
+    options.c_cflag |= CS8; /* Select 8 data bits */
+
+#ifdef CNEW_RTSCTS
+    options.c_cflag &= ~CNEW_RTSCTS; // no hw flow control
+#endif
+
+    options.c_iflag &= ~(IXON | IXOFF | IXANY); // no sw flow control
+
+    // raw input mode   
+    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
+    // raw output mode   
+    options.c_oflag &= ~OPOST;
+
+
+
+    if (tcsetattr(serial_fd, TCSANOW, &options))
+    {
+        close();
+        return false;
+    }
+
+#else
+
+    // using Linux extension ...
+    struct termios2 tio;
+
+    ioctl(serial_fd, TCGETS2, &tio);
+    bzero(&tio, sizeof(struct termios2));
+
+    tio.c_cflag = BOTHER;
+    tio.c_cflag |= (CLOCAL | CREAD | CS8); //8 bit no hardware handshake
+
+    tio.c_cflag &= ~CSTOPB;   //1 stop bit
+    tio.c_cflag &= ~CRTSCTS;  //No CTS
+    tio.c_cflag &= ~PARENB;   //No Parity
+
+#ifdef CNEW_RTSCTS
+    tio.c_cflag &= ~CNEW_RTSCTS; // no hw flow control
+#endif
+
+    tio.c_iflag &= ~(IXON | IXOFF | IXANY); // no sw flow control
+
+
+    tio.c_cc[VMIN] = 0;         //min chars to read
+    tio.c_cc[VTIME] = 0;        //time in 1/10th sec wait
+
+    tio.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
+    // raw output mode   
+    tio.c_oflag &= ~OPOST;
+
+    tio.c_ispeed = baudrate;
+    tio.c_ospeed = baudrate;
+
+
+    ioctl(serial_fd, TCSETS2, &tio);
+
+#endif
+
+
+    tcflush(serial_fd, TCIFLUSH);
+
+    if (fcntl(serial_fd, F_SETFL, FNDELAY))
+    {
+        close();
+        return false;
+    }
+
+
+    _is_serial_opened = true;
+    _operation_aborted = false;
+
+    //Clear the DTR bit to let the motor spin
+    clearDTR();
+    do {
+        // create self pipeline for wait cancellation
+        if (pipe(_selfpipe) == -1) break;
+
+        int flags = fcntl(_selfpipe[0], F_GETFL);
+        if (flags == -1)
+            break;
+
+        flags |= O_NONBLOCK;                /* Make read end nonblocking */
+        if (fcntl(_selfpipe[0], F_SETFL, flags) == -1)
+            break;
+
+        flags = fcntl(_selfpipe[1], F_GETFL);
+        if (flags == -1)
+            break;
+
+        flags |= O_NONBLOCK;                /* Make write end nonblocking */
+        if (fcntl(_selfpipe[1], F_SETFL, flags) == -1)
+            break;
+
+    } while (0);
+    
+    return true;
+}
+
+void raw_serial::close()
+{
+    if (serial_fd != -1)
+        ::close(serial_fd);
+    serial_fd = -1;
+    
+    if (_selfpipe[0] != -1)
+        ::close(_selfpipe[0]);
+
+    if (_selfpipe[1] != -1)
+        ::close(_selfpipe[1]);
+
+    _selfpipe[0] = _selfpipe[1] = -1;
+
+    _operation_aborted = false;
+    _is_serial_opened = false;
+}
+
+int raw_serial::senddata(const unsigned char * data, size_t size)
+{
+// FIXME: non-block io should be used
+    if (!isOpened()) return 0;
+
+    if (data == NULL || size ==0) return 0;
+    
+    size_t tx_len = 0;
+    required_tx_cnt = 0;
+    do {
+        int ans = ::write(serial_fd, data + tx_len, size-tx_len);
+        
+        if (ans == -1) return tx_len;
+        
+        tx_len += ans;
+        required_tx_cnt = tx_len;
+    }while (tx_len<size);
+    
+    
+    return tx_len;
+}
+
+
+int raw_serial::recvdata(unsigned char * data, size_t size)
+{
+    if (!isOpened()) return 0;
+    
+    int ans = ::read(serial_fd, data, size);
+    
+    if (ans == -1) ans=0;
+    required_rx_cnt = ans;
+    return ans;
+}
+
+
+void raw_serial::flush( _u32 flags)
+{
+    tcflush(serial_fd,TCIFLUSH); 
+}
+
+int raw_serial::waitforsent(_u32 timeout, size_t * returned_size)
+{
+    if (returned_size) *returned_size = required_tx_cnt;
+    return 0;
+}
+
+int raw_serial::waitforrecv(_u32 timeout, size_t * returned_size)
+{
+    if (!isOpened() ) return -1;
+   
+    if (returned_size) *returned_size = required_rx_cnt;
+    return 0;
+}
+
+int raw_serial::waitfordata(size_t data_count, _u32 timeout, size_t * returned_size)
+{
+    size_t length = 0;
+    if (returned_size==NULL) returned_size=(size_t *)&length;
+    *returned_size = 0;
+
+    int max_fd;
+    fd_set input_set;
+    struct timeval timeout_val;
+
+    /* Initialize the input set */
+    FD_ZERO(&input_set);
+    FD_SET(serial_fd, &input_set);
+
+    if (_selfpipe[0] != -1)
+        FD_SET(_selfpipe[0], &input_set);
+
+    max_fd =  std::max<int>(serial_fd, _selfpipe[0]) + 1;
+
+    /* Initialize the timeout structure */
+    timeout_val.tv_sec = timeout / 1000;
+    timeout_val.tv_usec = (timeout % 1000) * 1000;
+
+    if ( isOpened() )
+    {
+        int nread;
+
+        if ( ioctl(serial_fd, FIONREAD, &nread) == -1) return ANS_DEV_ERR;
+
+        *returned_size = nread;
+
+        if (*returned_size >= data_count)
+        {
+            return 0;
+        }
+    }
+
+    while ( isOpened() )
+    {
+        /* Do the select */
+        int n = ::select(max_fd, &input_set, NULL, NULL, &timeout_val);
+
+        if (n < 0)
+        {
+            // select error
+            *returned_size =  0;
+            return ANS_DEV_ERR;
+        }
+        else if (n == 0)
+        {
+            // time out
+            *returned_size =0;
+            return ANS_TIMEOUT;
+        }
+        else
+        {
+            if (FD_ISSET(_selfpipe[0], &input_set)) {   
+                // require aborting the current operation
+                int ch;
+                for (;;) {                    
+                    if (::read(_selfpipe[0], &ch, 1) == -1) {
+                        break;
+                    }
+                    
+                }
+
+                // treat as  timeout
+                *returned_size = 0;
+                return ANS_TIMEOUT;
+            }
+
+            // data avaliable
+            assert (FD_ISSET(serial_fd, &input_set));
+
+
+            if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) return ANS_DEV_ERR;
+            if (*returned_size >= data_count)
+            {
+                return 0;
+            }
+        }
+        
+    }
+
+    *returned_size=0;
+    return ANS_DEV_ERR;
+}
+
+size_t raw_serial::rxqueue_count()
+{
+    if  ( !isOpened() ) return 0;
+    size_t remaining;
+    
+    if (::ioctl(serial_fd, FIONREAD, &remaining) == -1) return 0;
+    return remaining;
+}
+
+void raw_serial::setDTR()
+{
+    if ( !isOpened() ) return;
+
+    uint32_t dtr_bit = TIOCM_DTR;
+    ioctl(serial_fd, TIOCMBIS, &dtr_bit);
+}
+
+void raw_serial::clearDTR()
+{
+    if ( !isOpened() ) return;
+
+    uint32_t dtr_bit = TIOCM_DTR;
+    ioctl(serial_fd, TIOCMBIC, &dtr_bit);
+}
+
+void raw_serial::_init()
+{
+    serial_fd = -1;  
+    _portName[0] = 0;
+    required_tx_cnt = required_rx_cnt = 0;
+    _operation_aborted = false;
+    _selfpipe[0] = _selfpipe[1] = -1;
+}
+
+void raw_serial::cancelOperation()
+{
+    _operation_aborted = true;
+    if (_selfpipe[1] == -1) return;
+
+    (int)::write(_selfpipe[1], "x", 1);
+}
+
+_u32 raw_serial::getTermBaudBitmap(_u32 baud)
+{
+#define BAUD_CONV( _baud_) case _baud_:  return B##_baud_ 
+switch (baud) {
+        BAUD_CONV(1200);
+        BAUD_CONV(1800);
+        BAUD_CONV(2400);
+        BAUD_CONV(4800);
+        BAUD_CONV(9600);
+        BAUD_CONV(19200);
+        BAUD_CONV(38400);
+        BAUD_CONV(57600);
+        BAUD_CONV(115200);
+        BAUD_CONV(230400);
+        BAUD_CONV(460800);
+        BAUD_CONV(500000);
+        BAUD_CONV(576000);
+        BAUD_CONV(921600);
+        BAUD_CONV(1000000);
+        BAUD_CONV(1152000);
+        BAUD_CONV(1500000);
+        BAUD_CONV(2000000);
+        BAUD_CONV(2500000);
+        BAUD_CONV(3000000);
+        BAUD_CONV(3500000);
+        BAUD_CONV(4000000);
+    }
+    return -1;
+}
+
+}}} //end rp::arch::net
+
+//begin rp::hal
+namespace rp{ namespace hal{
+
+serial_rxtx * serial_rxtx::CreateRxTx()
+{
+    return new rp::arch::net::raw_serial();
+}
+
+void serial_rxtx::ReleaseRxTx(serial_rxtx *rxtx)
+{
+    delete rxtx;
+}
+
+}} //end rp::hal

third_party/rplidar_sdk/src/arch/linux/net_serial.h

@@ -0,0 +1,90 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+
+#include "hal/abs_rxtx.h"
+
+namespace rp{ namespace arch{ namespace net{
+
+class raw_serial : public rp::hal::serial_rxtx
+{
+public:
+    enum{
+        SERIAL_RX_BUFFER_SIZE = 512,
+        SERIAL_TX_BUFFER_SIZE = 128,
+    };
+
+    raw_serial();
+    virtual ~raw_serial();
+    virtual bool bind(const char * portname, uint32_t baudrate, uint32_t flags = 0);
+    virtual bool open();
+    virtual void close();
+    virtual void flush( _u32 flags);
+    
+    virtual int waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL);
+
+    virtual int senddata(const unsigned char * data, size_t size);
+    virtual int recvdata(unsigned char * data, size_t size);
+
+    virtual int waitforsent(_u32 timeout = -1, size_t * returned_size = NULL);
+    virtual int waitforrecv(_u32 timeout = -1, size_t * returned_size = NULL);
+
+    virtual size_t rxqueue_count();
+
+    virtual void setDTR();
+    virtual void clearDTR();
+
+    _u32 getTermBaudBitmap(_u32 baud);
+
+    virtual void cancelOperation();
+
+protected:
+    bool open(const char * portname, uint32_t baudrate, uint32_t flags = 0);
+    void _init();
+
+    char _portName[200];
+    uint32_t _baudrate;
+    uint32_t _flags;
+
+    int serial_fd;
+
+    size_t required_tx_cnt;
+    size_t required_rx_cnt;
+
+    int    _selfpipe[2];
+    bool   _operation_aborted;
+};
+
+}}}

third_party/rplidar_sdk/src/arch/linux/net_socket.cpp

@@ -0,0 +1,893 @@
+/*
+ *  RoboPeak Project
+ *  HAL Layer - Socket Interface
+ *  Copyright 2009 - 2013 RoboPeak Project
+ *
+ *  POXIS Implementation
+ */
+
+
+#include "sdkcommon.h"
+#include "../../hal/socket.h"
+
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <netinet/tcp.h>
+#include <arpa/inet.h>
+#include <netdb.h>
+
+#include <net/if.h>
+#include <sys/ioctl.h>
+#include <linux/can.h>
+#include <linux/can/raw.h>
+
+
+
+namespace rp{ namespace net {
+
+
+static inline int _halAddrTypeToOSType(SocketAddress::address_type_t type) 
+{
+    switch (type) {
+        case SocketAddress::ADDRESS_TYPE_INET:
+            return AF_INET;
+        case SocketAddress::ADDRESS_TYPE_INET6:
+            return AF_INET6;
+        case SocketAddress::ADDRESS_TYPE_UNSPEC:
+            return AF_UNSPEC;
+
+        default:
+            assert(!"should not reach here");
+            return AF_UNSPEC;
+    }
+}
+
+
+SocketAddress::SocketAddress() 
+{
+    _platform_data = reinterpret_cast<void *>(new sockaddr_storage);
+    memset(_platform_data, 0, sizeof(sockaddr_storage));
+    
+    reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
+}
+
+SocketAddress::SocketAddress(const SocketAddress & src)
+{
+    _platform_data = reinterpret_cast<void *>(new sockaddr_storage);
+    memcpy(_platform_data, src._platform_data, sizeof(sockaddr_storage));
+}
+
+
+
+SocketAddress::SocketAddress(const char * addrString, int port, SocketAddress::address_type_t type)
+{
+    _platform_data = reinterpret_cast<void *>(new sockaddr_storage);
+    memset(_platform_data, 0, sizeof(sockaddr_storage));
+    
+    // default to ipv4 in case the following operation fails
+    reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
+
+    setAddressFromString(addrString, type);
+    setPort(port);
+}
+
+SocketAddress::SocketAddress(void * platform_data)
+ : _platform_data(platform_data)
+{}
+
+SocketAddress & SocketAddress::operator = (const SocketAddress &src)
+{
+    memcpy(_platform_data, src._platform_data, sizeof(sockaddr_storage));
+    return *this;
+}
+
+
+SocketAddress::~SocketAddress()
+{
+    delete reinterpret_cast<sockaddr_storage *>(_platform_data);
+}
+
+SocketAddress::address_type_t SocketAddress::getAddressType() const
+{
+    switch(reinterpret_cast<const sockaddr_storage *>(_platform_data)->ss_family) {
+        case AF_INET:
+            return ADDRESS_TYPE_INET;
+        case AF_INET6:
+            return ADDRESS_TYPE_INET6;
+        default:
+            assert(!"should not reach here");
+            return ADDRESS_TYPE_INET;
+    }
+}
+
+int SocketAddress::getPort() const
+{
+    switch (getAddressType()) {
+        case ADDRESS_TYPE_INET:
+            return (int)ntohs(reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_port);
+        case ADDRESS_TYPE_INET6:
+            return (int)ntohs(reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_port);
+        default:
+            return 0;
+    }
+}
+
+u_result SocketAddress::setPort(int port)
+{
+    switch (getAddressType()) {
+        case ADDRESS_TYPE_INET:
+            reinterpret_cast<sockaddr_in *>(_platform_data)->sin_port = htons((short)port);
+            break;
+        case ADDRESS_TYPE_INET6:
+            reinterpret_cast<sockaddr_in6 *>(_platform_data)->sin6_port = htons((short)port);
+            break;
+        default:
+            return RESULT_OPERATION_FAIL;
+    }
+    return RESULT_OK;
+}
+
+u_result SocketAddress::setAddressFromString(const char * address_string,  SocketAddress::address_type_t type)
+{
+    int ans = 0;
+    int prevPort = getPort();
+    switch (type) {
+        case ADDRESS_TYPE_INET:
+            reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
+            ans = inet_pton(AF_INET, 
+                            address_string, 
+                            &reinterpret_cast<sockaddr_in *>(_platform_data)->sin_addr);
+        break;
+
+
+        case ADDRESS_TYPE_INET6:
+            
+            reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET6;
+            ans = inet_pton(AF_INET6, 
+                            address_string, 
+                            &reinterpret_cast<sockaddr_in6  *>(_platform_data)->sin6_addr);
+        break;
+
+        default:
+            return RESULT_INVALID_DATA;
+
+    }
+    setPort(prevPort);
+
+    return ans<=0?RESULT_INVALID_DATA:RESULT_OK;
+}
+
+
+u_result SocketAddress::getAddressAsString(char * buffer, size_t buffersize) const
+{
+    int net_family = reinterpret_cast<const sockaddr_storage *>(_platform_data)->ss_family;
+    const char *ans = NULL;
+    switch (net_family) {
+        case AF_INET:
+            ans = inet_ntop(net_family, &reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr,
+                            buffer, buffersize);
+        break;
+
+        case AF_INET6:
+            ans = inet_ntop(net_family, &reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr,
+                            buffer, buffersize);
+
+        break;
+    }
+    return ans==NULL?RESULT_OPERATION_FAIL:RESULT_OK;
+}
+
+
+
+size_t SocketAddress::LoopUpHostName(const char * hostname, const char * sevicename, std::vector<SocketAddress> &addresspool , bool performDNS, SocketAddress::address_type_t type)
+{
+    struct addrinfo hints;
+    struct addrinfo *result;
+    int ans;
+
+    memset(&hints, 0, sizeof(struct addrinfo));
+    hints.ai_family = _halAddrTypeToOSType(type);
+    hints.ai_flags = AI_PASSIVE;
+
+    if (!performDNS) {
+        hints.ai_family |= AI_NUMERICSERV | AI_NUMERICHOST;
+    
+    }
+
+    ans = getaddrinfo(hostname, sevicename, &hints, &result);
+
+    addresspool.clear();
+
+    if (ans != 0) {
+        // hostname loopup failed
+        return 0;
+    }
+
+    
+    for (struct addrinfo * cursor = result; cursor != NULL; cursor = cursor->ai_next) {
+        if (cursor->ai_family == ADDRESS_TYPE_INET || cursor->ai_family == ADDRESS_TYPE_INET6) {
+            sockaddr_storage * storagebuffer = new sockaddr_storage;
+            assert(sizeof(sockaddr_storage) >= cursor->ai_addrlen);
+            memcpy(storagebuffer, cursor->ai_addr, cursor->ai_addrlen);
+            addresspool.push_back(SocketAddress(storagebuffer));
+        }
+    }
+
+    
+    freeaddrinfo(result);
+
+    return addresspool.size();
+}
+
+
+u_result SocketAddress::getRawAddress(_u8 * buffer, size_t bufferSize) const
+{
+     switch (getAddressType()) {
+        case ADDRESS_TYPE_INET:
+            if (bufferSize < sizeof(in_addr::s_addr)) return RESULT_INSUFFICIENT_MEMORY;
+
+            memcpy(buffer, &reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr.s_addr, sizeof(reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr.s_addr));
+
+            
+            break;
+        case ADDRESS_TYPE_INET6:
+            if (bufferSize < sizeof(in6_addr::s6_addr)) return RESULT_INSUFFICIENT_MEMORY;
+            memcpy(buffer, reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr.s6_addr, sizeof(reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr.s6_addr));
+
+            break;
+        default:
+            return RESULT_OPERATION_FAIL;
+    }
+    return RESULT_OK;
+}
+
+
+void SocketAddress::setLoopbackAddress(SocketAddress::address_type_t type)
+{
+
+    int prevPort = getPort();
+    switch (type) {
+        case ADDRESS_TYPE_INET:
+            {
+                sockaddr_in * addrv4 = reinterpret_cast<sockaddr_in *>(_platform_data);
+                addrv4->sin_family = AF_INET;
+                addrv4->sin_addr.s_addr = htonl(INADDR_LOOPBACK);
+            }
+            break;
+        case ADDRESS_TYPE_INET6:
+            {
+                sockaddr_in6  * addrv6 = reinterpret_cast<sockaddr_in6  *>(_platform_data);
+                addrv6->sin6_family = AF_INET6;
+                addrv6->sin6_addr = in6addr_loopback;
+
+            }
+            break;
+        default:
+            return;
+    }
+
+    setPort(prevPort);
+}
+
+void SocketAddress::setBroadcastAddressIPv4()
+{
+
+    int prevPort = getPort();
+    sockaddr_in * addrv4 = reinterpret_cast<sockaddr_in *>(_platform_data);
+    addrv4->sin_family = AF_INET;
+    addrv4->sin_addr.s_addr = htonl(INADDR_BROADCAST);
+    setPort(prevPort);
+
+}
+
+void SocketAddress::setAnyAddress(SocketAddress::address_type_t type)
+{
+    int prevPort = getPort();
+    switch (type) {
+        case ADDRESS_TYPE_INET:
+            {
+                sockaddr_in * addrv4 = reinterpret_cast<sockaddr_in *>(_platform_data);
+                addrv4->sin_family = AF_INET;
+                addrv4->sin_addr.s_addr = htonl(INADDR_ANY);
+            }
+            break;
+        case ADDRESS_TYPE_INET6:
+            {
+                sockaddr_in6  * addrv6 = reinterpret_cast<sockaddr_in6  *>(_platform_data);
+                addrv6->sin6_family = AF_INET6;
+                addrv6->sin6_addr = in6addr_any;
+
+            }
+            break;
+        default:
+            return;
+    }
+
+    setPort(prevPort);
+
+
+}
+
+
+}}
+
+
+
+///--------------------------------
+
+
+namespace rp { namespace arch { namespace net{ 
+
+using namespace rp::net;
+
+class _single_thread StreamSocketImpl : public StreamSocket
+{
+public:
+
+    StreamSocketImpl(int fd)
+        : _socket_fd(fd)
+    {
+        assert(fd>=0);
+        int bool_true = 1;
+        ::setsockopt( _socket_fd, SOL_SOCKET, SO_REUSEADDR , (char *)&bool_true, sizeof(bool_true) );
+
+        enableNoDelay(true);
+        this->setTimeout(DEFAULT_SOCKET_TIMEOUT, SOCKET_DIR_BOTH);
+    }
+
+    virtual ~StreamSocketImpl() 
+    {
+        close(_socket_fd);
+    }
+
+    virtual void dispose()
+    {
+        delete this;
+    }
+
+
+    virtual u_result bind(const SocketAddress & localaddr)
+    {
+        const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(localaddr.getPlatformData());
+        assert(addr);
+        int ans = ::bind(_socket_fd, addr, sizeof(sockaddr_storage));
+        if (ans) {
+            return RESULT_OPERATION_FAIL;
+        } else {
+            return RESULT_OK;
+        }
+    }
+
+    virtual u_result getLocalAddress(SocketAddress & localaddr)
+    {
+        struct sockaddr * addr = reinterpret_cast<struct sockaddr *>( const_cast<void *>(localaddr.getPlatformData())); //donnot do this at home...
+        assert(addr);
+
+        size_t actualsize =  sizeof(sockaddr_storage);
+        int ans = ::getsockname(_socket_fd, addr, (socklen_t*)&actualsize);
+
+        assert(actualsize <= sizeof(sockaddr_storage));
+        assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6);
+
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk)
+    {
+        int ans;
+        timeval tv;
+        tv.tv_sec = timeout / 1000; 
+        tv.tv_usec = (timeout % 1000) * 1000; 
+
+        if (msk & SOCKET_DIR_RD) {
+             ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv) );
+             if (ans) return RESULT_OPERATION_FAIL;
+        }
+
+        if (msk & SOCKET_DIR_WR) {
+             ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv) );
+             if (ans) return RESULT_OPERATION_FAIL;
+        }
+
+        return RESULT_OK;
+    }
+  
+    virtual u_result connect(const SocketAddress & pairAddress)
+    {
+        const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(pairAddress.getPlatformData());
+        int ans = ::connect(_socket_fd, addr, sizeof(sockaddr_storage));
+        if (!ans) return RESULT_OK;
+
+
+        switch (errno) {
+            case EAFNOSUPPORT:
+                return RESULT_OPERATION_NOT_SUPPORT;
+#if 0
+            case EINPROGRESS:
+                return RESULT_OK; //treat async connection as good status
+#endif
+            case ETIMEDOUT:
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                return RESULT_OPERATION_FAIL;
+        }
+    }
+      
+    virtual u_result listen(int backlog)
+    {
+        int ans = ::listen( _socket_fd,   backlog);
+
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual StreamSocket * accept(SocketAddress * pairAddress) 
+    {
+        size_t addrsize;
+        addrsize = sizeof(sockaddr_storage);
+        int pair_socket = ::accept( _socket_fd, pairAddress?reinterpret_cast<struct sockaddr *>(const_cast<void *>(pairAddress->getPlatformData())):NULL
+            , (socklen_t*)&addrsize);
+
+        if (pair_socket>=0) {
+            return new StreamSocketImpl(pair_socket);
+        } else {
+            return NULL;
+        }
+    }
+
+    virtual u_result waitforIncomingConnection(_u32 timeout)
+    {
+        return waitforData(timeout);
+    }
+
+    virtual u_result send(const void * buffer, size_t len) 
+    {
+        size_t ans = ::send( _socket_fd, buffer, len, MSG_NOSIGNAL);
+        if (ans == len) {
+            return RESULT_OK;
+        } else {
+            switch (errno) {
+                case EAGAIN:
+#if EWOULDBLOCK!=EAGAIN
+                case EWOULDBLOCK:
+#endif
+                    return RESULT_OPERATION_TIMEOUT;
+                default:
+                    return RESULT_OPERATION_FAIL;
+            }
+        }
+        
+    }
+
+
+    virtual u_result recv(void *buf, size_t len, size_t & recv_len)
+    {
+        size_t ans = ::recv( _socket_fd, buf, len, 0);
+        if (ans == (size_t)-1) {
+            recv_len = 0;  
+
+            switch (errno) {
+                case EAGAIN:
+#if EWOULDBLOCK!=EAGAIN
+                case EWOULDBLOCK:
+#endif
+                    return RESULT_OPERATION_TIMEOUT;
+                default:
+                    return RESULT_OPERATION_FAIL;
+            }
+
+            
+
+        } else {
+            recv_len = ans;
+            return RESULT_OK;
+        }
+    }
+
+#if 0
+    virtual u_result recvNoWait(void *buf, size_t len, size_t & recv_len)
+    {
+        size_t ans = ::recv( _socket_fd, buf, len, MSG_DONTWAIT);
+        if (ans == (size_t)-1) {
+            recv_len = 0;  
+            if (errno == EAGAIN || errno == EWOULDBLOCK) {
+                return RESULT_OK; 
+            } else {
+                return RESULT_OPERATION_FAIL;
+            }
+
+
+        } else {
+            recv_len = ans;
+            return RESULT_OK;
+        }
+
+    }
+#endif
+
+    virtual u_result getPeerAddress(SocketAddress & peerAddr)
+    {
+        struct sockaddr * addr = reinterpret_cast<struct sockaddr *>(const_cast<void *>(peerAddr.getPlatformData())); //donnot do this at home...
+        assert(addr);
+        size_t actualsize =  sizeof(sockaddr_storage);
+        int ans = ::getpeername(_socket_fd, addr, (socklen_t*)&actualsize);
+
+        assert(actualsize <=  sizeof(sockaddr_storage));
+        assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6);
+
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+
+    }
+
+    virtual u_result shutdown(socket_direction_mask mask)
+    {
+        int shutdw_opt ;
+
+        switch (mask) {
+            case SOCKET_DIR_RD:
+                shutdw_opt = SHUT_RD;
+                break;
+            case SOCKET_DIR_WR:
+                shutdw_opt = SHUT_WR;
+                break;
+            case SOCKET_DIR_BOTH:
+            default:
+                shutdw_opt = SHUT_RDWR;
+        }
+
+        int ans = ::shutdown(_socket_fd, shutdw_opt);
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result enableKeepAlive(bool enable)
+    {
+        int bool_true = enable?1:0;
+        return ::setsockopt( _socket_fd, SOL_SOCKET, SO_KEEPALIVE , &bool_true, sizeof(bool_true) )?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result enableNoDelay(bool enable ) 
+    {
+        int bool_true = enable?1:0;
+        return ::setsockopt( _socket_fd, IPPROTO_TCP, TCP_NODELAY,&bool_true, sizeof(bool_true) )?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result waitforSent(_u32 timeout ) 
+    {
+        fd_set wrset;
+        FD_ZERO(&wrset);
+        FD_SET(_socket_fd, &wrset);
+
+        timeval tv;
+        tv.tv_sec = timeout / 1000;
+        tv.tv_usec = (timeout % 1000) * 1000;
+        int ans = ::select(_socket_fd+1, NULL, &wrset, NULL, &tv);
+
+        switch (ans) {
+            case 1:
+                // fired
+                return RESULT_OK;
+            case 0:
+                // timeout
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                delay(0); //relax cpu
+                return RESULT_OPERATION_FAIL;
+        }
+    }
+
+    virtual u_result waitforData(_u32 timeout )
+    {
+        fd_set rdset;
+        FD_ZERO(&rdset);
+        FD_SET(_socket_fd, &rdset);
+
+        timeval tv;
+        tv.tv_sec = timeout / 1000;
+        tv.tv_usec = (timeout % 1000) * 1000;
+        int ans = ::select(_socket_fd+1, &rdset, NULL, NULL, &tv);
+
+        switch (ans) {
+            case 1:
+                // fired
+                return RESULT_OK;
+            case 0:
+                // timeout
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                delay(0); //relax cpu
+                return RESULT_OPERATION_FAIL;
+        }
+    }
+
+protected:
+    int  _socket_fd;
+
+
+};
+
+
+class _single_thread DGramSocketImpl : public DGramSocket
+{
+public:
+
+    DGramSocketImpl(int fd)
+        : _socket_fd(fd)
+    {
+        assert(fd>=0);
+        int bool_true = 1;
+        ::setsockopt( _socket_fd, SOL_SOCKET, SO_REUSEADDR | SO_BROADCAST , (char *)&bool_true, sizeof(bool_true) );
+        setTimeout(DEFAULT_SOCKET_TIMEOUT, SOCKET_DIR_BOTH);
+    }
+
+    virtual ~DGramSocketImpl() 
+    {
+        close(_socket_fd);
+    }
+
+    virtual void dispose()
+    {
+        delete this;
+    }
+
+
+    virtual u_result bind(const SocketAddress & localaddr)
+    {
+        const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(localaddr.getPlatformData());
+        assert(addr);
+        int ans = ::bind(_socket_fd, addr, sizeof(sockaddr_storage));
+        if (ans) {
+            return RESULT_OPERATION_FAIL;
+        } else {
+            return RESULT_OK;
+        }
+    }
+
+    virtual u_result getLocalAddress(SocketAddress & localaddr)
+    {
+        struct sockaddr * addr = reinterpret_cast<struct sockaddr *>(const_cast<void *>((localaddr.getPlatformData()))); //donnot do this at home...
+        assert(addr);
+
+        size_t actualsize =  sizeof(sockaddr_storage);
+        int ans = ::getsockname(_socket_fd, addr, (socklen_t*)&actualsize);
+
+        assert(actualsize <= sizeof(sockaddr_storage));
+        assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6);
+
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk)
+    {
+        int ans;
+        timeval tv;
+        tv.tv_sec = timeout / 1000; 
+        tv.tv_usec = (timeout % 1000) * 1000; 
+
+        if (msk & SOCKET_DIR_RD) {
+             ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv) );
+             if (ans) return RESULT_OPERATION_FAIL;
+        }
+
+        if (msk & SOCKET_DIR_WR) {
+             ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv) );
+             if (ans) return RESULT_OPERATION_FAIL;
+        }
+
+        return RESULT_OK;
+    }
+  
+
+    virtual u_result waitforSent(_u32 timeout ) 
+    {
+        fd_set wrset;
+        FD_ZERO(&wrset);
+        FD_SET(_socket_fd, &wrset);
+
+        timeval tv;
+        tv.tv_sec = timeout / 1000;
+        tv.tv_usec = (timeout % 1000) * 1000;
+        int ans = ::select(_socket_fd+1, NULL, &wrset, NULL, &tv);
+
+        switch (ans) {
+            case 1:
+                // fired
+                return RESULT_OK;
+            case 0:
+                // timeout
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                delay(0); //relax cpu
+                return RESULT_OPERATION_FAIL;
+        }
+    }
+
+    virtual u_result waitforData(_u32 timeout )
+    {
+        fd_set rdset;
+        FD_ZERO(&rdset);
+        FD_SET(_socket_fd, &rdset);
+
+        timeval tv;
+        tv.tv_sec = timeout / 1000;
+        tv.tv_usec = (timeout % 1000) * 1000;
+        int ans = ::select(_socket_fd+1, &rdset, NULL, NULL, &tv);
+
+        switch (ans) {
+            case 1:
+                // fired
+                return RESULT_OK;
+            case 0:
+                // timeout
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                delay(0); //relax cpu
+                return RESULT_OPERATION_FAIL;
+        }
+    }
+
+    virtual u_result sendTo(const SocketAddress * target, const void * buffer, size_t len)
+    {
+        const struct sockaddr * addr = target ? reinterpret_cast<const struct sockaddr *>(target->getPlatformData()) : NULL;
+        int dest_addr_size = (target ? sizeof(sockaddr_storage) : 0);
+        int ans = ::sendto(_socket_fd, (const char *)buffer, (int)len, 0, addr, dest_addr_size);
+        if (ans != -1) {
+            assert(ans == len);
+            return RESULT_OK;
+        } else {
+            switch (errno) {
+                case EAGAIN:
+#if EWOULDBLOCK!=EAGAIN
+                case EWOULDBLOCK:
+#endif
+                    return RESULT_OPERATION_TIMEOUT;
+
+                case EMSGSIZE:
+                    return RESULT_INVALID_DATA;
+                default:
+                    return RESULT_OPERATION_FAIL;
+            }
+        
+        }
+
+    }
+
+    virtual u_result setPairAddress(const SocketAddress* pairAddress)
+    {
+        sockaddr_storage unspecAddr;
+        unspecAddr.ss_family = AF_UNSPEC;
+
+        const struct sockaddr* addr = pairAddress ? reinterpret_cast<const struct sockaddr*>(pairAddress->getPlatformData()) : reinterpret_cast<const struct sockaddr*>(&unspecAddr);
+        int ans = ::connect(_socket_fd, addr, (int)sizeof(sockaddr_storage));
+        return ans ? RESULT_OPERATION_FAIL : RESULT_OK;
+
+    }
+    
+    virtual u_result clearRxCache()
+    {
+        timeval tv;
+        tv.tv_sec = 0;
+        tv.tv_usec = 0;
+        fd_set rdset;
+        FD_ZERO(&rdset);
+        FD_SET(_socket_fd, &rdset);
+
+        int res = -1;
+        char recv_data[2];
+        memset(recv_data, 0, sizeof(recv_data));
+        while (true) {
+            res = select(FD_SETSIZE, &rdset, nullptr, nullptr, &tv);
+            if (res == 0) break;
+            recv(_socket_fd, recv_data, 1, 0);
+        }
+        return RESULT_OK;
+    }
+
+    virtual u_result recvFrom(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr)
+    {
+        struct sockaddr * addr = (sourceAddr?reinterpret_cast<struct sockaddr *>(const_cast<void *>(sourceAddr->getPlatformData())):NULL);
+        size_t source_addr_size = (sourceAddr?sizeof(sockaddr_storage):0);
+
+        size_t ans = ::recvfrom( _socket_fd, buf, len, 0, addr, (socklen_t*)&source_addr_size);
+        if (ans == (size_t)-1) {
+            recv_len = 0;  
+            switch (errno) {
+                case EAGAIN:
+#if EWOULDBLOCK!=EAGAIN
+                case EWOULDBLOCK:
+#endif
+                    return RESULT_OPERATION_TIMEOUT;
+                default:
+                    return RESULT_OPERATION_FAIL;
+            }
+
+        } else {
+            recv_len = ans;
+            return RESULT_OK;
+        }
+
+    }
+
+#if 0
+    virtual u_result recvFromNoWait(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr)
+    {
+        struct sockaddr * addr = (sourceAddr?reinterpret_cast<struct sockaddr *>(const_cast<void *>(sourceAddr->getPlatformData())):NULL);
+        size_t source_addr_size = (sourceAddr?sizeof(sockaddr_storage):0);
+
+
+        size_t ans = ::recvfrom( _socket_fd, buf, len, MSG_DONTWAIT, addr, &source_addr_size);
+
+        if (ans == (size_t)-1) {
+            recv_len = 0;  
+            if (errno == EAGAIN || errno == EWOULDBLOCK) {
+                return RESULT_OK; 
+            } else {
+                return RESULT_OPERATION_FAIL;
+            }
+
+
+        } else {
+            recv_len = ans;
+            return RESULT_OK;
+        }
+
+    }
+#endif
+    
+protected:
+    int  _socket_fd;
+
+};
+
+
+}}}
+
+
+namespace rp { namespace net{ 
+
+
+static inline int _socketHalFamilyToOSFamily(SocketBase::socket_family_t family)
+{
+    switch (family) {
+        case SocketBase::SOCKET_FAMILY_INET:
+            return AF_INET;
+        case SocketBase::SOCKET_FAMILY_INET6:
+            return AF_INET6;
+        case SocketBase::SOCKET_FAMILY_RAW:
+            return AF_PACKET;
+        default:
+            assert(!"should not reach here");
+            return AF_INET; // force treating as IPv4 in release mode
+    }
+
+}
+
+StreamSocket * StreamSocket::CreateSocket(SocketBase::socket_family_t family)
+{
+    if (family == SOCKET_FAMILY_RAW) return NULL;
+
+
+    int socket_family = _socketHalFamilyToOSFamily(family);
+    int socket_fd = ::socket(socket_family, SOCK_STREAM, 0);
+    if (socket_fd == -1) return NULL;
+
+    StreamSocket * newborn = static_cast<StreamSocket *>(new rp::arch::net::StreamSocketImpl(socket_fd));
+    return newborn;
+
+}
+
+
+DGramSocket * DGramSocket::CreateSocket(SocketBase::socket_family_t family)
+{
+    int socket_family = _socketHalFamilyToOSFamily(family);
+
+
+    int socket_fd = ::socket(socket_family, (family==SOCKET_FAMILY_RAW)?SOCK_RAW:SOCK_DGRAM, 0);
+    if (socket_fd == -1) return NULL;
+
+    DGramSocket * newborn = static_cast<DGramSocket *>(new rp::arch::net::DGramSocketImpl(socket_fd));
+    return newborn;
+
+}
+
+
+}}
+

third_party/rplidar_sdk/src/arch/linux/thread.hpp

@@ -0,0 +1,185 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "arch/linux/arch_linux.h"
+
+#include <sched.h>
+#include <sys/types.h>
+#include <sys/syscall.h>
+#include <sys/time.h>
+#include <sys/resource.h>
+
+namespace rp{ namespace hal{
+
+Thread Thread::create(thread_proc_t proc, void * data)
+{
+    Thread newborn(proc, data);
+    
+    // tricky code, we assume pthread_t is not a structure but a word size value
+    assert( sizeof(newborn._handle) >= sizeof(pthread_t));
+
+    pthread_create((pthread_t *)&newborn._handle, NULL, (void * (*)(void *))proc, data);
+
+    return newborn;
+}
+
+u_result Thread::terminate()
+{
+    if (!this->_handle) return RESULT_OK;
+    
+    return pthread_cancel((pthread_t)this->_handle)==0?RESULT_OK:RESULT_OPERATION_FAIL;
+}
+
+u_result Thread::SetSelfPriority( priority_val_t p)
+{
+
+    pid_t selfTid = syscall(SYS_gettid);
+
+        // check whether current schedule policy supports priority levels
+    int current_policy = SCHED_OTHER;
+    struct sched_param current_param;
+    int nice = 0;
+    int ans;
+
+    if (sched_getparam(selfTid, &current_param))
+    {
+        // cannot retreieve values
+        return RESULT_OPERATION_FAIL;
+    }   
+
+    int pthread_priority_min;
+
+#if 1
+    pthread_priority_min = sched_get_priority_min(SCHED_RR);
+#else
+    pthread_priority_min = 1;
+#endif
+	int pthread_priority = 0 ;
+
+	switch(p)
+	{
+	case PRIORITY_REALTIME:
+		//pthread_priority = pthread_priority_max;
+        current_policy = SCHED_RR;
+        pthread_priority = pthread_priority_min + 1;
+        nice = 0;
+		break;
+	case PRIORITY_HIGH:
+		//pthread_priority = (pthread_priority_max + pthread_priority_min)/2;
+        current_policy = SCHED_RR;
+        pthread_priority = pthread_priority_min;
+        nice = 0;
+		break;
+	case PRIORITY_NORMAL:
+        pthread_priority = 0;
+        current_policy = SCHED_OTHER;
+        nice = 0;
+        break;
+	case PRIORITY_LOW:
+        pthread_priority = 0;
+        current_policy = SCHED_OTHER;
+        nice = 10;
+        break;
+	case PRIORITY_IDLE:
+		pthread_priority = 0;
+        current_policy = SCHED_IDLE;
+        nice = 0;
+		break;
+	}
+    // change the inhertiable behavior
+    current_policy |= SCHED_RESET_ON_FORK;
+
+    current_param.__sched_priority = pthread_priority;
+
+  
+
+    
+    // do not use pthread version as it will make the priority be inherited by a thread child
+	if ( (ans = sched_setscheduler(selfTid, current_policy , &current_param)) )
+	{
+        if (ans == EPERM)
+        {
+            //DBG_PRINT("warning, current process hasn't the right permission to set threads priority\n");
+        }
+		return RESULT_OPERATION_FAIL;
+	}
+
+
+    if ((current_policy == SCHED_OTHER) || (current_policy == SCHED_BATCH))
+    {
+        if (setpriority(PRIO_PROCESS, selfTid, nice)) {
+            return RESULT_OPERATION_FAIL;
+        }
+    }
+    
+
+	return  RESULT_OK;
+}
+
+Thread::priority_val_t Thread::getPriority()
+{
+	if (!this->_handle) return PRIORITY_NORMAL;
+
+    int current_policy;
+    struct sched_param current_param;
+    if (pthread_getschedparam( (pthread_t) this->_handle, &current_policy, &current_param))
+    {
+        // cannot retreieve values
+        return PRIORITY_NORMAL;
+    }   
+
+    int pthread_priority_max = sched_get_priority_max(SCHED_RR);
+    int pthread_priority_min = sched_get_priority_min(SCHED_RR);
+
+    if (current_param.__sched_priority ==(pthread_priority_max ))
+	{
+		return PRIORITY_REALTIME;
+	}
+	if (current_param.__sched_priority >=(pthread_priority_max + pthread_priority_min)/2)
+	{
+		return PRIORITY_HIGH;
+	}
+	return PRIORITY_NORMAL;
+}
+
+u_result Thread::join(unsigned long timeout)
+{
+    if (!this->_handle) return RESULT_OK;
+    
+    pthread_join((pthread_t)(this->_handle), NULL);
+    this->_handle = 0;
+    return RESULT_OK;
+}
+
+}}

third_party/rplidar_sdk/src/arch/linux/timer.cpp

@@ -0,0 +1,52 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "arch/linux/arch_linux.h"
+
+namespace rp{ namespace arch{
+_u64 rp_getus()
+{
+    struct timespec t;
+    t.tv_sec = t.tv_nsec = 0;
+    clock_gettime(CLOCK_MONOTONIC, &t);
+    return t.tv_sec*1000000LL + t.tv_nsec/1000;
+}
+_u64 rp_getms()
+{
+    struct timespec t;
+    t.tv_sec = t.tv_nsec = 0;
+    clock_gettime(CLOCK_MONOTONIC, &t);
+    return t.tv_sec*1000L + t.tv_nsec/1000000L;
+}
+}}

third_party/rplidar_sdk/src/arch/linux/timer.h

@@ -0,0 +1,59 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+
+#include "hal/types.h"
+
+#include <unistd.h>
+static inline void delay(_word_size_t ms){
+    while (ms>=1000){
+        usleep(1000*1000);
+        ms-=1000;
+    };
+    if (ms!=0)
+        usleep(ms*1000);
+}
+
+// TODO: the highest timer interface should be clock_gettime
+namespace rp{ namespace arch{
+
+_u64 rp_getus();
+_u64 rp_getms();
+
+}}
+
+#define getms() rp::arch::rp_getms()
+#define getus() rp::arch::rp_getus()
+

third_party/rplidar_sdk/src/dataunpacker/dataunnpacker_commondef.h

@@ -0,0 +1,60 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+
+ /*
+  *  Sample Data Unpacker System
+  *  External Reference and dependencies
+  */
+
+  /*
+	* Redistribution and use in source and binary forms, with or without
+	* modification, are permitted provided that the following conditions are met:
+	*
+	* 1. Redistributions of source code must retain the above copyright notice,
+	*    this list of conditions and the following disclaimer.
+	*
+	* 2. Redistributions in binary form must reproduce the above copyright notice,
+	*    this list of conditions and the following disclaimer in the documentation
+	*    and/or other materials provided with the distribution.
+	*
+	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+	* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+	* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+	* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+	* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+	* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+	* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+	* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+	* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+	* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+	* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+	*
+	*/
+
+#pragma once
+
+#include "sdkcommon.h"
+#include "hal/abs_rxtx.h"
+#include "hal/thread.h"
+#include "hal/types.h"
+#include "hal/assert.h"
+#include "hal/locker.h"
+#include "hal/socket.h"
+#include "hal/event.h"
+#include "hal/waiter.h"
+#include "hal/byteorder.h"
+#include "sl_lidar_driver.h"
+#include "sl_crc.h" 
+#include <algorithm>
+#include <memory>
+
+#define CONF_NO_BOOST_CRC_SUPPORT
+
+#include "dataupacker_namespace.h"
+
+

third_party/rplidar_sdk/src/dataunpacker/dataunnpacker_internal.h

@@ -0,0 +1,74 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+
+ /*
+  *  Sample Data Unpacker System
+  *  Internal Definition
+  */
+
+  /*
+	* Redistribution and use in source and binary forms, with or without
+	* modification, are permitted provided that the following conditions are met:
+	*
+	* 1. Redistributions of source code must retain the above copyright notice,
+	*    this list of conditions and the following disclaimer.
+	*
+	* 2. Redistributions in binary form must reproduce the above copyright notice,
+	*    this list of conditions and the following disclaimer in the documentation
+	*    and/or other materials provided with the distribution.
+	*
+	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+	* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+	* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+	* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+	* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+	* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+	* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+	* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+	* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+	* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+	* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+	*
+	*/
+
+#pragma once
+
+BEGIN_DATAUNPACKER_NS()
+
+
+class LIDARSampleDataUnpackerInner: public LIDARSampleDataUnpacker
+{
+public:
+	LIDARSampleDataUnpackerInner(LIDARSampleDataListener& l): LIDARSampleDataUnpacker(l){}
+
+	virtual void publishHQNode(_u64 timestamp_uS, const rplidar_response_measurement_node_hq_t* node) = 0;
+	virtual void publishDecodingErrorMsg(int errorType, _u8 ansType, const void* payload, size_t size) = 0;
+	virtual void publishCustomData(_u8 ansType, _u32 customCode, const void* payload, size_t size) = 0;
+	virtual void publishNewScanReset() = 0;
+
+
+	virtual _u64 getCurrentTimestamp_uS() = 0;
+
+};
+
+class IDataUnpackerHandler
+{
+public:
+	IDataUnpackerHandler() {}
+	virtual ~IDataUnpackerHandler() {}
+
+
+	virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size) = 0;
+
+	virtual _u8 getSampleAnswerType() const = 0;
+	virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size) = 0;
+	virtual void reset() = 0;
+
+};
+
+END_DATAUNPACKER_NS()

third_party/rplidar_sdk/src/dataunpacker/dataunpacker.cpp

@@ -0,0 +1,259 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+
+ /*
+  *  Sample Data Unpacker System
+  *
+  */
+
+  /*
+	* Redistribution and use in source and binary forms, with or without
+	* modification, are permitted provided that the following conditions are met:
+	*
+	* 1. Redistributions of source code must retain the above copyright notice,
+	*    this list of conditions and the following disclaimer.
+	*
+	* 2. Redistributions in binary form must reproduce the above copyright notice,
+	*    this list of conditions and the following disclaimer in the documentation
+	*    and/or other materials provided with the distribution.
+	*
+	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+	* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+	* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+	* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+	* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+	* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+	* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+	* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+	* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+	* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+	* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+	*
+	*/
+
+#include "dataunnpacker_commondef.h"
+#include "dataunpacker.h"
+#include "dataunnpacker_internal.h"
+
+
+#include <map>
+
+
+#define REGISTER_HANDLER(_c_) {     \
+		auto newBorn = new unpacker::_c_();   \
+		if (!newBorn) return false; \
+		handlerList.push_back(newBorn); \
+	} 
+
+// How to include new handlers?
+// 1. add extra include line below if a new handle is to be included
+// 2. update the code in function _registerDataUnpackerHandlers
+#include "unpacker/handler_capsules.h"
+#include "unpacker/handler_hqnode.h"
+#include "unpacker/handler_normalnode.h"
+
+
+#define  DEF_REGISTER_HANDLER_LIST
+
+
+BEGIN_DATAUNPACKER_NS()
+
+
+static bool _registerDataUnpackerHandlers(std::vector<IDataUnpackerHandler *> & handlerList)
+{
+	REGISTER_HANDLER(UnpackerHandler_NormalNode);
+	REGISTER_HANDLER(UnpackerHandler_HQNode);
+	REGISTER_HANDLER(UnpackerHandler_CapsuleNode);
+	REGISTER_HANDLER(UnpackerHandler_UltraCapsuleNode);
+	REGISTER_HANDLER(UnpackerHandler_DenseCapsuleNode);
+	REGISTER_HANDLER(UnpackerHandler_UltraDenseCapsuleNode);
+	return true;
+}
+
+
+class LIDARSampleDataUnpackerImpl : public LIDARSampleDataUnpackerInner
+{
+public:
+
+	void registerHandler(_u8 ansType, IDataUnpackerHandler* handler)
+	{
+		_handlerMap[ansType] = handler;
+	}
+
+
+	void unregisterAllHandlers()
+	{
+		for (auto itr = _handlerMap.begin(); itr != _handlerMap.end(); ++itr)
+		{
+			delete itr->second;
+		}
+		_handlerMap.clear();
+	}
+
+	LIDARSampleDataUnpackerImpl(LIDARSampleDataListener& l)
+		: LIDARSampleDataUnpackerInner(l)
+		, _enabled(false)
+		, _lastActiveAnsType(0)
+		, _lastActiveHandler(nullptr)
+	{
+
+	}
+
+	virtual ~LIDARSampleDataUnpackerImpl()
+	{
+		unregisterAllHandlers();
+	}
+
+
+	virtual void updateUnpackerContext(UnpackerContextType type, const void* data, size_t size)
+	{
+	
+		// notify the handlers ...
+		for (auto itr = _handlerMap.begin(); itr != _handlerMap.end(); ++itr)
+		{
+			itr->second->onUnpackerContextSet(type, data, size);
+		}
+	}
+
+	virtual bool onSampleData(_u8 ansType, const void* buffer, size_t size) {
+		if (!_enabled) return false;
+
+
+		if (_lastActiveAnsType != ansType) {
+			onDeselectHandler();
+
+			auto itr = _handlerMap.find(ansType);
+			if (itr != _handlerMap.end()) {
+				onSelectHandler(ansType, itr->second);
+			}
+			else {
+				onSelectHandler(ansType, nullptr);
+			}
+			
+		}
+
+		if (_lastActiveHandler) {
+			_lastActiveHandler->onData(this, reinterpret_cast<const _u8 *>(buffer), size);
+			return true;
+		}
+		else {
+			return false;
+		}
+	}
+
+	virtual void reset()
+	{
+		clearCache();
+		_lastActiveHandler = nullptr;
+		_lastActiveAnsType = 0;
+
+	}
+
+	virtual void enable()
+	{
+		_enabled = true;
+		reset();
+	}
+
+	virtual void disable()
+	{
+		_enabled = false;
+		reset();
+
+	}
+
+	virtual void clearCache()
+	{
+		if (_lastActiveHandler) {
+			_lastActiveHandler->reset();
+		}
+	}
+
+	virtual _u64 getCurrentTimestamp_uS() {
+		return getus();
+	}
+
+	virtual void publishHQNode(_u64 timestamp_uS, const rplidar_response_measurement_node_hq_t* node)
+	{
+		_listener.onHQNodeDecoded(timestamp_uS, node);
+	}
+
+
+	virtual void publishDecodingErrorMsg(int errorType, _u8 ansType, const void* payload, size_t size)
+	{
+		_listener.onDecodingError(errorType, ansType, payload, size);
+
+	}
+
+	virtual void publishCustomData(_u8 ansType, _u32 customCode, const void* payload, size_t size)
+	{
+		_listener.onCustomSampleDataDecoded(ansType, customCode, payload, size);
+	}
+
+
+	virtual void publishNewScanReset()
+	{
+		_listener.onHQNodeScanResetReq();
+	}
+protected:
+
+	void onSelectHandler(_u8 ansType, IDataUnpackerHandler* handler)
+	{
+		_lastActiveHandler = handler;
+		_lastActiveAnsType = ansType;
+	}
+
+	void onDeselectHandler()
+	{
+		reset();
+	}
+
+
+protected:
+	bool _enabled;
+	std::map<_u8, IDataUnpackerHandler*> _handlerMap;
+
+	_u8 _lastActiveAnsType;
+	IDataUnpackerHandler* _lastActiveHandler;
+};
+
+LIDARSampleDataUnpacker* LIDARSampleDataUnpacker::CreateInstance(LIDARSampleDataListener& listener)
+{
+	LIDARSampleDataUnpackerImpl* impl = new LIDARSampleDataUnpackerImpl(listener);
+	
+	std::vector<IDataUnpackerHandler*> list;
+	if (!_registerDataUnpackerHandlers(list)) {
+		delete  impl;
+		for (auto itr = list.begin(); itr != list.end(); ++itr) {
+			delete* itr;
+		}
+		impl = nullptr;
+	}
+
+	for (auto itr = list.begin(); itr != list.end(); ++itr) {
+		impl->registerHandler((*itr)->getSampleAnswerType(), (*itr));
+	}
+	return impl;
+}
+
+void LIDARSampleDataUnpacker::ReleaseInstance(LIDARSampleDataUnpacker* unpacker) {
+	delete unpacker;
+}
+
+LIDARSampleDataUnpacker::~LIDARSampleDataUnpacker() {
+
+}
+
+LIDARSampleDataUnpacker::LIDARSampleDataUnpacker(LIDARSampleDataListener& l) 
+	: _listener(l)
+{
+
+}
+
+
+END_DATAUNPACKER_NS()

third_party/rplidar_sdk/src/dataunpacker/dataunpacker.h

@@ -0,0 +1,93 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+
+ /*
+  *  Sample Data Unpacker System
+  *
+  */
+
+  /*
+	* Redistribution and use in source and binary forms, with or without
+	* modification, are permitted provided that the following conditions are met:
+	*
+	* 1. Redistributions of source code must retain the above copyright notice,
+	*    this list of conditions and the following disclaimer.
+	*
+	* 2. Redistributions in binary form must reproduce the above copyright notice,
+	*    this list of conditions and the following disclaimer in the documentation
+	*    and/or other materials provided with the distribution.
+	*
+	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+	* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+	* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+	* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+	* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+	* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+	* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+	* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+	* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+	* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+	* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+	*
+	*/
+
+
+#pragma once
+
+#include "dataupacker_namespace.h"
+
+BEGIN_DATAUNPACKER_NS()
+
+
+class LIDARSampleDataListener
+{
+
+
+public:
+	virtual void onHQNodeScanResetReq() = 0;
+	virtual void onHQNodeDecoded(_u64 timestamp_uS, const rplidar_response_measurement_node_hq_t* node) = 0;
+	virtual void onCustomSampleDataDecoded(_u8 ansType, _u32 customCode, const void* data, size_t size) {}
+
+	virtual void onDecodingError(int errMsg, _u8 ansType, const void* payload, size_t size) {}
+};
+
+class LIDARSampleDataUnpacker
+{
+public:
+	enum {
+		ERR_EVENT_ON_EXP_ENCODER_RESET = 0x8001,
+		ERR_EVENT_ON_EXP_CHECKSUM_ERR = 0x8002,
+	};
+
+	enum UnpackerContextType {
+		UNPACKER_CONTEXT_TYPE_LIDAR_UNKNOWN = 0,
+		UNPACKER_CONTEXT_TYPE_LIDAR_TIMING = 1,
+		UNPACKER_CONTEXT_TYPE_TRIANGULATION_OPTICAL_FACTOR = 2,
+
+	};
+
+	virtual ~LIDARSampleDataUnpacker();
+	static LIDARSampleDataUnpacker* CreateInstance(LIDARSampleDataListener& listener);
+	static void ReleaseInstance(LIDARSampleDataUnpacker*);
+
+	virtual void updateUnpackerContext(UnpackerContextType type, const void* data, size_t size) = 0;
+
+	virtual void enable() = 0;
+	virtual void disable() = 0;
+
+	virtual bool onSampleData(_u8 ansType, const void* buffer, size_t size) = 0;
+	virtual void reset() = 0;
+	virtual void clearCache() = 0;
+
+protected:
+	LIDARSampleDataUnpacker(LIDARSampleDataListener&);
+	LIDARSampleDataListener& _listener;
+
+};
+
+END_DATAUNPACKER_NS()

third_party/rplidar_sdk/src/dataunpacker/dataupacker_namespace.h

@@ -0,0 +1,5 @@
+#pragma once
+
+
+#define BEGIN_DATAUNPACKER_NS()  namespace sl{ namespace internal{
+#define END_DATAUNPACKER_NS()  }}

third_party/rplidar_sdk/src/dataunpacker/unpacker/handler_capsules.h

@@ -0,0 +1,149 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+
+ /*
+  *  Sample Data Unpacker System
+  *  Capsule Style Sample Node Handlers
+  */
+
+  /*
+	* Redistribution and use in source and binary forms, with or without
+	* modification, are permitted provided that the following conditions are met:
+	*
+	* 1. Redistributions of source code must retain the above copyright notice,
+	*    this list of conditions and the following disclaimer.
+	*
+	* 2. Redistributions in binary form must reproduce the above copyright notice,
+	*    this list of conditions and the following disclaimer in the documentation
+	*    and/or other materials provided with the distribution.
+	*
+	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+	* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+	* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+	* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+	* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+	* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+	* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+	* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+	* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+	* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+	* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+	*
+	*/
+
+#pragma once
+
+BEGIN_DATAUNPACKER_NS()
+
+namespace unpacker {
+
+class UnpackerHandler_CapsuleNode : public IDataUnpackerHandler {
+public:
+	UnpackerHandler_CapsuleNode();
+	virtual ~UnpackerHandler_CapsuleNode();
+
+	virtual _u8 getSampleAnswerType() const;
+	virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size);
+	virtual void reset();
+	virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size);
+protected:
+
+	void _onScanNodeCapsuleData(rplidar_response_capsule_measurement_nodes_t &, LIDARSampleDataUnpackerInner* engine);
+
+	std::vector<_u8> _cached_scan_node_buf;
+	int              _cached_scan_node_buf_pos;
+	bool             _is_previous_capsuledataRdy;
+
+	rplidar_response_capsule_measurement_nodes_t _cached_previous_capsuledata;
+	_u64             _cached_last_data_timestamp_us;
+
+	SlamtecLidarTimingDesc _cachedTimingDesc;
+};
+
+class UnpackerHandler_UltraCapsuleNode : public IDataUnpackerHandler {
+public:
+	UnpackerHandler_UltraCapsuleNode();
+	virtual ~UnpackerHandler_UltraCapsuleNode();
+
+	virtual _u8 getSampleAnswerType() const;
+	virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size);
+	virtual void reset();
+	virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size);
+protected:
+	void _onScanNodeUltraCapsuleData(rplidar_response_ultra_capsule_measurement_nodes_t&, LIDARSampleDataUnpackerInner* engine);
+
+
+	std::vector<_u8> _cached_scan_node_buf;
+	int              _cached_scan_node_buf_pos;
+	bool             _is_previous_capsuledataRdy;
+
+	rplidar_response_ultra_capsule_measurement_nodes_t _cached_previous_ultracapsuledata;
+	_u64             _cached_last_data_timestamp_us;
+
+	SlamtecLidarTimingDesc _cachedTimingDesc;
+
+};
+
+
+
+class UnpackerHandler_DenseCapsuleNode : public IDataUnpackerHandler {
+public:
+	UnpackerHandler_DenseCapsuleNode();
+	virtual ~UnpackerHandler_DenseCapsuleNode();
+
+	virtual _u8 getSampleAnswerType() const;
+	virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size);
+	virtual void reset();
+	virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size);
+protected:
+	void _onScanNodeDenseCapsuleData(rplidar_response_dense_capsule_measurement_nodes_t&, LIDARSampleDataUnpackerInner* engine);
+
+
+	std::vector<_u8> _cached_scan_node_buf;
+	int              _cached_scan_node_buf_pos;
+	bool             _is_previous_capsuledataRdy;
+
+	rplidar_response_dense_capsule_measurement_nodes_t _cached_previous_dense_capsuledata;
+	_u64             _cached_last_data_timestamp_us;
+
+	SlamtecLidarTimingDesc _cachedTimingDesc;
+
+};
+
+
+class UnpackerHandler_UltraDenseCapsuleNode : public IDataUnpackerHandler {
+public:
+	UnpackerHandler_UltraDenseCapsuleNode();
+	virtual ~UnpackerHandler_UltraDenseCapsuleNode();
+
+	virtual _u8 getSampleAnswerType() const;
+	virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size);
+	virtual void reset();
+	virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size);
+protected:
+	void _onScanNodeUltraDenseCapsuleData(rplidar_response_ultra_dense_capsule_measurement_nodes_t&, LIDARSampleDataUnpackerInner* engine);
+
+	std::vector<_u8> _cached_scan_node_buf;
+	int              _cached_scan_node_buf_pos;
+	bool             _is_previous_capsuledataRdy;
+
+	rplidar_response_ultra_dense_capsule_measurement_nodes_t _cached_previous_ultra_dense_capsuledata;
+	_u64             _cached_last_data_timestamp_us;
+
+
+
+	int              _last_node_sync_bit;
+	int              _last_dist_q2;
+
+	SlamtecLidarTimingDesc _cachedTimingDesc;
+};
+
+
+}
+
+END_DATAUNPACKER_NS()

third_party/rplidar_sdk/src/dataunpacker/unpacker/handler_hqnode.cpp

@@ -0,0 +1,192 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+
+ /*
+  *  Sample Data Unpacker System
+  *  HQNode Sample Node Handler
+  */
+
+  /*
+	* Redistribution and use in source and binary forms, with or without
+	* modification, are permitted provided that the following conditions are met:
+	*
+	* 1. Redistributions of source code must retain the above copyright notice,
+	*    this list of conditions and the following disclaimer.
+	*
+	* 2. Redistributions in binary form must reproduce the above copyright notice,
+	*    this list of conditions and the following disclaimer in the documentation
+	*    and/or other materials provided with the distribution.
+	*
+	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+	* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+	* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+	* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+	* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+	* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+	* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+	* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+	* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+	* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+	* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+	*
+	*/
+
+#include "../dataunnpacker_commondef.h"
+#include "../dataunpacker.h"
+#include "../dataunnpacker_internal.h"
+
+#ifdef CONF_NO_BOOST_CRC_SUPPORT
+#include "sl_crc.h" 
+#endif
+
+#include "handler_hqnode.h"
+
+BEGIN_DATAUNPACKER_NS()
+	
+namespace unpacker{
+
+
+static _u64 _getSampleDelayOffsetInHQMode(const SlamtecLidarTimingDesc& timing)
+{
+    // FIXME: to eval
+    // 
+    // guess channel baudrate by LIDAR model ....
+    const _u64 channelBaudRate = timing.native_baudrate? timing.native_baudrate:1000000;
+
+    _u64 tranmissionDelay = 1000000ULL * sizeof(rplidar_response_measurement_node_hq_t) * 10 / channelBaudRate;
+
+    if (timing.native_interface_type == LIDARInterfaceType::LIDAR_INTERFACE_ETHERNET)
+    {
+        tranmissionDelay = 100; //dummy value
+    }
+
+    // center of the sample duration
+    const _u64 sampleDelay = (timing.sample_duration_uS >> 1);
+    const _u64 sampleFilterDelay = timing.sample_duration_uS;
+
+    return sampleFilterDelay + sampleDelay + tranmissionDelay + timing.linkage_delay_uS;
+}
+
+UnpackerHandler_HQNode::UnpackerHandler_HQNode()
+    : _cached_scan_node_buf_pos(0)
+{
+    _cached_scan_node_buf.resize(sizeof(rplidar_response_hq_capsule_measurement_nodes_t));
+    memset(&_cachedTimingDesc, 0, sizeof(_cachedTimingDesc));
+}
+
+UnpackerHandler_HQNode::~UnpackerHandler_HQNode()
+{
+
+}
+
+_u8 UnpackerHandler_HQNode::getSampleAnswerType() const
+{
+	return RPLIDAR_ANS_TYPE_MEASUREMENT_HQ;
+}
+
+void UnpackerHandler_HQNode::onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t cnt)
+{
+
+    for (size_t pos = 0; pos < cnt; ++pos)
+    {
+        _u8 current_data = data[pos];
+
+        switch (_cached_scan_node_buf_pos)
+        {
+        case 0: // expect the sync byte
+        {
+            if (current_data == RPLIDAR_RESP_MEASUREMENT_HQ_SYNC) {
+                // pass
+            }
+            else {
+                continue;
+            }
+        }
+        break;
+
+        case sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 1 - 4:    // get bytes to calculate crc ready
+        {
+           
+        }
+        break;
+
+        case sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 1: // new data ready
+        {
+            _cached_scan_node_buf[sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 1] = current_data;
+            _cached_scan_node_buf_pos = 0;
+            rplidar_response_hq_capsule_measurement_nodes_t* nodesData = reinterpret_cast<rplidar_response_hq_capsule_measurement_nodes_t*>(&_cached_scan_node_buf[0]);
+
+#ifdef CONF_NO_BOOST_CRC_SUPPORT
+            _u32 crcCalc = crc32::getResult(&_cached_scan_node_buf[0], sizeof(sl_lidar_response_hq_capsule_measurement_nodes_t) - 4);
+
+
+#else
+            // calculate crc with boost crc method
+            boost::crc_optimal<32, 0x04C11DB7, 0xFFFFFFFF, 0xFFFFFFFF, true, true> mycrc;
+            std::vector<_u8> crcInputData;
+            crcInputData.resize(sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 4);
+            memcpy(&crcInputData[0], nodesData, sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 4);
+            //supplement crcInputData to mutiples of 4
+            int leftBytes = 4 - (crcInputData.size() & 3);
+            for (int i = 0; i < leftBytes; i++)
+                crcInputData.push_back(0);
+            mycrc.process_bytes(&crcInputData[0], crcInputData.size());
+            _u32 crcCalc = mycrc.checksum();
+            
+#endif
+
+            _u32 recvCRC = nodesData->crc32;
+#ifdef _CPU_ENDIAN_BIG
+            recvCRC = le32_to_cpu(recvCRC);
+            nodesData->time_stamp = le64_to_cpu(nodesData->time_stamp);
+#endif
+            if (recvCRC == crcCalc)
+            {
+                for (size_t pos = 0; pos < _countof(nodesData->node_hq); ++pos)
+                {
+                    rplidar_response_measurement_node_hq_t hqNode = nodesData->node_hq[pos];
+#ifdef _CPU_ENDIAN_BIG
+                    hqNode.angle_z_q14 = le16_to_cpu(hqNode.angle_z_q14);
+                    hqNode.dist_mm_q2 = le32_to_cpu(hqNode.dist_mm_q2);
+#endif
+                    engine->publishHQNode(engine->getCurrentTimestamp_uS() - _getSampleDelayOffsetInHQMode(_cachedTimingDesc), &hqNode);
+                }
+            }
+            else  //crc check not passed 
+            {
+                engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_CHECKSUM_ERR
+                    , RPLIDAR_ANS_TYPE_MEASUREMENT_HQ, nodesData, sizeof(*nodesData));
+            }
+            continue;
+        }
+        break;
+
+
+        }
+        _cached_scan_node_buf[_cached_scan_node_buf_pos++] = current_data;
+    }
+
+}
+
+
+void UnpackerHandler_HQNode::onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size)
+{
+    if (type == LIDARSampleDataUnpacker::UNPACKER_CONTEXT_TYPE_LIDAR_TIMING) {
+        assert(size == sizeof(_cachedTimingDesc));
+        _cachedTimingDesc = *reinterpret_cast<const SlamtecLidarTimingDesc*>(data);
+    }
+}
+
+void UnpackerHandler_HQNode::reset()
+{
+    _cached_scan_node_buf_pos = 0;
+}
+}
+
+
+END_DATAUNPACKER_NS()

third_party/rplidar_sdk/src/dataunpacker/unpacker/handler_hqnode.h

@@ -0,0 +1,63 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+
+ /*
+  *  Sample Data Unpacker System
+  *  HQNode Sample Node Handler
+  */
+
+  /*
+	* Redistribution and use in source and binary forms, with or without
+	* modification, are permitted provided that the following conditions are met:
+	*
+	* 1. Redistributions of source code must retain the above copyright notice,
+	*    this list of conditions and the following disclaimer.
+	*
+	* 2. Redistributions in binary form must reproduce the above copyright notice,
+	*    this list of conditions and the following disclaimer in the documentation
+	*    and/or other materials provided with the distribution.
+	*
+	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+	* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+	* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+	* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+	* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+	* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+	* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+	* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+	* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+	* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+	* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+	*
+	*/
+
+#pragma once
+
+BEGIN_DATAUNPACKER_NS()
+
+namespace unpacker {
+
+	class UnpackerHandler_HQNode : public IDataUnpackerHandler {
+	public:
+		UnpackerHandler_HQNode();
+		virtual ~UnpackerHandler_HQNode();
+
+		virtual _u8 getSampleAnswerType() const;
+		virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size);
+		virtual void reset();
+		virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size);
+
+	protected:
+		std::vector<_u8> _cached_scan_node_buf;
+		int              _cached_scan_node_buf_pos;
+		SlamtecLidarTimingDesc _cachedTimingDesc;
+	};
+
+}
+
+END_DATAUNPACKER_NS()

third_party/rplidar_sdk/src/dataunpacker/unpacker/handler_normalnode.cpp

@@ -0,0 +1,159 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+
+ /*
+  *  Sample Data Unpacker System
+  *  Normal Sample Node Handler
+  */
+
+  /*
+	* Redistribution and use in source and binary forms, with or without
+	* modification, are permitted provided that the following conditions are met:
+	*
+	* 1. Redistributions of source code must retain the above copyright notice,
+	*    this list of conditions and the following disclaimer.
+	*
+	* 2. Redistributions in binary form must reproduce the above copyright notice,
+	*    this list of conditions and the following disclaimer in the documentation
+	*    and/or other materials provided with the distribution.
+	*
+	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+	* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+	* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+	* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+	* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+	* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+	* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+	* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+	* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+	* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+	* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+	*
+	*/
+
+#include "../dataunnpacker_commondef.h"
+#include "../dataunpacker.h"
+#include "../dataunnpacker_internal.h"
+
+
+#include "handler_normalnode.h"
+
+BEGIN_DATAUNPACKER_NS()
+	
+namespace unpacker{
+
+
+static _u64 _getSampleDelayOffsetInLegacyMode(const SlamtecLidarTimingDesc& timing)
+{
+    // guess channel baudrate by LIDAR model ....
+    const _u64 channelBaudRate = timing.native_baudrate? timing.native_baudrate:115200;
+
+    _u64 tranmissionDelay = 1000000ULL * sizeof(rplidar_response_measurement_node_t) * 10 / channelBaudRate;
+
+    if (timing.native_interface_type == LIDARInterfaceType::LIDAR_INTERFACE_ETHERNET)
+    {
+        tranmissionDelay = 100; //dummy value
+    }
+
+    // center of the sample duration
+    const _u64 sampleDelay = (timing.sample_duration_uS >> 1);
+    const _u64 sampleFilterDelay = timing.sample_duration_uS;
+
+    return sampleFilterDelay + sampleDelay + tranmissionDelay + timing.linkage_delay_uS;
+}
+
+UnpackerHandler_NormalNode::UnpackerHandler_NormalNode()
+    : _cached_scan_node_buf_pos(0)
+{
+    _cached_scan_node_buf.resize(sizeof(rplidar_response_measurement_node_t));
+    memset(&_cachedTimingDesc, 0, sizeof(_cachedTimingDesc));
+;}
+
+UnpackerHandler_NormalNode::~UnpackerHandler_NormalNode()
+{
+
+}
+
+_u8 UnpackerHandler_NormalNode::getSampleAnswerType() const
+{
+	return RPLIDAR_ANS_TYPE_MEASUREMENT;
+}
+
+void UnpackerHandler_NormalNode::onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t cnt)
+{
+    for (size_t pos = 0; pos < cnt; ++pos) {
+        _u8 current_data = data[pos];
+        switch (_cached_scan_node_buf_pos) {
+        case 0: // expect the sync bit and its reverse in this byte
+        {
+            _u8 tmp = (current_data >> 1);
+            if ((tmp ^ current_data) & 0x1) {
+                // pass
+            }
+            else {
+                continue;
+            }
+
+        }
+        break;
+        case 1: // expect the highest bit to be 1
+        {
+            if (current_data & RPLIDAR_RESP_MEASUREMENT_CHECKBIT) {
+                // pass
+            }
+            else {
+                _cached_scan_node_buf_pos = 0;
+                continue;
+            }
+        }
+        break;
+        case sizeof(rplidar_response_measurement_node_t) - 1: // new data ready
+        {
+            _cached_scan_node_buf[sizeof(rplidar_response_measurement_node_t) - 1] = current_data;
+            _cached_scan_node_buf_pos = 0;
+
+            rplidar_response_measurement_node_t* node = reinterpret_cast<rplidar_response_measurement_node_t*>(&_cached_scan_node_buf[0]);
+#ifdef _CPU_ENDIAN_BIG
+            node->angle_q6_checkbit = le16_to_cpu(node->angle_q6_checkbit);
+            node->distance_q2 = le16_to_cpu(node->distance_q2);
+#endif
+            //cast node to rplidar_response_measurement_node_hq_t
+            rplidar_response_measurement_node_hq_t hqNode;
+            hqNode.angle_z_q14 = (((node->angle_q6_checkbit) >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) << 8) / 90;  //transfer to q14 Z-angle
+            hqNode.dist_mm_q2 = node->distance_q2;
+            hqNode.flag = (node->sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT);  // trasfer syncbit to HQ flag field
+            hqNode.quality = (node->sync_quality >> RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT;  //remove the last two bits and then make quality from 0-63 to 0-255
+            
+            
+            engine->publishHQNode(engine->getCurrentTimestamp_uS() - _getSampleDelayOffsetInLegacyMode(_cachedTimingDesc), &hqNode);
+            continue;
+
+        }
+        break;
+        }
+        _cached_scan_node_buf[_cached_scan_node_buf_pos++] = current_data;
+    }
+}
+
+
+void UnpackerHandler_NormalNode::onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size)
+{
+    if (type == LIDARSampleDataUnpacker::UNPACKER_CONTEXT_TYPE_LIDAR_TIMING) {
+        assert(size == sizeof(_cachedTimingDesc));
+        _cachedTimingDesc = *reinterpret_cast<const SlamtecLidarTimingDesc*>(data);
+    }
+}
+
+void UnpackerHandler_NormalNode::reset()
+{
+    _cached_scan_node_buf_pos = 0;
+}
+}
+
+
+END_DATAUNPACKER_NS()

third_party/rplidar_sdk/src/dataunpacker/unpacker/handler_normalnode.h

@@ -0,0 +1,63 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+
+ /*
+  *  Sample Data Unpacker System
+  *  Normal Sample Node Handler
+  */
+
+  /*
+	* Redistribution and use in source and binary forms, with or without
+	* modification, are permitted provided that the following conditions are met:
+	*
+	* 1. Redistributions of source code must retain the above copyright notice,
+	*    this list of conditions and the following disclaimer.
+	*
+	* 2. Redistributions in binary form must reproduce the above copyright notice,
+	*    this list of conditions and the following disclaimer in the documentation
+	*    and/or other materials provided with the distribution.
+	*
+	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+	* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+	* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+	* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+	* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+	* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+	* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+	* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+	* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+	* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+	* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+	*
+	*/
+
+#pragma once
+
+BEGIN_DATAUNPACKER_NS()
+	
+namespace unpacker{
+
+class UnpackerHandler_NormalNode : public IDataUnpackerHandler {
+public:
+	UnpackerHandler_NormalNode();
+	virtual ~UnpackerHandler_NormalNode();
+
+	virtual _u8 getSampleAnswerType() const;
+	virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size);
+	virtual void reset();
+	virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size);
+protected:
+	std::vector<_u8> _cached_scan_node_buf;
+	int              _cached_scan_node_buf_pos;
+
+	SlamtecLidarTimingDesc _cachedTimingDesc;
+};
+
+}
+
+END_DATAUNPACKER_NS()

third_party/rplidar_sdk/src/hal/abs_rxtx.h

@@ -0,0 +1,88 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once 
+
+#include "hal/types.h"
+
+namespace rp{ namespace hal{
+
+class serial_rxtx
+{
+public:
+    enum{
+        ANS_OK      = 0,
+        ANS_TIMEOUT = -1,
+        ANS_DEV_ERR = -2,
+    };
+
+    static serial_rxtx * CreateRxTx();
+    static void ReleaseRxTx( serial_rxtx * );
+
+    serial_rxtx():_is_serial_opened(false){}
+    virtual ~serial_rxtx(){}
+
+    virtual void flush( _u32 flags) = 0;
+
+    virtual bool bind(const char * portname, _u32 baudrate, _u32 flags = 0) = 0;
+    virtual bool open() = 0;
+    virtual void close()  = 0;
+    
+    virtual int waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL) = 0;
+
+    virtual int senddata(const unsigned char * data, size_t size) = 0;
+    virtual int recvdata(unsigned char * data, size_t size) = 0;
+
+    virtual int waitforsent(_u32 timeout = -1, size_t * returned_size = NULL) = 0;
+    virtual int waitforrecv(_u32 timeout = -1, size_t * returned_size = NULL) = 0;
+
+    virtual size_t rxqueue_count() = 0;
+
+    virtual void setDTR() = 0;
+    virtual void clearDTR() = 0;
+    virtual void cancelOperation() {}
+
+    virtual bool isOpened()
+    {
+        return _is_serial_opened;
+    }
+
+protected:
+    volatile bool   _is_serial_opened;
+};
+
+}}
+
+
+

third_party/rplidar_sdk/src/hal/assert.h

@@ -0,0 +1,18 @@
+#ifndef _INFRA_HAL_ASSERT_H
+#define _INFRA_HAL_ASSERT_H
+
+#ifdef WIN32
+#include <crtdbg.h>
+#ifndef assert
+#define assert(x) _ASSERT(x)
+#endif
+#elif defined(_AVR_)
+#define assert(x) 
+#elif defined(__GNUC__)
+#ifndef assert
+#define assert(x) 
+#endif
+#else
+#error assert.h cannot identify your platform
+#endif 
+#endif

third_party/rplidar_sdk/src/hal/byteops.h

@@ -0,0 +1,94 @@
+/*
+ *  RoboPeak Project
+ *  Copyright 2009 - 2013
+ *  
+ *  RPOS - Byte Operations
+ *
+ */
+
+#pragma once
+
+// byte swapping operations for compiling time
+
+#define __static_byteswap_16(x)  ((_u16)(				\
+	(((_u16)(x) & (_u16)0x00FFU) << 8) |			\
+	(((_u16)(x) & (_u16)0xFF00U) >> 8)))
+
+#define __static_byteswap_32(x) ((_u32)(				\
+	(((_u32)(x) & (_u32)0x000000FFUL) << 24) |		\
+	(((_u32)(x) & (_u32)0x0000FF00UL) <<  8) |		\
+	(((_u32)(x) & (_u32)0x00FF0000UL) >>  8) |		\
+	(((_u32)(x) & (_u32)0xFF000000UL) >> 24)))
+
+#define __static_byteswap_64(x) ((_u64)(				\
+	(((_u64)(x) & (_u64)0x00000000000000ffULL) << 56) |	\
+	(((_u64)(x) & (_u64)0x000000000000ff00ULL) << 40) |	\
+	(((_u64)(x) & (_u64)0x0000000000ff0000ULL) << 24) |	\
+	(((_u64)(x) & (_u64)0x00000000ff000000ULL) <<  8) |	\
+	(((_u64)(x) & (_u64)0x000000ff00000000ULL) >>  8) |	\
+	(((_u64)(x) & (_u64)0x0000ff0000000000ULL) >> 24) |	\
+	(((_u64)(x) & (_u64)0x00ff000000000000ULL) >> 40) |	\
+	(((_u64)(x) & (_u64)0xff00000000000000ULL) >> 56)))
+
+
+#define __fast_swap(a, b) do { (a) ^= (b); (b) ^= (a); (a) ^= (b); } while(0)
+
+
+static inline _u16 __byteswap_16(_u16 x)
+{
+#ifdef __arch_byteswap_16
+    return __arch_byteswap_16(x);
+#else
+    return __static_byteswap_16(x);
+#endif
+}
+
+static inline _u32 __byteswap_32(_u32 x)
+{
+#ifdef __arch_byteswap_32
+    return __arch_byteswap_32(x);
+#else
+    return __static_byteswap_32(x);
+#endif
+}
+
+static inline _u64 __byteswap_64(_u64 x)
+{
+#ifdef __arch_byteswap_64
+    return __arch_byteswap_64(x);
+#else
+    return __static_byteswap_64(x);
+#endif
+}
+
+
+#ifdef float
+static inline float __byteswap_float(float x)
+{
+#ifdef __arch_byteswap_float
+    return __arch_byteswap_float(x);
+#else
+    _u8 * raw = (_u8 *)&x;
+    __fast_swap(raw[0], raw[3]);
+    __fast_swap(raw[1], raw[2]);
+    return x;
+#endif
+}
+#endif
+
+
+#ifdef double
+static inline double __byteswap_double(double x)
+{
+#ifdef __arch_byteswap_double
+    return __arch_byteswap_double(x);
+#else
+    _u8 * raw = (_u8 *)&x;
+    __fast_swap(raw[0], raw[7]);
+    __fast_swap(raw[1], raw[6]);
+    __fast_swap(raw[2], raw[5]);
+    __fast_swap(raw[3], raw[4]);
+    return x;
+#endif
+}
+#endif

third_party/rplidar_sdk/src/hal/byteorder.h

@@ -0,0 +1,112 @@
+/*
+ *  RoboPeak Project
+ *  Copyright 2009 - 2013
+ *  
+ *  RPOS - Endianness Helper
+ *
+ */
+
+#pragma once
+
+
+#if !defined(_CPU_ENDIAN_BIG) && !defined(_CPU_ENDIAN_SMALL)
+// CPU Endianness is not specified, assume little endian.
+#define _CPU_ENDIAN_SMALL
+#endif
+
+#if defined(_CPU_ENDIAN_BIG) && defined(_CPU_ENDIAN_SMALL)
+#error "_CPU_ENDIAN_BIG and _CPU_ENDIAN_SMALL cannot be defined at the same time."
+#endif
+
+#include "hal/byteops.h"
+
+#if defined(_CPU_ENDIAN_SMALL)
+
+
+// we don't want to conflict with the Linux kernel...
+#ifndef __KERNEL__ 
+#define constant_cpu_to_le64(x) ((_u64)(x))
+#define constant_le64_to_cpu(x) ((_u64)(x))
+#define constant_cpu_to_le32(x) ((_u32)(x))
+#define constant_le32_to_cpu(x) ((_u32)(x))
+#define constant_cpu_to_le16(x) ((_u16)(x))
+#define constant_le16_to_cpu(x) ((_u16)(x))
+#define constant_cpu_to_be64(x) (__static_byteswap_64((x)))
+#define constant_be64_to_cpu(x) __static_byteswap_64((_u64)(x))
+#define constant_cpu_to_be32(x) (__static_byteswap_32((x)))
+#define constant_be32_to_cpu(x) __static_byteswap_32((_u32)(x))
+#define constant_cpu_to_be16(x) (__static_byteswap_16((x)))
+#define constant_be16_to_cpu(x) __static_byteswap_16((_u16)(x))
+
+#define cpu_to_le64(x) ((_u64)(x))
+#define le64_to_cpu(x) ((_u64)(x))
+#define cpu_to_le32(x) ((_u32)(x))
+#define le32_to_cpu(x) ((_u32)(x))
+#define cpu_to_le16(x) ((_u16)(x))
+#define le16_to_cpu(x) ((_u16)(x))
+#define cpu_to_be64(x) (__byteswap_64((x)))
+#define be64_to_cpu(x) __byteswap_64((_u64)(x))
+#define cpu_to_be32(x) (__byteswap_32((x)))
+#define be32_to_cpu(x) __byteswap_32((_u32)(x))
+#define cpu_to_be16(x) (__byteswap_16((x)))
+#define be16_to_cpu(x) __byteswap_16((_u16)(x))
+#endif
+
+#define cpu_to_float_le(x) ((float)x)
+#define float_le_to_cpu(x) ((float)x)
+
+#define cpu_to_float_be(x) __byteswap_float(x)
+#define float_be_to_cpu(x) __byteswap_float(x)
+
+#define cpu_to_double_le(x) ((double)x)
+#define double_le_to_cpu(x) ((double)x)
+
+#define cpu_to_double_be(x) __byteswap_double(x)
+#define double_be_to_cpu(x) __byteswap_double(x)
+
+#else
+
+// we don't want to conflict with the Linux kernel...
+#ifndef __KERNEL__ 
+#define constant_cpu_to_le64(x) (__static_byteswap_64((x)))
+#define constant_le64_to_cpu(x) __static_byteswap_64((_u64)(x))
+#define constant_cpu_to_le32(x) (__static_byteswap_32((x)))
+#define constant_le32_to_cpu(x) __static_byteswap_32((_u32)(x))
+#define constant_cpu_to_le16(x) (__static_byteswap_16((x)))
+#define constant_le16_to_cpu(x) __static_byteswap_16((_u16)(x))
+#define constant_cpu_to_be64(x) ((_u64)(x))
+#define constant_be64_to_cpu(x) ((_u64)(x))
+#define constant_cpu_to_be32(x) ((_u32)(x))
+#define constant_be32_to_cpu(x) ((_u32)(x))
+#define constant_cpu_to_be16(x) ((_u16)(x))
+#define constant_be16_to_cpu(x) ((_u16)(x))
+
+#define cpu_to_le64(x) (__byteswap_64((x)))
+#define le64_to_cpu(x) __byteswap_64((_u64)(x))
+#define cpu_to_le32(x) (__byteswap_32((x)))
+#define le32_to_cpu(x) __byteswap_32((_u32)(x))
+#define cpu_to_le16(x) (__byteswap_16((x)))
+#define le16_to_cpu(x) __byteswap_16((_u16)(x))
+#define cpu_to_be64(x) ((_u64)(x))
+#define be64_to_cpu(x) ((_u64)(x))
+#define cpu_to_be32(x) ((_u32)(x))
+#define be32_to_cpu(x) ((_u32)(x))
+#define cpu_to_be16(x) ((_u16)(x))
+#define be16_to_cpu(x) ((_u16)(x))
+#endif
+
+
+#define cpu_to_float_le(x) __byteswap_float(x)
+#define float_le_to_cpu(x) __byteswap_float(x)
+
+#define cpu_to_float_be(x) ((float)x)
+#define float_be_to_cpu(x) ((float)x)
+
+
+#define cpu_to_double_le(x) __byteswap_double(x)
+#define double_le_to_cpu(x) __byteswap_double(x)
+
+#define cpu_to_double_be(x) ((double)x)
+#define double_be_to_cpu(x) ((double)x)
+
+#endif

third_party/rplidar_sdk/src/hal/event.h

@@ -0,0 +1,206 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+namespace rp{ namespace hal{
+
+class Event
+{
+public:
+    
+    enum
+    {
+        EVENT_OK = 1,
+        EVENT_TIMEOUT = 0xFFFFFFFF,
+        EVENT_FAILED = 0,
+    };
+    
+    Event(bool isAutoReset = true, bool isSignal = false)
+#ifdef _WIN32
+        : _event(NULL)
+#else
+        : _is_signalled(isSignal)
+        , _isAutoReset(isAutoReset)
+#endif
+    {
+#ifdef _WIN32
+        _event = CreateEvent(NULL, isAutoReset?FALSE:TRUE, isSignal?TRUE:FALSE, NULL); 
+#else
+        pthread_mutex_init(&_cond_locker, NULL);
+        pthread_condattr_init(&_cond_attr);
+#ifdef _MACOS
+        // sadly, there is no monotonic clock support for pthread cond variable on MACOS
+        // if time slew is a big issue, try to reimplement it using kqueue/kevent
+#else
+        pthread_condattr_setclock(&_cond_attr, CLOCK_MONOTONIC);
+#endif
+        pthread_cond_init(&_cond_var, &_cond_attr);
+       
+#endif
+    }
+
+    ~ Event()
+    {
+        release();
+    }
+
+    void set( bool isSignal = true )
+    {
+        if (isSignal){
+#ifdef _WIN32
+            SetEvent(_event);
+#else
+            pthread_mutex_lock(&_cond_locker);
+               
+            if ( _is_signalled == false )
+            {
+                _is_signalled = true;
+                pthread_cond_signal(&_cond_var);
+            }
+            pthread_mutex_unlock(&_cond_locker);
+#endif
+        }
+        else
+        {
+#ifdef _WIN32
+            ResetEvent(_event);
+#else
+            pthread_mutex_lock(&_cond_locker);
+            _is_signalled = false;
+            pthread_mutex_unlock(&_cond_locker);
+#endif
+        }
+    }
+    
+    unsigned long wait( unsigned long timeout = 0xFFFFFFFF )
+    {
+#ifdef _WIN32
+        switch (WaitForSingleObject(_event, timeout==0xFFFFFFF?INFINITE:(DWORD)timeout))
+        {
+        case WAIT_FAILED:
+            return EVENT_FAILED;
+        case WAIT_OBJECT_0:
+            return EVENT_OK;
+        case WAIT_TIMEOUT:
+            return EVENT_TIMEOUT;
+        }
+        return EVENT_OK;
+#else
+        unsigned long ans = EVENT_OK;
+        pthread_mutex_lock( &_cond_locker );
+
+        if ( !_is_signalled )
+        {
+            
+                if (timeout == 0xFFFFFFFF){
+                    pthread_cond_wait(&_cond_var,&_cond_locker);
+                }else
+                {
+                    int timewaitresult = 0;
+#ifdef _MACOS
+                    timespec wait_time;
+                    
+                    wait_time.tv_sec = timeout / 1000;
+                    wait_time.tv_nsec = (timeout%1000)*1000000ULL;
+                
+                    timewaitresult = pthread_cond_timedwait_relative_np(&_cond_var,&_cond_locker,&wait_time);
+#else
+                    timespec wait_time;
+                    clock_gettime(CLOCK_MONOTONIC, &wait_time);
+
+                    wait_time.tv_sec += timeout / 1000;
+                    wait_time.tv_nsec += (timeout%1000)*1000000ULL;
+                
+                    if (wait_time.tv_nsec >= 1000000000)
+                    {
+                       ++wait_time.tv_sec;
+                       wait_time.tv_nsec -= 1000000000;
+                    }
+                    timewaitresult = pthread_cond_timedwait(&_cond_var,&_cond_locker,&wait_time);
+#endif
+
+                    switch (timewaitresult)
+                    {
+                    case 0:
+                        // signalled
+                        break;
+                    case ETIMEDOUT:
+                        // time up
+                        ans = EVENT_TIMEOUT;
+                        goto _final;
+                        break;
+                    default:
+                        ans = EVENT_FAILED;
+                        goto _final;
+                    }
+       
+            }
+        }
+          
+        //assert(_is_signalled);
+
+        if ( _isAutoReset )
+        {
+            _is_signalled = false;
+        }
+_final:
+        pthread_mutex_unlock( &_cond_locker );
+
+        return ans;
+#endif
+        
+    }
+protected:
+
+    void release()
+    {
+#ifdef _WIN32
+        CloseHandle(_event);
+#else
+        pthread_mutex_destroy(&_cond_locker);
+        pthread_cond_destroy(&_cond_var);
+#endif
+    }
+
+#ifdef _WIN32
+        HANDLE _event;
+#else
+        pthread_cond_t         _cond_var;
+        pthread_mutex_t        _cond_locker;
+        pthread_condattr_t     _cond_attr;
+        bool                   _is_signalled;
+        bool                   _isAutoReset;
+#endif
+};
+}}

third_party/rplidar_sdk/src/hal/locker.h

@@ -0,0 +1,205 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+namespace rp{ namespace hal{ 
+
+class Locker
+{
+public:
+    enum LOCK_STATUS
+    {
+        LOCK_OK = 1,
+        LOCK_TIMEOUT = -1,
+        LOCK_FAILED = 0
+    };
+
+    Locker(bool recusive = false){
+#ifdef _WIN32
+        _lock = NULL;
+#endif
+        init(recusive);
+    }
+
+    ~Locker()
+    {
+        release();
+    }
+
+    Locker::LOCK_STATUS lock(unsigned long timeout = 0xFFFFFFFF)
+    {
+#ifdef _WIN32
+        switch (WaitForSingleObject(_lock, timeout==0xFFFFFFF?INFINITE:(DWORD)timeout))
+        {
+        case WAIT_ABANDONED:
+            return LOCK_FAILED;
+        case WAIT_OBJECT_0:
+            return LOCK_OK;
+        case WAIT_TIMEOUT:
+            return LOCK_TIMEOUT;
+        }
+
+#else
+#ifdef _MACOS
+        if (timeout !=0 ) {
+            if (pthread_mutex_lock(&_lock) == 0) return LOCK_OK;
+        }
+#else
+        if (timeout == 0xFFFFFFFF){
+            if (pthread_mutex_lock(&_lock) == 0) return LOCK_OK;
+        }
+#endif
+        else if (timeout == 0)
+        {
+            if (pthread_mutex_trylock(&_lock) == 0) return LOCK_OK;
+        }
+#ifndef _MACOS
+        else
+        {
+            timespec wait_time;
+            timeval now;
+            gettimeofday(&now,NULL);
+
+            wait_time.tv_sec = timeout/1000 + now.tv_sec;
+            wait_time.tv_nsec = (timeout%1000)*1000000 + now.tv_usec*1000;
+        
+            if (wait_time.tv_nsec >= 1000000000)
+            {
+               ++wait_time.tv_sec;
+               wait_time.tv_nsec -= 1000000000;
+            }
+            switch (pthread_mutex_timedlock(&_lock,&wait_time))
+            {
+            case 0:
+                return LOCK_OK;
+            case ETIMEDOUT:
+                return LOCK_TIMEOUT;
+            }
+        }
+#endif
+#endif
+
+        return LOCK_FAILED;
+    }
+
+
+    void unlock()
+    {
+#ifdef _WIN32
+        if (_recusive) {
+            ReleaseMutex(_lock);
+        } else {
+            ReleaseSemaphore(_lock, 1, NULL);
+        }
+#else
+        pthread_mutex_unlock(&_lock);
+#endif
+    }
+
+#ifdef _WIN32
+    HANDLE getLockHandle()
+    {
+        return _lock;
+    }
+#else
+    pthread_mutex_t *getLockHandle()
+    {
+        return &_lock;
+    }
+#endif
+
+
+
+protected:
+    void    init(bool recusive)
+    {
+        
+#ifdef _WIN32
+        if (_recusive = recusive) {
+            _lock = CreateMutex(NULL, FALSE, NULL);
+        } else {
+            _lock = CreateSemaphore(NULL, 1, 1, NULL);
+        }
+#else
+        
+        if (recusive) {
+            pthread_mutexattr_t attr;
+            pthread_mutexattr_init(&attr);
+            pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE);
+            pthread_mutex_init(&_lock, &attr);
+        } else {
+            pthread_mutex_init(&_lock, NULL);
+        }
+#endif
+    }
+
+    void    release()
+    {
+        unlock(); //force unlock before release
+#ifdef _WIN32
+
+        if (_lock) CloseHandle(_lock);
+        _lock = NULL;
+#else
+        pthread_mutex_destroy(&_lock);
+#endif
+    }
+
+#ifdef _WIN32
+    HANDLE  _lock;
+    bool _recusive;
+#else
+    pthread_mutex_t _lock;
+#endif
+    
+};
+
+class AutoLocker
+{
+public :
+    AutoLocker(Locker &l): _binded(l)
+    {
+        _binded.lock();
+    }
+
+    void forceUnlock() {
+        _binded.unlock();
+    }
+    ~AutoLocker() {_binded.unlock();}
+    Locker & _binded;
+};
+
+
+}}
+

third_party/rplidar_sdk/src/hal/socket.h

@@ -0,0 +1,149 @@
+/*
+ *  RoboPeak Project
+ *  HAL Layer - Socket Interface
+ *  Copyright 2009 - 2013 RoboPeak Project
+ */
+
+#pragma once
+
+#include <vector>
+
+namespace rp{ namespace net {
+
+class _single_thread SocketAddress 
+{
+
+public:
+    enum address_type_t {
+        ADDRESS_TYPE_UNSPEC = 0,
+        ADDRESS_TYPE_INET   = 1,
+        ADDRESS_TYPE_INET6  = 2,
+    };
+
+public:
+
+
+
+    SocketAddress();
+    SocketAddress(const char * addrString, int port, address_type_t = ADDRESS_TYPE_INET);
+    // do not use this function, internal usage
+    SocketAddress(void * platform_data);
+    SocketAddress(const SocketAddress &);
+    
+    SocketAddress & operator = (const SocketAddress &);
+
+    virtual ~SocketAddress();
+    
+    virtual int getPort() const;
+    virtual u_result setPort(int port);
+
+    virtual u_result setAddressFromString(const char * address_string,  address_type_t = ADDRESS_TYPE_INET);
+    virtual u_result getAddressAsString(char * buffer, size_t buffersize) const;
+    
+    virtual address_type_t getAddressType() const; 
+
+    virtual u_result getRawAddress(_u8 * buffer, size_t bufferSize) const;
+
+    const void * getPlatformData() const {
+        return _platform_data;
+    }
+
+    virtual void setLoopbackAddress(address_type_t = ADDRESS_TYPE_INET);
+    virtual void setBroadcastAddressIPv4();
+    virtual void setAnyAddress(address_type_t = ADDRESS_TYPE_INET);
+
+public:
+    static size_t LoopUpHostName(const char * hostname, const char * sevicename, std::vector<SocketAddress> &addresspool , bool performDNS = true, address_type_t = ADDRESS_TYPE_INET);
+
+protected:
+    void * _platform_data;
+};
+
+
+
+class SocketBase 
+{
+public:
+    enum socket_family_t {
+        SOCKET_FAMILY_INET  = 0,
+        SOCKET_FAMILY_INET6 = 1,
+        SOCKET_FAMILY_RAW   = 2,
+    };
+
+
+    enum socket_direction_mask {
+        SOCKET_DIR_RD = 0x1,
+        SOCKET_DIR_WR = 0x2,
+        SOCKET_DIR_BOTH = (SOCKET_DIR_RD | SOCKET_DIR_WR),
+    };
+
+    enum {
+        DEFAULT_SOCKET_TIMEOUT = 10000, //10sec
+    };
+
+    virtual ~SocketBase() {}
+    virtual void dispose() = 0;
+    virtual u_result bind(const SocketAddress & ) = 0;
+    
+    virtual u_result getLocalAddress(SocketAddress & ) = 0;
+    virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk = SOCKET_DIR_BOTH) = 0;
+
+    virtual u_result waitforSent(_u32 timeout  = DEFAULT_SOCKET_TIMEOUT) = 0;
+    virtual u_result waitforData(_u32 timeout  = DEFAULT_SOCKET_TIMEOUT)  = 0;
+protected:
+    SocketBase() {} 
+};
+
+
+class _single_thread StreamSocket : public SocketBase
+{
+public:
+
+    enum {
+        MAX_BACKLOG = 128,
+    };
+
+    static StreamSocket * CreateSocket(socket_family_t family = SOCKET_FAMILY_INET);
+    
+    virtual u_result connect(const SocketAddress & pairAddress) = 0;
+    
+    virtual u_result listen(int backlog = MAX_BACKLOG) = 0;
+    virtual StreamSocket * accept(SocketAddress * pairAddress = NULL) = 0;
+    virtual u_result waitforIncomingConnection(_u32 timeout  = DEFAULT_SOCKET_TIMEOUT) = 0;
+
+    virtual u_result send(const void * buffer, size_t len) = 0;
+    
+    virtual u_result recv(void *buf, size_t len, size_t & recv_len) = 0;
+    
+    virtual u_result getPeerAddress(SocketAddress & ) = 0;
+
+    virtual u_result shutdown(socket_direction_mask mask) = 0;
+
+    virtual u_result enableKeepAlive(bool enable = true) = 0;
+    
+    virtual u_result enableNoDelay(bool enable = true) = 0;
+
+protected:
+    virtual ~StreamSocket() {} // use dispose();
+    StreamSocket() {}
+};
+
+class _single_thread DGramSocket: public SocketBase
+{
+
+public:
+
+    static DGramSocket * CreateSocket(socket_family_t family = SOCKET_FAMILY_INET);
+       
+    virtual u_result setPairAddress(const SocketAddress * pairAddress) = 0;
+    virtual u_result sendTo(const SocketAddress * target, const void * buffer, size_t len) = 0;
+    virtual u_result recvFrom(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr = NULL) = 0;
+    virtual u_result clearRxCache() = 0;
+    
+protected:
+    virtual ~DGramSocket() {} // use dispose();
+
+    DGramSocket() {}
+};
+
+}}

third_party/rplidar_sdk/src/hal/thread.cpp

@@ -0,0 +1,48 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "sdkcommon.h"
+#include "hal/thread.h"
+
+#if defined(_WIN32)
+#include "arch/win32/winthread.hpp"
+#elif defined(_MACOS)
+#include "arch/macOS/thread.hpp"
+#elif defined(__GNUC__)
+#include "arch/linux/thread.hpp"
+#else
+#error no threading implemention found for this platform.
+#endif
+
+

third_party/rplidar_sdk/src/hal/thread.h

@@ -0,0 +1,94 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+
+#include "hal/types.h"
+#define CLASS_THREAD(c , x ) \
+	rp::hal::Thread::create_member<c, &c::x>(this )
+
+namespace rp{ namespace hal{
+
+class Thread
+{
+public:
+    enum priority_val_t
+	{
+		PRIORITY_REALTIME = 0,
+		PRIORITY_HIGH     = 1,
+		PRIORITY_NORMAL   = 2,
+		PRIORITY_LOW      = 3,
+		PRIORITY_IDLE     = 4,
+	};
+
+    template <class T, u_result (T::*PROC)(void)>
+    static Thread create_member(T * pthis)
+    {
+		return create(_thread_thunk<T,PROC>, pthis);
+	}
+
+	template <class T, u_result (T::*PROC)(void) >
+	static _word_size_t THREAD_PROC _thread_thunk(void * data)
+	{
+		return (static_cast<T *>(data)->*PROC)();
+	}
+	static Thread create(thread_proc_t proc, void * data = NULL );
+
+public:
+    ~Thread() { }
+    Thread():  _data(NULL),_func(NULL),_handle(0)  {}
+    _word_size_t getHandle(){ return _handle;}
+    u_result terminate();
+    void *getData() { return _data;}
+    u_result join(unsigned long timeout = -1);
+	
+    // disabled as on platforms like Linux, the priority will be inherited by the child thread
+    // which may caused unexpected behavior.
+    // Please using Thread::SetSelfPriority instead
+    // u_result setPriority( priority_val_t p);
+	priority_val_t getPriority();
+
+    static u_result SetSelfPriority(priority_val_t p);
+
+
+    bool operator== ( const Thread & right) { return this->_handle == right._handle; }
+protected:
+    Thread( thread_proc_t proc, void * data ): _data(data),_func(proc), _handle(0)  {}
+    void * _data;
+    thread_proc_t _func;
+    _word_size_t _handle;
+};
+
+}}
+

third_party/rplidar_sdk/src/hal/types.h

@@ -0,0 +1,119 @@
+/*
+ *      Common Data Types for RP
+ */
+
+#ifndef _INFRA_HAL_TYPES_H_
+#define _INFRA_HAL_TYPES_H_
+
+//Basic types
+//
+#ifdef WIN32
+
+//fake stdint.h for VC only
+
+typedef signed   char     int8_t;
+typedef unsigned char     uint8_t;
+
+typedef __int16           int16_t;
+typedef unsigned __int16  uint16_t;
+
+typedef __int32           int32_t;
+typedef unsigned __int32  uint32_t;
+
+typedef __int64           int64_t;
+typedef unsigned __int64  uint64_t;
+
+
+#define RPMODULE_EXPORT  __declspec(dllexport)
+#define RPMODULE_IMPORT  __declspec(dllimport)
+
+#else
+
+#include <stdint.h>
+
+#define RPMODULE_EXPORT
+#define RPMODULE_IMPORT
+
+#endif
+
+
+//based on stdint.h
+typedef int8_t         _s8;
+typedef uint8_t        _u8;
+
+typedef int16_t        _s16;
+typedef uint16_t       _u16;
+
+typedef int32_t        _s32;
+typedef uint32_t       _u32;
+
+typedef int64_t        _s64;
+typedef uint64_t       _u64;
+
+#define __small_endian
+
+#ifndef __GNUC__
+#define __attribute__(x)
+#endif
+
+
+// The _word_size_t uses actual data bus width of the current CPU
+#ifdef _AVR_
+typedef _u8            _word_size_t;
+#define THREAD_PROC    
+#elif defined (WIN64)
+typedef _u64           _word_size_t;
+#define THREAD_PROC    __stdcall
+#elif defined (WIN32)
+typedef size_t         _word_size_t;
+#define THREAD_PROC    __stdcall
+#elif defined (__GNUC__)
+typedef unsigned long  _word_size_t;
+#define THREAD_PROC   
+#elif defined (__ICCARM__)
+typedef _u32            _word_size_t;
+#define THREAD_PROC  
+#endif
+
+
+
+//#define __le 
+//#define __be
+
+#define _multi_thread
+#define _single_thread
+
+typedef uint32_t u_result;
+
+#define RESULT_OK                       0
+#define RESULT_FAIL_BIT                 0x80000000
+#define RESULT_ALREADY_DONE             0x20
+#define RESULT_INVALID_DATA             (0x8000 | RESULT_FAIL_BIT)
+#define RESULT_OPERATION_FAIL           (0x8001 | RESULT_FAIL_BIT)
+#define RESULT_OPERATION_TIMEOUT        (0x8002 | RESULT_FAIL_BIT)
+#define RESULT_OPERATION_STOP           (0x8003 | RESULT_FAIL_BIT)
+#define RESULT_OPERATION_NOT_SUPPORT    (0x8004 | RESULT_FAIL_BIT)
+#define RESULT_FORMAT_NOT_SUPPORT       (0x8005 | RESULT_FAIL_BIT)
+#define RESULT_INSUFFICIENT_MEMORY      (0x8006 | RESULT_FAIL_BIT)
+#define RESULT_OPERATION_ABORTED        (0x8007 | RESULT_FAIL_BIT)
+#define RESULT_NOT_FOUND                (0x8008 | RESULT_FAIL_BIT)
+#define RESULT_RECONNECTING             (0x8009 | RESULT_FAIL_BIT)
+
+#define IS_OK(x)    ( ((x) & RESULT_FAIL_BIT) == 0 )
+#define IS_FAIL(x)  ( ((x) & RESULT_FAIL_BIT) )
+
+
+typedef _word_size_t (THREAD_PROC * thread_proc_t ) ( void * );
+
+
+#if defined (_BUILD_AS_DLL)
+#if defined (_BUILD_DLL_EXPORT)
+#define RPMODULE_IMPEXP  RPMODULE_EXPORT
+#else
+#define RPMODULE_IMPEXP  RPMODULE_IMPORT
+#endif
+#else
+#define RPMODULE_IMPEXP 
+#endif
+
+#endif

third_party/rplidar_sdk/src/hal/util.h

@@ -0,0 +1,67 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+
+
+//------
+/* _countof helper */
+#if !defined(_countof)
+#if !defined(__cplusplus)
+#define _countof(_Array) (sizeof(_Array) / sizeof(_Array[0]))
+#else
+extern "C++"
+{
+template <typename _CountofType, size_t _SizeOfArray>
+char (*__countof_helper( _CountofType (&_Array)[_SizeOfArray]))[_SizeOfArray];
+#define _countof(_Array) sizeof(*__countof_helper(_Array))
+}
+#endif
+#endif
+
+/* _offsetof helper */
+#if !defined(offsetof)
+#define offsetof(_structure, _field) ((_word_size_t)&(((_structure *)0x0)->_field))
+#endif
+
+
+#define BEGIN_STATIC_CODE( _blockname_ ) \
+    static class _static_code_##_blockname_ {   \
+    public:     \
+        _static_code_##_blockname_ () 
+
+
+#define END_STATIC_CODE( _blockname_ ) \
+    }   _instance_##_blockname_;
+

third_party/rplidar_sdk/src/hal/waiter.h

@@ -0,0 +1,44 @@
+/*
+ * For synchronize asynchrous operations
+ *
+ * Copyright 2010 Robopeak Team
+ */
+#pragma once
+
+#ifdef _AVR_
+#error there is no implementation for waiter.h on AVR platforms
+#else
+
+#include "hal/event.h"
+
+namespace rp{ namespace hal{
+
+	template<typename ResultT>
+	class Waiter : public Event
+	{
+	public:
+		Waiter() 
+			: Event()
+		{
+		}
+
+		~Waiter() 
+		{}
+
+		ResultT waitForResult()
+		{
+			wait();
+			return result;
+		}
+
+		void setResult(ResultT result)
+		{
+			this->result = result;
+			set();
+		}
+
+		volatile ResultT result;
+	};
+}}
+
+#endif

third_party/rplidar_sdk/src/rplidar_driver.cpp

@@ -0,0 +1,199 @@
+/*
+ * Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+#include "sdkcommon.h"
+#include "hal/abs_rxtx.h"
+#include "hal/thread.h"
+#include "hal/types.h"
+#include "hal/assert.h"
+#include "hal/locker.h"
+#include "hal/socket.h"
+#include "hal/event.h"
+#include "rplidar_driver.h"
+#include "sl_crc.h" 
+#include <algorithm>
+
+namespace rp { namespace standalone{ namespace rplidar {
+
+    RPlidarDriver::RPlidarDriver(){}
+
+    RPlidarDriver::RPlidarDriver(sl_u32 channelType) 
+        :_channelType(channelType)
+    {
+    }
+
+    RPlidarDriver::~RPlidarDriver() {}
+
+    RPlidarDriver * RPlidarDriver::CreateDriver(_u32 drivertype)
+    {
+        //_channelType = drivertype;
+        return  new RPlidarDriver(drivertype);
+    }
+
+    void RPlidarDriver::DisposeDriver(RPlidarDriver * drv)
+    {
+        delete drv;
+    }
+
+    u_result RPlidarDriver::connect(const char *path, _u32 portOrBaud, _u32 flag)
+    {
+        switch (_channelType)
+        {
+        case CHANNEL_TYPE_SERIALPORT:
+            _channel = (*createSerialPortChannel(path, portOrBaud));
+            break;
+        case CHANNEL_TYPE_TCP:
+            _channel = *createTcpChannel(path, portOrBaud);
+            break;
+        case CHANNEL_TYPE_UDP:
+            _channel = *createUdpChannel(path, portOrBaud);
+            break;
+        }
+        if (!(bool)_channel) return SL_RESULT_OPERATION_FAIL;
+        
+        _lidarDrv = *createLidarDriver();
+
+        if (!(bool)_lidarDrv) return SL_RESULT_OPERATION_FAIL;
+
+        sl_result ans =(_lidarDrv)->connect(_channel);
+        return ans;
+    }
+
+    void RPlidarDriver::disconnect()
+    {
+        (_lidarDrv)->disconnect();
+    }
+
+    bool RPlidarDriver::isConnected() 
+    { 
+        return (_lidarDrv)->isConnected();
+    }
+     
+    u_result RPlidarDriver::reset(_u32 timeout)
+    {
+        return (_lidarDrv)->reset();
+    }
+
+    u_result RPlidarDriver::getAllSupportedScanModes(std::vector<RplidarScanMode>& outModes, _u32 timeoutInMs)
+    {
+        return (_lidarDrv)->getAllSupportedScanModes(outModes, timeoutInMs);
+    }
+
+    u_result RPlidarDriver::getTypicalScanMode(_u16& outMode, _u32 timeoutInMs)
+    {
+        return (_lidarDrv)->getTypicalScanMode(outMode, timeoutInMs);
+    }
+
+    u_result RPlidarDriver::startScan(bool force, bool useTypicalScan, _u32 options, RplidarScanMode* outUsedScanMode)
+    {
+        return (_lidarDrv)->startScan(force, useTypicalScan, options, outUsedScanMode);
+    }
+
+    u_result RPlidarDriver::startScanExpress(bool force, _u16 scanMode, _u32 options, RplidarScanMode* outUsedScanMode, _u32 timeout)
+    {
+        return (_lidarDrv)->startScanExpress(force, scanMode, options, outUsedScanMode, timeout);
+    }
+    
+    u_result RPlidarDriver::getHealth(rplidar_response_device_health_t & health, _u32 timeout)
+    {
+        return (_lidarDrv)->getHealth(health, timeout);
+    }
+
+    u_result RPlidarDriver::getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout)
+    {
+        return (_lidarDrv)->getDeviceInfo(info, timeout);
+    }
+
+    u_result RPlidarDriver::setMotorPWM(_u16 pwm)
+    {
+        return (_lidarDrv)->setMotorSpeed(pwm);
+    }   
+    
+    u_result RPlidarDriver::checkMotorCtrlSupport(bool & support, _u32 timeout)
+    {
+        MotorCtrlSupport motorSupport;
+        u_result ans = (_lidarDrv)->checkMotorCtrlSupport(motorSupport, timeout);
+        if (motorSupport == MotorCtrlSupportNone)
+            support = false;
+        return ans;
+    }
+
+    u_result RPlidarDriver::setLidarIpConf(const rplidar_ip_conf_t& conf, _u32 timeout)
+	{
+		return (_lidarDrv)->setLidarIpConf(conf, timeout);
+	}
+
+    u_result RPlidarDriver::getLidarIpConf(rplidar_ip_conf_t& conf, _u32 timeout)
+    {
+        return (_lidarDrv)->getLidarIpConf(conf, timeout);
+    }
+
+    u_result RPlidarDriver::getDeviceMacAddr(_u8* macAddrArray, _u32 timeoutInMs)
+	{
+		return (_lidarDrv)->getDeviceMacAddr(macAddrArray, timeoutInMs);
+	}
+
+    u_result RPlidarDriver::stop(_u32 timeout) 
+    { 
+        return (_lidarDrv)->stop(timeout);
+    }
+
+    u_result RPlidarDriver::grabScanDataHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count, _u32 timeout)
+    {
+        return (_lidarDrv)->grabScanDataHq(nodebuffer, count, timeout);
+    }
+
+    u_result RPlidarDriver::ascendScanData(rplidar_response_measurement_node_hq_t * nodebuffer, size_t count)
+    {
+        return (_lidarDrv)->ascendScanData(nodebuffer, count);
+    }
+    
+    u_result RPlidarDriver::getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count)
+    {
+        return RESULT_OPERATION_NOT_SUPPORT;
+    }
+
+    u_result RPlidarDriver::getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count)
+    {
+        return (_lidarDrv)->getScanDataWithIntervalHq(nodebuffer, count);
+    }
+
+    u_result RPlidarDriver::startMotor()
+    {
+        return (_lidarDrv)->setMotorSpeed(DEFAULT_MOTOR_SPEED);
+    }
+    u_result RPlidarDriver::stopMotor()
+    {
+        return (_lidarDrv)->setMotorSpeed(0);
+    }
+
+}}}

third_party/rplidar_sdk/src/sdkcommon.h

@@ -0,0 +1,51 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#if defined(_WIN32)
+
+#include "arch/win32/arch_win32.h"
+#elif defined(_MACOS)
+#include "arch/macOS/arch_macOS.h"
+#elif defined(__GNUC__)
+#include "arch/linux/arch_linux.h"
+#else
+#error "unsupported target"
+#endif
+
+#include "hal/types.h"
+#include "hal/assert.h"
+
+#include "rplidar.h"
+
+#include "hal/util.h"

third_party/rplidar_sdk/src/sl_async_transceiver.cpp

@@ -0,0 +1,416 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+
+
+#include "sdkcommon.h"
+#include "hal/abs_rxtx.h"
+#include "hal/thread.h"
+#include "hal/types.h"
+#include "hal/assert.h"
+#include "hal/locker.h"
+#include "hal/socket.h"
+#include "hal/event.h"
+
+#include "sl_async_transceiver.h"
+
+
+
+namespace sl { namespace internal {
+
+
+
+
+ProtocolMessage::ProtocolMessage()
+        : len(0)
+        , cmd(0)
+        , data(NULL)
+        , _databufsize(0)
+		, _usingOutterData(false)
+{
+    _changeBufSize();
+}
+
+ProtocolMessage::ProtocolMessage(_u8 cmd, const void* buffer, size_t size)
+	: len(size)
+	, cmd(cmd)
+	, data(NULL)
+    , _databufsize(0)
+	, _usingOutterData(false)
+{
+    _changeBufSize();
+	if (buffer)
+	{
+		memcpy(data, buffer, size);
+	}
+}
+
+ProtocolMessage::ProtocolMessage(const ProtocolMessage& srcMsg)
+	: len(srcMsg.len)
+	, cmd(srcMsg.cmd)
+	, data(NULL)
+    , _databufsize(0)
+	, _usingOutterData(false)
+{
+    _changeBufSize( true );
+	if (srcMsg.data && len)
+	{
+		memcpy(data, srcMsg.data, len);
+	}
+}
+
+ProtocolMessage::~ProtocolMessage()
+{
+	this->cleanData();
+}
+
+ProtocolMessage& ProtocolMessage::operator =(const ProtocolMessage& srcMessage)
+{
+	this->cleanData();
+
+
+	this->len = srcMessage.len;
+	this->cmd = srcMessage.cmd;
+
+    _changeBufSize( true );
+	if (srcMessage.data && len)
+	{
+		memcpy(data, srcMessage.data, len);
+	}
+
+	return *this;
+}
+
+void ProtocolMessage::setDataBuf(_u8 *buffer, size_t size)
+{
+	this->cleanData();
+
+	len = size;
+	data = buffer;
+    _databufsize = size;
+	_usingOutterData = true;
+}
+
+void ProtocolMessage::fillData(const void * buffer, size_t size)
+{
+	len = size;
+    _changeBufSize();
+    if (buffer)
+	    memcpy(data, buffer, size);
+}
+
+void ProtocolMessage::cleanData()
+{
+	if (data) 
+	{
+		if (!_usingOutterData)
+		{
+			delete [] data;
+		}
+		data = NULL;
+		len = 1;
+        _databufsize = 0;
+	}
+}
+
+void ProtocolMessage::_changeBufSize( bool force_compact)
+{
+    size_t actual_size  = getPayloadSize();
+
+    size_t new_buf_size = actual_size;
+
+
+    if (!_usingOutterData)
+    {
+        // nothing to do
+        if ( new_buf_size == _databufsize ) return;
+
+        if ( new_buf_size < _databufsize){
+
+            if ( (_databufsize >> 1) < new_buf_size)
+            {
+                // reuse the current buffer
+                if (!force_compact) return;
+            }else
+            {
+                // the current buffer size is much bigger, we need to release it to save memory
+            }
+        }
+    }
+
+    // we need to change the buffer
+    cleanData();
+    // the cleanData() will reset the length info, so we need to restore it
+    len = actual_size;
+    data = new _u8[new_buf_size];
+    _databufsize = new_buf_size;
+}
+
+
+AsyncTransceiver::AsyncTransceiver(IAsyncProtocolCodec& codec)
+	: _bindedChannel(NULL)
+	, _codec(codec)
+	, _isWorking(false)
+    , _workingFlag(0)
+{
+
+}
+
+AsyncTransceiver::~AsyncTransceiver()
+{
+    unbindAndClose();
+}
+
+u_result AsyncTransceiver::openChannelAndBind(IChannel* channel)
+{
+    if (!channel) return RESULT_INVALID_DATA;
+
+    unbindAndClose();
+	u_result ans = RESULT_OK;
+	do 
+	{
+		rp::hal::AutoLocker l(_opLocker);
+
+        // try to open the channel ...
+        Result<nullptr_t> ans = SL_RESULT_OK;
+
+        if (!channel->open()) {
+            ans= RESULT_OPERATION_FAIL;
+            break;
+        }
+
+
+        // force a flush to clear any pending data
+        channel->flush();
+
+		_dataEvt.set(false);
+
+		_isWorking = true;
+        _workingFlag = 0;
+        _bindedChannel = channel;
+
+
+		_decoderThread = CLASS_THREAD(AsyncTransceiver, _proc_decoderThread);
+		_rxThread = CLASS_THREAD(AsyncTransceiver, _proc_rxThread);
+
+        
+
+
+	} while (0);
+
+	return ans;
+}
+
+void AsyncTransceiver::unbindAndClose()
+{
+	rp::hal::AutoLocker l(_opLocker);
+	if (!_isWorking) return;
+
+    assert(_bindedChannel);
+
+    
+	_isWorking = false;
+	_dataEvt.set(); // set signal to wake up threads
+
+	_decoderThread.join();
+	_rxThread.join();
+
+
+    _bindedChannel->close();
+
+    _bindedChannel = NULL;
+
+
+    for (std::list< Buffer* >::iterator itr = _rxQueue.begin(); itr != _rxQueue.end(); ++itr)
+    {
+        // Fixed vs upstream (`delete [] *itr;`): the queue holds scalar
+        // `new Buffer()` allocations, so array delete is UB — glibc aborts
+        // with "double free or corruption" when a buffer is still queued at
+        // teardown, i.e. whenever the lidar is closed while scanning.
+        delete *itr;
+    }
+    _rxQueue.clear();
+
+}
+
+u_result AsyncTransceiver::sendMessage(message_autoptr_t& msg)
+{
+    assert(msg);
+
+    if (!_isWorking) return RESULT_OPERATION_NOT_SUPPORT;
+
+    rp::hal::AutoLocker l(_opLocker);
+
+    size_t requiredBufferSize = _codec.estimateLength(msg);
+
+    if (requiredBufferSize == 0) {
+        // nothing to send
+        return RESULT_OK;
+    }
+
+    u_result ans = RESULT_OK;
+
+    _u8* txBuffer = new _u8[requiredBufferSize];
+
+    do {
+  
+        if (!txBuffer) {
+            return RESULT_INSUFFICIENT_MEMORY;
+        }
+
+        _codec.onEncodeData(msg, txBuffer, &requiredBufferSize);
+
+        int txSize = _bindedChannel->write(txBuffer, requiredBufferSize);
+
+        if (txSize < 0) ans = RESULT_OPERATION_FAIL;
+
+    } while (0);
+
+
+    delete[] txBuffer;
+    return ans;
+}
+
+sl_result AsyncTransceiver::_proc_rxThread()
+{
+    assert(_bindedChannel);
+
+    rp::hal::Thread::SetSelfPriority(rp::hal::Thread::PRIORITY_HIGH);
+
+    u_result result;
+    size_t hintedSize = 0;
+    while (_isWorking)
+    {
+        result = _bindedChannel->waitForDataExt(hintedSize, 1000);
+
+        if (IS_FAIL(result))
+        {
+            // timeout is allowed
+            if (result == RESULT_OPERATION_TIMEOUT) {
+                continue;
+            }
+            if (_isWorking) {
+                _workingFlag |= WORKING_FLAG_ERROR;
+                _codec.onChannelError(result);
+                break;
+            }
+        }
+
+        // no data in buffer, sleep and wait for the next round
+        if (!hintedSize)
+        {
+            continue;
+        }
+
+
+        Buffer* decodeBuffer = new Buffer();
+        
+        decodeBuffer->data = new _u8[hintedSize];
+
+        decodeBuffer->size = _bindedChannel->read(decodeBuffer->data, hintedSize);
+#ifdef _DEBUG_DUMP_PACKET
+        printf("Revc: %d\n", decodeBuffer->size);
+#endif
+         
+        if  (!decodeBuffer->size) {
+            delete decodeBuffer;
+
+            
+            _workingFlag |= WORKING_FLAG_ERROR;
+            _codec.onChannelError(RESULT_OPERATION_ABORTED);
+            break;
+        }
+
+        assert(hintedSize >= decodeBuffer->size);
+
+
+#ifdef _DEBUG_DUMP_PACKET
+        printf("=== Dump RX Packet, size = %d ===\n", decodeBuffer->size);
+        for (int pos = 0; pos < decodeBuffer->size; pos++)
+        {
+            printf("%02x ", decodeBuffer->data[pos]);
+        }
+        printf("\n=== END ===\n");
+#endif
+
+        _rxLocker.lock();
+        _rxQueue.push_back(decodeBuffer);
+        _dataEvt.set();
+        _rxLocker.unlock();
+
+
+    }
+    _workingFlag |= WORKING_FLAG_RX_DISABLED;
+    return RESULT_OK;
+}
+
+sl_result AsyncTransceiver::_proc_decoderThread()
+{
+
+    assert(_bindedChannel);
+    rp::hal::Thread::SetSelfPriority(rp::hal::Thread::PRIORITY_HIGH);
+    _codec.onDecodeReset();
+    
+
+    while (_isWorking)
+    {
+        _rxLocker.lock();
+
+        if (_rxQueue.empty())
+        {
+            _rxLocker.unlock();
+
+            if (_dataEvt.wait(1000))
+                continue;
+
+            _rxLocker.lock();
+        }
+        assert(!_rxQueue.empty());
+
+        Buffer * bufferToDecode = _rxQueue.front();
+        _rxQueue.pop_front();
+
+        _rxLocker.unlock();
+
+        //cout<<"decoding "<< bufferToDecode->size <<" bytes of data"<<endl;
+        _codec.onDecodeData(bufferToDecode->data, bufferToDecode->size);
+
+
+        delete bufferToDecode;
+    }
+
+    return RESULT_OK;
+
+}
+
+
+}}

third_party/rplidar_sdk/src/sl_async_transceiver.h

@@ -0,0 +1,167 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+#pragma once
+
+#include <list>
+#include <memory>
+
+namespace sl { namespace internal {
+
+
+class _single_thread ProtocolMessage {
+
+public:
+	size_t len;			
+	_u8 cmd;
+protected:
+	_u8* data;
+	size_t _databufsize;
+
+public:
+	ProtocolMessage();
+	ProtocolMessage(_u8 cmd, const void* buffer, size_t size);
+	ProtocolMessage(const ProtocolMessage& srcMsg);
+	virtual ~ProtocolMessage();
+
+	ProtocolMessage& operator=(const ProtocolMessage& srcMessage);
+
+	// avoid use this method, pls. use fillData instead
+	void setDataBuf(_u8* buffer, size_t size);
+
+	_u8* getDataBuf() { return data; }
+
+	void fillData(const void* buffer, size_t size);
+	void cleanData();
+
+	size_t getPayloadSize() const
+	{
+		return len;
+	}
+
+protected:
+
+	// change the data buffer to fix the new payload size
+	// the existing buffer will be reused if possible.
+	// all the existing payload data will lose
+	void _changeBufSize(bool force_compact = false);
+	bool _usingOutterData;
+};
+
+
+
+typedef std::shared_ptr<ProtocolMessage> message_autoptr_t;
+
+
+class IAsyncProtocolCodec {
+public:
+	IAsyncProtocolCodec() {}
+	virtual ~IAsyncProtocolCodec()  {}
+
+	virtual void   onChannelError(u_result errCode) {}
+
+	virtual void   onDecodeReset() {}
+	virtual void   onDecodeData(const void* buffer, size_t size) = 0;
+
+
+	virtual size_t estimateLength(message_autoptr_t& message) = 0;
+	virtual void   onEncodeData(message_autoptr_t& message, _u8* txbuffer, size_t* size) = 0;
+
+};
+
+class AsyncTransceiver {
+public:
+
+	enum working_flag_t
+	{
+		WORKING_FLAG_RX_DISABLED = 0x1L << 0,
+		WORKING_FLAG_TX_DISABLED = 0x1L << 1,
+
+		WORKING_FLAG_ERROR = 0x1L << 31,
+	};
+
+
+	AsyncTransceiver(IAsyncProtocolCodec& codec);
+	~AsyncTransceiver();
+
+
+
+	u_result openChannelAndBind(IChannel* channel);
+	void     unbindAndClose();
+
+	IChannel* getBindedChannel() const {
+		return _bindedChannel;
+	}
+	
+	u_result sendMessage(message_autoptr_t& msg);
+
+protected:
+
+	sl_result _proc_rxThread();
+	sl_result _proc_decoderThread();
+
+protected:
+
+
+	rp::hal::Locker _opLocker;
+	rp::hal::Locker _rxLocker;
+	rp::hal::Event  _dataEvt;
+
+	IChannel* _bindedChannel;
+	IAsyncProtocolCodec& _codec;
+
+
+	bool _isWorking;
+	_u32 _workingFlag;
+
+	rp::hal::Thread _rxThread;
+	rp::hal::Thread _decoderThread;
+
+	struct Buffer {
+		size_t size;
+		_u8* data;
+
+
+		Buffer() : size(0), data(NULL){}
+
+		~Buffer() {
+			if (data) {
+				delete[] data;
+				data = NULL;
+			}
+		}
+	};
+	std::list< Buffer * > _rxQueue;
+};
+
+
+}}

third_party/rplidar_sdk/src/sl_crc.cpp

@@ -0,0 +1,102 @@
+/*
+ * Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+#include "sl_crc.h"  
+
+namespace sl {namespace crc32 {
+    
+    static sl_u32 table[256];//crc32_table
+    sl_u32 bitrev(sl_u32 input, sl_u16 bw)
+    {
+        sl_u16 i;
+        sl_u32 var;
+        var = 0;
+        for (i = 0; i < bw; i++) {
+            if (input & 0x01) {
+                var |= 1 << (bw - 1 - i);
+            }
+            input >>= 1;
+        }
+        return var;
+    }
+
+    void init(sl_u32 poly)
+    {
+        sl_u16 i;
+        sl_u16 j;
+        sl_u32 c;
+
+        poly = bitrev(poly, 32);
+        for (i = 0; i < 256; i++) {
+            c = i;
+            for (j = 0; j < 8; j++) {
+                if (c & 1)
+                    c = poly ^ (c >> 1);
+                else
+                    c = c >> 1;
+            }
+            table[i] = c;
+        }
+    }
+
+    sl_u32 cal(sl_u32 crc, void* input, sl_u16 len)
+    {
+        sl_u16 i;
+        sl_u8 index;
+        sl_u8* pch;
+        pch = (unsigned char*)input;
+        sl_u8 leftBytes = 4 - (len & 0x3);
+
+        for (i = 0; i < len; i++) {
+            index = (unsigned char)(crc^*pch);
+            crc = (crc >> 8) ^ table[index];
+            pch++;
+        }
+
+        for (i = 0; i < leftBytes; i++) {//zero padding
+            index = (unsigned char)(crc ^ 0);
+            crc = (crc >> 8) ^ table[index];
+        }
+        return crc ^ 0xffffffff;
+    }
+
+    sl_result getResult(sl_u8 *ptr, sl_u32 len) 
+    {
+        static sl_u8 tmp;
+        if (tmp != 1) {
+            init(0x4C11DB7);
+            tmp = 1;
+        }
+
+        return cal(0xFFFFFFFF, ptr, len);
+    }
+}}

third_party/rplidar_sdk/src/sl_lidarprotocol_codec.cpp

@@ -0,0 +1,238 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+
+
+#include "sdkcommon.h"
+#include "hal/byteorder.h"
+#include "hal/abs_rxtx.h"
+#include "hal/thread.h"
+#include "hal/types.h"
+#include "hal/assert.h"
+#include "hal/locker.h"
+#include "hal/socket.h"
+#include "hal/event.h"
+
+#include "sl_lidar_driver.h"
+#include "sl_crc.h" 
+#include <algorithm>
+
+#include "sl_async_transceiver.h"
+#include "sl_lidarprotocol_codec.h"
+
+
+
+namespace sl { namespace internal {
+
+
+
+RPLidarProtocolCodec::RPLidarProtocolCodec()
+    : IAsyncProtocolCodec()
+    , _listener(NULL)
+    , _op_locker(true)
+{
+    onDecodeReset();
+}
+
+void RPLidarProtocolCodec::exitLoopMode() {
+    onDecodeReset();
+}
+
+
+
+void RPLidarProtocolCodec::setMessageListener(IProtocolMessageListener* listener)
+{
+    rp::hal::AutoLocker l(_op_locker);
+    _listener = listener;
+}
+
+size_t RPLidarProtocolCodec::estimateLength(message_autoptr_t& message)
+{
+    size_t actualSize = 2; //1-byte's sync byte, 1-byte's cmd byte
+
+    if (message->cmd & RPLIDAR_CMDFLAG_HAS_PAYLOAD) {
+        actualSize += (message->getPayloadSize() & 0xFF);
+        actualSize += 2; //1-byte for size field, 1-byte for checksum
+    }
+
+    return actualSize;
+}
+
+
+void RPLidarProtocolCodec::onEncodeData(message_autoptr_t& message, _u8* buffer, size_t* size)
+{
+    _u8 checksum = 0;
+    size_t writeSize = std::min<size_t>(*size, estimateLength(message));
+    size_t currentPos = 0;
+
+    while (currentPos < writeSize) {
+        _u8 currentTxByte;
+        switch (currentPos) {
+        case 0: // sync byte
+            currentTxByte = RPLIDAR_CMD_SYNC_BYTE;
+            break;
+        case 1: // cmd byte
+            currentTxByte = message->cmd;
+            break;
+        case 2: // size byte
+            currentTxByte = (_u8)message->getPayloadSize();
+            break;
+        default:
+        {
+            size_t payloadPos = currentPos - 3;
+            if (payloadPos == message->getPayloadSize()) {
+                // checksum byte
+                currentTxByte = checksum;
+                assert(currentPos + 1 == writeSize);
+            }
+            else {
+                // payload
+                currentTxByte = message->getDataBuf()[payloadPos];
+            }
+        }
+        }
+
+
+        checksum ^= currentTxByte;
+        buffer[currentPos++] = currentTxByte;
+    } while (0);
+
+    *size = currentPos;
+}
+
+void   RPLidarProtocolCodec::onDecodeReset() {
+    rp::hal::AutoLocker autolock(_op_locker);
+    // flush the pending data
+    _decodingMessage.cleanData();
+    // reset to initial state
+    _rx_pos = 0;
+    _working_states = STATUS_WAIT_SYNC1;
+}
+
+
+void RPLidarProtocolCodec::onDecodeData(const void* buffer, size_t size)
+{
+    rp::hal::AutoLocker autolock(_op_locker);
+
+    const _u8* data = reinterpret_cast<const _u8*>(buffer);
+    const _u8* dataEnd = data + size;
+
+
+    while (data != dataEnd) {
+        _u8 currentByte = *data;
+        ++data;
+
+        switch (_working_states & ((_u32)STATUS_LOOP_MODE_FLAG - 1)) {
+        case STATUS_WAIT_SYNC1:
+            if (currentByte == RPLIDAR_ANS_SYNC_BYTE1) {
+                _working_states = STATUS_WAIT_SYNC2;
+            }
+            break;
+        case STATUS_WAIT_SYNC2:
+            if (currentByte == RPLIDAR_ANS_SYNC_BYTE2) {
+                _working_states = STATUS_WAIT_SIZE_FLAG;
+                _rx_pos = 0; // init rx pos for recv size and flag
+            }
+            else {
+                // reset to the initial state
+                _working_states = STATUS_WAIT_SYNC1;
+            }
+            break;
+        case STATUS_WAIT_SIZE_FLAG:
+        {
+            assert(sizeof(_decodingMessage.len) >= 4);
+            _u8* byteArr = reinterpret_cast<_u8*>(&_decodingMessage.len);
+            byteArr[_rx_pos++] = currentByte;
+
+            if (_rx_pos == 4) {
+                _working_states = STATUS_WAIT_TYPE;
+                _decodingMessage.len = le32_to_cpu(_decodingMessage.len);
+
+                // 30bit size + 2bit flag has been received
+                _u32 flagbits = (_u32)(_decodingMessage.len >> RPLIDAR_ANS_HEADER_SUBTYPE_SHIFT);
+                if (flagbits & RPLIDAR_ANS_PKTFLAG_LOOP) {
+                    _working_states |= STATUS_LOOP_MODE_FLAG;
+                }
+                _decodingMessage.len = (_decodingMessage.len & RPLIDAR_ANS_HEADER_SIZE_MASK);
+                // alloc buffer
+                _decodingMessage.fillData(NULL, _decodingMessage.getPayloadSize());
+                _rx_pos = 0;
+            }
+        }
+        break;
+        case STATUS_WAIT_TYPE:
+            // save the type field as a cmd 
+            _decodingMessage.cmd = currentByte;
+
+            // recv payload...
+            _working_states = (_working_states & STATUS_LOOP_MODE_FLAG)
+                | STATUS_RECV_PAYLOAD;
+
+            if (!_decodingMessage.getPayloadSize()) {
+                // zero payload packet? 
+                _working_states = STATUS_WAIT_SYNC1;
+            }
+            break;
+        case STATUS_RECV_PAYLOAD:
+            _decodingMessage.getDataBuf()[_rx_pos++] = currentByte;
+
+            if ((size_t)_rx_pos == _decodingMessage.getPayloadSize()) {
+                if (_working_states & STATUS_LOOP_MODE_FLAG) {
+                    // rewind to the payload recv status in loop mode
+                    _rx_pos = 0;
+                }
+                else {
+                    // reset the decoder
+                    _working_states = STATUS_WAIT_SYNC1;
+                }
+
+                IProtocolMessageListener* cachedLister = _listener;
+
+                autolock.forceUnlock(); //unlock the oplock to prevent deadlock
+
+
+                if (cachedLister) {
+                    cachedLister->onProtocolMessageDecoded(_decodingMessage);
+                }
+
+                _op_locker.lock(); // relock it
+            }
+            break;
+        }
+
+    }
+}
+
+
+
+
+}}

third_party/rplidar_sdk/src/sl_lidarprotocol_codec.h

@@ -0,0 +1,88 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+
+#pragma once
+
+#include "sl_async_transceiver.h"
+
+namespace sl { namespace internal {
+
+
+class IProtocolMessageListener {
+public:
+    virtual void onProtocolMessageDecoded(const ProtocolMessage&) = 0;
+};
+
+
+class RPLidarProtocolCodec : public IAsyncProtocolCodec
+{
+public:
+
+    enum {
+        STATUS_WAIT_SYNC1 = 0x0,
+        STATUS_WAIT_SYNC2 = 0x1,
+        STATUS_WAIT_SIZE_FLAG = 0x2,
+        STATUS_WAIT_TYPE = 0x3,
+        STATUS_RECV_PAYLOAD = 0x4,
+        STATUS_LOOP_MODE_FLAG = 0x80000000,
+    };
+
+    RPLidarProtocolCodec();
+
+    void exitLoopMode();
+
+
+    virtual size_t estimateLength(message_autoptr_t& message);
+
+
+    virtual void onEncodeData(message_autoptr_t& message, _u8* txbuffer, size_t* size);
+
+    virtual void   onDecodeReset();
+    virtual void   onDecodeData(const void* buffer, size_t size);
+    
+    void setMessageListener(IProtocolMessageListener* l);
+
+protected:
+
+    IProtocolMessageListener* _listener;
+    ProtocolMessage          _decodingMessage;
+    rp::hal::Locker          _op_locker;
+                            
+    _u32                     _working_states;
+    int                      _rx_pos;
+};
+
+}}
+
+
+

third_party/rplidar_sdk/src/sl_serial_channel.cpp

@@ -0,0 +1,146 @@
+/*
+ * Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+#include "sl_lidar_driver.h"
+#include "hal/abs_rxtx.h"
+#include "hal/socket.h"
+
+
+namespace sl {
+    
+    class SerialPortChannel : public ISerialPortChannel
+    {
+    public:
+        SerialPortChannel(const std::string& device, int baudrate) :_rxtxSerial(rp::hal::serial_rxtx::CreateRxTx())
+        {
+            _device = device;
+            _baudrate = baudrate;
+        }
+
+        ~SerialPortChannel()
+        {
+            if (_rxtxSerial)
+                delete _rxtxSerial;
+        }
+
+        bool bind(const std::string& device, sl_s32 baudrate)
+        {
+            _closePending = false;
+            return _rxtxSerial->bind(device.c_str(), baudrate);
+        }
+
+        bool open()
+        {
+            if(!bind(_device, _baudrate))
+                return false;
+            return _rxtxSerial->open();
+        }
+
+        void close()
+        {
+            _closePending = true;
+            _rxtxSerial->cancelOperation();
+            _rxtxSerial->close();
+        }
+        void flush()
+        {
+            _rxtxSerial->flush(0);
+        }
+
+        sl_result waitForDataExt(size_t& size_hint, sl_u32 timeoutInMs)
+        {
+            _word_size_t result;
+            size_t size_holder;
+            size_hint = 0;
+
+            if (_closePending) return  RESULT_OPERATION_TIMEOUT;
+
+            if (!_rxtxSerial->isOpened()) {
+                return RESULT_OPERATION_FAIL;
+            }
+
+            result = _rxtxSerial->waitfordata(1, timeoutInMs, &size_holder);
+            size_hint = size_holder;
+            if (result == (_word_size_t)rp::hal::serial_rxtx::ANS_DEV_ERR)
+                return RESULT_OPERATION_FAIL;
+            if (result == (_word_size_t)rp::hal::serial_rxtx::ANS_TIMEOUT)
+                return RESULT_OPERATION_TIMEOUT;
+
+            return RESULT_OK;
+        }
+
+        bool waitForData(size_t size, sl_u32 timeoutInMs, size_t* actualReady)
+        {
+            if (_closePending) return false;
+            return (_rxtxSerial->waitfordata(size, timeoutInMs, actualReady) == rp::hal::serial_rxtx::ANS_OK);
+        }
+
+        int write(const void* data, size_t size)
+        {
+           return _rxtxSerial->senddata((const sl_u8 * )data, size);
+        }
+
+        int read(void* buffer, size_t size)
+        {
+            size_t lenRec = 0;
+            lenRec = _rxtxSerial->recvdata((sl_u8 *)buffer, size);
+            return (int)lenRec;
+        }
+
+        void clearReadCache()
+        {
+           
+        }
+
+        void setDTR(bool dtr)
+        {
+            dtr ? _rxtxSerial->setDTR() : _rxtxSerial->clearDTR();
+        }
+
+        int getChannelType() {
+            return CHANNEL_TYPE_SERIALPORT;
+        }
+
+    private:
+        rp::hal::serial_rxtx  * _rxtxSerial;
+        bool _closePending;
+        std::string _device;
+        int _baudrate;
+
+    };
+
+    Result<IChannel*> createSerialPortChannel(const std::string& device, int baudrate)
+    {
+        return new  SerialPortChannel(device, baudrate);
+    }
+
+}

third_party/rplidar_sdk/src/sl_tcp_channel.cpp

@@ -0,0 +1,124 @@
+/*
+ * Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+#include "sl_lidar_driver.h"
+#include "hal/abs_rxtx.h"
+#include "hal/socket.h"
+
+
+namespace sl {
+    
+    class TcpChannel : public IChannel
+    {
+    public:
+        TcpChannel(const std::string& ip, int port) : _binded_socket(rp::net::StreamSocket::CreateSocket()) {
+            _ip = ip;
+            _port = port;
+        }
+
+        bool bind(const std::string & ip, sl_s32 port)
+        {
+            _socket = rp::net::SocketAddress(ip.c_str(), port);
+            return true;
+        }
+
+        bool open()
+        {
+            if(!bind(_ip, _port))
+                return false;
+            return IS_OK(_binded_socket->connect(_socket));
+            
+        }
+
+        void close()
+        {
+            _binded_socket->dispose();
+            _binded_socket = NULL;
+        }
+        void flush()
+        {
+        
+        }
+
+        sl_result waitForDataExt(size_t& size_hint, sl_u32 timeoutInMs)
+        {
+            u_result ans;
+            size_hint = 0;
+            ans = _binded_socket->waitforData(timeoutInMs);
+
+            switch (ans) {
+            case RESULT_OK:
+                size_hint = 1024; //dummy value
+                break;
+            }
+
+            return ans;
+        }
+
+        bool waitForData(size_t size, sl_u32 timeoutInMs, size_t* actualReady)
+        {
+            if (actualReady)
+                *actualReady = size;
+            return (_binded_socket->waitforData(timeoutInMs) == RESULT_OK);
+
+        }
+
+        int write(const void* data, size_t size)
+        {
+            return _binded_socket->send(data, size);
+        }
+
+        int read(void* buffer, size_t size)
+        {
+            size_t lenRec = 0;
+            _binded_socket->recv(buffer, size, lenRec);
+            return (int)lenRec;
+        }
+
+        void clearReadCache() {}
+
+        void setStatus(_u32 flag){}
+
+        int getChannelType() {
+            return CHANNEL_TYPE_TCP;
+        }
+    private:
+        rp::net::StreamSocket * _binded_socket;
+        rp::net::SocketAddress _socket;
+        std::string _ip;
+        int _port;
+    };
+    Result<IChannel*> createTcpChannel(const std::string& ip, int port)
+    {
+        return new  TcpChannel(ip, port);
+    }
+}

third_party/rplidar_sdk/src/sl_udp_channel.cpp

@@ -0,0 +1,129 @@
+/*
+ * Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+#include "sl_lidar_driver.h"
+#include "hal/abs_rxtx.h"
+#include "hal/socket.h"
+
+
+namespace sl {
+	class UdpChannel : public IChannel
+	{
+	public:
+		UdpChannel(const std::string& ip, int port) : _binded_socket(rp::net::DGramSocket::CreateSocket()) {
+            _ip = ip;
+            _port = port;
+        }
+
+		bool bind(const std::string & ip, sl_s32 port)
+        {
+            _socket = rp::net::SocketAddress(ip.c_str(), port);
+            return true;
+        }
+
+        bool open()
+        {
+            if(!bind(_ip, _port))
+                return false;
+            return SL_IS_OK(_binded_socket->setPairAddress(&_socket));         
+        }
+
+        void close()
+        {
+            _binded_socket->dispose();
+            _binded_socket = NULL;
+        }
+        void flush()
+        {
+            clearReadCache();
+        }
+
+        sl_result waitForDataExt(size_t& size_hint, sl_u32 timeoutInMs)
+        {
+            u_result ans;
+            size_hint = 0;
+            ans = _binded_socket->waitforData(timeoutInMs);
+
+            switch (ans) {
+            case RESULT_OK:
+                size_hint = 1024; //dummy value
+                break;
+            }
+
+            return ans;
+        }
+
+        bool waitForData(size_t size, sl_u32 timeoutInMs, size_t* actualReady)
+        {
+            if (actualReady)
+                *actualReady = size;
+            return (_binded_socket->waitforData(timeoutInMs) == RESULT_OK);
+
+        }
+
+        int write(const void* data, size_t size)
+        {
+            return _binded_socket->sendTo(nullptr, data, size);
+        }
+
+        int read(void* buffer, size_t size)
+        {
+            size_t actualGet;
+
+            u_result ans = _binded_socket->recvFrom(buffer, size, actualGet);
+            if (IS_FAIL(ans)) return 0;
+            return actualGet;
+        
+        }
+
+        void clearReadCache() {
+            _binded_socket->clearRxCache();
+        }
+
+        void setStatus(_u32 flag){}
+        
+        int getChannelType() {
+            return CHANNEL_TYPE_UDP;
+        }
+
+	private:
+		rp::net::DGramSocket * _binded_socket;
+		rp::net::SocketAddress _socket;
+        std::string _ip;
+        int _port;
+	};
+
+    Result<IChannel*> createUdpChannel(const std::string& ip, int port)
+    {
+        return new  UdpChannel(ip, port);
+    }
+}