diff --git a/CMakeLists.txt b/CMakeLists.txt index e34fa97..f0e1fdc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ -cmake_minimum_required(VERSION 3.10) -project(lidarlib VERSION 1.0.0 LANGUAGES CXX) +cmake_minimum_required(VERSION 3.16) +project(xlidar_driver VERSION 2.0.0 LANGUAGES CXX) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) @@ -9,77 +9,48 @@ endif() find_package(Threads REQUIRED) -option(BUILD_SHARED_LIBS "Build shared (.so) libraries instead of static" ON) +option(XLIDAR_BUILD_EXAMPLES "Build example/demo binaries" ON) -set(LIDARLIB_SOURCES - src/olei_lidar.cpp - src/sick_lidar.cpp - src/espe_lidar.cpp - src/lidar_config.cpp -) +# Where the rplidar plugin finds the Slamtec SDK sources (include/ + src/). +# Empty + not auto-detected -> the rplidar plugin is skipped with a warning. +set(XLIDAR_RPLIDAR_SDK_DIR "" CACHE PATH "Path to the Slamtec RPLIDAR SDK (dir containing include/ and src/)") -add_library(lidarlib ${LIDARLIB_SOURCES}) -set_target_properties(lidarlib PROPERTIES - VERSION ${PROJECT_VERSION} - SOVERSION ${PROJECT_VERSION_MAJOR} - POSITION_INDEPENDENT_CODE ON -) -target_include_directories(lidarlib PUBLIC - $ - $ -) -target_link_libraries(lidarlib PUBLIC Threads::Threads) +add_subdirectory(src) +add_subdirectory(plugins) -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(espe_example examples/espe_example.cpp) - target_link_libraries(espe_example PRIVATE lidarlib) - - add_executable(lidar_app examples/lidar_app.cpp) - target_link_libraries(lidar_app PRIVATE lidarlib) +if(XLIDAR_BUILD_EXAMPLES) + add_subdirectory(examples) endif() -# install + find_package() support +# ── install + find_package() support ───────────────────────────────────────── include(GNUInstallDirs) include(CMakePackageConfigHelpers) -install(TARGETS lidarlib - EXPORT lidarlibTargets +install(TARGETS lidar_manager + 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 - FILE lidarlibTargets.cmake - NAMESPACE lidarlib:: - DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/lidarlib +install(EXPORT xlidar_driverTargets + FILE xlidar_driverTargets.cmake + NAMESPACE xlidar:: + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/xlidar_driver ) configure_package_config_file( - cmake/lidarlibConfig.cmake.in - ${CMAKE_CURRENT_BINARY_DIR}/lidarlibConfig.cmake - INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/lidarlib + cmake/xlidar_driverConfig.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/xlidar_driverConfig.cmake + 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}/lidarlibConfigVersion.cmake - DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/lidarlib + ${CMAKE_CURRENT_BINARY_DIR}/xlidar_driverConfig.cmake + ${CMAKE_CURRENT_BINARY_DIR}/xlidar_driverConfigVersion.cmake + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/xlidar_driver ) diff --git a/README.md b/README.md index 1597813..288fe31 100644 --- a/README.md +++ b/README.md @@ -1,125 +1,155 @@ -# Lidarlib +# xlidar-driver -Thư viện C++17 thu nhận dữ liệu lidar 2D cho **OLEI** (UDP), **SICK** -(TCP/UDP) và **ESPE** (TCP/UDP) +SDK driver lidar 2D cho Linux, kiến trúc **plugin nạp động**: mỗi driver là +một file `.so` độc lập, cùng implement một interface chung +`xlidar::LidarDriverInterface`; host chỉ cần facade +**`xlidar::LidarManager`** (build ra `liblidar_manager.so`) để khám phá +plugin, đọc metadata và tạo instance thiết bị. -Mọi driver cùng implement một interface `lidarlib::Lidar`, khởi tạo qua một -factory duy nhất `lidarlib::make_lidar()`, output thống nhất theo định dạng -ROS `sensor_msgs/LaserScan`. +Driver đi kèm: **Slamtec RPLIDAR** (serial), **OLEI** (UDP), **SICK TiM** +(TCP/CoLa-A), **SICK nanoScan3** (UDP safety), **ESPE LGA60** (TCP/UDP). +Output thống nhất theo định dạng ROS `sensor_msgs/LaserScan`. -## Tính năng +## Kiến trúc -- **Đa hãng, một API** — OLEI (Family A/B/C), SICK (TiM 5xx/7xx, nanoScan3) - và ESPE (LGA60) dùng chung interface: `open()` / `recv_scan()` / callback / - `close()`. -- **Tự nhận diện giao thức** — phân biệt họ giao thức OLEI theo frame ID từng - gói; chế độ `AUTO` tự dò model từ dữ liệu (Family B/C). -- **Chẩn đoán thiết bị** — đọc trạng thái tự chẩn đoán nhúng trong stream: - lỗi motor/điện áp/nhiệt độ (OLEI), kính bẩn/pollution (SICK TiM), - contamination/manipulation (nanoScan3). -- **Xử lý lỗi tường minh** — `ErrorCode` phân loại từ `errno` thật; - lifecycle an toàn với mọi thứ tự gọi `open()`/`close()`. -- **Đa luồng an toàn** — mỗi instance độc lập hoàn toàn, chạy mỗi lidar một - thread không cần khóa. -- **Cấu hình JSON** — khai báo danh sách lidar trong `config.json`, - load/save bằng API kèm sẵn. +``` +xlidar_driver/ +├── CMakeLists.txt +├── include/ # API public — host chỉ include từ đây +│ ├── lidar_interface.hpp # LidarDriverInterface, DriverInfo, DeviceConfig, +│ │ # LaserScan/ScanResult, ErrorCode, plugin ABI +│ ├── lidar_diagnostics.hpp # Diagnostics + bit lỗi từng hãng +│ └── lidar_manager.hpp # LidarManager, PluginRegistry, config.json +├── src/ +│ ├── CMakeLists.txt # → liblidar_manager.so +│ ├── lidar_manager.cpp # dlopen/dlsym + load/save config.json +│ └── json_mini.hpp +├── plugins/ # mỗi thư mục → một plugin .so +│ ├── CMakeLists.txt # helper xlidar_add_plugin() +│ ├── common/plugin_helpers.hpp +│ ├── driver_rplidar/ # → driver_rplidar.so (rplidar_c1_driver) +│ ├── driver_olei/ # → driver_olei.so (olei_lidar_driver) +│ ├── driver_sick_code/ # → driver_sick_code.so (sick_tim_driver) +│ ├── driver_sick_safety/ # → driver_sick_safety.so (sick_nanoscan3_driver) +│ └── driver_espe/ # → driver_espe.so (espe_lga60_driver) +├── examples/ # list_drivers, example, lidar_app +└── docs/diagnostics.md # nghiên cứu layout dữ liệu chẩn đoán +``` + +- **LidarManager** quét thư mục plugin, `dlopen` từng `.so`, resolve hai + entry point C rồi đăng ký vào `available_drivers()` — map + `` trong đó `PluginRegistry = {DriverInfo, + file_path}`. +- **Plugin ABI** — mỗi plugin export đúng hai symbol C: + + ```cpp + XLIDAR_PLUGIN_EXPORT void get_driver_info(xlidar::DriverInfo* out); + XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface* + create_driver_instance(const xlidar::DeviceConfig* cfg); + ``` + +- **DriverInfo** — plugin tự đăng ký danh tính: `vendor`, `model` (dòng + thiết bị driver phụ trách — một driver có thể cover cả series), `driver_id` + (định danh duy nhất, ví dụ `rplidar_c1_driver`), `description` (mô tả ngắn: + thiết bị hỗ trợ, transport, ghi chú), kèm metadata cho UI: `transport` + (`serial`/`udp`/`tcp`), `transport_selectable`, `supported_models`. +- Plugin được giữ nguyên trong bộ nhớ tới khi manager bị hủy — **manager + phải sống lâu hơn mọi instance nó tạo ra**. ## Cài đặt -Yêu cầu: Linux, CMake ≥ 3.10, trình dịch C++17. Không có dependency ngoài -(chỉ pthread). +Yêu cầu: Linux, CMake ≥ 3.16, C++17. Không có dependency ngoài (pthread, +dl). Plugin rplidar cần thêm source SDK của Slamtec. ```bash -cmake -S . -B build -DCMAKE_BUILD_TYPE=Release +cmake -S . -B build -DCMAKE_BUILD_TYPE=Release \ + -DXLIDAR_RPLIDAR_SDK_DIR=/path/to/rplidar_sdk # dir chứa include/ + src/ cmake --build build -j"$(nproc)" -cmake --install build --prefix "$HOME/.local" # hoặc sudo với /usr/local ``` -Tùy chọn CMake: `-DLIDARLIB_BUILD_EXAMPLES=OFF` (tắt binary demo), -`-DBUILD_SHARED_LIBS=OFF` (build static). +Kết quả: `build/src/liblidar_manager.so`, plugin trong `build/plugins/*.so`, +demo trong `build/examples/`. Không truyền `XLIDAR_RPLIDAR_SDK_DIR` thì các +vị trí quen thuộc được tự dò (`third_party/rplidar_sdk`, +`../rplidar_sdk/sdk`, `../xloc-monorepo/xlocd/deps/rplidar_sdk`); không thấy +SDK thì plugin rplidar bị bỏ qua, phần còn lại build bình thường. -Dùng từ project khác: - -```cmake -find_package(lidarlib REQUIRED) -target_link_libraries(my_app PRIVATE lidarlib::lidarlib) -``` +Tùy chọn: `-DXLIDAR_BUILD_EXAMPLES=OFF`. Hỗ trợ `cmake --install` + +`find_package(xlidar_driver)` (target `xlidar::lidar_manager`). ## Sử dụng -### Đọc scan +### Khám phá driver và đọc scan ```cpp -#include "lidarlib/lidarlib.hpp" // toàn bộ API trong một include +#include "lidar_manager.hpp" // kéo theo lidar_interface.hpp -lidarlib::LidarConfig c{"front", "192.168.1.10", 2368, "AUTO", false, "OLEI"}; -std::unique_ptr lidar = lidarlib::make_lidar(c); +xlidar::LidarManager manager("plugins"); +manager.load_all_plugins(); -if (lidar->open() != lidarlib::ErrorCode::Ok) { - fprintf(stderr, "open: %s\n", lidarlib::to_string(lidar->last_error())); +for (const auto& [id, plugin] : manager.available_drivers()) + printf("%s — %s\n", id.c_str(), plugin.info.description.c_str()); + +xlidar::DeviceConfig cfg; +cfg.driver_id = "sick_tim_driver"; +cfg.model = "SICK-TIM7xx"; +cfg.ip = "192.168.0.1"; // port 0 = port mặc định của driver + +auto lidar = manager.create_lidar_device(cfg); // nullptr nếu driver_id lạ +if (lidar->open() != xlidar::ErrorCode::Ok) { + fprintf(stderr, "open: %s\n", xlidar::to_string(lidar->last_error())); return 1; } -lidarlib::ScanResult r; +xlidar::ScanResult r; if (lidar->recv_scan(r, 1000)) { // r.scan : LaserScan — điểm đo, format ROS // r.info : ExtraInfo — metadata tuỳ model -} else { - // Timeout / DeviceDisconnected — xem lidar->last_error() } ``` -Có thể khởi tạo driver trực tiếp không qua factory: - -```cpp -lidarlib::Driver olei(lidarlib::MODEL_AUTO, "192.168.100.100", 2368); -lidarlib::SickDriver tim (lidarlib::MODEL_SICK_TIM571, "192.168.0.1", 2111); -lidarlib::NanoScanDriver nano(lidarlib::MODEL_SICK_NANOSCAN3, "0.0.0.0", 6060); -lidarlib::EspeDriver espe(lidarlib::MODEL_ESPE_LGA60, "192.168.1.88", 8080); -``` +Driver serial (rplidar) dùng `cfg.serial_port` + `cfg.baudrate` thay cho +`ip`/`port`. Xem `examples/example.cpp`. ### Chế độ callback -Thay cho `recv_scan()` blocking: - ```cpp -lidar->set_scan_callback([](const lidarlib::ScanResult& r) { /* mỗi vòng quét */ }); +lidar->set_scan_callback([](const xlidar::ScanResult& r) { /* mỗi vòng quét */ }); while (running) lidar->spin_once(); ``` -### Chẩn đoán thiết bị +Callback chỉ phát từ `spin_once()` — chọn một kiểu bơm dữ liệu: +`recv_scan()` (poll) hoặc callback + `spin_once()`. -Thiết bị nhúng thông tin tự chẩn đoán trong stream dữ liệu; driver decode -sẵn qua `get_diagnostics()` (chi tiết layout từng giao thức: -[docs/diagnostics.md](docs/diagnostics.md)): +### Sẵn sàng & chẩn đoán thiết bị ```cpp -lidarlib::Diagnostics d = lidar->get_diagnostics(); -if (!d.valid) { - // chưa decode được vòng quét nào -} else if (d.has_fault()) { - // OLEI Family A - d.monitor_fault(); // motor/giám sát bất thường - d.voltage_fault(); // điện áp ngoài dải - d.temperature_fault(); // nhiệt độ bất thường - // SICK - d.sick_error(); // TiM: device error - d.pollution_error(); // TiM: kính bẩn nặng +if (!lidar->wait_ready(5000)) { /* chưa có scan sạch nào trong 5s */ } + +xlidar::Diagnostics d = lidar->get_diagnostics(); +if (d.has_fault()) { + d.monitor_fault(); // OLEI: motor/giám sát + d.voltage_fault(); // OLEI: điện áp + d.temperature_fault(); // OLEI: nhiệt độ + d.sick_error(); // SICK TiM: device error + d.pollution_error(); // SICK TiM: kính bẩn nặng d.contamination_error(); // nanoScan3: kính bẩn nặng d.manipulation(); // nanoScan3: nghi bị che/can thiệp - printf("fault: %s\n", lidarlib::to_string(d).c_str()); + d.espe_fault(); // ESPE: từ lỗi thiết bị + d.rplidar_fault(); // RPLIDAR: health = Error (kèm rplidar_error_code) + printf("fault: %s\n", xlidar::to_string(d).c_str()); } else if (d.has_warning()) { - // pollution_warning() / contamination_warning() — kính bẩn nhẹ, nên lau + // pollution/contamination warning, rplidar health warning } ``` -`d.healthy()` = đã có dữ liệu và không có fault. Khuyến nghị giám sát: cảm -biến khỏe khi và chỉ khi `recv_scan()` thành công đều đặn **và** -`get_diagnostics().has_fault() == false`. +`d.healthy()` = đã có dữ liệu và không fault. `is_ready(max_age_ms)` = đang +mở + healthy + scan mới nhất chưa quá hạn — diagnostics chỉ refresh qua +`recv_scan()`/`spin_once()`, nên gọi từ chính thread bơm dữ liệu. Layout +chẩn đoán từng giao thức: [docs/diagnostics.md](docs/diagnostics.md). ### Xử lý lỗi & lifecycle -`open()` trả về `ErrorCode`; `last_error()` giữ kết quả của lần gọi gần nhất. +`open()` trả về `ErrorCode`; `last_error()` giữ kết quả gần nhất. | ErrorCode | Ý nghĩa | |---|---| @@ -127,166 +157,109 @@ biến khỏe khi và chỉ khi `recv_scan()` thành công đều đặn **và** | `AlreadyOpen` | `open()` khi đang mở — kết nối cũ giữ nguyên | | `NotOpen` | `recv_scan()`/`spin_once()` khi chưa `open()` | | `InvalidAddress` | Chuỗi IP không hợp lệ | -| `PortInUse` | Port local đã bị chiếm | -| `BindFailed` / `SocketError` | Lỗi bind khác / không tạo được socket | -| `ConnectionRefused` / `ConnectionFailed` / `Timeout` | TCP connect bị từ chối / không tới được / quá thời hạn | -| `HandshakeFailed` | TCP nối được nhưng lệnh start-stream thất bại | +| `PortInUse` / `BindFailed` / `SocketError` | Lỗi bind/socket (driver UDP) | +| `ConnectionRefused` / `ConnectionFailed` / `Timeout` | TCP connect bị từ chối / không tới được / quá hạn | +| `HandshakeFailed` | Nối được nhưng lệnh start-stream/start-scan thất bại | +| `SerialError` | Cổng serial không tồn tại / không mở được (rplidar) | +| `DeviceError` | Thiết bị tự báo fault (rplidar health check lúc `open()`) | | `DeviceDisconnected` | Thiết bị đóng kết nối / lỗi recv giữa chừng | +| `InvalidConfig` | `DeviceConfig` không dùng được với driver | -Lifecycle an toàn với mọi thứ tự gọi: `close()` là idempotent, `open()` lặp -trả `AlreadyOpen` và không đụng kết nối đang chạy, sau `close()` có thể -`open()` lại (state được reset). +Lifecycle an toàn với mọi thứ tự gọi: `close()` idempotent, `open()` lặp trả +`AlreadyOpen`, sau `close()` mở lại được. Mỗi instance độc lập hoàn toàn — +mỗi lidar một thread, không cần khóa. ### Cấu hình JSON -`lidar_app` (binary demo) đọc `config.json`, mở từng lidar một thread: +`lidar_app` (binary demo) đọc `config.json`, mở mỗi lidar một thread; API: +`xlidar::load_config(path)` / `save_config(path, cfg)` / +`manager.create_from_config_file(path)`. ```json { "lidars": [ - {"name":"front", "ip":"192.168.100.100", "port":2368, "brand":"OLEI", "model":"AUTO", "inverted":false}, - {"name":"rear", "ip":"192.168.100.100", "port":2369, "brand":"OLEI", "model":"AUTO", "inverted":true}, - {"name":"sick1", "ip":"192.168.0.1", "port":2111, "brand":"SICK", "model":"SICK-TIM571"}, - {"name":"nano1", "ip":"0.0.0.0", "port":6060, "brand":"SICK", "model":"SICK-nanoScan3"}, - {"name":"espe1", "ip":"192.168.1.88", "port":8080, "brand":"ESPE", "model":"ESPE-LGA60"} + {"name":"front", "driver_id":"olei_lidar_driver", "model":"AUTO", "ip":"192.168.100.100", "port":2368}, + {"name":"sick1", "driver_id":"sick_tim_driver", "model":"SICK-TIM7xx", "ip":"192.168.0.1", "port":2111}, + {"name":"nano1", "driver_id":"sick_nanoscan3_driver","model":"SICK-nanoScan3", "ip":"0.0.0.0", "port":6060}, + {"name":"espe1", "driver_id":"espe_lga60_driver", "model":"ESPE-LGA60", "ip":"192.168.1.88", "port":8080}, + {"name":"rp1", "driver_id":"rplidar_c1_driver", "model":"AUTO", "serial_port":"/dev/ttyUSB0", "baudrate":460800} ] } ``` +Các trường `DeviceConfig` (mọi trường có default, chỉ khai báo cái cần): + | Trường | Ý nghĩa | |---|---| -| `brand` | `"OLEI"` (mặc định), `"SICK"` hoặc `"ESPE"` | -| `model` | Tên trong bảng model bên dưới; tên lạ → mặc định của hãng | -| `inverted` | `true` nếu lidar lắp úp ngược — driver tự đảo góc (mọi hãng) | -| `use_udp` | Chỉ ESPE: `true` để dùng transport UDP thay vì TCP | -| `angle_min_deg` / `angle_max_deg` | Tuỳ chọn: remap tuyến tính góc output sang cửa sổ này (không cắt điểm). Bỏ trống = tắt | +| `driver_id` | Plugin phụ trách thiết bị (bắt buộc) | +| `model` | Một trong `supported_models` của driver; tên lạ → mặc định của driver | +| `ip` / `port` | Driver mạng: UDP = địa chỉ bind cục bộ, TCP = địa chỉ thiết bị; `port` 0 = mặc định của driver | +| `serial_port` / `baudrate` | Driver serial (rplidar); mặc định `/dev/ttyUSB0` @ 460800 | +| `inverted` | `true` nếu lidar lắp úp ngược — driver tự đảo góc | +| `use_udp` | Chỉ driver `transport_selectable` (ESPE): chuyển sang UDP | +| `angle_min_deg` / `angle_max_deg` | Cửa sổ FOV hợp lệ (hệ góc có dấu, 0 = phía trước): điểm ngoài cửa sổ thành NaN. Bỏ trống = tắt | +| `range_min_m` / `range_max_m` | Ghi đè dải đo; 0 = theo model | +| `remap_angle_min_deg` / `remap_angle_max_deg` | (legacy) remap tuyến tính góc output, không cắt điểm | +| `extra` | map chuỗi→chuỗi cho tuỳ chọn riêng của driver | -Đọc/ghi bằng `lidarlib::load_config(path)` / `lidarlib::save_config(path, cfg)`. +File format cũ của lidarlib (`{"brand": "OLEI", ...}`) được migrate tự động +khi load: `brand`+`model` → `driver_id`, cặp `angle_*_deg` cũ → remap. -## Model hỗ trợ +## Driver đi kèm -### OLEI (UDP, port mặc định 2368) +| driver_id | Plugin | Vendor / dòng máy | Transport | Model hỗ trợ | Ghi chú | +|---|---|---|---|---|---| +| `rplidar_c1_driver` | `driver_rplidar.so` | Slamtec RPLIDAR | serial | `AUTO`, `C1` | Build trên SDK vendor; default C1 @ 460800; A/S series dùng được với baud tương ứng. Health check lúc `open()`, model/firmware tự nhận | +| `olei_lidar_driver` | `driver_olei.so` | OLEI 2D | udp | `AUTO`, `VB`, `VF`, `LR-1F`, `LR-1FMI`, `LR-1BS5`, `LR-16F`, `GS1-5` | Tự nhận diện giao thức Family A/B/C theo frame ID từng gói; `AUTO` tự dò model (Family B/C). Port mặc định 2368 | +| `sick_tim_driver` | `driver_sick_code.so` | SICK TiM 5xx/7xx | tcp | `SICK-TIM5xx`, `SICK-TIM571`, `SICK-TIM7xx` | SOPAS/CoLa-A; `open()` tự start stream. Port 2111. Verify trên TiM781S thật | +| `sick_nanoscan3_driver` | `driver_sick_safety.so` | SICK nanoScan3/microScan3 | udp | `SICK-nanoScan3` | Receiver thụ động UDP safety-data; đích UDP cấu hình sẵn bằng Safety Designer. Port 6060. Chưa verify phần cứng | +| `espe_lga60_driver` | `driver_espe.so` | ESPE LGA60 | tcp (+udp) | `ESPE-LGA60` | FOV 320°; `open()` gửi `RAuto`; tham số thiết bị theo tool Windows của hãng. Port 8080. Chưa verify phần cứng | -| Constant | FOV (°) | Range (m) | Giao thức | -|---|---|---|---| -| `MODEL_VB` | −135…135 | 0.05…30 | Family A | -| `MODEL_VF` | −180…180 | 0.05…30 | Family A | -| `MODEL_LR1F` | −180…180 | 0.05…50 | Family A (0° thiết bị hướng đuôi, offset +180°) | -| `MODEL_LR1FMI` | −180…180 | 0.05…30 | Family B, ~2400 điểm/vòng (0° hướng đuôi) | -| `MODEL_LR1BS5` | −180…180 | 0.05…30 | Family B | -| `MODEL_LR16F` | −135…135 | 0.05…30 | 3D 16-line | -| `MODEL_GS15` | −180…180 | 0.05…30 | Family C/V3 — chưa verify phần cứng | -| `MODEL_AUTO` | −180…180 | 0.05…30 | Tự dò model (Family B/C) | - -Driver nhận diện họ giao thức theo frame ID từng gói: **Family A** `0xFAF0` -(header 20 B, 3 B/điểm, CRC32) · **Family B** `0xFEF0` (header 40 B kèm tên -model ASCII, 8 B/điểm) · **Family C/V3** `0xFEAC` (header 48 B, 2–4 B/điểm). -Tên model đọc từ packet: `detected_model()` hoặc `result.info.detected_model`. - -### SICK - -| Constant | FOV (°) | Range (m) | Transport | -|---|---|---|---| -| `MODEL_SICK_TIM5XX` | −135…135 | 0.05…10 | TCP/SOPAS (CoLa-A), port 2111 | -| `MODEL_SICK_TIM571` | −135…135 | 0.05…25 | TCP/SOPAS, port 2111 | -| `MODEL_SICK_TIM7XX` | −135…135 | 0.05…25 | TCP/SOPAS, port 2111 — verify trên TiM781S thật | -| `MODEL_SICK_NANOSCAN3` | −137.5…137.5 | 0.05…40 | UDP safety-data, port 6060 | - -- **TiM (`SickDriver`)** — `open()` tự gửi lệnh start-stream; góc output đã - quy về 0° = phía trước. -- **nanoScan3 (`NanoScanDriver`)** — receiver UDP thụ động; đích UDP phải - cấu hình sẵn trong SICK Safety Designer. Chưa verify phần cứng thật. - -### ESPE - -| Constant | FOV (°) | Range (m) | Transport | -|---|---|---|---| -| `MODEL_ESPE_LGA60` | −160…160 | 0.05…50 | TCP (mặc định) hoặc UDP, port 8080 | - -- **LGA60 (`EspeDriver`)** — laser scanner FOV 320°, thiết bị quét - 20°→340° với 0° hướng đuôi (offset −180° để output 0° = phía trước). - `open()` tự gửi lệnh start-capture `RAuto`; các tham số thiết bị (tốc độ - quay, độ phân giải 0.025–0.5°, mức lọc nhiễu) lấy theo cấu hình đã nạp - bằng phần mềm Windows của hãng — driver không tự đổi. Frame dữ liệu - `HISN` (header big-endian, điểm đo little-endian: distance mm + - intensity); frame vùng `WSimu` (nếu thiết bị gửi) được đọc lấy mã lỗi. - Chuyển transport UDP qua tham số `use_udp` của constructor hoặc trường - `use_udp` trong config JSON. Mặc định của hãng: IP 192.168.1.88, port - 8080. Port từ driver ROS gốc của hãng — chưa verify trên phần cứng thật. +Chi tiết giao thức từng hãng (frame layout, offset góc, đơn vị) nằm trong +comment đầu mỗi file plugin và [docs/diagnostics.md](docs/diagnostics.md). ## Kiểu dữ liệu -### `ScanResult` +### `ScanResult` = `{ LaserScan scan; ExtraInfo info; }` -Kết quả một vòng quét: `{ LaserScan scan; ExtraInfo info; }`. +`LaserScan` cùng field và đơn vị với ROS `sensor_msgs/LaserScan`: -### `LaserScan` - -Cùng field và đơn vị với ROS `sensor_msgs/LaserScan`: - -| Field | Kiểu | Ý nghĩa | -|---|---|---| -| `angle_min` / `angle_max` | `float` | Góc điểm đầu/cuối (rad), unwrap liên tục | -| `angle_increment` | `float` | Bước góc (rad); góc điểm *i* = `angle_min + i·increment` | -| `ranges` | `vector` | Khoảng cách (m), theo thứ tự quét | -| `intensities` | `vector` | Cường độ phản xạ 0–255 | -| `range_min` / `range_max` | `float` | Dải đo hợp lệ (m), lấy từ `ModelConfig` | -| `timestamp_ms` | `uint32_t` | Đồng hồ thiết bị (ms); 0 nếu giao thức không có | -| `time_increment` / `scan_time` | `float` | Luôn 0 (thiết bị không cung cấp) | - -### `ExtraInfo` - -Metadata tuỳ giao thức; trường thiết bị không có giữ `std::nullopt`: - -| Field | Nguồn | Ý nghĩa | -|---|---|---| -| `detected_model` | mọi driver | Tên model thực đọc từ dữ liệu (hoặc tên cấu hình) | -| `error_status` | OLEI Family A | Byte lỗi thiết bị (xem Diagnostics) | -| `distance_scale_mm` | OLEI A/B | Hệ số mm/count của khoảng cách | -| `rotation_raw` | OLEI Family A | Tốc độ motor (raw) | -| `scan_frequency_raw`, `input_status`, `output_status`, `field_status`, `status_flags` | OLEI Family C, SICK TiM | Trạng thái I/O, field an toàn, cờ trạng thái (raw) | -| `sick_device_status` | SICK TiM | Cặp Device Status: 0 ok · 1 error · 2 pollution warning · 4 pollution error | -| `nano_general_state` | nanoScan3 | Byte 0 block General System State (bit `kNanoState*`) | -| `espe_error_status` | ESPE LGA60 | Từ lỗi thiết bị trong frame vùng `WSimu` (chỉ có khi host poll area data) | - -### `Diagnostics` - -Trạng thái tự chẩn đoán đã decode (`lidarlib/diagnostics.hpp`), trả về từ -`get_diagnostics()` hoặc `decode_diagnostics(result.info)`: - -| API | Ý nghĩa | +| Field | Ý nghĩa | |---|---| -| `valid` | Đã decode được ít nhất một vòng quét | -| `monitor_fault()` / `voltage_fault()` / `temperature_fault()` | OLEI Family A: motor / điện áp / nhiệt độ bất thường | -| `sick_error()` / `pollution_warning()` / `pollution_error()` | SICK TiM: lỗi thiết bị / kính bẩn nhẹ / kính bẩn nặng | -| `contamination_warning()` / `contamination_error()` / `manipulation()` | nanoScan3: kính bẩn / nghi bị can thiệp | -| `espe_fault()` | ESPE LGA60: từ lỗi thiết bị khác 0 (ý nghĩa bit chưa verify) | -| `has_fault()` | Gộp mọi nguồn lỗi | -| `has_warning()` | Gộp các cảnh báo kính bẩn (vẫn đo được) | -| `healthy()` | `valid && !has_fault()` | -| `to_string(d)` | Chuỗi log một dòng: `no data` / `ok` / `WARN: …` / `FAULT: …` | +| `angle_min` / `angle_max` / `angle_increment` | Góc (rad); góc điểm *i* = `angle_min + i·increment`. Driver mạng: hệ góc có dấu, 0 = phía trước; rplidar: hệ góc thiết bị [0, 2π) | +| `ranges` | Khoảng cách (m); NaN = điểm không hợp lệ (rplidar/FOV filter), ∞ = không có phản hồi (nanoScan3/ESPE) | +| `intensities` | Cường độ 0–255 | +| `range_min` / `range_max` | Dải đo hợp lệ (m) | +| `timestamp_ms` | Đồng hồ thiết bị (ms); 0 nếu giao thức không có | +| `time_increment` / `scan_time` | Chỉ rplidar đo được (chu kỳ grab thực); driver khác = 0 | -### `ModelConfig` & `LidarConfig` +`ExtraInfo`: metadata thô tuỳ giao thức (`detected_model`, byte lỗi OLEI, +status SICK, health RPLIDAR, ...) — field thiết bị không có giữ +`std::nullopt`. `Diagnostics` (từ `get_diagnostics()` hoặc +`decode_diagnostics(info)`) là bản decode tiện dùng: `has_fault()` / +`has_warning()` / `healthy()` / `to_string()` , kèm `model`, `firmware` +(rplidar), `device_timestamp_ms`. -- `ModelConfig` — thông số một model: tên, FOV, dải đo, offset góc, cửa sổ - remap. Các preset `MODEL_*` khai báo sẵn trong header. -- `LidarConfig` — một entry cấu hình runtime: `{name, ip, port, model, - inverted, brand}`, dùng với `make_lidar()` và file JSON. +## Viết một plugin mới -## Cấu trúc source +1. Tạo `plugins/driver_/` với `_driver.cpp` + `CMakeLists.txt` + (`xlidar_add_plugin(driver_ _driver.cpp)`), thêm + `add_subdirectory` vào `plugins/CMakeLists.txt`. +2. Implement class kế thừa `xlidar::LidarDriverInterface` — đủ + `open`/`close`/`recv_scan`/`spin_once`/`set_scan_callback`/ + `detected_model`/`is_open`/`get_driver_info`, và cập nhật + `get_diagnostics()` + `mark_scan_decoded()` mỗi vòng quét. +3. Export hai entry point: -| File | Vai trò | -|---|---| -| `include/lidarlib/lidarlib.hpp` | Include tổng hợp toàn bộ API | -| `include/lidarlib/lidar.hpp` | Kiểu dữ liệu, interface `Lidar`, driver OLEI, preset `MODEL_*` | -| `include/lidarlib/sick_lidar.hpp` | `SickDriver`, `NanoScanDriver`, preset `MODEL_SICK_*` | -| `include/lidarlib/espe_lidar.hpp` | `EspeDriver`, preset `MODEL_ESPE_LGA60` | -| `include/lidarlib/diagnostics.hpp` | `Diagnostics`, bit lỗi, `decode_diagnostics()` | -| `include/lidarlib/error.hpp` | `enum class ErrorCode` + `to_string()` | -| `include/lidarlib/config.hpp` | `LidarConfig`, load/save JSON, `make_lidar()` | -| `src/olei_lidar.cpp` | Parse Family A/B/C, CRC, gom vòng quét | -| `src/sick_lidar.cpp` | Parse CoLa-A (TiM) + safety-data UDP (nanoScan3) | -| `src/espe_lidar.cpp` | Parse frame `HISN`/`WSimu` (LGA60), gom vòng quét | -| `src/lidar_config.cpp` | Bảng model/brand, config JSON, factory | -| `docs/diagnostics.md` | Nghiên cứu layout dữ liệu chẩn đoán từng giao thức | -| `examples/` | Demo: một lidar, hai lidar song song, SICK TiM, nanoScan3, app khung | + ```cpp + XLIDAR_PLUGIN_EXPORT void get_driver_info(xlidar::DriverInfo* out) { *out = kDriverInfo; } + XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface* + create_driver_instance(const xlidar::DeviceConfig* cfg) { return new MyDriver(...); } + ``` + +4. Chọn `driver_id` duy nhất, mô tả `description` rõ driver phụ trách nhóm + thiết bị nào; dùng `apply_device_config()` trong `plugins/common` để tôn + trọng các cửa sổ góc/dải đo chung. + +Plugin build với `-fvisibility=hidden` — chỉ hai entry point lộ ra ngoài. +Manager và plugin phải build cùng toolchain (chúng trao đổi kiểu C++). diff --git a/cmake/lidarlibConfig.cmake.in b/cmake/lidarlibConfig.cmake.in deleted file mode 100644 index 661baef..0000000 --- a/cmake/lidarlibConfig.cmake.in +++ /dev/null @@ -1,6 +0,0 @@ -@PACKAGE_INIT@ - -include(CMakeFindDependencyMacro) -find_dependency(Threads) - -include("${CMAKE_CURRENT_LIST_DIR}/lidarlibTargets.cmake") diff --git a/cmake/xlidar_driverConfig.cmake.in b/cmake/xlidar_driverConfig.cmake.in new file mode 100644 index 0000000..9b20bdd --- /dev/null +++ b/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) diff --git a/config.json b/config.json index 42a8c19..ee560c5 100644 --- a/config.json +++ b/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","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","ip":"192.168.100.22","port":2111,"inverted":false},{"name":"rp1","driver_id":"rplidar_c1_driver","model":"AUTO","serial_port":"/dev/ttyUSB0","baudrate":460800,"inverted":false}]} diff --git a/docs/diagnostics.md b/docs/diagnostics.md index 2d76a7d..255f2a5 100644 --- a/docs/diagnostics.md +++ b/docs/diagnostics.md @@ -1,9 +1,9 @@ -# Nghiên cứu: Dữ liệu chẩn đoán (diagnosis) của lidar OLEI & SICK +# Nghiên cứu: Dữ liệu chẩn đoán (diagnosis) của các driver xlidar Tài liệu này tổng hợp những gì các gói dữ liệu OLEI mang theo về **tình trạng thiết bị** (self-diagnostics), ngoài dữ liệu điểm quét. Kết quả nghiên cứu này -là cơ sở cho API `lidarlib::Diagnostics` / `Lidar::get_diagnostics()` -(header `include/lidarlib/diagnostics.hpp`). +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 @@ -76,7 +76,7 @@ an toàn kiểu safety-scanner). Các trường chẩn đoán (port từ driver Vì bit map chưa xác minh, driver **truyền nguyên giá trị raw** qua `Diagnostics` (các trường `std::optional`) thay vì decode sai. Khi có tài liệu V3 chính thức hoặc thiết bị GS1-5 để thử, bổ sung decode tại -`decode_diagnostics()` trong `src/olei_lidar.cpp`. +`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`) @@ -126,7 +126,22 @@ Driver đọc byte này vào `info.nano_general_state`, decode qua Lưu ý: block này **chỉ có mặt nếu được tick chọn** trong cấu hình data output của Safety Designer — thiếu block thì trường giữ `nullopt`. -## 6. Chẩn đoán tầng transport (mọi driver) +## 6. Slamtec RPLIDAR (serial — health qua SDK) + +RPLIDAR không nhúng chẩn đoán trong stream điểm quét; thay vào đó SDK có lệnh +`getHealth()` trả về `status` (0 = OK, 1 = Warning, 2 = Error) kèm +`error_code` 16-bit. Driver (`plugins/driver_rplidar`) gọi health check +**một lần lúc `open()`** — status Error thì `open()` trả `DeviceError` và +không dùng thiết bị; Warning vẫn chạy nhưng để lại dấu. + +- Snapshot health nằm ở `Diagnostics::rplidar_health_status` / + `rplidar_error_code`, decode qua `rplidar_fault()` / `rplidar_warning()`. +- `Diagnostics::model` (`slamtec-0xNN` từ device info) và + `Diagnostics::firmware` (`fw M.mm hw H`) được tự nhận lúc `open()`. +- Chẩn đoán runtime chủ yếu là gián tiếp: `recv_scan()` timeout / mất kết + nối serial (`Timeout` / `DeviceDisconnected`), giống mục transport dưới đây. + +## 7. Chẩn đoán tầng transport (mọi driver) Ngoài dữ liệu trên wire, bản thân driver cung cấp lớp chẩn đoán kết nối: @@ -140,9 +155,9 @@ Ngoài dữ liệu trên wire, bản thân driver cung cấp lớp chẩn đoán Chiến lược giám sát khuyến nghị cho app: coi cảm biến **healthy** khi và chỉ khi `recv_scan()` thành công đều đặn **và** `get_diagnostics().has_fault() == false`. -## 7. Kiểm tra sẵn sàng: `is_ready()` / `wait_ready()` +## 8. Kiểm tra sẵn sàng: `is_ready()` / `wait_ready()` -Chiến lược trên được gói sẵn trong hai hàm của `Lidar`: +Chiến lược trên được gói sẵn trong hai hàm của `LidarDriverInterface`: ```cpp lidar->open(); @@ -166,14 +181,14 @@ if (!lidar->is_ready()) { /* mất dữ liệu hoặc thiết bị báo fault */ vẫn ready); app muốn chặt hơn thì tự kiểm tra thêm `!get_diagnostics().has_warning()`. -## 8. API +## 9. API ```cpp -#include "lidarlib/lidarlib.hpp" +#include "lidar_manager.hpp" -lidarlib::ScanResult r; +xlidar::ScanResult r; if (lidar->recv_scan(r, 1000)) { - lidarlib::Diagnostics d = lidar->get_diagnostics(); + xlidar::Diagnostics d = lidar->get_diagnostics(); if (!d.valid) { // chưa có scan nào được decode @@ -182,7 +197,7 @@ if (lidar->recv_scan(r, 1000)) { if (d.voltage_fault()) /* điện áp bất thường */; if (d.temperature_fault()) /* nhiệt độ bất thường */; if (d.monitor_fault()) /* motor/giám sát bất thường */; - printf("lidar fault: %s\n", lidarlib::to_string(d).c_str()); + printf("lidar fault: %s\n", xlidar::to_string(d).c_str()); } // SICK: cảnh báo kính bẩn — chưa phải fault nhưng nên lên lịch lau @@ -197,7 +212,7 @@ if (lidar->recv_scan(r, 1000)) { } ``` -- `Lidar::get_diagnostics()` — snapshot từ vòng quét decode gần nhất; gọi từ +- `LidarDriverInterface::get_diagnostics()` — snapshot từ vòng quét decode gần nhất; gọi từ cùng thread đang bơm `recv_scan()`/`spin_once()` (driver không khóa nội bộ). - `decode_diagnostics(const ExtraInfo&)` — hàm free, decode trực tiếp từ `ScanResult::info` nếu app muốn gắn chẩn đoán với đúng scan cụ thể. @@ -207,7 +222,7 @@ if (lidar->recv_scan(r, 1000)) { nano contamination error/manipulation); `has_warning()` gộp các mức cảnh báo kính bẩn. -## 9. Hướng mở rộng +## 10. Hướng mở rộng - **SICK SOPAS query chủ động**: `sRN SCdevicestate` (0=busy, 1=ready, 2=error), `sRN LCMstate` (mức nhiễm bẩn chi tiết) — cần cơ chế diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt new file mode 100644 index 0000000..3e19309 --- /dev/null +++ b/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() diff --git a/examples/espe_example.cpp b/examples/espe_example.cpp deleted file mode 100644 index 217d706..0000000 --- a/examples/espe_example.cpp +++ /dev/null @@ -1,39 +0,0 @@ -// ESPE LGA60 driver example (TCP, port 8080 theo cấu hình mặc định của hãng). -#include "lidarlib/espe_lidar.hpp" -#include - -int main() { - lidarlib::EspeDriver drv(lidarlib::MODEL_ESPE_LGA60, "192.168.1.88", 8080); - - lidarlib::ErrorCode err = drv.open(); - if (err != lidarlib::ErrorCode::Ok) { - fprintf(stderr, "Không kết nối được tới lidar ESPE: %s\n", - lidarlib::to_string(err)); - return 1; - } - - for (int i = 0; i < 10; ++i) { - lidarlib::ScanResult result; - if (!drv.recv_scan(result, 1000)) { - fprintf(stderr, "Timeout hoặc lỗi nhận dữ liệu: %s\n", - lidarlib::to_string(drv.last_error())); - continue; - } - const lidarlib::LaserScan& scan = result.scan; - printf("Scan #%d: %zu điểm, model=%s, diag=%s\n", - i, scan.ranges.size(), result.info.detected_model.c_str(), - lidarlib::to_string(drv.get_diagnostics()).c_str()); - - for (size_t j = 0; j < 20 && j < scan.ranges.size(); ++j) { - float angle_deg = (scan.angle_min + j * scan.angle_increment) * 180.f / 3.14159265f; - printf(" [%zu] angle=%.2f° dist=%.3fm intensity=%.0f\n", - j, angle_deg, scan.ranges[j], scan.intensities[j]); - } - } - - drv.close(); - return 0; -} - -// Build: -// g++ -std=c++17 -O2 -Iinclude -o espe_example examples/espe_example.cpp src/espe_lidar.cpp src/olei_lidar.cpp diff --git a/examples/example.cpp b/examples/example.cpp index 87ab76e..3fadb88 100644 --- a/examples/example.cpp +++ b/examples/example.cpp @@ -1,57 +1,71 @@ -// OLEI driver example. -#include "lidarlib/lidar.hpp" +// Open one lidar through the manager and read 10 scans. +// ./example [plugins_dir] +// ./example olei_lidar_driver +// ./example rplidar_c1_driver +#include "lidar_manager.hpp" #include -int main() { - lidarlib::Driver drv(lidarlib::MODEL_VB); +int main(int argc, char** argv) { + if (argc < 2) { + fprintf(stderr, "usage: %s [plugins_dir]\n", argv[0]); + return 1; + } + const std::string plugins_dir = (argc > 2) ? argv[2] : "plugins"; - lidarlib::ErrorCode err = drv.open(); - if (err != lidarlib::ErrorCode::Ok) { - fprintf(stderr, "Không mở được socket: %s\n", lidarlib::to_string(err)); + xlidar::LidarManager manager(plugins_dir); + 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; } - // Chờ cảm biến sẵn sàng: đã nhận được ít nhất một scan hoàn chỉnh - // và thiết bị không báo lỗi (motor/điện áp/nhiệt độ...) - if (!drv.wait_ready(5000)) { - fprintf(stderr, "Cảm biến chưa sẵn sàng: %s (diag: %s)\n", - lidarlib::to_string(drv.last_error()), - lidarlib::to_string(drv.get_diagnostics()).c_str()); - drv.close(); + // 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; - if (!drv.recv_scan(result, 2000)) { - fprintf(stderr, "Timeout hoặc lỗi nhận packet\n"); + xlidar::ScanResult result; + if (!lidar->recv_scan(result, 2000)) { + fprintf(stderr, "recv_scan failed: %s\n", xlidar::to_string(lidar->last_error())); 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()); + const xlidar::LaserScan& scan = result.scan; + printf("scan #%d: %zu points, ts=%u ms, model=%s\n", + i, scan.ranges.size(), scan.timestamp_ms, + result.info.detected_model.c_str()); - // Chẩn đoán thiết bị từ scan mới nhất - lidarlib::Diagnostics diag = drv.get_diagnostics(); - printf(" diag: %s\n", lidarlib::to_string(diag).c_str()); - if (diag.has_fault()) { - if (diag.monitor_fault()) printf(" !! lỗi monitor/motor\n"); - if (diag.voltage_fault()) printf(" !! điện áp bất thường\n"); - if (diag.temperature_fault()) printf(" !! nhiệt độ bất thường\n"); - } + // 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 < 20 && j < scan.ranges.size(); ++j) { + for (size_t j = 0; j < 5 && j < scan.ranges.size(); ++j) { float angle_deg = (scan.angle_min + j * scan.angle_increment) * 180.f / 3.14159265f; printf(" [%zu] angle=%.2f° dist=%.3fm intensity=%.0f\n", j, angle_deg, scan.ranges[j], scan.intensities[j]); } } - drv.close(); + lidar->close(); return 0; } - -// Build: -// g++ -std=c++17 -O2 -Iinclude -o example examples/example.cpp src/olei_lidar.cpp diff --git a/examples/lidar_app.cpp b/examples/lidar_app.cpp index 338c2e2..873db3c 100644 --- a/examples/lidar_app.cpp +++ b/examples/lidar_app.cpp @@ -1,6 +1,6 @@ // Headless skeleton app: loads config.json, one reader thread per lidar. -// ./lidar_app [config.json] -#include "lidarlib/lidarlib.hpp" +// ./lidar_app [config.json] [plugins_dir] +#include "lidar_manager.hpp" #include #include #include @@ -13,33 +13,31 @@ namespace { std::atomic g_running{true}; void on_signal(int) { g_running = false; } -void run_lidar(lidarlib::LidarConfig cfg) { - std::unique_ptr lidar = lidarlib::make_lidar(cfg); - lidarlib::ErrorCode err = lidar->open(); - if (err != lidarlib::ErrorCode::Ok) { - fprintf(stderr, "[%s] khong mo duoc %s %s:%u (%s)\n", - cfg.name.c_str(), cfg.brand.c_str(), cfg.ip.c_str(), cfg.port, - lidarlib::to_string(err)); +void run_lidar(xlidar::LidarManager& manager, xlidar::DeviceConfig cfg) { + std::unique_ptr lidar = manager.create_lidar_device(cfg); + if (!lidar) return; + + 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", - cfg.name.c_str(), cfg.brand.c_str(), cfg.ip.c_str(), cfg.port, - cfg.model.c_str(), cfg.inverted); + printf("[%s] opened %s (model=%s, inverted=%d)\n", + cfg.name.c_str(), cfg.driver_id.c_str(), 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; - const lidarlib::ExtraInfo& info = result.info; - - printf("[%s] %zu diem | ts=%u ms | model=%s | err=0x%02X\n", - cfg.name.c_str(), scan.ranges.size(), scan.timestamp_ms, - info.detected_model.c_str(), info.error_status); + printf("[%s] %zu points | ts=%u ms | model=%s | diag=%s\n", + cfg.name.c_str(), result.scan.ranges.size(), result.scan.timestamp_ms, + result.info.detected_model.c_str(), + xlidar::to_string(lidar->get_diagnostics()).c_str()); } lidar->close(); - printf("[%s] da dong\n", cfg.name.c_str()); + printf("[%s] closed\n", cfg.name.c_str()); } } // namespace @@ -48,23 +46,30 @@ int main(int argc, char** argv) { setvbuf(stdout, nullptr, _IOLBF, 0); const std::string config_path = (argc > 1) ? argv[1] : "config.json"; + const std::string plugins_dir = (argc > 2) ? argv[2] : "plugins"; - lidarlib::Config cfg = lidarlib::load_config(config_path); - lidarlib::save_config(config_path, cfg); // ensure the file exists + xlidar::LidarManager manager(plugins_dir); + 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 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; diff --git a/examples/list_drivers.cpp b/examples/list_drivers.cpp new file mode 100644 index 0000000..4803abf --- /dev/null +++ b/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 + +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; +} diff --git a/examples/nanoscan_example.cpp b/examples/nanoscan_example.cpp deleted file mode 100644 index 4a91f02..0000000 --- a/examples/nanoscan_example.cpp +++ /dev/null @@ -1,39 +0,0 @@ -// SICK nanoScan3 example. The sensor's UDP output target must be configured -// in SICK Safety Designer; this driver only binds a local UDP port. -#include "lidarlib/sick_lidar.hpp" -#include - -int main() { - lidarlib::NanoScanDriver drv(lidarlib::MODEL_SICK_NANOSCAN3, "0.0.0.0", 6060); - - lidarlib::ErrorCode err = drv.open(); - if (err != lidarlib::ErrorCode::Ok) { - fprintf(stderr, "Không mở được UDP socket cho nanoScan3: %s\n", - lidarlib::to_string(err)); - return 1; - } - - for (int i = 0; i < 10; ++i) { - lidarlib::ScanResult result; - if (!drv.recv_scan(result, 2000)) { - fprintf(stderr, "Timeout hoặc lỗi nhận UDP datagram\n"); - break; - } - const lidarlib::LaserScan& scan = result.scan; - const lidarlib::ExtraInfo& info = result.info; - printf("Scan #%d: %zu điểm, ts=%u, model=%s\n", - i, scan.ranges.size(), scan.timestamp_ms, info.detected_model.c_str()); - - for (size_t j = 0; j < 20 && j < scan.ranges.size(); ++j) { - float angle_deg = (scan.angle_min + j * scan.angle_increment) * 180.f / 3.14159265f; - printf(" [%zu] angle=%.2f° dist=%.3fm intensity=%.0f\n", - j, angle_deg, scan.ranges[j], scan.intensities[j]); - } - } - - drv.close(); - return 0; -} - -// Build: -// g++ -std=c++17 -O2 -Iinclude -o nanoscan_example examples/nanoscan_example.cpp src/sick_lidar.cpp diff --git a/examples/sick_example.cpp b/examples/sick_example.cpp deleted file mode 100644 index f3a83c0..0000000 --- a/examples/sick_example.cpp +++ /dev/null @@ -1,39 +0,0 @@ -// SICK TiM driver example (SOPAS/CoLa-A, TCP). -#include "lidarlib/sick_lidar.hpp" -#include - -int main() { - lidarlib::SickDriver drv(lidarlib::MODEL_SICK_TIM571, "192.168.0.1", 2111); - - lidarlib::ErrorCode err = drv.open(); - if (err != lidarlib::ErrorCode::Ok) { - fprintf(stderr, "Không kết nối được TCP tới lidar SICK: %s\n", - lidarlib::to_string(err)); - return 1; - } - - for (int i = 0; i < 10; ++i) { - lidarlib::ScanResult result; - if (!drv.recv_scan(result, 2000)) { - fprintf(stderr, "Timeout hoặc lỗi nhận telegram\n"); - break; - } - const lidarlib::LaserScan& scan = result.scan; - const lidarlib::ExtraInfo& info = result.info; - printf("Scan #%d: %zu điểm, ts=%u ms, err=0x%02X, model=%s\n", - i, scan.ranges.size(), scan.timestamp_ms, info.error_status, - info.detected_model.c_str()); - - for (size_t j = 0; j < 20 && j < scan.ranges.size(); ++j) { - float angle_deg = (scan.angle_min + j * scan.angle_increment) * 180.f / 3.14159265f; - printf(" [%zu] angle=%.2f° dist=%.3fm intensity=%.0f\n", - j, angle_deg, scan.ranges[j], scan.intensities[j]); - } - } - - drv.close(); - return 0; -} - -// Build: -// g++ -std=c++17 -O2 -Iinclude -o sick_example examples/sick_example.cpp src/sick_lidar.cpp diff --git a/examples/test_dual.cpp b/examples/test_dual.cpp deleted file mode 100644 index e2c0400..0000000 --- a/examples/test_dual.cpp +++ /dev/null @@ -1,47 +0,0 @@ -// Test 2 Olei lidars (front + rear) concurrently. -#include "lidarlib/lidar.hpp" -#include -#include - -static void run_lidar(const char* tag, const lidarlib::ModelConfig& cfg, - const std::string& local_ip, uint16_t port, bool inverted, int n_scans) { - lidarlib::Driver drv(cfg, local_ip, port, inverted); - lidarlib::ErrorCode err = drv.open(); - if (err != lidarlib::ErrorCode::Ok) { - fprintf(stderr, "[%s] Khong mo duoc socket tren %s:%u (%s)\n", - tag, local_ip.c_str(), port, lidarlib::to_string(err)); - return; - } - printf("[%s] Da bind %s:%u, dang doi scan...\n", tag, local_ip.c_str(), port); - - for (int i = 0; i < n_scans; ++i) { - lidarlib::ScanResult result; - if (!drv.recv_scan(result, 2000)) { - fprintf(stderr, "[%s] Timeout/loi nhan packet (scan #%d)\n", tag, i); - continue; - } - const lidarlib::LaserScan& scan = result.scan; - const lidarlib::ExtraInfo& info = result.info; - printf("[%s] Scan #%d: %zu diem, ts=%u ms, err=0x%02X, model=%s\n", - tag, i, scan.ranges.size(), scan.timestamp_ms, info.error_status, - info.detected_model.c_str()); - for (size_t j = 0; j < 20 && j < scan.ranges.size(); ++j) { - float angle_deg = (scan.angle_min + j * scan.angle_increment) * 180.f / 3.14159265f; - printf(" [%zu] angle=%.2f dist=%.3fm intensity=%.0f\n", - j, angle_deg, scan.ranges[j], scan.intensities[j]); - } - } - drv.close(); -} - -int main() { - // Real headers (UDP sniff): front = "OLELR-1BS2", rear = "OLELR-1BS5". - std::thread t_front(run_lidar, "front/scan_1", lidarlib::MODEL_AUTO, - "192.168.100.100", 2368, false, 5); - std::thread t_rear(run_lidar, "rear/scan_2", lidarlib::MODEL_AUTO, - "192.168.100.100", 2369, true, 5); - - t_front.join(); - t_rear.join(); - return 0; -} diff --git a/include/lidarlib/diagnostics.hpp b/include/lidar_diagnostics.hpp similarity index 68% rename from include/lidarlib/diagnostics.hpp rename to include/lidar_diagnostics.hpp index 12e539d..e31b8c5 100644 --- a/include/lidarlib/diagnostics.hpp +++ b/include/lidar_diagnostics.hpp @@ -1,15 +1,16 @@ #pragma once +// xlidar-driver — device self-diagnostics decoded from the data stream. #include #include #include #include -namespace lidarlib { +namespace xlidar { -struct ExtraInfo; // lidar.hpp +struct ExtraInfo; // lidar_interface.hpp -// Family A (0xFAF0) error_status bits, header byte [5]. Bits 3-7 are reserved -// on the wire; a nonzero reserved bit is still reported as a fault. +// 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 @@ -28,25 +29,32 @@ inline constexpr uint8_t kNanoStateContaminationError = 1u << 3; inline constexpr uint8_t kNanoStateReferenceContour = 1u << 4; inline constexpr uint8_t kNanoStateManipulation = 1u << 5; -// Device self-diagnostics decoded from the data stream. Fields the family -// doesn't carry stay std::nullopt (see docs/diagnostics.md for the per-family -// wire layout). valid stays false until the driver has decoded one full scan. +// 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; + +// Device self-diagnostics decoded from the data stream. Fields the device +// family doesn't carry stay std::nullopt (see docs/diagnostics.md for the +// per-family wire layout). valid stays false until the driver has decoded one +// full scan. struct Diagnostics { bool valid = false; std::string model = "AUTO"; + 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 - // Family A error byte (0 = no fault; Family B/C don't carry it) + // OLEI Family A error byte (0 = no fault; Family B/C don't carry it) uint8_t error_status = 0; bool monitor_fault() const { return (error_status & kFaultMonitor) != 0; } bool voltage_fault() const { return (error_status & kFaultVoltage) != 0; } bool temperature_fault() const { return (error_status & kFaultTemperature) != 0; } - // Family A only — raw motor speed field, unit unverified + // OLEI Family A only — raw motor speed field, unit unverified std::optional rotation_raw; - // Family C / V3 (GS1-5) only — raw passthroughs, bit meanings unverified + // OLEI Family C / V3 (GS1-5) only — raw passthroughs, bit meanings unverified std::optional scan_frequency_raw; std::optional input_status; std::optional output_status; @@ -73,17 +81,30 @@ struct Diagnostics { bool espe_fault() const { return espe_error_status && *espe_error_status != 0; } + // RPLIDAR — SDK getHealth() status (refreshed at open(); the streaming + // protocol carries no health) plus the device error code that goes with it. + std::optional rplidar_health_status; + std::optional rplidar_error_code; + + bool rplidar_fault() const { return rplidar_health_status && *rplidar_health_status == kRplidarHealthError; } + bool rplidar_warning() const { return rplidar_health_status && *rplidar_health_status == kRplidarHealthWarning; } + // Fault = device says something is wrong now; warning = degraded but - // still measuring (dirty optics) — schedule cleaning/service. + // still measuring (dirty optics, weak motor) — schedule cleaning/service. bool has_fault() const { return error_status != 0 || sick_error() || pollution_error() - || contamination_error() || manipulation() || espe_fault(); + || contamination_error() || manipulation() || espe_fault() + || rplidar_fault(); + } + bool has_warning() const { + return pollution_warning() || contamination_warning() || rplidar_warning(); } - bool has_warning() const { return pollution_warning() || contamination_warning(); } bool healthy() const { return valid && !has_fault(); } }; // Decode the diagnostic fields of one scan; sets valid = true. +// 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: pollution" / @@ -93,6 +114,7 @@ inline std::string to_string(const Diagnostics& d) { if (!d.has_fault()) return d.has_warning() ? std::string("WARN:") + (d.pollution_warning() ? " pollution" : "") + (d.contamination_warning() ? " contamination" : "") + + (d.rplidar_warning() ? " rplidar" : "") : "ok"; std::string s = "FAULT:"; @@ -108,6 +130,12 @@ inline std::string to_string(const Diagnostics& d) { std::snprintf(buf, sizeof(buf), " espe(0x%04X)", *d.espe_error_status); s += buf; } + if (d.rplidar_fault()) { + char buf[32]; + std::snprintf(buf, sizeof(buf), " rplidar(0x%04X)", + d.rplidar_error_code ? *d.rplidar_error_code : 0); + s += buf; + } if (uint8_t rest = d.error_status & ~(kFaultMonitor | kFaultVoltage | kFaultTemperature)) { char buf[24]; std::snprintf(buf, sizeof(buf), " reserved(0x%02X)", rest); @@ -116,4 +144,4 @@ inline std::string to_string(const Diagnostics& d) { return s; } -} // namespace lidarlib +} // namespace xlidar diff --git a/include/lidar_interface.hpp b/include/lidar_interface.hpp new file mode 100644 index 0000000..8d33d2b --- /dev/null +++ b/include/lidar_interface.hpp @@ -0,0 +1,383 @@ +#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 +#include +#include +#include +#include +#include +#include +#include + +namespace xlidar { + +// ── Error codes ───────────────────────────────────────────────────────────── + +// Result of open() and the sticky status behind last_error(). Ok == 0 so +// `if (err != ErrorCode::Ok)` reads naturally at call sites. +enum class ErrorCode { + Ok = 0, + + // Lifecycle misuse — the call was refused, the instance state is unchanged. + AlreadyOpen, // open() called while already open + NotOpen, // recv_scan()/spin_once() called before open() + + // open() failures + SocketError, // socket() creation failed + InvalidAddress, // ip string is not a valid IPv4 address + PortInUse, // bind: local port already taken (EADDRINUSE/EACCES) + BindFailed, // bind failed for another reason + ConnectionRefused, // TCP connect refused (device up, port closed) + ConnectionFailed, // TCP connect failed (unreachable, no route, ...) + HandshakeFailed, // connected, but the start-stream command failed + SerialError, // serial port open/configure failed (serial drivers) + DeviceError, // device rejected a command / reported a hard fault + + // Runtime failures + Timeout, // no (complete) scan within timeout_ms + DeviceDisconnected, // peer closed the connection / socket or serial error + + // Configuration errors + InvalidConfig, // DeviceConfig is not usable by this driver +}; + +inline const char* to_string(ErrorCode e) { + switch (e) { + case ErrorCode::Ok: return "Ok"; + case ErrorCode::AlreadyOpen: return "AlreadyOpen"; + case ErrorCode::NotOpen: return "NotOpen"; + case ErrorCode::SocketError: return "SocketError"; + case ErrorCode::InvalidAddress: return "InvalidAddress"; + case ErrorCode::PortInUse: return "PortInUse"; + case ErrorCode::BindFailed: return "BindFailed"; + case ErrorCode::ConnectionRefused: return "ConnectionRefused"; + case ErrorCode::ConnectionFailed: return "ConnectionFailed"; + case ErrorCode::HandshakeFailed: return "HandshakeFailed"; + case ErrorCode::SerialError: return "SerialError"; + case ErrorCode::DeviceError: return "DeviceError"; + case ErrorCode::Timeout: return "Timeout"; + case ErrorCode::DeviceDisconnected: return "DeviceDisconnected"; + case ErrorCode::InvalidConfig: return "InvalidConfig"; + } + return "Unknown"; +} + +// ── Scan data ─────────────────────────────────────────────────────────────── + +// ROS sensor_msgs/LaserScan-shaped output (radians, meters, seconds). +// ranges[i] is at angle_min + i*angle_increment, in sweep order. +struct LaserScan { + uint32_t timestamp_ms = 0; // device clock (ms); 0 if not on the wire + float angle_min = 0.f; // rad + float angle_max = 0.f; // rad + float angle_increment = 0.f; // rad + float time_increment = 0.f; // sec — not exposed by most devices, 0 then + float scan_time = 0.f; // sec — not exposed by most devices, 0 then + float range_min = 0.f; // m — from ModelConfig, not measured + float range_max = 0.f; // m — from ModelConfig, not measured + std::vector ranges; // m + std::vector 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 rotation_raw; + + // OLEI Family C / V3 (GS1-5) only — raw passthroughs, unverified + std::optional distance_ratio_raw; + std::optional scan_frequency_raw; + std::optional input_status; + std::optional output_status; + std::optional field_status; + std::optional status_flags; + + // SICK TiM only — LMDscandata status pair (word0<<8)|word1: + // 0 ok, 1 error, 2 pollution warning, 4 pollution error. + std::optional sick_device_status; + + // SICK nanoScan3 only — General System State byte 0 (see kNanoState* bits). + std::optional 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 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 rplidar_health_status; + std::optional rplidar_error_code; +}; + +struct ScanResult { + LaserScan scan; + ExtraInfo info; +}; + +// Decode the diagnostic fields of one scan; sets valid = true. +inline Diagnostics decode_diagnostics(const ExtraInfo& info) { + Diagnostics d; + d.valid = true; + d.model = info.detected_model; + d.error_status = info.error_status; + d.rotation_raw = info.rotation_raw; + d.scan_frequency_raw = info.scan_frequency_raw; + d.input_status = info.input_status; + d.output_status = info.output_status; + d.field_status = info.field_status; + d.status_flags = info.status_flags; + d.sick_device_status = info.sick_device_status; + d.nano_general_state = info.nano_general_state; + d.espe_error_status = info.espe_error_status; + d.rplidar_health_status = info.rplidar_health_status; + d.rplidar_error_code = info.rplidar_error_code; + 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; + +// ── 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::use_udp. +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"; +} + +// 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 → use_udp switches TCP/UDP + std::vector supported_models; // valid DeviceConfig::model values +}; + +// Settings for one lidar instance. `name` is the unique key across saves. +// Which fields matter depends on the driver's transport: +// 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 + + // 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 + bool use_udp = false; // only for transport-selectable drivers (ESPE) + + // 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 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.ip == b.ip && a.port == b.port && + a.serial_port == b.serial_port && a.baudrate == b.baudrate && + a.inverted == b.inverted && a.use_udp == b.use_udp && + 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( + deadline - std::chrono::steady_clock::now()).count(); + if (left <= 0) return false; + recv_scan(tmp, static_cast(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" diff --git a/include/lidar_manager.hpp b/include/lidar_manager.hpp new file mode 100644 index 0000000..322e4ee --- /dev/null +++ b/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 +#include +#include +#include +#include + +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 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& 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 + create_lidar_device(const std::string& driver_id, const DeviceConfig& cfg); + + // Convenience: driver_id taken from cfg.driver_id. + std::unique_ptr 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> + 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 available_; + std::map loaded_; + mutable std::mutex mutex_; +}; + +} // namespace xlidar diff --git a/include/lidarlib/config.hpp b/include/lidarlib/config.hpp deleted file mode 100644 index ab555a0..0000000 --- a/include/lidarlib/config.hpp +++ /dev/null @@ -1,63 +0,0 @@ -#pragma once -#include "lidarlib/lidar.hpp" -#include -#include -#include - -namespace lidarlib { - -// Settings for one lidar. `name` is the unique key across saves. -struct LidarConfig { - std::string name = "lidar"; - std::string ip = "0.0.0.0"; - uint16_t port = 2368; - std::string model = "AUTO"; - bool inverted = false; // unit mounted upside-down → mirror the scan - std::string brand = "OLEI"; // "OLEI", "SICK" or "ESPE" - bool use_udp = false; // ESPE only: UDP instead of TCP transport - - // Output angle window (deg): scan angles are remapped onto - // [angle_min_deg, angle_max_deg] without dropping points. - // Defaults (±360) = off. - float angle_min_deg = -360.f; - float angle_max_deg = 360.f; - - friend bool operator==(const LidarConfig& a, const LidarConfig& b) { - return a.name == b.name && a.ip == b.ip && a.port == b.port && - a.model == b.model && a.inverted == b.inverted && a.brand == b.brand && - a.use_udp == b.use_udp && - a.angle_min_deg == b.angle_min_deg && a.angle_max_deg == b.angle_max_deg; - } - friend bool operator!=(const LidarConfig& a, const LidarConfig& b) { return !(a == b); } -}; - -struct Config { - std::vector 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& model_names(); - -const std::vector& brand_names(); - -// Subset of model_names() valid for `brand`; empty if unknown. -const std::vector& model_names_for_brand(const std::string& brand); - -// Returns defaults if the file doesn't exist (without creating it). -Config load_config(const std::string& path); - -void save_config(const std::string& path, const Config& cfg); - -// Build a ready-to-open lidar from one LidarConfig — the only entry point an -// app needs. brand "SICK" → SICK driver (model "SICK-nanoScan3" → UDP -// NanoScanDriver, others → TCP SickDriver); brand "ESPE" → TCP EspeDriver; -// anything else → OLEI UDP. -// Unknown/cross-brand model falls back to the brand default. Never nullptr. -std::unique_ptr make_lidar(const LidarConfig& cfg); - -} // namespace lidarlib diff --git a/include/lidarlib/error.hpp b/include/lidarlib/error.hpp deleted file mode 100644 index 1ec8627..0000000 --- a/include/lidarlib/error.hpp +++ /dev/null @@ -1,46 +0,0 @@ -#pragma once - -namespace lidarlib { - -// Result of open() and the sticky status behind last_error(). Ok == 0 so -// `if (err != ErrorCode::Ok)` reads naturally at call sites. -enum class ErrorCode { - Ok = 0, - - // Lifecycle misuse — the call was refused, the instance state is unchanged. - AlreadyOpen, // open() called while already open - NotOpen, // recv_scan()/spin_once() called before open() - - // open() failures - SocketError, // socket() creation failed - InvalidAddress, // ip string is not a valid IPv4 address - PortInUse, // bind: local port already taken (EADDRINUSE/EACCES) - BindFailed, // bind failed for another reason - ConnectionRefused, // TCP connect refused (device up, port closed) - ConnectionFailed, // TCP connect failed (unreachable, no route, ...) - HandshakeFailed, // connected, but the start-stream command failed - - // Runtime failures - Timeout, // no (complete) scan within timeout_ms - DeviceDisconnected, // peer closed the connection / socket recv error -}; - -inline const char* to_string(ErrorCode e) { - switch (e) { - case ErrorCode::Ok: return "Ok"; - case ErrorCode::AlreadyOpen: return "AlreadyOpen"; - case ErrorCode::NotOpen: return "NotOpen"; - case ErrorCode::SocketError: return "SocketError"; - case ErrorCode::InvalidAddress: return "InvalidAddress"; - case ErrorCode::PortInUse: return "PortInUse"; - case ErrorCode::BindFailed: return "BindFailed"; - case ErrorCode::ConnectionRefused: return "ConnectionRefused"; - case ErrorCode::ConnectionFailed: return "ConnectionFailed"; - case ErrorCode::HandshakeFailed: return "HandshakeFailed"; - case ErrorCode::Timeout: return "Timeout"; - case ErrorCode::DeviceDisconnected: return "DeviceDisconnected"; - } - return "Unknown"; -} - -} // namespace lidarlib diff --git a/include/lidarlib/lidar.hpp b/include/lidarlib/lidar.hpp deleted file mode 100644 index e59b705..0000000 --- a/include/lidarlib/lidar.hpp +++ /dev/null @@ -1,236 +0,0 @@ -#pragma once -#include "lidarlib/diagnostics.hpp" -#include "lidarlib/error.hpp" -#include -#include -#include -#include -#include -#include - -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 ranges; // m - std::vector 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 rotation_raw; - - // Family C / V3 (GS1-5) only — raw passthroughs, unverified - std::optional distance_ratio_raw; - std::optional scan_frequency_raw; - std::optional input_status; - std::optional output_status; - std::optional field_status; - std::optional status_flags; - - // SICK TiM only — LMDscandata status pair (word0<<8)|word1: - // 0 ok, 1 error, 2 pollution warning, 4 pollution error. - std::optional sick_device_status; - - // SICK nanoScan3 only — General System State byte 0 (see kNanoState* bits). - std::optional 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 espe_error_status; -}; - -struct ScanResult { - LaserScan scan; - ExtraInfo info; -}; - -// Per-model configuration. scan_angle_* use the signed system [-180,180]: -// 0 = ahead, + = left, - = right. range_min/max are datasheet placeholders. -struct ModelConfig { - const char* name; - float scan_angle_min; // deg - float scan_angle_max; // deg - float range_min_m = 0.05f; - float range_max_m = 30.f; - // Added to the raw device angle so output 0° = ahead (LR-1F/1FMI report 0° - // at the back: +180; SICK TiM puts the front at 90°: -90). - float angle_offset_deg = 0.f; - // Output remap window (see make_lidar / remap_scan_window): shifts the - // scan's angles onto [out_angle_min, out_angle_max] without dropping points. - bool remap_angles = false; - float out_angle_min = 0.f; // deg - float out_angle_max = 0.f; // deg -}; - -inline constexpr ModelConfig MODEL_VB { "VB", -135.f, 135.f, 0.05f, 30.f }; // 2D 270° -inline constexpr ModelConfig MODEL_VF { "VF", -180.f, 180.f, 0.05f, 30.f }; // 2D 360° -inline constexpr ModelConfig MODEL_LR1F { "LR-1F", -180.f, 180.f, 0.05f, 50.f, 180.f }; // 2D 360° 50m; device 0° = rear -inline constexpr ModelConfig MODEL_LR1FMI { "LR-1FMI", -180.f, 180.f, 0.05f, 30.f, 180.f }; // 2D 360° (Family B); device 0° = rear -inline constexpr ModelConfig MODEL_LR1BS5 { "LR-1BS5", -180.f, 180.f, 0.05f, 30.f }; // 2D 360° (Family B) -inline constexpr ModelConfig MODEL_LR16F { "LR-16F", -135.f, 135.f, 0.05f, 30.f }; // 3D 16 line -inline constexpr ModelConfig MODEL_GS15 { "GS1-5", -180.f, 180.f, 0.05f, 30.f }; // 2D 360° - -// Model unknown ahead of time: Family B/C packets carry enough to auto-detect; -// Family A doesn't, so the wide default FOV is kept. -inline constexpr ModelConfig MODEL_AUTO { "AUTO", -180.f, 180.f, 0.05f, 30.f }; - -using ScanCallback = std::function; - -// Unified driver interface returned by make_lidar(); OLEI and SICK drivers -// both derive from it. -class Lidar { -public: - virtual ~Lidar() = default; - - // ErrorCode::Ok on success. Calling open() on an already-open instance - // returns AlreadyOpen and leaves the connection untouched. - virtual ErrorCode open() = 0; - // Idempotent: safe to call before open() or more than once. - virtual void close() = 0; - // Block until one full scan; false on error/timeout (see last_error()). - // timeout_ms = 0 → block indefinitely. No default on purpose: drivers - // differ (OLEI 1000, SICK 2000). - virtual bool recv_scan(ScanResult& out, int timeout_ms) = 0; - // The callback fires only from spin_once() — recv_scan() never invokes - // it. Pick one pump style: recv_scan() to poll, or callback + spin_once(). - virtual void set_scan_callback(ScanCallback cb) = 0; - // Process one unit of input (may block on the socket while the device is - // silent); fires the scan callback when a scan completed. False on error. - virtual bool spin_once() = 0; - virtual const char* detected_model() const = 0; - - virtual bool is_open() const = 0; - // Status of the most recent open()/recv_scan()/spin_once() call. - ErrorCode last_error() const { return last_error_; } - - // Device self-diagnostics from the newest fully decoded scan. valid stays - // false until one scan has been seen. Updated by recv_scan()/spin_once(); - // call from the same thread that pumps them. - virtual Diagnostics get_diagnostics() const { return {}; } - - // True when the sensor is usable right now: connection open, at least one - // fault-free scan decoded, and that scan no older than max_age_ms - // (0 = skip the age check). Diagnostics only refresh from - // recv_scan()/spin_once(), so unless something is pumping them this goes - // stale and reports not-ready; call from the pump thread. - bool is_ready(int max_age_ms = 3000) const { - if (!is_open() || !get_diagnostics().healthy()) return false; - if (max_age_ms <= 0) return true; - return last_scan_time_.time_since_epoch().count() != 0 - && std::chrono::steady_clock::now() - last_scan_time_ - <= std::chrono::milliseconds(max_age_ms); - } - - // Pump recv_scan() until is_ready() or timeout_ms elapses; false on - // timeout (see last_error() for the underlying failure). Scans consumed - // while waiting are discarded and the scan callback does not fire — - // intended for startup, before handing the pump to the main loop. - bool wait_ready(int timeout_ms = 5000) { - const auto deadline = std::chrono::steady_clock::now() - + std::chrono::milliseconds(timeout_ms); - ScanResult tmp; - while (!is_ready()) { - if (!is_open()) return false; - const auto left = std::chrono::duration_cast( - deadline - std::chrono::steady_clock::now()).count(); - if (left <= 0) return false; - recv_scan(tmp, static_cast(left)); - } - return true; - } - -protected: - ErrorCode set_error(ErrorCode e) { last_error_ = e; return e; } - // Drivers call this each time a full scan is decoded; feeds the freshness - // side of is_ready(). - void mark_scan_decoded() { last_scan_time_ = std::chrono::steady_clock::now(); } - -private: - ErrorCode last_error_ = ErrorCode::Ok; - std::chrono::steady_clock::time_point last_scan_time_{}; -}; - -// OLEI UDP driver. -class Driver : public Lidar { -public: - using ScanCallback = lidarlib::ScanCallback; - - // ip: local bind address; port: UDP port the lidar sends to; - // inverted: unit mounted upside-down → mirror every angle. - explicit Driver(const ModelConfig& cfg, - const std::string& ip = "0.0.0.0", - uint16_t port = 2368, - bool inverted = false); - ~Driver(); - - Driver(const Driver&) = delete; - Driver& operator=(const Driver&) = delete; - - ErrorCode open() override; - void close() override; - bool recv_scan(ScanResult& out, int timeout_ms = 1000) override; - void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); } - bool spin_once() override; - bool is_open() const override { return sock_fd_ >= 0; } - - // Model name read from the Family B/C header; "AUTO" until one is seen. - const char* detected_model() const override { return detected_model_name_.c_str(); } - - Diagnostics get_diagnostics() const override { return latest_diag_; } - -private: - bool poll_packet(); // one recvfrom() + dispatch to the family parser - bool parse_family_a(const uint8_t* buf, int len); // ID=0xFAF0 - bool parse_family_b(const uint8_t* buf, int len); // ID=0xFEF0 - bool parse_family_c(const uint8_t* buf, int len); // Magic=0xFEAC (GS1-5) - - void push_point(float signed_angle_deg, float dist_m, uint8_t intensity); - void flush_scan(); - - ModelConfig cfg_; - std::string ip_; - uint16_t port_; - bool inverted_ = false; - int sock_fd_ = -1; - ScanCallback cb_; - - // Per-revolution accumulation buffers (index-aligned) - std::vector pending_angle_deg_; - std::vector pending_dist_m_; - std::vector pending_intensity_; - uint32_t pending_ts_ = 0; - uint8_t pending_err_ = 0; - float last_angle_ = -1.f; // wrap detection, device space [0,360) - - ExtraInfo pending_info_; - - // Snapshot for get_diagnostics(); refreshed by flush_scan(). - Diagnostics latest_diag_; - - ScanResult ready_result_; - bool scan_ready_ = false; - - // Per-instance so two drivers on two threads don't race. - uint8_t recv_buf_[4096]; - - bool auto_detect_ = false; - bool model_locked_ = false; - std::string detected_model_name_ = "AUTO"; -}; - -} // namespace lidarlib diff --git a/include/lidarlib/lidarlib.hpp b/include/lidarlib/lidarlib.hpp deleted file mode 100644 index b8ef873..0000000 --- a/include/lidarlib/lidarlib.hpp +++ /dev/null @@ -1,7 +0,0 @@ -#pragma once -// One-include convenience header for the whole public API. -#include "lidarlib/error.hpp" -#include "lidarlib/lidar.hpp" -#include "lidarlib/sick_lidar.hpp" -#include "lidarlib/espe_lidar.hpp" -#include "lidarlib/config.hpp" diff --git a/include/lidarlib/sick_lidar.hpp b/include/lidarlib/sick_lidar.hpp deleted file mode 100644 index ab9e719..0000000 --- a/include/lidarlib/sick_lidar.hpp +++ /dev/null @@ -1,118 +0,0 @@ -#pragma once -#include "lidarlib/lidar.hpp" -#include -#include -#include - -namespace lidarlib { - -// SICK TiM presets. FOV/range from datasheets; scan_angle_* are informational -// only and do NOT filter points. angle_offset_deg = -90 because the TiM wire -// frame puts 90° at the device front. -inline constexpr ModelConfig MODEL_SICK_TIM5XX { "SICK-TIM5xx", -135.f, 135.f, 0.05f, 10.f, -90.f }; // TiM551/561, 270°, 10m -inline constexpr ModelConfig MODEL_SICK_TIM571 { "SICK-TIM571", -135.f, 135.f, 0.05f, 25.f, -90.f }; // TiM571, 270°, 25m -inline constexpr ModelConfig MODEL_SICK_TIM7XX { "SICK-TIM7xx", -135.f, 135.f, 0.05f, 25.f, -90.f }; // TiM781, 270°, 25m - -// SICK TiM5xx/7xx over SOPAS/CoLa-A (TCP, default port 2111). -// Verified against a real TiM781S (FW V5.11). NOT verified: NumEncoders > 0, -// the 8-bit channel branch, and the TIM5xx/TIM571 FOV/range numbers. -class SickDriver : public Lidar { -public: - using ScanCallback = lidarlib::ScanCallback; - - // inverted: unit mounted upside-down → mirror the scan. - explicit SickDriver(const ModelConfig& cfg, - const std::string& ip, - uint16_t port = 2111, - bool inverted = false); - ~SickDriver(); - - SickDriver(const SickDriver&) = delete; - SickDriver& operator=(const SickDriver&) = delete; - - // Connect + send "sEN LMDscandata 1" to start continuous scan output. - ErrorCode open() override; - void close() override; - bool recv_scan(ScanResult& out, int timeout_ms = 2000) override; - void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); } - bool spin_once() override; - bool is_open() const override { return sock_fd_ >= 0; } - - // No model string on the wire — returns the configured name. - const char* detected_model() const override { return detected_model_name_.c_str(); } - - Diagnostics get_diagnostics() const override { return latest_diag_; } - -private: - bool send_telegram(const std::string& body); - bool read_telegram(std::string& out, int timeout_ms); - bool parse_lmdscandata(const std::string& telegram, ScanResult& out); - - ModelConfig cfg_; - std::string detected_model_name_; // owned copy of cfg_.name (stable lifetime) - std::string ip_; - uint16_t port_; - bool inverted_ = false; - int sock_fd_ = -1; - ScanCallback cb_; - - // Snapshot for get_diagnostics(); refreshed by parse_lmdscandata(). - Diagnostics latest_diag_; - - // Leftover TCP bytes carried across telegram boundaries; per-instance. - std::string recv_buf_; -}; - -inline constexpr ModelConfig MODEL_SICK_NANOSCAN3 { "SICK-nanoScan3", -137.5f, 137.5f, 0.05f, 40.f }; - -// SICK nanoScan3 / microScan3 safety-scanner binary UDP output. Layout ported -// from SICK's open-source sick_safetyscanners; NOT verified on real hardware. -// Passive UDP receiver: the sensor's UDP target must be configured up front in -// SICK Safety Designer — this class does no CoLa2/TCP handshake. -class NanoScanDriver : public Lidar { -public: - using ScanCallback = lidarlib::ScanCallback; - - // ip: local bind address; port: local UDP port the sensor sends to; - // inverted: unit mounted upside-down → mirror the scan. - explicit NanoScanDriver(const ModelConfig& cfg, - const std::string& ip = "0.0.0.0", - uint16_t port = 6060, - bool inverted = false); - ~NanoScanDriver(); - - NanoScanDriver(const NanoScanDriver&) = delete; - NanoScanDriver& operator=(const NanoScanDriver&) = delete; - - ErrorCode open() override; - void close() override; - bool recv_scan(ScanResult& out, int timeout_ms = 1000) override; - void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); } - bool spin_once() override; - bool is_open() const override { return sock_fd_ >= 0; } - - // No model string on the wire — returns the configured name. - const char* detected_model() const override { return detected_model_name_.c_str(); } - - Diagnostics get_diagnostics() const override { return latest_diag_; } - -private: - int recv_datagram(int timeout_ms); - bool parse_packet(const uint8_t* buf, int len, ScanResult& out); - - ModelConfig cfg_; - std::string detected_model_name_; // owned copy of cfg_.name (stable lifetime) - std::string ip_; - uint16_t port_; - bool inverted_ = false; - int sock_fd_ = -1; - ScanCallback cb_; - - // Snapshot for get_diagnostics(); refreshed by parse_packet(). - Diagnostics latest_diag_; - - // Per-instance; sized for a full safety-data packet (max ~2751 beams). - std::vector recv_buf_; -}; - -} // namespace lidarlib diff --git a/plugins/CMakeLists.txt b/plugins/CMakeLists.txt new file mode 100644 index 0000000..29a8648 --- /dev/null +++ b/plugins/CMakeLists.txt @@ -0,0 +1,29 @@ +# Driver plugins. Every plugin is a self-contained MODULE library named +# .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( ) — 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_code) +add_subdirectory(driver_sick_safety) +add_subdirectory(driver_espe) +add_subdirectory(driver_rplidar) diff --git a/plugins/common/plugin_helpers.hpp b/plugins/common/plugin_helpers.hpp new file mode 100644 index 0000000..4bbc75c --- /dev/null +++ b/plugins/common/plugin_helpers.hpp @@ -0,0 +1,152 @@ +// 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace xlidar { + +inline constexpr float kDeg2Rad = 3.14159265358979323846f / 180.f; + +// Remap a finished scan's angular window onto [min_deg, max_deg]. Only +// angle_min/angle_max/angle_increment are rewritten; points are untouched. +inline void remap_scan_window(LaserScan& scan, float min_deg, float max_deg) { + const float new_min = min_deg * kDeg2Rad; + const float new_max = max_deg * kDeg2Rad; + const float old_span = scan.angle_max - scan.angle_min; + if (old_span > 0.f) + scan.angle_increment *= (new_max - new_min) / old_span; + scan.angle_min = new_min; + scan.angle_max = new_max; +} + +// Mirror a finished scan for a unit mounted upside-down: reverse the point +// order and negate the angular window. Apply before remap_scan_window(). +inline void invert_scan(LaserScan& scan) { + std::reverse(scan.ranges.begin(), scan.ranges.end()); + std::reverse(scan.intensities.begin(), scan.intensities.end()); + const float new_min = -scan.angle_max; + scan.angle_max = -scan.angle_min; + scan.angle_min = new_min; +} + +// 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(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::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(p[0] | (p[1] << 8)); +} +inline uint32_t le32(const uint8_t* p) { + return static_cast(p[0]) + | (static_cast(p[1]) << 8) + | (static_cast(p[2]) << 16) + | (static_cast(p[3]) << 24); +} +inline int32_t le_i32(const uint8_t* p) { return static_cast(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(&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 diff --git a/plugins/driver_espe/CMakeLists.txt b/plugins/driver_espe/CMakeLists.txt new file mode 100644 index 0000000..09f9fb5 --- /dev/null +++ b/plugins/driver_espe/CMakeLists.txt @@ -0,0 +1 @@ +xlidar_add_plugin(driver_espe espe_driver.cpp) diff --git a/src/espe_lidar.cpp b/plugins/driver_espe/espe_driver.cpp similarity index 85% rename from src/espe_lidar.cpp rename to plugins/driver_espe/espe_driver.cpp index 923cf93..4cd9fd8 100644 --- a/src/espe_lidar.cpp +++ b/plugins/driver_espe/espe_driver.cpp @@ -1,19 +1,19 @@ -#include "lidarlib/espe_lidar.hpp" -#include "lidar_bytes.hpp" -#include "lidar_net.hpp" +// ESPE LGA60 — "HISN" range frames + "WSimu" area frames over TCP/UDP. +#include "espe_driver.hpp" +#include "plugin_helpers.hpp" #include #include #include #include #include -#include #include -#include -#include #include +#include +#include +#include -namespace lidarlib { +namespace xlidar { namespace { // "RAuto" + fixed tail — puts the device into continuous measurement output. @@ -219,10 +219,7 @@ void EspeDriver::finish_scan() { scan.range_min = cfg_.range_min_m; scan.range_max = cfg_.range_max_m; - if (inverted_) - invert_scan(scan); - if (cfg_.remap_angles) - remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max); + finalize_scan(scan, cfg_, inverted_); ExtraInfo& info = ready_result_.info; info = ExtraInfo{}; @@ -263,4 +260,40 @@ bool EspeDriver::spin_once() { return true; } -} // namespace lidarlib +// ── plugin registration ───────────────────────────────────────────────────── + +namespace { + +const DriverInfo kDriverInfo = [] { + DriverInfo info; + info.vendor = "ESPE"; + info.model = "LGA60"; + info.driver_id = "espe_lga60_driver"; + info.description = "ESPE LGA60 320° laser scanner — TCP by default, UDP via " + "use_udp; 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; // use_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; + const uint16_t port = cfg->port ? cfg->port : 8080; + return new EspeDriver(apply_device_config(MODEL_ESPE_LGA60, *cfg), + cfg->ip, port, cfg->use_udp, cfg->inverted); +} diff --git a/include/lidarlib/espe_lidar.hpp b/plugins/driver_espe/espe_driver.hpp similarity index 91% rename from include/lidarlib/espe_lidar.hpp rename to plugins/driver_espe/espe_driver.hpp index c84086e..8404282 100644 --- a/include/lidarlib/espe_lidar.hpp +++ b/plugins/driver_espe/espe_driver.hpp @@ -1,9 +1,13 @@ +// ESPE LGA60 laser scanner over TCP or UDP — plugin-private header. #pragma once -#include "lidarlib/lidar.hpp" -#include -#include +#include "lidar_interface.hpp" -namespace lidarlib { +#include +#include +#include +#include + +namespace xlidar { // ESPE LGA60-320: 320° FOV, device sweeps 20°..340° with 0° at the rear // (angle_offset_deg = -180 so output 0° = ahead). Range per datasheet page; @@ -14,10 +18,8 @@ inline constexpr ModelConfig MODEL_ESPE_LGA60 { "ESPE-LGA60", -160.f, 160.f, 0.0 // ROS driver; NOT verified on real hardware. open() sends the "RAuto" start // command; device parameters (spin rate, resolution, filters) are whatever // the vendor Windows config tool programmed — this driver does not set them. -class EspeDriver : public Lidar { +class EspeDriver : public LidarDriverInterface { public: - using ScanCallback = lidarlib::ScanCallback; - // ip: device address; use_udp selects the transport the device is // configured for (vendor default is TCP); inverted: unit mounted // upside-down → mirror the scan. @@ -31,6 +33,8 @@ public: EspeDriver(const EspeDriver&) = delete; EspeDriver& operator=(const EspeDriver&) = delete; + DriverInfo get_driver_info() const override; + // Connect + send the start-capture command. ErrorCode open() override; void close() override; @@ -80,4 +84,4 @@ private: bool scan_ready_ = false; }; -} // namespace lidarlib +} // namespace xlidar diff --git a/plugins/driver_olei/CMakeLists.txt b/plugins/driver_olei/CMakeLists.txt new file mode 100644 index 0000000..175b133 --- /dev/null +++ b/plugins/driver_olei/CMakeLists.txt @@ -0,0 +1 @@ +xlidar_add_plugin(driver_olei olei_driver.cpp) diff --git a/src/olei_lidar.cpp b/plugins/driver_olei/olei_driver.cpp similarity index 83% rename from src/olei_lidar.cpp rename to plugins/driver_olei/olei_driver.cpp index 15dbadd..0ffa162 100644 --- a/src/olei_lidar.cpp +++ b/plugins/driver_olei/olei_driver.cpp @@ -1,16 +1,17 @@ -#include "lidarlib/lidar.hpp" -#include "lidar_bytes.hpp" +// OLEI 2D lidars over UDP — Family A (0xFAF0), Family B (0xFEF0) and +// Family C / Protocol V3 (0xFEAC, GS1-5) packet parsing. +#include "olei_driver.hpp" +#include "plugin_helpers.hpp" #include #include #include -#include -#include #include -#include #include +#include +#include -namespace lidarlib { +namespace xlidar { // Normalize into (-180, 180]: 0 = ahead, + = left, - = right. static inline float to_signed_deg(float deg) { @@ -39,32 +40,16 @@ static constexpr uint16_t FRAME_ID_A = 0xFAF0; // 2D Ethernet (VB, VF, LR-1F) static constexpr uint16_t FRAME_ID_B = 0xFEF0; // LR-1BS5 / LR-1BS2 Ethernet variant static constexpr uint16_t FRAME_ID_C = 0xFEAC; // Protocol V3 (GS1-5) -Diagnostics decode_diagnostics(const ExtraInfo& info) { - Diagnostics d; - d.valid = true; - d.model = info.detected_model; - d.error_status = info.error_status; - d.rotation_raw = info.rotation_raw; - d.scan_frequency_raw = info.scan_frequency_raw; - d.input_status = info.input_status; - d.output_status = info.output_status; - d.field_status = info.field_status; - d.status_flags = info.status_flags; - d.sick_device_status = info.sick_device_status; - d.nano_general_state = info.nano_general_state; - d.espe_error_status = info.espe_error_status; - return d; -} - -Driver::Driver(const ModelConfig& cfg, const std::string& ip, uint16_t port, bool inverted) +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); } -Driver::~Driver() { close(); } +OleiDriver::~OleiDriver() { close(); } -ErrorCode Driver::open() { +ErrorCode OleiDriver::open() { if (is_open()) return set_error(ErrorCode::AlreadyOpen); sockaddr_in addr{}; @@ -105,14 +90,14 @@ ErrorCode Driver::open() { return set_error(ErrorCode::Ok); } -void Driver::close() { +void OleiDriver::close() { if (sock_fd_ >= 0) { ::close(sock_fd_); sock_fd_ = -1; } } -bool Driver::recv_scan(ScanResult& out, int timeout_ms) { +bool OleiDriver::recv_scan(ScanResult& out, int timeout_ms) { if (!is_open()) { set_error(ErrorCode::NotOpen); return false; } scan_ready_ = false; @@ -133,7 +118,7 @@ bool Driver::recv_scan(ScanResult& out, int timeout_ms) { return true; } -bool Driver::spin_once() { +bool OleiDriver::spin_once() { if (!poll_packet()) return false; if (scan_ready_) { scan_ready_ = false; @@ -142,7 +127,7 @@ bool Driver::spin_once() { return true; } -bool Driver::poll_packet() { +bool OleiDriver::poll_packet() { if (!is_open()) { set_error(ErrorCode::NotOpen); return false; } uint8_t* buf = recv_buf_; sockaddr_in from{}; @@ -165,7 +150,7 @@ bool Driver::poll_packet() { } // Append with angle-unwrapping so the ±180° seam stays a continuous ramp. -void Driver::push_point(float signed_angle_deg, float dist_m, uint8_t intensity) { +void OleiDriver::push_point(float signed_angle_deg, float dist_m, uint8_t intensity) { float angle = signed_angle_deg; if (!pending_angle_deg_.empty()) { float prev = pending_angle_deg_.back(); @@ -177,7 +162,7 @@ void Driver::push_point(float signed_angle_deg, float dist_m, uint8_t intensity) pending_intensity_.push_back(intensity); } -void Driver::flush_scan() { +void OleiDriver::flush_scan() { if (pending_angle_deg_.empty()) return; const size_t n = pending_angle_deg_.size(); @@ -195,8 +180,8 @@ void Driver::flush_scan() { scan.ranges.assign(pending_dist_m_.begin(), pending_dist_m_.end()); scan.intensities.assign(pending_intensity_.begin(), pending_intensity_.end()); - if (cfg_.remap_angles) - remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max); + // Inversion already happened per point (maybe_invert), so inverted=false. + finalize_scan(scan, cfg_, /*inverted=*/false); ExtraInfo& info = ready_result_.info; info = pending_info_; @@ -215,7 +200,7 @@ void Driver::flush_scan() { } // Family A (0xFAF0): 20B header + 3B blocks (u16 dist, u8 intensity). -bool Driver::parse_family_a(const uint8_t* buf, int len) { +bool OleiDriver::parse_family_a(const uint8_t* buf, int len) { static constexpr int HEADER_LEN = 20; static constexpr int BLOCK_LEN = 3; @@ -267,7 +252,7 @@ bool Driver::parse_family_a(const uint8_t* buf, int len) { // Family B (0xFEF0): 40B header (model string at [7-16]) + 8B blocks // (u16 angle ×0.01°, u16 dist, u16 signal). No timestamp/error on the wire. -bool Driver::parse_family_b(const uint8_t* buf, int len) { +bool OleiDriver::parse_family_b(const uint8_t* buf, int len) { static constexpr int HEADER_LEN = 40; static constexpr int BLOCK_LEN = 8; @@ -338,7 +323,7 @@ bool Driver::parse_family_b(const uint8_t* buf, int len) { // Family C / Protocol V3 (0xFEAC, GS1-5): 48B header + 2 or 4B points depending // on Types. Ported from the C# driver OleiGS15Driver.cs; NOT verified on real // hardware. Angle = (FirstIndex + i) * (360 / NumPointsScan) - 180. -bool Driver::parse_family_c(const uint8_t* buf, int len) { +bool OleiDriver::parse_family_c(const uint8_t* buf, int len) { static constexpr int HEADER_LEN = 48; if (len < HEADER_LEN) return false; @@ -424,4 +409,49 @@ 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; + 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); +} diff --git a/plugins/driver_olei/olei_driver.hpp b/plugins/driver_olei/olei_driver.hpp new file mode 100644 index 0000000..5df5b37 --- /dev/null +++ b/plugins/driver_olei/olei_driver.hpp @@ -0,0 +1,89 @@ +// OLEI 2D lidars over UDP (Family A / B / C protocols) — plugin-private header. +#pragma once +#include "lidar_interface.hpp" + +#include +#include + +namespace xlidar { + +// Model presets. Family B/C packets carry enough to auto-detect the model; +// Family A doesn't, so MODEL_AUTO keeps the wide default FOV. +inline constexpr ModelConfig MODEL_VB { "VB", -135.f, 135.f, 0.05f, 30.f }; // 2D 270° +inline constexpr ModelConfig MODEL_VF { "VF", -180.f, 180.f, 0.05f, 30.f }; // 2D 360° +inline constexpr ModelConfig MODEL_LR1F { "LR-1F", -180.f, 180.f, 0.05f, 50.f, 180.f }; // 2D 360° 50m; device 0° = rear +inline constexpr ModelConfig MODEL_LR1FMI { "LR-1FMI", -180.f, 180.f, 0.05f, 30.f, 180.f }; // 2D 360° (Family B); device 0° = rear +inline constexpr ModelConfig MODEL_LR1BS5 { "LR-1BS5", -180.f, 180.f, 0.05f, 30.f }; // 2D 360° (Family B) +inline constexpr ModelConfig MODEL_LR16F { "LR-16F", -135.f, 135.f, 0.05f, 30.f }; // 3D 16 line +inline constexpr ModelConfig MODEL_GS15 { "GS1-5", -180.f, 180.f, 0.05f, 30.f }; // 2D 360° (Family C/V3) +inline constexpr ModelConfig MODEL_AUTO { "AUTO", -180.f, 180.f, 0.05f, 30.f }; + +// OLEI UDP driver. +class OleiDriver : public LidarDriverInterface { +public: + // ip: local bind address; port: UDP port the lidar sends to; + // inverted: unit mounted upside-down → mirror every angle. + explicit OleiDriver(const ModelConfig& cfg, + const std::string& ip = "0.0.0.0", + uint16_t port = 2368, + bool inverted = false); + ~OleiDriver(); + + OleiDriver(const OleiDriver&) = delete; + OleiDriver& operator=(const OleiDriver&) = delete; + + DriverInfo get_driver_info() const override; + + ErrorCode open() override; + void close() override; + bool recv_scan(ScanResult& out, int timeout_ms = 1000) override; + void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); } + bool spin_once() override; + bool is_open() const override { return sock_fd_ >= 0; } + + // Model name read from the Family B/C header; "AUTO" until one is seen. + const char* detected_model() const override { return detected_model_name_.c_str(); } + + Diagnostics get_diagnostics() const override { return latest_diag_; } + +private: + bool poll_packet(); // one recvfrom() + dispatch to the family parser + bool parse_family_a(const uint8_t* buf, int len); // ID=0xFAF0 + bool parse_family_b(const uint8_t* buf, int len); // ID=0xFEF0 + bool parse_family_c(const uint8_t* buf, int len); // Magic=0xFEAC (GS1-5) + + void push_point(float signed_angle_deg, float dist_m, uint8_t intensity); + void flush_scan(); + + ModelConfig cfg_; + std::string ip_; + uint16_t port_; + bool inverted_ = false; + int sock_fd_ = -1; + ScanCallback cb_; + + // Per-revolution accumulation buffers (index-aligned) + std::vector pending_angle_deg_; + std::vector pending_dist_m_; + std::vector 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 diff --git a/plugins/driver_rplidar/CMakeLists.txt b/plugins/driver_rplidar/CMakeLists.txt new file mode 100644 index 0000000..f6486b0 --- /dev/null +++ b/plugins/driver_rplidar/CMakeLists.txt @@ -0,0 +1,35 @@ +# The rplidar plugin compiles the vendor SDK straight into the plugin .so. +# XLIDAR_RPLIDAR_SDK_DIR must point at a directory holding include/ + src/; +# when unset, common local layouts are probed. Without an SDK the plugin is +# skipped (the rest of the build is unaffected). + +if(NOT XLIDAR_RPLIDAR_SDK_DIR) + foreach(candidate + ${CMAKE_SOURCE_DIR}/third_party/rplidar_sdk + ${CMAKE_SOURCE_DIR}/../rplidar_sdk/sdk + ${CMAKE_SOURCE_DIR}/../xloc-monorepo/xlocd/deps/rplidar_sdk) + if(EXISTS ${candidate}/include/sl_lidar.h) + set(XLIDAR_RPLIDAR_SDK_DIR ${candidate}) + break() + endif() + endforeach() +endif() + +if(NOT XLIDAR_RPLIDAR_SDK_DIR OR NOT EXISTS ${XLIDAR_RPLIDAR_SDK_DIR}/include/sl_lidar.h) + message(WARNING "driver_rplidar: Slamtec SDK not found " + "(set -DXLIDAR_RPLIDAR_SDK_DIR=...) — plugin skipped") + return() +endif() + +message(STATUS "driver_rplidar: using SDK at ${XLIDAR_RPLIDAR_SDK_DIR}") + +file(GLOB_RECURSE RPLIDAR_SDK_SOURCES CONFIGURE_DEPENDS + ${XLIDAR_RPLIDAR_SDK_DIR}/src/*.cpp) +# The SDK ships win32/macOS arch files; keep Linux only. +list(FILTER RPLIDAR_SDK_SOURCES EXCLUDE REGEX "arch/(win32|macOS)/") + +xlidar_add_plugin(driver_rplidar rplidar_driver.cpp ${RPLIDAR_SDK_SOURCES}) +target_include_directories(driver_rplidar SYSTEM PRIVATE + ${XLIDAR_RPLIDAR_SDK_DIR}/include + ${XLIDAR_RPLIDAR_SDK_DIR}/src +) diff --git a/plugins/driver_rplidar/rplidar_driver.cpp b/plugins/driver_rplidar/rplidar_driver.cpp new file mode 100644 index 0000000..482f36c --- /dev/null +++ b/plugins/driver_rplidar/rplidar_driver.cpp @@ -0,0 +1,326 @@ +// Slamtec RPLIDAR over serial (C1 defaults), built on the vendor SDK +// (sl_lidar.h). Ported from xlocd's embedded rplidar driver — the scan math +// (angle/distance decoding, inversion, FOV window, NaN invalid points) is +// kept identical. +// +// Unlike the network drivers, angles are reported in the DEVICE frame +// [0, 2π), 0 = ahead, ascending — exactly what the SDK's ascendScanData +// yields (and what xlocd's engine expects). +#include "lidar_interface.hpp" +#include "plugin_helpers.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#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(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(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(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(info.model)); + char firmware_buf[48]; + std::snprintf(firmware_buf, sizeof(firmware_buf), "fw %u.%02u hw %u", + static_cast(info.firmware_version >> 8), + static_cast(info.firmware_version & 0xFF), + static_cast(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(health.error_code); + refresh_diag_from_health(); + disconnect(); + return set_error(ErrorCode::DeviceError); + } + health_status_ = health.status; + health_error_code_ = static_cast(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 nodes(kMaxNodesPerScan); + std::size_t count = nodes.size(); + const auto grabbed = lidar_->grabScanDataHq( + nodes.data(), count, + timeout_ms > 0 ? static_cast(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(grab_time - last_grab_).count() + : kDefaultScanTimeS; + last_grab_ = grab_time; + have_last_grab_ = true; + + const float angle_first = node_angle_rad(nodes.front()); + const float angle_last = node_angle_rad(nodes[count - 1]); + if (angle_last <= angle_first) { + set_error(ErrorCode::Timeout); // malformed revolution — treat as a miss + return false; + } + + LaserScan& scan = out.scan; + scan = LaserScan{}; + // Inverted mount -> mirror the angles (angle' = 2π - angle) and walk + // the nodes backwards to keep ascending order. + if (inverted_) { + scan.angle_min = kTwoPi - angle_last; + scan.angle_max = kTwoPi - angle_first; + } else { + scan.angle_min = angle_first; + scan.angle_max = angle_last; + } + scan.angle_increment = (scan.angle_max - scan.angle_min) / static_cast(count - 1); + scan.scan_time = scan_time; + scan.time_increment = scan_time / static_cast(count); + scan.range_min = cfg_.range_min_m; + scan.range_max = cfg_.range_max_m; + + // Valid FOV window — only filter when narrower than the full circle. + const bool apply_angle_window = + cfg_.fov_filter && (cfg_.fov_min_deg > -180.0F || cfg_.fov_max_deg < 180.0F); + + scan.ranges.reserve(count); + scan.intensities.reserve(count); + for (std::size_t i = 0; i < count; ++i) { + const std::size_t node_index = inverted_ ? count - 1 - i : i; + // dist = 0 is the SDK's "no return" sentinel; together with + // out-of-range / out-of-window points it becomes NaN. + const float distance = node_distance_m(nodes[node_index]); + bool valid = nodes[node_index].dist_mm_q2 != 0 && + distance >= scan.range_min && distance <= scan.range_max; + if (valid && apply_angle_window) { + const float grid_angle = scan.angle_min + scan.angle_increment * static_cast(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::quiet_NaN()); + scan.intensities.push_back(static_cast(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 health_status_; + std::optional 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; + // "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); +} diff --git a/plugins/driver_sick_code/CMakeLists.txt b/plugins/driver_sick_code/CMakeLists.txt new file mode 100644 index 0000000..e79b5ae --- /dev/null +++ b/plugins/driver_sick_code/CMakeLists.txt @@ -0,0 +1 @@ +xlidar_add_plugin(driver_sick_code sick_code_driver.cpp) diff --git a/src/sick_lidar.cpp b/plugins/driver_sick_code/sick_code_driver.cpp similarity index 50% rename from src/sick_lidar.cpp rename to plugins/driver_sick_code/sick_code_driver.cpp index 40c891d..6253adf 100644 --- a/src/sick_lidar.cpp +++ b/plugins/driver_sick_code/sick_code_driver.cpp @@ -1,21 +1,20 @@ -#include "lidarlib/sick_lidar.hpp" -#include "lidar_bytes.hpp" -#include "lidar_net.hpp" +// SICK TiM 5xx/7xx — SOPAS/CoLa-A ASCII telegrams over TCP ("sSN/sRA +// LMDscandata" parsing). +#include "sick_code_driver.hpp" +#include "plugin_helpers.hpp" #include #include -#include #include #include -#include #include -#include #include -#include -#include #include +#include +#include +#include -namespace lidarlib { +namespace xlidar { namespace { constexpr char kStx = 0x02; @@ -41,20 +40,16 @@ std::vector tokenize(const std::string& s) { } return out; } - -constexpr size_t kNanoRecvBufSize = 65536; -// nanoScan3 DerivedValues store angles as int32 in 1/4194304 degree. -constexpr double kNanoAngleResolution = 4194304.0; } // namespace -SickDriver::SickDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port, - bool inverted) +SickCodeDriver::SickCodeDriver(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) {} -SickDriver::~SickDriver() { close(); } +SickCodeDriver::~SickCodeDriver() { close(); } -ErrorCode SickDriver::open() { +ErrorCode SickCodeDriver::open() { if (is_open()) return set_error(ErrorCode::AlreadyOpen); sockaddr_in addr{}; @@ -84,7 +79,7 @@ ErrorCode SickDriver::open() { return set_error(ErrorCode::Ok); } -void SickDriver::close() { +void SickCodeDriver::close() { if (sock_fd_ >= 0) { send_telegram("sEN LMDscandata 0"); // best-effort ::close(sock_fd_); @@ -92,7 +87,7 @@ void SickDriver::close() { } } -bool SickDriver::send_telegram(const std::string& body) { +bool SickCodeDriver::send_telegram(const std::string& body) { if (sock_fd_ < 0) return false; std::string framed; framed.reserve(body.size() + 2); @@ -111,7 +106,7 @@ bool SickDriver::send_telegram(const std::string& body) { // CoLa-A has no length prefix, so ETX is the only frame boundary; recv_buf_ // carries leftover bytes across calls. -bool SickDriver::read_telegram(std::string& out, int timeout_ms) { +bool SickCodeDriver::read_telegram(std::string& out, int timeout_ms) { if (!is_open()) { set_error(ErrorCode::NotOpen); return false; } for (;;) { @@ -144,7 +139,7 @@ bool SickDriver::read_telegram(std::string& out, int timeout_ms) { } } -bool SickDriver::recv_scan(ScanResult& out, int timeout_ms) { +bool SickCodeDriver::recv_scan(ScanResult& out, int timeout_ms) { for (;;) { std::string telegram; if (!read_telegram(telegram, timeout_ms)) return false; @@ -153,7 +148,7 @@ bool SickDriver::recv_scan(ScanResult& out, int timeout_ms) { } } -bool SickDriver::spin_once() { +bool SickCodeDriver::spin_once() { std::string telegram; if (!read_telegram(telegram, 0)) return false; @@ -165,7 +160,7 @@ bool SickDriver::spin_once() { // CoLa-A "sSN/sRA LMDscandata": space-separated ASCII hex tokens, field order // per SICK's Telegram Listing. "DIST1" → ranges, "RSSI1" → intensities. -bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out) { +bool SickCodeDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out) { std::vector tok = tokenize(telegram); if (tok.size() < 20) return false; if (tok[0] != "sSN" && tok[0] != "sRA") return false; @@ -259,10 +254,7 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out) if (scan.intensities.size() != scan.ranges.size()) scan.intensities.assign(scan.ranges.size(), 0.f); - if (inverted_) - invert_scan(scan); - if (cfg_.remap_angles) - remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max); + finalize_scan(scan, cfg_, inverted_); ExtraInfo& info = out.info; info = ExtraInfo{}; @@ -280,211 +272,48 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out) return true; } -// ── NanoScanDriver — SICK nanoScan3/microScan3 safety-scanner UDP output ──── +// ── plugin registration ───────────────────────────────────────────────────── -NanoScanDriver::NanoScanDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port, - bool inverted) - : cfg_(cfg), detected_model_name_(cfg.name ? cfg.name : ""), ip_(ip), port_(port), - inverted_(inverted), recv_buf_(kNanoRecvBufSize) {} +namespace { -NanoScanDriver::~NanoScanDriver() { close(); } +const DriverInfo kDriverInfo = [] { + DriverInfo info; + info.vendor = "SICK"; + info.model = "TiM5xx/TiM7xx"; + info.driver_id = "sick_tim_driver"; + info.description = "SICK TiM 2D lidars (TiM551/561/571/781, ...) over " + "SOPAS/CoLa-A ASCII telegrams on TCP. open() starts the " + "LMDscandata stream. Default port 2111. Verified on a " + "real TiM781S."; + info.transport = Transport::Tcp; + info.supported_models = {"SICK-TIM5xx", "SICK-TIM571", "SICK-TIM7xx"}; + return info; +}(); -ErrorCode NanoScanDriver::open() { - 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(&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); +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; } -void NanoScanDriver::close() { - if (sock_fd_ >= 0) { - ::close(sock_fd_); - sock_fd_ = -1; - } +} // namespace + +DriverInfo SickCodeDriver::get_driver_info() const { return kDriverInfo; } + +} // namespace xlidar + +XLIDAR_PLUGIN_EXPORT void get_driver_info(xlidar::DriverInfo* out) { + *out = xlidar::kDriverInfo; } -int NanoScanDriver::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(n); +XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface* +create_driver_instance(const xlidar::DeviceConfig* cfg) { + using namespace xlidar; + 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 SickCodeDriver(apply_device_config(*preset, *cfg), cfg->ip, port, cfg->inverted); } - -// 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 tele; - std::vector 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(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(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(total), out)) { - set_error(ErrorCode::Ok); - return true; - } - } - } -} - -bool NanoScanDriver::spin_once() { - int n = recv_datagram(0); - if (n < 0) return false; - - ScanResult result; - if (!parse_packet(recv_buf_.data(), n, result)) return true; - if (cb_) cb_(result); - return true; -} - -// SICK safety-scanner data packet (LE), layout ported from sick_safetyscanners: -// DataHeader offset table at fixed offsets (derivedValues @36, measurementData -// @40); DerivedValues holds multiplicationFactor/startAngle/resolution; -// MeasurementData is u32 numBeams then 4 B/beam (u16 dist, u8 reflect, u8 status). -bool NanoScanDriver::parse_packet(const uint8_t* buf, int len, ScanResult& out) { - if (len < 52) return false; - - uint16_t gss_off = le16(buf + 32); // General System State block - uint16_t gss_size = le16(buf + 34); - uint16_t dv_off = le16(buf + 36); - uint16_t dv_size = le16(buf + 38); - uint16_t md_off = le16(buf + 40); - uint16_t md_size = le16(buf + 42); - - if (dv_off == 0 || dv_size == 0 || md_off == 0 || md_size == 0) return false; - if (static_cast(dv_off) + 20 > len) return false; - if (static_cast(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(start_raw) / kNanoAngleResolution; - double res_deg = static_cast(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(md_off) + 4 + static_cast(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::infinity(); - } else { - scan.ranges[i] = static_cast(distance) * - static_cast(mult_factor) * 1e-3f; // mm -> m - } - scan.intensities[i] = static_cast(reflect); - } - - scan.angle_min = (static_cast(start_deg) + cfg_.angle_offset_deg) * kDeg2Rad; - scan.angle_increment = static_cast(res_deg * kDeg2Rad); - scan.angle_max = scan.angle_min + - scan.angle_increment * static_cast(num_beams - 1); - scan.time_increment = 0.f; - scan.scan_time = 0.f; - scan.range_min = cfg_.range_min_m; - scan.range_max = cfg_.range_max_m; - // Raw device time from the DataHeader — an opaque tag, not ms since power-on. - scan.timestamp_ms = le32(buf + 28); - - if (inverted_) - invert_scan(scan); - if (cfg_.remap_angles) - remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max); - - ExtraInfo& info = out.info; - info = ExtraInfo{}; - info.detected_model = cfg_.name; - // Byte 0 holds the run/standby/contamination/manipulation flags - // (kNanoState*); the block is absent when not configured in the sensor. - if (gss_off != 0 && gss_size != 0 && static_cast(gss_off) < len) - info.nano_general_state = buf[gss_off]; - - latest_diag_ = decode_diagnostics(info); - latest_diag_.device_timestamp_ms = scan.timestamp_ms; - mark_scan_decoded(); - - return true; -} - -} // namespace lidarlib diff --git a/plugins/driver_sick_code/sick_code_driver.hpp b/plugins/driver_sick_code/sick_code_driver.hpp new file mode 100644 index 0000000..ad045f9 --- /dev/null +++ b/plugins/driver_sick_code/sick_code_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 +#include + +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 SickCodeDriver : public LidarDriverInterface { +public: + // inverted: unit mounted upside-down → mirror the scan. + explicit SickCodeDriver(const ModelConfig& cfg, + const std::string& ip, + uint16_t port = 2111, + bool inverted = false); + ~SickCodeDriver(); + + SickCodeDriver(const SickCodeDriver&) = delete; + SickCodeDriver& operator=(const SickCodeDriver&) = 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 diff --git a/plugins/driver_sick_safety/CMakeLists.txt b/plugins/driver_sick_safety/CMakeLists.txt new file mode 100644 index 0000000..2fd2daa --- /dev/null +++ b/plugins/driver_sick_safety/CMakeLists.txt @@ -0,0 +1 @@ +xlidar_add_plugin(driver_sick_safety sick_safety_driver.cpp) diff --git a/plugins/driver_sick_safety/sick_safety_driver.cpp b/plugins/driver_sick_safety/sick_safety_driver.cpp new file mode 100644 index 0000000..b66c32c --- /dev/null +++ b/plugins/driver_sick_safety/sick_safety_driver.cpp @@ -0,0 +1,261 @@ +// SICK nanoScan3 / microScan3 — binary safety-data UDP packets, with +// application-layer "MS3 " fragment reassembly. +#include "sick_safety_driver.hpp" +#include "plugin_helpers.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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(&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(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 tele; + std::vector 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(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(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(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(dv_off) + 20 > len) return false; + if (static_cast(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(start_raw) / kNanoAngleResolution; + double res_deg = static_cast(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(md_off) + 4 + static_cast(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::infinity(); + } else { + scan.ranges[i] = static_cast(distance) * + static_cast(mult_factor) * 1e-3f; // mm -> m + } + scan.intensities[i] = static_cast(reflect); + } + + scan.angle_min = (static_cast(start_deg) + cfg_.angle_offset_deg) * kDeg2Rad; + scan.angle_increment = static_cast(res_deg * kDeg2Rad); + scan.angle_max = scan.angle_min + + scan.angle_increment * static_cast(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(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; + const uint16_t port = cfg->port ? cfg->port : 6060; + return new SickSafetyDriver(apply_device_config(MODEL_SICK_NANOSCAN3, *cfg), + cfg->ip, port, cfg->inverted); +} diff --git a/plugins/driver_sick_safety/sick_safety_driver.hpp b/plugins/driver_sick_safety/sick_safety_driver.hpp new file mode 100644 index 0000000..53b74be --- /dev/null +++ b/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 +#include +#include + +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 recv_buf_; +}; + +} // namespace xlidar diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt new file mode 100644 index 0000000..5c937c5 --- /dev/null +++ b/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 + $ + $ +) +target_link_libraries(lidar_manager PUBLIC Threads::Threads PRIVATE ${CMAKE_DL_LIBS}) diff --git a/src/lidar_bytes.hpp b/src/lidar_bytes.hpp deleted file mode 100644 index c98b14d..0000000 --- a/src/lidar_bytes.hpp +++ /dev/null @@ -1,53 +0,0 @@ -// Internal helpers shared by the driver TUs — not part of the public API. -#pragma once -#include "lidarlib/lidar.hpp" -#include -#include -#include - -namespace lidarlib { - -inline constexpr float kDeg2Rad = 3.14159265358979323846f / 180.f; - -// Remap a finished scan's angular window onto [min_deg, max_deg]. Only -// angle_min/angle_max/angle_increment are rewritten; points are untouched. -inline void remap_scan_window(LaserScan& scan, float min_deg, float max_deg) { - const float new_min = min_deg * kDeg2Rad; - const float new_max = max_deg * kDeg2Rad; - const float old_span = scan.angle_max - scan.angle_min; - if (old_span > 0.f) - scan.angle_increment *= (new_max - new_min) / old_span; - scan.angle_min = new_min; - scan.angle_max = new_max; -} - -// Mirror a finished scan for a unit mounted upside-down: reverse the point -// order and negate the angular window. Apply before remap_scan_window(). -inline void invert_scan(LaserScan& scan) { - std::reverse(scan.ranges.begin(), scan.ranges.end()); - std::reverse(scan.intensities.begin(), scan.intensities.end()); - const float new_min = -scan.angle_max; - scan.angle_max = -scan.angle_min; - scan.angle_min = new_min; -} - -// Little-endian readers (bounds are the caller's responsibility). -inline uint8_t le_u8 (const uint8_t* p) { return p[0]; } -inline uint16_t le16(const uint8_t* p) { - return static_cast(p[0] | (p[1] << 8)); -} -inline uint32_t le32(const uint8_t* p) { - return static_cast(p[0]) - | (static_cast(p[1]) << 8) - | (static_cast(p[2]) << 16) - | (static_cast(p[3]) << 24); -} -inline int32_t le_i32(const uint8_t* p) { return static_cast(le32(p)); } - -inline float bits_to_float(uint32_t bits) { - float f; - std::memcpy(&f, &bits, sizeof(f)); - return f; -} - -} // namespace lidarlib diff --git a/src/lidar_config.cpp b/src/lidar_config.cpp deleted file mode 100644 index f9146f6..0000000 --- a/src/lidar_config.cpp +++ /dev/null @@ -1,176 +0,0 @@ -#include "lidarlib/config.hpp" -#include "lidarlib/sick_lidar.hpp" -#include "lidarlib/espe_lidar.hpp" -#include "json_mini.hpp" -#include -#include -#include -#include -#include - -namespace lidarlib { - -namespace { - -struct ModelEntry { const char* name; const ModelConfig* cfg; const char* brand; }; - -constexpr ModelEntry kModels[] = { - { "AUTO", &MODEL_AUTO, "OLEI" }, - { "VB", &MODEL_VB, "OLEI" }, - { "VF", &MODEL_VF, "OLEI" }, - { "LR-1F", &MODEL_LR1F, "OLEI" }, - { "LR-1FMI", &MODEL_LR1FMI, "OLEI" }, - { "LR-1BS5", &MODEL_LR1BS5, "OLEI" }, - { "LR-16F", &MODEL_LR16F, "OLEI" }, - { "GS1-5", &MODEL_GS15, "OLEI" }, - { "SICK-TIM5xx", &MODEL_SICK_TIM5XX, "SICK" }, - { "SICK-TIM571", &MODEL_SICK_TIM571, "SICK" }, - { "SICK-TIM7xx", &MODEL_SICK_TIM7XX, "SICK" }, - { "SICK-nanoScan3", &MODEL_SICK_NANOSCAN3, "SICK" }, - { "ESPE-LGA60", &MODEL_ESPE_LGA60, "ESPE" }, -}; - -json::Value to_json(const LidarConfig& c) { - json::Value v = json::Value::make_object(); - v.set("name", json::Value::make_string(c.name)); - v.set("ip", json::Value::make_string(c.ip)); - v.set("port", json::Value::make_number(c.port)); - v.set("brand", json::Value::make_string(c.brand)); - v.set("model", json::Value::make_string(c.model)); - v.set("inverted", json::Value::make_bool(c.inverted)); - v.set("use_udp", json::Value::make_bool(c.use_udp)); - v.set("angle_min_deg", json::Value::make_number(c.angle_min_deg)); - v.set("angle_max_deg", json::Value::make_number(c.angle_max_deg)); - return v; -} - -LidarConfig lidar_from_json(const json::Value& v, const LidarConfig& def) { - LidarConfig c = def; - c.name = v.get_string("name", def.name); - c.ip = v.get_string("ip", def.ip); - c.port = static_cast(v.get_number("port", def.port)); - c.brand = v.get_string("brand", def.brand); - c.model = v.get_string("model", def.model); - c.inverted = v.get_bool("inverted", def.inverted); - c.use_udp = v.get_bool("use_udp", def.use_udp); - c.angle_min_deg = static_cast(v.get_number("angle_min_deg", def.angle_min_deg)); - c.angle_max_deg = static_cast(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& model_names() { - static const std::vector names = [] { - std::vector v; - for (const auto& e : kModels) v.push_back(e.name); - return v; - }(); - return names; -} - -const std::vector& brand_names() { - static const std::vector names = {"OLEI", "SICK", "ESPE"}; - return names; -} - -const std::vector& model_names_for_brand(const std::string& brand) { - static const std::vector empty; - static const auto by_brand = [] { - std::vector>> 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 make_lidar(const LidarConfig& cfg) { - // Anything but the exact strings "SICK"/"ESPE" is OLEI (keeps brand-less - // configs working). Brand name and fallback model are decided together so - // a new brand adds exactly one branch here plus one construction case. - const bool is_sick = (cfg.brand == "SICK"); - const bool is_espe = (cfg.brand == "ESPE"); - - const char* brand; - const ModelConfig* fallback; - if (is_sick) { brand = "SICK"; fallback = &MODEL_SICK_TIM571; } - else if (is_espe) { brand = "ESPE"; fallback = &MODEL_ESPE_LGA60; } - else { brand = "OLEI"; fallback = &MODEL_AUTO; } - - const ModelConfig* model = model_by_name_for_brand(cfg.model, brand); - if (!model) model = fallback; - - ModelConfig mc = *model; - if (cfg.angle_min_deg > -360.f || cfg.angle_max_deg < 360.f) { - mc.remap_angles = true; - mc.out_angle_min = cfg.angle_min_deg; - mc.out_angle_max = cfg.angle_max_deg; - } - - if (is_sick) { - if (model == &MODEL_SICK_NANOSCAN3) - return std::make_unique(mc, cfg.ip, cfg.port, cfg.inverted); - return std::make_unique(mc, cfg.ip, cfg.port, cfg.inverted); - } - if (is_espe) - return std::make_unique(mc, cfg.ip, cfg.port, cfg.use_udp, cfg.inverted); - return std::make_unique(mc, cfg.ip, cfg.port, cfg.inverted); -} - -} // namespace lidarlib diff --git a/src/lidar_manager.cpp b/src/lidar_manager.cpp new file mode 100644 index 0000000..2c382f1 --- /dev/null +++ b/src/lidar_manager.cpp @@ -0,0 +1,234 @@ +// 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 +#include +#include +#include +#include +#include +#include + +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)); + 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("use_udp", json::Value::make_bool(c.use_udp)); + v.set("angle_min_deg", json::Value::make_number(c.angle_min_deg)); + v.set("angle_max_deg", json::Value::make_number(c.angle_max_deg)); + 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(v.get_number("port", c.port)); + c.serial_port = v.get_string("serial_port", c.serial_port); + c.baudrate = static_cast(v.get_number("baudrate", c.baudrate)); + c.inverted = v.get_bool("inverted", c.inverted); + c.use_udp = v.get_bool("use_udp", c.use_udp); + c.range_min_m = static_cast(v.get_number("range_min_m", c.range_min_m)); + c.range_max_m = static_cast(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(v.get_number("angle_min_deg", c.remap_angle_min_deg)); + c.remap_angle_max_deg = + static_cast(v.get_number("angle_max_deg", c.remap_angle_max_deg)); + } else { + c.angle_min_deg = static_cast(v.get_number("angle_min_deg", c.angle_min_deg)); + c.angle_max_deg = static_cast(v.get_number("angle_max_deg", c.angle_max_deg)); + c.remap_angle_min_deg = + static_cast(v.get_number("remap_angle_min_deg", c.remap_angle_min_deg)); + c.remap_angle_max_deg = + static_cast(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 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 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( + ::dlsym(handle, XLIDAR_GET_DRIVER_INFO_SYMBOL)); + auto create_fn = reinterpret_cast( + ::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 +LidarManager::create_lidar_device(const std::string& driver_id, const DeviceConfig& cfg) { + xlidar_create_driver_instance_fn create = nullptr; + { + std::lock_guard 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(create(&cfg)); +} + +std::vector> +LidarManager::create_from_config_file(const std::string& path) { + std::vector> 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 diff --git a/src/lidar_net.hpp b/src/lidar_net.hpp deleted file mode 100644 index 137abc8..0000000 --- a/src/lidar_net.hpp +++ /dev/null @@ -1,51 +0,0 @@ -// Internal socket helpers shared by the TCP driver TUs — not part of the public API. -#pragma once -#include "lidarlib/error.hpp" -#include -#include -#include -#include -#include -#include - -namespace lidarlib { - -// Non-blocking connect with a bounded timeout — a blocking connect() to an -// unreachable device would stall for the OS default (~2 min on Linux). -// Enables TCP_NODELAY on success; the fd is returned to blocking mode either -// way. The caller owns the fd and closes it on failure. -inline ErrorCode connect_tcp_with_timeout(int fd, const sockaddr_in& addr, int timeout_ms) { - int flags = ::fcntl(fd, F_GETFL, 0); - ::fcntl(fd, F_SETFL, flags | O_NONBLOCK); - - ErrorCode conn_err = ErrorCode::Ok; - int rc = ::connect(fd, reinterpret_cast(&addr), sizeof(addr)); - if (rc < 0 && errno != EINPROGRESS) { - conn_err = (errno == ECONNREFUSED) ? ErrorCode::ConnectionRefused - : ErrorCode::ConnectionFailed; - } else if (rc < 0) { - fd_set wfds; FD_ZERO(&wfds); FD_SET(fd, &wfds); - timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 }; - rc = ::select(fd + 1, nullptr, &wfds, nullptr, &tv); - if (rc == 0) { - conn_err = ErrorCode::Timeout; - } else if (rc < 0) { - conn_err = ErrorCode::ConnectionFailed; - } else { - int err = 0; socklen_t errlen = sizeof(err); - ::getsockopt(fd, SOL_SOCKET, SO_ERROR, &err, &errlen); - if (err != 0) - conn_err = (err == ECONNREFUSED) ? ErrorCode::ConnectionRefused - : ErrorCode::ConnectionFailed; - } - } - ::fcntl(fd, F_SETFL, flags); - - if (conn_err == ErrorCode::Ok) { - int nodelay = 1; - ::setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, &nodelay, sizeof(nodelay)); - } - return conn_err; -} - -} // namespace lidarlib