From 59880871b0a2c0f0c1e362ffbeeab55369c413f0 Mon Sep 17 00:00:00 2001 From: QUYVN Date: Wed, 1 Jul 2026 10:14:52 +0700 Subject: [PATCH] Fix Family B angle decode + add LR-1FMI model MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit parse_family_b() dùng sai hệ số góc 0.25°/LSB; theo spec Olei chính hãng (Olei.LidarSensor/LidarDataBlock.GetAngleDegrees) AngleRaw là 0.01°/LSB. Sai 25× khiến điểm bị gán nhầm góc → một phòng bị bôi thành vòng tròn trên RViz. Đã verify với thiết bị thật OLELR-1FMI: sau khi sửa ra 2400 điểm/vòng, 0–359.9°, đúng hình học môi trường. - Đổi hệ số góc 0.25° → 0.01° trong parse_family_b(). - Bỏ qua block invalid (AngleRaw >= 0xFF00) theo spec. - Dò ranh giới vòng quay PER-POINT thay vì per-packet (một gói có thể chứa >1 vòng), tránh gộp nhiều vòng vào một scan. - Thêm model LR-1FMI (360°, 0.01°/LSB, ~2400 pts/rev) vào bảng model + kModelTable, đặt "1FMI" trước "1F" để khớp đúng chuỗi tên. Co-Authored-By: Claude Opus 4.8 --- .gitignore | 12 + CMakeLists.txt | 82 ++++++ README.md | 346 ++++++++++++++++++++++ cmake/lidarlibConfig.cmake.in | 6 + config.json | 1 + examples/example.cpp | 48 ++++ examples/lidar_app.cpp | 83 ++++++ examples/sick_example.cpp | 40 +++ examples/test_dual.cpp | 55 ++++ include/lidarlib/config.hpp | 80 ++++++ include/lidarlib/lidar.hpp | 230 +++++++++++++++ include/lidarlib/lidarlib.hpp | 13 + include/lidarlib/sick_lidar.hpp | 91 ++++++ src/json_mini.hpp | 238 ++++++++++++++++ src/olei_config.cpp | 156 ++++++++++ src/olei_lidar.cpp | 488 ++++++++++++++++++++++++++++++++ src/sick_lidar.cpp | 319 +++++++++++++++++++++ 17 files changed, 2288 insertions(+) create mode 100644 .gitignore create mode 100644 CMakeLists.txt create mode 100644 README.md create mode 100644 cmake/lidarlibConfig.cmake.in create mode 100644 config.json create mode 100644 examples/example.cpp create mode 100644 examples/lidar_app.cpp create mode 100644 examples/sick_example.cpp create mode 100644 examples/test_dual.cpp create mode 100644 include/lidarlib/config.hpp create mode 100644 include/lidarlib/lidar.hpp create mode 100644 include/lidarlib/lidarlib.hpp create mode 100644 include/lidarlib/sick_lidar.hpp create mode 100644 src/json_mini.hpp create mode 100644 src/olei_config.cpp create mode 100644 src/olei_lidar.cpp create mode 100644 src/sick_lidar.cpp diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..8257234 --- /dev/null +++ b/.gitignore @@ -0,0 +1,12 @@ +# Build output +/build/ +*.o +*.so +*.so.* +*.a + +# Editor / OS +.vscode/ +.idea/ +*.swp +.DS_Store diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..927ce82 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,82 @@ +cmake_minimum_required(VERSION 3.10) +project(lidarlib VERSION 1.0.0 LANGUAGES CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +find_package(Threads REQUIRED) + +option(BUILD_SHARED_LIBS "Build shared (.so) libraries instead of static" ON) + +# ── lidarlib: OLEI (UDP) + SICK (TCP) drivers, the unified make_lidar() +# factory, and config.json load/save. No web UI. ── +set(LIDARLIB_SOURCES + src/olei_lidar.cpp + src/sick_lidar.cpp + src/olei_config.cpp +) + +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) + +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) + + # Headless skeleton app: load config.json -> make_lidar() each -> print scans. + # Replace its print loop with your own GUI; this is the integration template. + add_executable(lidar_app examples/lidar_app.cpp) + target_link_libraries(lidar_app PRIVATE lidarlib) +endif() + +# ── install + find_package() support ── +include(GNUInstallDirs) +include(CMakePackageConfigHelpers) + +install(TARGETS lidarlib + EXPORT lidarlibTargets + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} +) +install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) + +install(EXPORT lidarlibTargets + FILE lidarlibTargets.cmake + NAMESPACE lidarlib:: + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/lidarlib +) + +configure_package_config_file( + cmake/lidarlibConfig.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/lidarlibConfig.cmake + INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/lidarlib +) +write_basic_package_version_file( + ${CMAKE_CURRENT_BINARY_DIR}/lidarlibConfigVersion.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 +) diff --git a/README.md b/README.md new file mode 100644 index 0000000..85c1211 --- /dev/null +++ b/README.md @@ -0,0 +1,346 @@ +# Lidarlib + +Thư viện C++17 cho lidar (OLEI + SICK), build bằng CMake ra **shared lib `.so`** +(`lidarlib::lidarlib`), hỗ trợ `find_package()` để link vào project khác. Tự nhận +diện họ giao thức (Family A/B/C) theo từng gói, hỗ trợ chạy nhiều lidar song +song, gộp cả OLEI (UDP) và SICK (TCP) sau **một hàm config duy nhất** +`lidarlib::make_lidar()`. **Không có Web UI** — người dùng tự viết giao diện trên +API C++ này (include header + link `.so`). + +## Tóm tắt API (cho người viết giao diện) + +```cpp +#include "lidarlib/lidarlib.hpp" // gộp toàn bộ API trong 1 include + +// 1) MỘT hàm config: từ LidarConfig -> handle chung (OLEI hoặc SICK) +lidarlib::LidarConfig c{"front", "192.168.1.10", 2368, "AUTO", false, "OLEI"}; +std::unique_ptr lidar = lidarlib::make_lidar(c); +lidar->open(); + +// 2) HAI loại dữ liệu mỗi vòng quét +lidarlib::ScanResult r; +lidar->recv_scan(r, 1000); +// r.scan : lidarlib::LaserScan — chung cho mọi lidar, đúng format sensor_msgs/LaserScan của ROS +// r.info : lidarlib::ExtraInfo — thông tin thêm, tuỳ family/model thực tế + +// (tuỳ chọn) lưu/đọc danh sách lidar ra file: +lidarlib::Config cfg = lidarlib::load_config("config.json"); +lidarlib::save_config("config.json", cfg); +``` + +`lidarlib::Lidar` là interface chung; `lidarlib::Driver` (OLEI/UDP) và `lidarlib::SickDriver` +(SICK/TCP) đều kế thừa nó, nên giao diện chỉ cần thao tác qua `lidarlib::Lidar*` mà +không phải phân biệt hãng. Vẫn có thể `new` thẳng `Driver`/`SickDriver` nếu muốn. + +## Kiến trúc + +| File | Vai trò | +|------|---------| +| `include/lidarlib/lidarlib.hpp` | Header tổng hợp — include 1 dòng là có cả data model + 2 driver + config + factory | +| `include/lidarlib/lidar.hpp` | API public: `LaserScan`, `ExtraInfo`, `ScanResult`, `ModelConfig`, interface `Lidar`, class `Driver` | +| `src/olei_lidar.cpp` | Parse Family A (0xFAF0), Family B (0xFEF0), Family C/V3 (0xFEAC), CRC, gom scan | +| `include/lidarlib/config.hpp` + `src/olei_config.cpp` | `Config`/`LidarConfig` (gồm `brand`: `"OLEI"`/`"SICK"`), load/save `config.json`, tra cứu `ModelConfig` theo tên/theo hãng, và **hàm config `make_lidar()`** | +| `src/json_mini.hpp` | Parser/serializer JSON tối giản, chỉ dùng nội bộ cho `olei_config` (load/save `config.json`) | +| `include/lidarlib/sick_lidar.hpp` + `src/sick_lidar.cpp` | Driver riêng cho lidar **SICK TiM5xx/7xx** — giao thức SOPAS/CoLa-A qua TCP (port 2111), khác hoàn toàn UDP binary của OLEI. **Đã verify bằng TiM781S thật** (xem mục riêng bên dưới) | +| `examples/example.cpp` | Demo 1 lidar, `recv_scan()` blocking | +| `examples/test_dual.cpp` | Demo 2 lidar song song (2 thread) | +| `examples/sick_example.cpp` | Demo driver SICK TiM, `recv_scan()` blocking qua TCP | +| `examples/lidar_app.cpp` | Khung app headless: đọc `config.json` → `make_lidar()` từng con → in scan. Thay vòng `printf` bằng giao diện của bạn | +| `CMakeLists.txt` | Build `lidarlib` (OLEI + SICK + factory + config, chỉ phụ thuộc pthread) thành `.so`, cài `install()`/`find_package()` | + +Ba họ giao thức được driver tự nhận diện theo Frame ID/magic trong từng gói: + +- **Family A** (`0xFAF0`) — VB/VF/LR-1F. Header 20B + block 3B/điểm. Có CRC32. +- **Family B** (`0xFEF0`) — LR-1BS5/LR-1BS2. Header 40B (preamble `0x010F` + + frame id ở offset [2-3], chuỗi tên model ASCII ở offset [7-17)) + block + 8B/điểm. +- **Family C / protocol V3** (`0xFEAC`) — GS1-5. Header 48B, block 2/4B/điểm + tùy byte `Types`. **Port từ driver C# `OleiGS15Driver.cs` + (RobotNet10.RobotApp), CHƯA verify bằng phần cứng GS1-5 thật** (không có + thiết bị để sniff) — chỉ test bằng packet giả lập tự dựng theo đúng cấu trúc + header. + +## Output: 2 loại + +`Driver::recv_scan()` (và callback `set_scan_callback`) trả về +`ScanResult { LaserScan scan; ExtraInfo info; }` mỗi khi gom đủ 1 vòng quay: + +```cpp +lidarlib::Driver drv(lidarlib::MODEL_AUTO, "192.168.100.100", 2369); +drv.open(); +lidarlib::ScanResult result; +drv.recv_scan(result, 2000); +printf("%zu diem, model=%s\n", result.scan.ranges.size(), result.info.detected_model.c_str()); +``` + +**`LaserScan`** — cùng tên field/đơn vị với `sensor_msgs/LaserScan` của ROS +(radian, mét, giây): + +| Field | Ý nghĩa | +|---|---| +| `timestamp_ms` | Đồng hồ thiết bị (ms từ lúc bật nguồn); = 0 nếu family không có (xem `ExtraInfo`) | +| `angle_min`/`angle_max`/`angle_increment` | rad — đã unwrap liên tục, KHÔNG bị giới hạn `[-π,π]` | +| `time_increment`/`scan_time` | Luôn = 0 — thiết bị không báo timing chi tiết đó | +| `range_min`/`range_max` | m — lấy từ `ModelConfig` (giá trị đặt sẵn, **không đo được mỗi scan**) | +| `ranges[]`/`intensities[]` | m / 0-255 (đọc lại thành float như ROS) | + +**`ExtraInfo`** — thông tin thêm tuỳ family/model thực tế của packet, field +nào thiết bị không có thì giữ `std::nullopt`: + +| Field | Family | Verify hardware? | +|---|---|---| +| `detected_model` | mọi family (qua `MODEL_AUTO`) | Family B verify bằng sniff sống | +| `error_status` | Family A | Verify | +| `distance_scale_mm` | Family A/B | Verify | +| `rotation_raw` | Family A | Raw, chưa decode ý nghĩa | +| `distance_ratio_raw`, `scan_frequency_raw`, `input_status`, `output_status`, `field_status`, `status_flags` | Family C/GS1-5 | Raw, **chưa verify hardware thật** | + +Vì sao cần "unwrap": góc từng điểm được lọc theo FOV ở hệ **có dấu** +`[-180, 180]` (0 = phía trước, dương = bên trái) — nhưng hệ này gãy ở biên +±180° đối với lidar quét 360°. Trước khi đưa vào `LaserScan`, driver unwrap +lại thành một dải góc liên tục trong từng vòng quay (`Driver::push_point()` +trong `src/olei_lidar.cpp`), nên `angle_min`/`angle_max`/`ranges[]` luôn đơn +điệu — đúng kiểu mảng mà `sensor_msgs/LaserScan` kỳ vọng. + +## Kết nối lidar + +Mạng tham chiếu trên host này (`/home/robotics`): + +``` +eth0: 192.168.100.100/24 + ├─ front (scan_1): DeviceIp 192.168.100.11, DevicePort 2368 + └─ rear (scan_2): DeviceIp 192.168.100.12, DevicePort 2369 +``` + +(Khớp với `RobotApp/RobotNet10.RobotApp/appsettings.json`, các mục +`Olei-front`/`Olei-rear`.) + +Kiểm tra kết nối trước khi test: + +```bash +ip -4 addr show eth0 # phải thấy inet 192.168.100.100/24 +ping -c1 192.168.100.11 # front +ping -c1 192.168.100.12 # rear +``` + +**Lưu ý quan trọng:** nếu `RobotNet10.RobotApp` đang chạy, nó bind sẵn port +2368/2369 (không bật `SO_REUSEPORT`) → driver standalone sẽ bind lỗi +(`Khong mo duoc socket... interface khong ton tai?`). Kiểm tra ai đang giữ port: + +```bash +ss -lunp | grep -E '2368|2369' +``` + +## Build + +```bash +cmake -S . -B build -DCMAKE_BUILD_TYPE=Release +cmake --build build -j"$(nproc)" +``` + +Sinh ra `build/liblidarlib.so` (driver OLEI + SICK + factory + config, không +phụ thuộc gì ngoài pthread) và 4 binary demo (`example`, `test_dual`, +`sick_example`, `lidar_app`). Tắt build demo bằng `-DLIDARLIB_BUILD_EXAMPLES=OFF`. +Muốn ra static lib `.a` thay vì `.so` thì thêm `-DBUILD_SHARED_LIBS=OFF`. + +Cài vào hệ thống để dùng `find_package(lidarlib)` từ project khác — `/usr/local` +chỉ root mới ghi được nên cần `sudo`, không thì `cmake --install` báo lỗi +permission denied: + +```bash +sudo cmake --install build --prefix /usr/local +``` + +Muốn cài không cần `sudo` thì đổi prefix sang thư mục riêng trong `$HOME` (vd +`~/.local`), rồi thêm `-DCMAKE_PREFIX_PATH=~/.local` khi configure project nào +gọi `find_package(lidarlib)`: + +```bash +cmake --install build --prefix "$HOME/.local" +``` + +```cmake +# trong CMakeLists.txt của project dùng thư viện này +find_package(lidarlib REQUIRED) +target_link_libraries(my_app PRIVATE lidarlib::lidarlib) +``` + +Sau khi link, giao diện chỉ cần `#include "lidarlib/lidarlib.hpp"` rồi gọi +`lidarlib::make_lidar()` — xem "Tóm tắt API" ở đầu README. + +Vẫn có thể build từng file bằng g++ thuần nếu không muốn dùng CMake: + +```bash +g++ -std=c++17 -O2 -pthread -Wall -Wextra -Iinclude -o test_dual examples/test_dual.cpp src/olei_lidar.cpp +g++ -std=c++17 -O2 -pthread -Wall -Wextra -Iinclude -o example examples/example.cpp src/olei_lidar.cpp +g++ -std=c++17 -O2 -pthread -Wall -Wextra -Iinclude -o lidar_app examples/lidar_app.cpp src/olei_lidar.cpp src/sick_lidar.cpp src/olei_config.cpp +g++ -std=c++17 -O2 -pthread -Wall -Wextra -Iinclude -o sick_example examples/sick_example.cpp src/sick_lidar.cpp +``` + +## Test thử + +### 2 lidar song song + +```bash +./build/test_dual +``` + +In ra 5 scan mỗi bên, kèm số điểm, timestamp, error status, và **model đã tự +dò được** (`model=...`). + +### 1 lidar + +Sửa model/IP/port trong `examples/example.cpp` rồi build lại, hoặc gọi trực tiếp: + +```cpp +lidarlib::Driver drv(lidarlib::MODEL_AUTO, "192.168.100.100", 2369); +drv.open(); +lidarlib::ScanResult result; +drv.recv_scan(result, 2000); +``` + +### Lidar lắp úp ngược + +Constructor có tham số thứ 4 `inverted` (mặc định `false`). Đặt `true` nếu +thiết bị bị lắp lật 180° quanh trục hướng về phía trước — driver tự đảo dấu +góc từng điểm (`angle = -angle`, chuẩn hóa lại về `-180..180`) để output luôn +đúng theo hệ quy chiếu xe, không phụ thuộc hướng lắp vật lý: + +```cpp +lidarlib::Driver drv(lidarlib::MODEL_AUTO, "192.168.100.100", 2369, /*inverted=*/true); +``` + +Đã verify bằng sniff sống: chạy `inverted=false` góc tăng dần theo thời gian, +chạy `inverted=true` góc giảm dần với cùng bước góc — đúng chữ ký của đảo dấu. + +## Cấu hình & chạy (lidar_app + config.json) + +Không còn Web UI. Cấu hình là một file JSON đơn giản — `examples/lidar_app.cpp` +đọc nó, mở từng lidar qua đúng **một hàm** `lidarlib::make_lidar()`, rồi đọc scan +trên mỗi thread. Đây là khung mẫu để bạn thay vòng `printf` bằng giao diện +riêng (Qt, ImGui, ROS node, v.v.). + +```bash +./build/lidar_app # đọc/tạo config.json cạnh chỗ chạy +./build/lidar_app my_config.json # đường dẫn config khác +``` + +`config.json` — danh sách lidar, không cố định số lượng. `brand` chọn loại +driver (`"OLEI"` = UDP, `"SICK"` = TCP/SOPAS); `model` tra trong bảng +`ModelConfig` (tên lạ → tự lùi về mặc định của hãng: `AUTO` cho OLEI, +`SICK-TIM571` cho SICK); `inverted` chỉ có tác dụng với OLEI. Bỏ trống `brand` +thì mặc định `"OLEI"` (tương thích file cũ). + +```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", "inverted":false} + ] +} +``` + +Trong code, đọc/ghi file bằng `lidarlib::load_config(path)` / `lidarlib::save_config(path, cfg)` +(file hỏng → trả về mặc định, không crash). Giao diện của bạn tự quyết khi nào +lưu — thư viện không tự bind port hay phục vụ HTTP gì cả. + +### Sniff packet thô (debug khi nghi ngờ offset header) + +```bash +python3 - <<'EOF' +import socket +s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) +s.bind(("192.168.100.100", 2369)) # đổi port theo lidar cần xem +data, addr = s.recvfrom(4096) +print("from", addr, "len", len(data)) +print(data[:40].hex(' ')) +EOF +``` + +Luôn ưu tiên capture thật hơn là tin comment trong code — comment mô tả ý +định lúc viết, không phải offset đã verify trên thiết bị thật. + +## Chọn `ModelConfig` + +| Constant | FOV (deg, có dấu) | range_min/max (m) | Khi dùng | +|---|---|---|---| +| `MODEL_VB` | -135…135 | 0.05…30 | 2D 270°, Family A | +| `MODEL_VF` | -180…180 | 0.05…30 | 2D 360°, Family A | +| `MODEL_LR1F` | -180…180 | 0.05…50 | 2D 360° 50m, Family A | +| `MODEL_LR1BS5` | -180…180 | 0.05…30 | 2D 360°, Family B | +| `MODEL_LR16F` | -135…135 | 0.05…30 | 3D 16-line | +| `MODEL_GS15` | -180…180 | 0.05…30 | 2D 360°, Family C/V3 — **chưa verify hardware thật** | +| `MODEL_AUTO` | -180…180 (mặc định, có thể tự thu hẹp) | 0.05…30 | Không biết trước model | + +`range_min_m`/`range_max_m` chỉ là giá trị đặt sẵn để điền vào +`LaserScan::range_min/range_max` (không đọc từ packet) — chỉnh trong +`include/lidarlib/lidar.hpp` theo datasheet thật của từng model nếu cần chính xác. + +`MODEL_AUTO`: chỉ có tác dụng tự-dò với **Family B** (đọc chuỗi tên model +trong header). Driver luôn lưu lại **tên thật** đọc từ packet (vd +`"OLELR-1BS5"`, `"OLELR-1BS2"`) — gọi `drv.detected_model()` hoặc +`result.info.detected_model` để xem (trả về `"AUTO"` nếu chưa nhận gói Family B +nào). FOV chỉ tự thu hẹp khi tên đó khớp một entry trong `kModelTable` +(`src/olei_lidar.cpp`); nếu không khớp, FOV giữ nguyên mặc định 360° +(`-180..180`, không mất điểm) — an toàn nhưng có thể giữ lại điểm ngoài FOV +thật của thiết bị nếu thiết bị đó không quét tròn. + +Family A không mang chuỗi tên model trong packet, nên `MODEL_AUTO` trên thiết +bị Family A cũng giữ nguyên FOV rộng — phải chỉ định model cụ thể (VD +`MODEL_VB`) nếu muốn thu hẹp FOV cho thiết bị góc hẹp. + +## Lidar SICK (TiM5xx/7xx) — driver riêng + +`lidarlib::SickDriver` (`include/lidarlib/sick_lidar.hpp` + `src/sick_lidar.cpp`) là +driver **độc lập** với `lidarlib::Driver` ở trên — không phải thêm 1 family vào +driver OLEI, vì giao thức khác hẳn: + +- Kết nối **TCP** (SOPAS, port mặc định 2111) tới lidar, không phải UDP + broadcast như OLEI. +- Telegram là **ASCII** (CoLa-A), đóng khung bằng `STX`(0x02)/`ETX`(0x03), + không có CRC32 như Family A. +- Thiết bị đứng im cho tới khi driver gửi lệnh `sEN LMDscandata 1` — `open()` + tự làm việc này; `close()` gửi `sEN LMDscandata 0` trước khi đóng socket. + +Output vẫn dùng chung `ScanResult`/`LaserScan`/`ExtraInfo` như driver OLEI nên +gọi giống hệt: + +```cpp +lidarlib::SickDriver drv(lidarlib::MODEL_SICK_TIM571, "192.168.0.1", 2111); +drv.open(); +lidarlib::ScanResult result; +drv.recv_scan(result, 2000); +``` + +| Constant | FOV (deg, có dấu) | range_min/max (m) | Khi dùng | +|---|---|---|---| +| `MODEL_SICK_TIM5XX` | -135…135 | 0.05…10 | TiM551/561, 270° | +| `MODEL_SICK_TIM571` | -135…135 | 0.05…25 | TiM571, 270° | +| `MODEL_SICK_TIM7XX` | -135…135 | 0.05…25 | TiM781, 270° | + +**Đã verify bằng phần cứng thật**: chạy trực tiếp với 1 con **SICK TiM781S** +(FW `V5.11-14.10.24`, DeviceIdent đọc qua `sRN DeviceIdent` trên cổng 2111) +tại `192.168.100.22:2111` — `MODEL_SICK_TIM7XX`. Kết quả khớp đúng datasheet +TiM781S: 811 điểm/scan trải từ -45°…225° (270° FOV), `angle_increment` = +0.333° (1/3°, đúng độ phân giải góc của dòng 781), khoảng cách 0.3-1.5m ổn +định qua nhiều scan liên tiếp, kênh `RSSI1` có giá trị intensity hợp lý +(không phải toàn 0), `error_status` = 0x00. Điều này xác nhận layout +`LMDscandata` trong `parse_lmdscandata()` (`src/sick_lidar.cpp`) — kênh +`DIST1`/`RSSI1`, scaling factor IEEE-754, start angle/step width — đọc đúng +trên hardware thật, không chỉ đúng theo tài liệu nữa. + +**Vẫn chưa verify**: chiều quy ước góc 0° (thẳng phía trước thiết bị hay +hướng khác — chưa đối chiếu với hướng lắp vật lý thật), thiết bị có encoder +(`NumEncoders > 0`, nhánh `next()`×2 chưa từng chạy qua vì test thực tế không +có encoder), và nhánh 8-bit channel (`Num8BitChannels`, thiết bị test chỉ +dùng kênh 16-bit). `MODEL_SICK_TIM5XX`/`MODEL_SICK_TIM571` (FOV/range theo +datasheet) cũng chưa test trên phần cứng — chỉ `MODEL_SICK_TIM7XX` đã chạy +thật. + +`SickDriver` nối vào `config.json` qua field `LidarConfig::brand` (`"OLEI"` +hoặc `"SICK"`, mặc định `"OLEI"`): `lidarlib::make_lidar()` thấy `brand=="SICK"` thì +trả về `lidarlib::SickDriver` (TCP/SOPAS) thay cho `lidarlib::Driver` (UDP), cùng kiểu +trả về `std::unique_ptr` nên phía gọi không phải phân biệt. Xem +`examples/lidar_app.cpp` (chạy theo config) hoặc `examples/sick_example.cpp` +(dùng thẳng `SickDriver` qua API C++). diff --git a/cmake/lidarlibConfig.cmake.in b/cmake/lidarlibConfig.cmake.in new file mode 100644 index 0000000..661baef --- /dev/null +++ b/cmake/lidarlibConfig.cmake.in @@ -0,0 +1,6 @@ +@PACKAGE_INIT@ + +include(CMakeFindDependencyMacro) +find_dependency(Threads) + +include("${CMAKE_CURRENT_LIST_DIR}/lidarlibTargets.cmake") diff --git a/config.json b/config.json new file mode 100644 index 0000000..42a8c19 --- /dev/null +++ b/config.json @@ -0,0 +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}]} diff --git a/examples/example.cpp b/examples/example.cpp new file mode 100644 index 0000000..c2d5a6f --- /dev/null +++ b/examples/example.cpp @@ -0,0 +1,48 @@ +// example.cpp — quick try-out of the OLEI LiDAR driver +#include "lidarlib/lidar.hpp" +#include + +int main() { + // ── pick a model ──────────────────────────────────────────────────────── + // lidarlib::Driver drv(lidarlib::MODEL_VF); // 2D 360° + // lidarlib::Driver drv(lidarlib::MODEL_LR1F); // 2D 360°, 50m + lidarlib::Driver drv(lidarlib::MODEL_VB); // 2D 270° + + if (!drv.open()) { + fprintf(stderr, "Không mở được socket\n"); + return 1; + } + + // ── option 1: blocking recv ───────────────────────────────────────────── + 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"); + 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()); + + // Print the first few points + 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]); + } + } + + // ── option 2: callback (your own loop) ────────────────────────────────── + // drv.set_scan_callback([](const lidarlib::ScanResult& result) { + // printf("Got scan: %zu pts\n", result.scan.ranges.size()); + // }); + // while (true) drv.spin_once(); + + drv.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 new file mode 100644 index 0000000..e1f2d2c --- /dev/null +++ b/examples/lidar_app.cpp @@ -0,0 +1,83 @@ +// lidar_app.cpp — headless skeleton app and integration template. +// +// Loads the lidar list from config.json, opens each one through the SINGLE +// config function lidarlib::make_lidar() (no per-brand branching), then reads scans +// on one thread per lidar and prints a one-line summary. There is no web UI: +// edit config.json directly, or build your own GUI on top of this same API. +// +// What a GUI author keeps: load_config() + make_lidar() + the recv_scan() loop. +// What a GUI author replaces: the printf() with their own rendering/persistence, +// and save_config() to write edits back. +// +// ./lidar_app [config.json] +#include "lidarlib/lidarlib.hpp" +#include +#include +#include +#include +#include +#include + +namespace { + +std::atomic g_running{true}; +void on_signal(int) { g_running = false; } + +// One reader thread per lidar. Owns the handle for its whole lifetime so the +// per-instance receive buffers never race another thread. +void run_lidar(lidarlib::LidarConfig cfg) { + std::unique_ptr lidar = lidarlib::make_lidar(cfg); // the one config call + if (!lidar->open()) { + fprintf(stderr, "[%s] khong mo duoc %s %s:%u\n", + cfg.name.c_str(), cfg.brand.c_str(), cfg.ip.c_str(), cfg.port); + 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); + + while (g_running) { + lidarlib::ScanResult result; + if (!lidar->recv_scan(result, 1000)) continue; // timeout -> retry + + // Output #1: ROS-shaped LaserScan (same for every lidar) + const lidarlib::LaserScan& scan = result.scan; + // Output #2: ExtraInfo (fields vary by model/family) + 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); + } + + lidar->close(); + printf("[%s] da dong\n", cfg.name.c_str()); +} + +} // namespace + +int main(int argc, char** argv) { + setvbuf(stdout, nullptr, _IOLBF, 0); // line-buffer so logs show promptly + + const std::string config_path = (argc > 1) ? argv[1] : "config.json"; + + lidarlib::Config cfg = lidarlib::load_config(config_path); + lidarlib::save_config(config_path, cfg); // ensure the file exists & is editable + + if (cfg.lidars.empty()) { + fprintf(stderr, "Khong co lidar nao trong %s\n", config_path.c_str()); + return 1; + } + + std::signal(SIGINT, on_signal); + std::signal(SIGTERM, on_signal); + + std::vector threads; + threads.reserve(cfg.lidars.size()); + for (const auto& lc : cfg.lidars) threads.emplace_back(run_lidar, lc); + + printf("Dang chay %zu lidar tu %s. Ctrl-C de dung.\n", + cfg.lidars.size(), config_path.c_str()); + for (auto& t : threads) t.join(); + return 0; +} diff --git a/examples/sick_example.cpp b/examples/sick_example.cpp new file mode 100644 index 0000000..20e2e2f --- /dev/null +++ b/examples/sick_example.cpp @@ -0,0 +1,40 @@ +// sick_example.cpp — quick try-out of the SICK TiM driver (SOPAS/CoLa-A, TCP) +// +// Verified against a real SICK TiM781S — see the caveat in +// include/lidarlib/sick_lidar.hpp for exactly what was (and wasn't) confirmed. +#include "lidarlib/sick_lidar.hpp" +#include + +int main() { + lidarlib::SickDriver drv(lidarlib::MODEL_SICK_TIM571, "192.168.0.1", 2111); + + if (!drv.open()) { + fprintf(stderr, "Không kết nối được TCP tới lidar SICK\n"); + return 1; + } + + for (int i = 0; i < 10; ++i) { + lidarlib::ScanResult result; + if (!drv.recv_scan(result, 2000)) { + fprintf(stderr, "Timeout hoặc lỗi nhận telegram\n"); + break; + } + const lidarlib::LaserScan& scan = result.scan; + const lidarlib::ExtraInfo& info = result.info; + printf("Scan #%d: %zu điểm, ts=%u ms, err=0x%02X, model=%s\n", + i, scan.ranges.size(), scan.timestamp_ms, info.error_status, + info.detected_model.c_str()); + + for (size_t j = 0; j < 20 && j < scan.ranges.size(); ++j) { + float angle_deg = (scan.angle_min + j * scan.angle_increment) * 180.f / 3.14159265f; + printf(" [%zu] angle=%.2f° dist=%.3fm intensity=%.0f\n", + j, angle_deg, scan.ranges[j], scan.intensities[j]); + } + } + + drv.close(); + return 0; +} + +// Build: +// g++ -std=c++17 -O2 -Iinclude -o sick_example examples/sick_example.cpp src/sick_lidar.cpp diff --git a/examples/test_dual.cpp b/examples/test_dual.cpp new file mode 100644 index 0000000..0cf99db --- /dev/null +++ b/examples/test_dual.cpp @@ -0,0 +1,55 @@ +// test_dual.cpp — test 2 Olei lidars (front + rear) concurrently, per appsettings.json +// Olei-front: scan_1, DeviceIp 192.168.100.11, LocalIp 192.168.100.100, DevicePort 2368 +// Olei-rear : scan_2, DeviceIp 192.168.100.12, LocalIp 192.168.100.100, DevicePort 2369 +#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); + if (!drv.open()) { + fprintf(stderr, "[%s] Khong mo duoc socket tren %s:%u (interface khong ton tai?)\n", + tag, local_ip.c_str(), port); + return; + } + printf("[%s] Da bind %s:%u, dang doi scan...\n", tag, local_ip.c_str(), port); + + for (int i = 0; i < n_scans; ++i) { + lidarlib::ScanResult result; + if (!drv.recv_scan(result, 2000)) { + fprintf(stderr, "[%s] Timeout/loi nhan packet (scan #%d)\n", tag, i); + continue; + } + const lidarlib::LaserScan& scan = result.scan; + const lidarlib::ExtraInfo& info = result.info; + printf("[%s] Scan #%d: %zu diem, ts=%u ms, err=0x%02X, model=%s\n", + tag, i, scan.ranges.size(), scan.timestamp_ms, info.error_status, + info.detected_model.c_str()); + for (size_t j = 0; j < 20 && j < scan.ranges.size(); ++j) { + float angle_deg = (scan.angle_min + j * scan.angle_increment) * 180.f / 3.14159265f; + printf(" [%zu] angle=%.2f dist=%.3fm intensity=%.0f\n", + j, angle_deg, scan.ranges[j], scan.intensities[j]); + } + } + drv.close(); +} + +int main() { + // Both front and rear are Family B in practice — front's real header + // string is "OLELR-1BS2", rear's is "OLELR-1BS5" (verified via live UDP + // sniff), NOT the VB (Family A) model the config name suggested. With + // MODEL_AUTO, the driver reads the real model name from the header and + // narrows the FOV when it matches a known entry in kModelTable + // (olei_lidar.cpp); "1BS5" matches (→ full 360°), but "1BS2" doesn't, so + // front currently stays at the unfiltered 360° default. Call + // drv.detected_model() to see which name was actually read. + 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/config.hpp b/include/lidarlib/config.hpp new file mode 100644 index 0000000..6013cf4 --- /dev/null +++ b/include/lidarlib/config.hpp @@ -0,0 +1,80 @@ +#pragma once +#include "lidarlib/lidar.hpp" +#include +#include +#include + +namespace lidarlib { + +// ─── Settings for one lidar ───────────────────────────────────────────────── +// `name` is the unique key used to match entries across saves (rename = old +// key removed, new key added) — useful if your GUI reconciles a running set of +// lidars against an edited config list. +struct LidarConfig { + std::string name = "lidar"; + std::string ip = "0.0.0.0"; + uint16_t port = 2368; + std::string model = "AUTO"; // must match an entry in model_names_for_brand(brand) + bool inverted = false; // only applies to brand "OLEI" — SickDriver has no equivalent + std::string brand = "OLEI"; // must match an entry in brand_names() — "OLEI" or "SICK" + + 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; + } + friend bool operator!=(const LidarConfig& a, const LidarConfig& b) { return !(a == b); } +}; + +// Any number of lidars — managed as a list so a consuming app/GUI can +// add/remove entries freely instead of being locked to a fixed front/rear pair. +struct Config { + std::vector lidars = { + {"front", "0.0.0.0", 2368, "AUTO", false}, + {"rear", "0.0.0.0", 2369, "AUTO", true}, + }; +}; + +// Known model name -> ModelConfig (matches the constants in lidar.hpp/sick_lidar.hpp). +// Returns nullptr if name doesn't match any entry. +const ModelConfig* model_by_name(const std::string& name); + +// Names accepted by model_by_name(), for populating a UI dropdown. +const std::vector& model_names(); + +// Brand names accepted in LidarConfig::brand ("OLEI", "SICK"), for populating +// a UI dropdown. +const std::vector& brand_names(); + +// Subset of model_names() valid for a given brand (e.g. "SICK" -> the +// MODEL_SICK_* names) — empty if `brand` doesn't match any entry in +// brand_names(). Used to filter the model dropdown once a brand is picked, +// and to validate that LidarConfig::model actually belongs to its brand. +const std::vector& model_names_for_brand(const std::string& brand); + +// Load config.json at `path`. If the file doesn't exist, returns defaults +// (and does NOT create the file — caller decides whether to save it). +Config load_config(const std::string& path); + +// Overwrite `path` with `cfg` serialized as JSON. +void save_config(const std::string& path, const Config& cfg); + +// ─── THE single config function ───────────────────────────────────────────── +// Build a ready-to-open lidar from one LidarConfig. This is the one entry point +// a GUI/app needs: `brand` selects the transport — exactly "SICK" → TCP, any +// other value (incl. "OLEI", empty, or an old config without the field) → OLEI +// UDP. `model` is resolved *for that brand*: a name that is unknown OR belongs +// to the other brand falls back to the brand's sensible default (MODEL_AUTO for +// OLEI, MODEL_SICK_TIM571 for SICK), so a mis-paired brand+model can't silently +// configure the wrong driver. `inverted` applies to OLEI only. Returns a unique +// handle to the unified Lidar interface — call ->open() then +// ->recv_scan(out, timeout_ms) to get ScanResult { LaserScan scan; ExtraInfo +// info; }. Never returns nullptr. +// +// lidarlib::LidarConfig c{"front", "192.168.1.10", 2368, "AUTO", false, "OLEI"}; +// auto lidar = lidarlib::make_lidar(c); +// lidar->open(); +// lidarlib::ScanResult r; +// lidar->recv_scan(r, 1000); // r.scan = LaserScan, r.info = ExtraInfo +std::unique_ptr make_lidar(const LidarConfig& cfg); + +} // namespace lidarlib diff --git a/include/lidarlib/lidar.hpp b/include/lidarlib/lidar.hpp new file mode 100644 index 0000000..79a87d2 --- /dev/null +++ b/include/lidarlib/lidar.hpp @@ -0,0 +1,230 @@ +#pragma once +#include +#include +#include +#include +#include + +namespace lidarlib { + +// ─── Default output: ROS sensor_msgs/LaserScan-shaped ────────────────────── +// Same field names/semantics as ROS's LaserScan message (radians, meters, +// seconds) so this can be bridged into a ROS node with a near-1:1 field copy. +// ranges[i]/intensities[i] correspond to angle = angle_min + i*angle_increment; +// the array spans exactly one revolution (or the model's FOV window) in the +// order the device actually swept it — angle_min/angle_max are NOT clamped to +// [-pi,pi], they just describe whatever contiguous window this revolution +// covered (matches how continuously-rotating lidars without a phase reset +// behave: the starting angle drifts slightly scan to scan). +struct LaserScan { + uint32_t timestamp_ms = 0; // device clock (ms since power-on); 0 if the + // family doesn't expose one (see ExtraInfo) + float angle_min = 0.f; // rad + float angle_max = 0.f; // rad + float angle_increment = 0.f; // rad + float time_increment = 0.f; // sec — device doesn't expose per-point timing, always 0 + float scan_time = 0.f; // sec — device doesn't expose per-scan timing, always 0 + float range_min = 0.f; // m — from ModelConfig, NOT measured per-scan + float range_max = 0.f; // m — from ModelConfig, NOT measured per-scan + std::vector ranges; // m + std::vector intensities; // 0-255 read back as float, like ROS does +}; + +// ─── Extra info: whatever diagnostic/header fields THIS family/model exposes ─ +// Fields the protocol family doesn't carry stay unset (std::nullopt). Several +// of these are raw, undecoded passthroughs of header bytes whose exact +// meaning hasn't been verified against real hardware/datasheet — see comments +// in olei_lidar.cpp next to where each is read. +struct ExtraInfo { + std::string detected_model = "AUTO"; // real model name read from the packet, or "AUTO" + uint8_t error_status = 0; // Family A only; BIT0=Monitor, BIT1=Voltage, BIT2=Temp + uint8_t distance_scale_mm = 0; // mm/count used to decode ranges this scan (0 = not reported) + + // Family A (0xFAF0) only — raw 16-bit "rotation info" header field, + // meaning not decoded/verified. + std::optional rotation_raw; + + // Family C / protocol V3 (0xFEAC, GS1-5) only — ported from the C# driver + // header layout, NOT cross-checked against real GS1-5 hardware. + 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; +}; + +// One complete revolution, in both forms at once. +struct ScanResult { + LaserScan scan; + ExtraInfo info; +}; + +// ─── Per-model configuration ─────────────────────────────────────────────── +// scan_angle_* use the SIGNED system [-180,180]: 0 = straight ahead, + = left, - = right. +// 360° lidars keep the full circle [-180,180]; narrow-FOV lidars (VB 270°) shrink it. +// range_min_m/range_max_m are sensor-spec placeholders (NOT read from any +// packet) used to fill LaserScan::range_min/range_max — adjust to the real +// datasheet values for each model if precision matters to your consumer. +struct ModelConfig { + const char* name; + float scan_angle_min; // deg — VB/LR-16F: -135, 360° models: -180 + float scan_angle_max; // deg — VB/LR-16F: 135, 360° models: 180 + float range_min_m = 0.05f; + float range_max_m = 30.f; +}; + +// Table of known models — the driver auto-detects the packet family (A=0xFAF0 / +// B=0xFEF0 / C=0xFEAC) per packet, so this config mainly decides the angular +// window (FOV) that gets kept. +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 }; // 2D 360° 50m +inline constexpr ModelConfig MODEL_LR1FMI { "LR-1FMI", -180.f, 180.f, 0.05f, 30.f }; // 2D 360°, 0.01°/LSB ~2400 pts/rev (Family B) +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° + +// Sentinel: model unknown ahead of time. Family B (0xFEF0) carries an ASCII +// model name string in its header (e.g. "OLELR-1BS5", verified via live UDP +// sniff) → the driver auto-detects it and narrows the FOV per the table +// above. Family C (0xFEAC, GS1-5) is identified by magic alone. Family A has +// no such string, so on a Family-A device MODEL_AUTO keeps the wide default +// FOV (-180..180, no points dropped) until the user specifies a concrete model. +inline constexpr ModelConfig MODEL_AUTO { "AUTO", -180.f, 180.f, 0.05f, 30.f }; + +// callback invoked whenever a complete scan is ready — shared by every driver +// (lidarlib::Driver, lidarlib::SickDriver) and the unified Lidar interface below. +using ScanCallback = std::function; + +// ─── Unified driver interface ─────────────────────────────────────────────── +// Common handle returned by lidarlib::make_lidar() (the single config function in +// config.hpp). Both the OLEI Driver (UDP) and the SICK SickDriver (TCP) derive +// from this, so a GUI/app can drive any supported lidar through one type and +// never branch on brand. Every call yields the same ScanResult { LaserScan +// scan; ExtraInfo info; } — output #1 (ROS-shaped LaserScan, identical across +// all models) and output #2 (ExtraInfo, model-specific extra fields). +class Lidar { +public: + virtual ~Lidar() = default; + + // Open the transport (UDP socket / TCP connection) and start receiving. + virtual bool open() = 0; + // Close the transport. + virtual void close() = 0; + // Block until one full scan is received; false on error/timeout. + // timeout_ms = 0 → block indefinitely. (No default here on purpose: the + // concrete drivers differ — OLEI 1000 ms, SICK 2000 ms — so callers using + // the interface must state the timeout they want.) + virtual bool recv_scan(ScanResult& out, int timeout_ms) = 0; + // Or set a callback and drive it from your own loop via spin_once(). + virtual void set_scan_callback(ScanCallback cb) = 0; + // Receive + dispatch the callback once (non-owning loop step). + virtual bool spin_once() = 0; + // Real model name read from the packet, or the configured name if the + // family carries none. See Driver::detected_model() for OLEI specifics. + virtual const char* detected_model() const = 0; +}; + +// ─── Driver ───────────────────────────────────────────────────────────────── +class Driver : public Lidar { +public: + // callback invoked whenever a complete scan is ready + using ScanCallback = lidarlib::ScanCallback; + + // ip : receiving host's bind address, usually "0.0.0.0" + // port : UDP port the lidar sends to (default 2368) + // cfg : model config + // inverted : set true if this physical unit is mounted upside-down + // (flipped 180° about its forward-facing axis). Mirrors every + // point's angle (angle = -angle) so output stays in the + // vehicle's frame regardless of mounting orientation — useful + // when e.g. front is mounted normally but rear is flipped. + explicit Driver(const ModelConfig& cfg, + const std::string& ip = "0.0.0.0", + uint16_t port = 2368, + bool inverted = false); + ~Driver(); + + // Non-copyable + Driver(const Driver&) = delete; + Driver& operator=(const Driver&) = delete; + + // Open the socket and start receiving + bool open() override; + + // Close the socket + void close() override; + + // Blocks until a full revolution has been received; returns false on error/timeout + // timeout_ms = 0 → block indefinitely + bool recv_scan(ScanResult& out, int timeout_ms = 1000) override; + + // Or use the callback (drive it from your own non-blocking loop) + void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); } + + // Receive + dispatch callback (call from your own loop) + bool spin_once() override; + + // The REAL model name read from the Family B/C header (only meaningful + // when the Driver was constructed with MODEL_AUTO). Always the actual + // string found in the packet (e.g. "OLELR-1BS2"), even when that model + // has no specific FOV entry in the table (FOV then stays at the 360° + // default). Returns "AUTO" if no Family B/C packet has been seen yet. + // Mirrored per-scan in ScanResult::info::detected_model. + const char* detected_model() const override { return detected_model_name_.c_str(); } + +private: + // ── parse Family A packet (ID=0xFAF0): 20B header, 3B block ── + bool parse_family_a(const uint8_t* buf, int len); + + // ── parse Family B packet (ID=0xFEF0): 40B header, 8B block ── + bool parse_family_b(const uint8_t* buf, int len); + + // ── parse Family C / protocol V3 packet (Magic=0xFEAC, GS1-5): 48B header ── + bool parse_family_c(const uint8_t* buf, int len); + + // Appends one point's angle (already signed+inverted+FOV-filtered by the + // caller), unwrapping it against the previous point in this revolution so + // the accumulated sequence stays continuous across the ±180° seam instead + // of jumping — required for LaserScan::angle_min/angle_max/ranges to stay + // monotonic for 360° devices. + void push_point(float signed_angle_deg, float dist_m, uint8_t intensity); + + // Once a full revolution is ready → flush into ready_result_ and fire the callback + void flush_scan(); + + ModelConfig cfg_; + std::string ip_; + uint16_t port_; + bool inverted_ = false; + int sock_fd_ = -1; + ScanCallback cb_; + + // Per-revolution accumulation buffers (parallel arrays, index-aligned) + std::vector pending_angle_deg_; // unwrapped, continuous + 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-around (revolution-boundary) detection, device space [0,360) + + // Per-revolution ExtraInfo accumulation — overwritten as packets for the + // in-progress revolution are parsed, then copied into ready_result_ on flush. + ExtraInfo pending_info_; + + // recv_scan()'s output, gated by a simple ready flag + ScanResult ready_result_; + bool scan_ready_ = false; + + // Per-instance receive buffer — NOT static, so that 2 lidars running on 2 + // threads don't overwrite each other's data (data race). + uint8_t recv_buf_[4096]; + + // Model auto-detection from the Family B/C header (see MODEL_AUTO) + 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 new file mode 100644 index 0000000..dc3e129 --- /dev/null +++ b/include/lidarlib/lidarlib.hpp @@ -0,0 +1,13 @@ +#pragma once +// ─── One-include convenience header ───────────────────────────────────────── +// Pull in the whole public API in a single line: +// +// #include "lidarlib/lidarlib.hpp" +// +// Gives you the data model (LaserScan / ExtraInfo / ScanResult), the unified +// Lidar interface, both concrete drivers (Driver = OLEI/UDP, SickDriver = +// SICK/TCP), the model/brand tables, config.json load/save, and the single +// config function lidarlib::make_lidar(). +#include "lidarlib/lidar.hpp" +#include "lidarlib/sick_lidar.hpp" +#include "lidarlib/config.hpp" diff --git a/include/lidarlib/sick_lidar.hpp b/include/lidarlib/sick_lidar.hpp new file mode 100644 index 0000000..1045420 --- /dev/null +++ b/include/lidarlib/sick_lidar.hpp @@ -0,0 +1,91 @@ +#pragma once +#include "lidarlib/lidar.hpp" +#include + +namespace lidarlib { + +// ─── SICK TiM5xx/7xx model presets ────────────────────────────────────────── +// FOV/range taken from SICK's public datasheets (NOT read from any packet — +// same placeholder convention as the OLEI ModelConfig constants in lidar.hpp). +// Scanning angle is 270° on every TiM5xx/7xx variant; only the rated range +// differs by model. +inline constexpr ModelConfig MODEL_SICK_TIM5XX { "SICK-TIM5xx", -135.f, 135.f, 0.05f, 10.f }; // TiM551/561, 270°, 10m +inline constexpr ModelConfig MODEL_SICK_TIM571 { "SICK-TIM571", -135.f, 135.f, 0.05f, 25.f }; // TiM571, 270°, 25m +inline constexpr ModelConfig MODEL_SICK_TIM7XX { "SICK-TIM7xx", -135.f, 135.f, 0.05f, 25.f }; // TiM781, 270°, 25m + +// ─── SickDriver — SICK TiM5xx/7xx over SOPAS/CoLa-A (TCP, default port 2111) ─ +// +// VERIFIED against a real SICK TiM781S (FW V5.11-14.10.24, MODEL_SICK_TIM7XX) +// on port 2111: 811 pts/scan over -45..225° (270° FOV), angle_increment = +// 0.333° (matches the 781's rated angular resolution), stable ranges, and a +// non-zero RSSI1 channel — confirms the DIST1/RSSI1 channel layout, the +// IEEE-754 scaling factor, and the start-angle/step-width decode in +// parse_lmdscandata() (sick_lidar.cpp) are correct on real hardware, not just +// per SICK's "Telegram Listing" doc. +// NOT yet verified: the angle-zero reference vs physical mounting direction, +// devices reporting NumEncoders > 0, the 8-bit-channel branch (the test unit +// only emitted 16-bit channels), and MODEL_SICK_TIM5XX/MODEL_SICK_TIM571's +// FOV/range numbers (only TiM7xx was tested). +// +// Protocol differences from lidarlib::Driver that justify a separate class +// instead of extending Driver: +// - Transport is TCP (a connection, request/response + streamed telegrams), +// not connectionless UDP broadcast. +// - Telegrams are ASCII (CoLa-A), framed by STX(0x02)/ETX(0x03) instead of +// the OLEI binary header+block layout — no CRC32 like Family A. +// - The device is passive until told to start: must send "sEN LMDscandata 1" +// before any scan telegram arrives. +class SickDriver : public Lidar { +public: + using ScanCallback = lidarlib::ScanCallback; // same callback shape, ScanResult-compatible + + // ip/port: SICK device's TCP endpoint (SOPAS default port 2111). + explicit SickDriver(const ModelConfig& cfg, + const std::string& ip, + uint16_t port = 2111); + ~SickDriver(); + + SickDriver(const SickDriver&) = delete; + SickDriver& operator=(const SickDriver&) = delete; + + // Connect + send "sEN LMDscandata 1" to start continuous scan output. + bool open() override; + + // Best-effort "sEN LMDscandata 0" then close the socket. + void close() override; + + // Blocks until one LMDscandata telegram has been parsed; returns false on + // error/timeout. timeout_ms = 0 → block indefinitely. + bool recv_scan(ScanResult& out, int timeout_ms = 2000) override; + + // Or use the callback (drive it from your own non-blocking loop) + void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); } + + // Receive + parse + dispatch callback (call from your own loop) + bool spin_once() override; + + // SICK telegrams carry no model string — returns the configured model name + // (e.g. "SICK-TIM7xx") so the unified Lidar interface stays consistent. + // Backed by an owned std::string (not cfg_.name, a borrowed const char*) so + // the pointer stays valid even if the ModelConfig was built from temporary + // storage — matching Driver::detected_model()'s ownership. + const char* detected_model() const override { return detected_model_name_.c_str(); } + +private: + bool send_telegram(const std::string& body); // wraps body with STX/ETX, writes to socket + bool read_telegram(std::string& out, int timeout_ms); // returns next STX..ETX frame, STX/ETX stripped + bool parse_lmdscandata(const std::string& telegram, ScanResult& out); + + ModelConfig cfg_; + std::string detected_model_name_; // owned copy of cfg_.name (stable lifetime) + std::string ip_; + uint16_t port_; + int sock_fd_ = -1; + ScanCallback cb_; + + // Accumulates bytes read from the TCP stream between telegram boundaries — + // per-instance (not static) so 2 SickDrivers on 2 threads don't race. + std::string recv_buf_; +}; + +} // namespace lidarlib diff --git a/src/json_mini.hpp b/src/json_mini.hpp new file mode 100644 index 0000000..f79c20e --- /dev/null +++ b/src/json_mini.hpp @@ -0,0 +1,238 @@ +// json_mini.hpp — minimal header-only JSON parse/serialize, just enough for +// flat-ish config objects (no comments, no streaming, no error recovery). +// Not a general-purpose JSON library — kept tiny on purpose. +#pragma once +#include +#include +#include +#include +#include +#include +#include + +namespace json { + +enum class Type { Null, Bool, Number, String, Object, Array }; + +struct Value { + Type type = Type::Null; + bool b = false; + double num = 0; + std::string str; + std::vector arr; + std::vector> obj; + + static Value make_object() { Value v; v.type = Type::Object; return v; } + static Value make_string(std::string s) { Value v; v.type = Type::String; v.str = std::move(s); return v; } + static Value make_number(double n) { Value v; v.type = Type::Number; v.num = n; return v; } + static Value make_bool(bool x) { Value v; v.type = Type::Bool; v.b = x; return v; } + + void set(const std::string& key, Value v) { + for (auto& kv : obj) if (kv.first == key) { kv.second = std::move(v); return; } + obj.emplace_back(key, std::move(v)); + } + + const Value* find(const std::string& key) const { + for (auto& kv : obj) if (kv.first == key) return &kv.second; + return nullptr; + } + + std::string get_string(const std::string& key, const std::string& def = "") const { + const Value* v = find(key); + return (v && v->type == Type::String) ? v->str : def; + } + double get_number(const std::string& key, double def = 0) const { + const Value* v = find(key); + return (v && v->type == Type::Number) ? v->num : def; + } + bool get_bool(const std::string& key, bool def = false) const { + const Value* v = find(key); + return (v && v->type == Type::Bool) ? v->b : def; + } + + std::string dump() const { + std::string out; + dump_to(out); + return out; + } + +private: + static void escape_into(const std::string& s, std::string& out) { + out += '"'; + for (char c : s) { + switch (c) { + case '"': out += "\\\""; break; + case '\\': out += "\\\\"; break; + case '\n': out += "\\n"; break; + default: out += c; break; + } + } + out += '"'; + } + + void dump_to(std::string& out) const { + switch (type) { + case Type::Null: out += "null"; break; + case Type::Bool: out += b ? "true" : "false"; break; + case Type::Number: { + if (num == static_cast(num)) out += std::to_string(static_cast(num)); + else out += std::to_string(num); + break; + } + case Type::String: escape_into(str, out); break; + case Type::Array: { + out += '['; + for (size_t i = 0; i < arr.size(); ++i) { + if (i) out += ','; + arr[i].dump_to(out); + } + out += ']'; + break; + } + case Type::Object: { + out += '{'; + for (size_t i = 0; i < obj.size(); ++i) { + if (i) out += ','; + escape_into(obj[i].first, out); + out += ':'; + obj[i].second.dump_to(out); + } + out += '}'; + break; + } + } + } +}; + +// ── Parser ────────────────────────────────────────────────────────────── +class ParseError : public std::runtime_error { +public: + explicit ParseError(const std::string& what) : std::runtime_error(what) {} +}; + +namespace detail { + +class Parser { +public: + explicit Parser(const std::string& s) : s_(s) {} + + Value parse() { + skip_ws(); + Value v = parse_value(); + skip_ws(); + return v; + } + +private: + const std::string& s_; + size_t pos_ = 0; + + char peek() const { + if (pos_ >= s_.size()) throw ParseError("unexpected end of JSON"); + return s_[pos_]; + } + char next() { return s_[pos_++]; } + void skip_ws() { while (pos_ < s_.size() && std::isspace(static_cast(s_[pos_]))) ++pos_; } + void expect(char c) { + if (pos_ >= s_.size() || s_[pos_] != c) + throw ParseError(std::string("expected '") + c + "'"); + ++pos_; + } + bool starts_with(const char* lit) { + size_t n = std::strlen(lit); + if (s_.compare(pos_, n, lit) == 0) { pos_ += n; return true; } + return false; + } + + Value parse_value() { + skip_ws(); + char c = peek(); + if (c == '{') return parse_object(); + if (c == '[') return parse_array(); + if (c == '"') return Value::make_string(parse_string()); + if (starts_with("true")) return Value::make_bool(true); + if (starts_with("false")) return Value::make_bool(false); + if (starts_with("null")) { Value v; v.type = Type::Null; return v; } + return parse_number(); + } + + Value parse_object() { + Value v = Value::make_object(); + expect('{'); + skip_ws(); + if (peek() == '}') { ++pos_; return v; } + while (true) { + skip_ws(); + std::string key = parse_string(); + skip_ws(); + expect(':'); + Value val = parse_value(); + v.obj.emplace_back(std::move(key), std::move(val)); + skip_ws(); + char c = next(); + if (c == ',') continue; + if (c == '}') break; + throw ParseError("expected ',' or '}' in object"); + } + return v; + } + + Value parse_array() { + Value v; v.type = Type::Array; + expect('['); + skip_ws(); + if (peek() == ']') { ++pos_; return v; } + while (true) { + v.arr.push_back(parse_value()); + skip_ws(); + char c = next(); + if (c == ',') continue; + if (c == ']') break; + throw ParseError("expected ',' or ']' in array"); + } + return v; + } + + std::string parse_string() { + expect('"'); + std::string out; + while (true) { + char c = next(); + if (c == '"') break; + if (c == '\\') { + char e = next(); + switch (e) { + case 'n': out += '\n'; break; + case 't': out += '\t'; break; + case '"': out += '"'; break; + case '\\': out += '\\'; break; + case '/': out += '/'; break; + default: out += e; break; + } + } else { + out += c; + } + } + return out; + } + + Value parse_number() { + size_t start = pos_; + if (pos_ < s_.size() && (s_[pos_] == '-' || s_[pos_] == '+')) ++pos_; + while (pos_ < s_.size() && + (std::isdigit(static_cast(s_[pos_])) || s_[pos_] == '.' || + s_[pos_] == 'e' || s_[pos_] == 'E' || s_[pos_] == '-' || s_[pos_] == '+')) + ++pos_; + if (pos_ == start) throw ParseError("invalid number"); + return Value::make_number(std::strtod(s_.substr(start, pos_ - start).c_str(), nullptr)); + } +}; + +} // namespace detail + +inline Value parse(const std::string& s) { + detail::Parser p(s); + return p.parse(); +} + +} // namespace json diff --git a/src/olei_config.cpp b/src/olei_config.cpp new file mode 100644 index 0000000..2bcca40 --- /dev/null +++ b/src/olei_config.cpp @@ -0,0 +1,156 @@ +#include "lidarlib/config.hpp" +#include "lidarlib/sick_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" }, +}; + +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)); + 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); + 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 { +// Like model_by_name() but only accepts a model that actually belongs to +// `brand` — so a mis-paired brand+model (e.g. brand="OLEI", model="SICK-TIM571") +// doesn't resolve to the other brand's preset. Returns nullptr if the name +// isn't a valid model for that brand. +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"}; + 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; // defaults + 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 -> fall back to defaults rather than crash + } + + 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 other than the exact string "SICK" is treated as OLEI (this also + // keeps old config.json files without a `brand` field working). + const bool is_sick = (cfg.brand == "SICK"); + + // Resolve the model *for this brand*: an unknown/empty name, OR a name that + // belongs to the other brand, falls back to the brand default (OLEI + // auto-detects from the packet; SICK has no model string in the wire + // protocol so we pick a mid-range preset). This prevents a mis-paired + // brand+model from silently configuring the wrong driver/FOV. + const ModelConfig* model = model_by_name_for_brand(cfg.model, is_sick ? "SICK" : "OLEI"); + if (!model) model = is_sick ? &MODEL_SICK_TIM571 : &MODEL_AUTO; + + if (is_sick) + return std::make_unique(*model, cfg.ip, cfg.port); + return std::make_unique(*model, cfg.ip, cfg.port, cfg.inverted); +} + +} // namespace lidarlib diff --git a/src/olei_lidar.cpp b/src/olei_lidar.cpp new file mode 100644 index 0000000..403c4af --- /dev/null +++ b/src/olei_lidar.cpp @@ -0,0 +1,488 @@ +#include "lidarlib/lidar.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace lidarlib { + +namespace { +constexpr float kDeg2Rad = 3.14159265358979323846f / 180.f; +} + +// ── Little-endian helpers ──────────────────────────────────────────────────── +static inline uint16_t le16(const uint8_t* p) { + return static_cast(p[0]) | (static_cast(p[1]) << 8); +} +static 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); +} + +// Normalize any angle into the SIGNED system (-180, 180]: 0 = straight ahead, +// + = left, - = right. This lets a model's FOV (e.g. VB -135…135) correctly +// filter lidars that report angles in 0–360 too. +static inline float to_signed_deg(float deg) { + deg = std::fmod(deg, 360.f); + if (deg < 0.f) deg += 360.f; // → [0,360) + if (deg > 180.f) deg -= 360.f; // → (-180,180] + return deg; +} + +// Mirror the angle when the unit is mounted upside-down (flipped 180° about +// its forward axis), so output angle stays correct relative to the vehicle +// frame regardless of physical mounting. Must run AFTER to_signed_deg() and +// BEFORE the FOV filter, since the FOV window is defined in vehicle frame. +static inline float maybe_invert(float signed_deg, bool inverted) { + return inverted ? to_signed_deg(-signed_deg) : signed_deg; +} + +// ── CRC32 (poly 0x04C11DB7, MSB-first) ────────────────────────────────────── +static uint32_t crc32_olei(const uint8_t* data, size_t len) { + uint32_t crc = 0xFFFFFFFF; + for (size_t i = 0; i < len; ++i) { + crc ^= static_cast(data[i]) << 24; + for (int b = 0; b < 8; ++b) + crc = (crc & 0x80000000u) ? (crc << 1) ^ 0x04C11DB7u : (crc << 1); + } + return crc; +} + +// ── Frame IDs ──────────────────────────────────────────────────────────────── +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) + +// ─── Constructor / Destructor ──────────────────────────────────────────────── +Driver::Driver(const ModelConfig& cfg, const std::string& ip, uint16_t port, bool inverted) + : cfg_(cfg), ip_(ip), port_(port), inverted_(inverted) +{ + auto_detect_ = (std::strcmp(cfg.name, "AUTO") == 0); +} + +Driver::~Driver() { close(); } + +// ─── open() ───────────────────────────────────────────────────────────────── +bool Driver::open() { + sock_fd_ = ::socket(AF_INET, SOCK_DGRAM, 0); + if (sock_fd_ < 0) return false; + + // Allow multiple sockets to bind the same port (run alongside another + // app / debugging). SO_REUSEPORT lets several listeners receive the same + // UDP stream — only works if EVERY socket on that port sets this flag. + int reuse = 1; + ::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)); +#ifdef SO_REUSEPORT + ::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEPORT, &reuse, sizeof(reuse)); +#endif + + sockaddr_in addr{}; + addr.sin_family = AF_INET; + addr.sin_port = htons(port_); + addr.sin_addr.s_addr = inet_addr(ip_.c_str()); + + if (::bind(sock_fd_, reinterpret_cast(&addr), sizeof(addr)) < 0) { + ::close(sock_fd_); + sock_fd_ = -1; + return false; + } + pending_angle_deg_.reserve(2048); + pending_dist_m_.reserve(2048); + pending_intensity_.reserve(2048); + return true; +} + +// ─── close() ──────────────────────────────────────────────────────────────── +void Driver::close() { + if (sock_fd_ >= 0) { + ::close(sock_fd_); + sock_fd_ = -1; + } +} + +// ─── recv_scan() — blocks until one full revolution is available ────────── +bool Driver::recv_scan(ScanResult& out, int timeout_ms) { + scan_ready_ = false; + + while (!scan_ready_) { + if (timeout_ms > 0) { + fd_set fds; FD_ZERO(&fds); FD_SET(sock_fd_, &fds); + timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 }; + int r = ::select(sock_fd_ + 1, &fds, nullptr, nullptr, &tv); + if (r <= 0) return false; // timeout or error + } + if (!spin_once()) return false; + } + out = std::move(ready_result_); + return true; +} + +// ─── spin_once() ──────────────────────────────────────────────────────────── +bool Driver::spin_once() { + // buf is the recv_buf_ member, NOT static → each Driver has its own + // memory, safe when 2 lidars receive concurrently on 2 threads. + uint8_t* buf = recv_buf_; + sockaddr_in from{}; + socklen_t fromlen = sizeof(from); + + ssize_t n = ::recvfrom(sock_fd_, buf, sizeof(recv_buf_), 0, + reinterpret_cast(&from), &fromlen); + if (n < 0) return false; + + // Distinguish protocol family by Frame ID (little-endian) + // Family A / C: Frame ID / magic sits right at bytes [0-1] + // Family B: has a 0x010F preamble at bytes [0-1], real Frame ID at bytes [2-3] + if (n < 4) return true; // too short, skip + uint16_t id_at_0 = le16(buf); // Family A (0xFAF0) or Family C (0xFEAC) + uint16_t frame_id_b = le16(buf + 2); // Family B: preamble 0x010F + real id at [2-3] + + if (id_at_0 == FRAME_ID_A) parse_family_a(buf, static_cast(n)); + else if (id_at_0 == FRAME_ID_C) parse_family_c(buf, static_cast(n)); + else if (frame_id_b == FRAME_ID_B) parse_family_b(buf, static_cast(n)); + // else: unknown family (3D LR-16F uses a different format, extend later) + + return true; +} + +// ─── push_point() — append with angle-unwrapping ─────────────────────────── +// `signed_angle_deg` is already signed+inverted+FOV-filtered by the caller. +// Unwrapping against the previous point (rather than re-deriving from device +// raw angle) keeps this identical for all 3 families and survives the ±180° +// seam: a 360° device's points cross from +179.x to -179.x mid-revolution in +// the signed system, which push_point() turns back into a continuous ramp so +// LaserScan::angle_min/angle_max/ranges stay meaningful (monotonic, ROS-style). +void Driver::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(); + while (angle - prev > 180.f) angle -= 360.f; + while (angle - prev < -180.f) angle += 360.f; + } + pending_angle_deg_.push_back(angle); + pending_dist_m_.push_back(dist_m); + pending_intensity_.push_back(intensity); +} + +// ─── flush_scan() — a revolution is complete ─────────────────────────────── +void Driver::flush_scan() { + if (pending_angle_deg_.empty()) return; + + const size_t n = pending_angle_deg_.size(); + + LaserScan& scan = ready_result_.scan; + scan.timestamp_ms = pending_ts_; + scan.angle_min = pending_angle_deg_.front() * kDeg2Rad; + scan.angle_max = pending_angle_deg_.back() * kDeg2Rad; + scan.angle_increment = (n > 1) + ? (scan.angle_max - scan.angle_min) / static_cast(n - 1) : 0.f; + scan.time_increment = 0.f; // device doesn't expose per-point timing + scan.scan_time = 0.f; // device doesn't expose per-scan timing + scan.range_min = cfg_.range_min_m; + scan.range_max = cfg_.range_max_m; + scan.ranges.assign(pending_dist_m_.begin(), pending_dist_m_.end()); + scan.intensities.assign(pending_intensity_.begin(), pending_intensity_.end()); + + ExtraInfo& info = ready_result_.info; + info = pending_info_; + info.detected_model = detected_model_name_; + info.error_status = pending_err_; + + pending_angle_deg_.clear(); + pending_dist_m_.clear(); + pending_intensity_.clear(); + pending_info_ = ExtraInfo{}; // reset per-revolution optional fields + scan_ready_ = true; + + if (cb_) cb_(ready_result_); +} + +// ─── parse_family_a() ─────────────────────────────────────────────────────── +// 20-byte header: +// [0-1] Frame ID = 0xFAF0 +// [2-3] Protocol = 0x0200 +// [4] Distance scale (mm/count) +// [5] Error status +// [6] Start angle (deg, uint8) +// [7] End angle (deg, uint8, exclusive) +// [8-9] Num points (uint16 LE) +// [10-11] Rotation info — raw, undecoded (exposed as ExtraInfo::rotation_raw) +// [12-15] Timestamp (uint32 LE, ms) +// [16-19] CRC32 of the block data +// 3-byte block × N: +// [0-1] Distance readout (uint16 LE) +// [2] Intensity (uint8) +bool Driver::parse_family_a(const uint8_t* buf, int len) { + static constexpr int HEADER_LEN = 20; + static constexpr int BLOCK_LEN = 3; + + if (len < HEADER_LEN) return false; + + // ── read header ── + // uint16_t protocol = le16(buf + 2); // 0x0200 + uint8_t dist_scale = buf[4]; // mm per count + uint8_t err_status = buf[5]; + float ang_start = static_cast(buf[6]); + // float ang_end = static_cast(buf[7]); // exclusive + uint16_t num_pts = le16(buf + 8); + uint16_t rotation_raw = le16(buf + 10); + uint32_t timestamp = le32(buf + 12); + uint32_t crc_packet = le32(buf + 16); + + // ── verify CRC (optional but recommended) ── + int block_bytes = len - HEADER_LEN; + if (block_bytes < num_pts * BLOCK_LEN) return false; // truncated packet + + uint32_t crc_calc = crc32_olei(buf + HEADER_LEN, static_cast(num_pts * BLOCK_LEN)); + if (crc_calc != crc_packet) return false; // CRC mismatch + + // ── detect wrap-around → flush the previous revolution ── + if (last_angle_ >= 0.f && ang_start < last_angle_ - 90.f) { + flush_scan(); + } + + // ── decode points ── + pending_ts_ = timestamp; + pending_err_ = err_status; + pending_info_.distance_scale_mm = dist_scale; + pending_info_.rotation_raw = rotation_raw; + + // scale=0 means the firmware didn't report it → default to 1 mm/count to avoid dist=0. + const float scale_mm = (dist_scale ? static_cast(dist_scale) : 1.f); + const float ang_end = static_cast(buf[7]); + + const uint8_t* blk = buf + HEADER_LEN; + for (uint16_t i = 0; i < num_pts; ++i, blk += BLOCK_LEN) { + uint16_t dist_raw = le16(blk); + uint8_t intensity = blk[2]; + + // Compute angle: linear interpolation within the packet's range (device-space) + float frac = (num_pts > 1) ? static_cast(i) / (num_pts - 1) : 0.f; + float angle = to_signed_deg(ang_start + frac * (ang_end - ang_start)); + angle = maybe_invert(angle, inverted_); + + // Filter out anything outside the model's FOV (already in the signed -180…180 system) + if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue; + + push_point(angle, dist_raw * scale_mm * 0.001f /* mm → m */, intensity); + } + + last_angle_ = ang_start; + return true; +} + +// ─── parse_family_b() ─────────────────────────────────────────────────────── +// 40-byte header: +// [0-1] 0x010F +// [2-3] 0xFEF0 (Frame ID) +// [4-5] 0x0200 (Protocol) +// [6] Distance scale +// [7-16] Model identifier string (e.g. "OLELR-1BS5") +// [17-39] Reserved +// 8-byte block × N: +// [0-1] AngleRaw (uint16 LE, × 0.01° → deg, 0–359.99); >= 0xFF00 = invalid point +// [2-3] Distance readout (uint16 LE); meters = value × DistanceScale / 1000 +// [4-5] Signal strength (uint16 LE) +// [6-7] Reserved +// NOTE: this header carries no timestamp/error field, so ScanResult::scan's +// timestamp_ms and info.error_status stay at their defaults (0) for Family B. +bool Driver::parse_family_b(const uint8_t* buf, int len) { + static constexpr int HEADER_LEN = 40; + static constexpr int BLOCK_LEN = 8; + + if (len < HEADER_LEN) return false; + + uint8_t dist_scale = buf[6]; + // scale=0 → default to 1 mm/count so distances don't collapse to zero. + const float scale_mm = (dist_scale ? static_cast(dist_scale) : 1.f); + pending_info_.distance_scale_mm = dist_scale; + if (auto_detect_ && !model_locked_) { + std::string raw(reinterpret_cast(buf + 7), 10); + size_t z = raw.find('\0'); + if (z != std::string::npos) raw.resize(z); + + if (!raw.empty()) { + detected_model_name_ = raw; + model_locked_ = true; + + static constexpr struct { const char* key; const ModelConfig* cfg; } kModelTable[] = { + { "1BS5", &MODEL_LR1BS5 }, + { "16F", &MODEL_LR16F }, + { "1FMI", &MODEL_LR1FMI }, // must precede "1F": "OLELR-1FMI" also contains "1F" + { "1F", &MODEL_LR1F }, + { "VF", &MODEL_VF }, + { "VB", &MODEL_VB }, + }; + for (const auto& entry : kModelTable) { + if (raw.find(entry.key) != std::string::npos) { + cfg_.scan_angle_min = entry.cfg->scan_angle_min; + cfg_.scan_angle_max = entry.cfg->scan_angle_max; + cfg_.range_min_m = entry.cfg->range_min_m; + cfg_.range_max_m = entry.cfg->range_max_m; + break; + } + } + } + } + + int num_pts = (len - HEADER_LEN) / BLOCK_LEN; + if (num_pts <= 0) return false; + + const uint8_t* blk = buf + HEADER_LEN; + // AngleRaw is 0.01°/LSB (0–359.99°), per the official Olei block spec — + // verified against real OLELR-1FMI geometry (a 0.25° scale smears a room + // into a circle). AngleRaw >= 0xFF00 marks an invalid point → skip it. + // The counter resets to 0 each revolution, but one packet is only a ~22° + // arc and the device can pack >1 revolution across packets, so the + // revolution boundary is detected PER POINT: a >90° drop between + // consecutive [0,360) angles ends the current revolution. + static constexpr uint16_t INVALID_ANGLE = 0xFF00; + for (int i = 0; i < num_pts; ++i, blk += BLOCK_LEN) { + uint16_t angle_raw = le16(blk); + if (angle_raw >= INVALID_ANGLE) continue; // invalid point + + float dev_deg = std::fmod(angle_raw * 0.01f, 360.f); // [0,360) + if (last_angle_ >= 0.f && dev_deg < last_angle_ - 90.f) { + flush_scan(); // revolution complete + } + last_angle_ = dev_deg; + + float angle = maybe_invert(to_signed_deg(angle_raw * 0.01f), inverted_); // -180…180 + float dist_m = le16(blk + 2) * scale_mm * 0.001f; // readout × scale → m + uint8_t intensity = static_cast(le16(blk + 4) >> 2); // 10-bit → 8-bit + + if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue; + + push_point(angle, dist_m, intensity); + } + + return true; +} + +// ─── parse_family_c() ─────────────────────────────────────────────────────── +// Protocol V3 (Olei GS1-5, magic 0xFEAC) — ported from the existing C# +// production driver OleiGS15Driver.cs (RobotNet10.RobotApp); NOT independently +// sniffed/verified against real GS1-5 hardware (no device was available to +// test this while writing the code). +// 48-byte header: +// [0-1] Magic = 0xFEAC +// [2-3] Version +// [4-7] PacketSize (uint32 LE) +// [8-9] HeaderSize (uint16 LE, usually = 48) +// [10] Distance ratio — read by the original C# driver but NOT applied +// (distance is always raw mm / 1000); same behavior kept here. +// Exposed raw as ExtraInfo::distance_ratio_raw. +// [11] Types: 0x00=2B/point (range only), 0x01=4B/point (range+intensity), +// 0x10=4B/point (first 2 bytes unused, range at [+2,+4)) +// [12-13] Scan number [14-15] Packet number +// [16-19] Timestamp decimal [20-23] Timestamp integer +// [24-25] Scan frequency raw [26-27] NumPointsScan (total points per revolution) +// [28-29] Input status [30-31] Output status +// [32-35] Field status +// [36-37] StartIndex [38-39] EndIndex +// [40-41] FirstIndex — index of this packet's first point within the full revolution +// [42-43] NumPointsPacket — number of points in this packet +// [44-47] Status flags +// All of [10], [24-25], [28-35], [44-47] are read and passed through raw in +// ExtraInfo — none of these are cross-verified against real hardware, same +// caveat as the rest of this family. +// Angle: angle = (FirstIndex + i) * (360 / NumPointsScan) - 180 → already in +// the signed system (-180..180); no fmod needed like Family B since the +// index always stays within [0, NumPointsScan). +bool Driver::parse_family_c(const uint8_t* buf, int len) { + static constexpr int HEADER_LEN = 48; + if (len < HEADER_LEN) return false; + + uint16_t header_size_field = le16(buf + 8); + uint8_t distance_ratio_raw = buf[10]; + uint8_t types = buf[11]; + uint16_t scan_frequency_raw = le16(buf + 24); + uint16_t num_pts_scan = le16(buf + 26); + uint16_t input_status = le16(buf + 28); + uint16_t output_status = le16(buf + 30); + uint32_t field_status = le32(buf + 32); + uint16_t first_index = le16(buf + 40); + uint16_t num_pts_packet = le16(buf + 42); + uint32_t status_flags = le32(buf + 44); + + if (num_pts_scan == 0) return false; // avoid divide-by-zero + + int header_size = (header_size_field == 0) ? HEADER_LEN : header_size_field; + if (header_size < HEADER_LEN || header_size > len) return false; + + int bytes_per_point = (types == 0x00) ? 2 : (types == 0x01 || types == 0x10) ? 4 : 0; + if (bytes_per_point == 0) return false; // unknown Types, layout unclear + + int payload_bytes = len - header_size; + int num_pts = num_pts_packet; + if (num_pts == 0 || num_pts * bytes_per_point > payload_bytes) { + num_pts = payload_bytes / bytes_per_point; + } + if (num_pts <= 0) return false; + + pending_info_.distance_ratio_raw = distance_ratio_raw; + pending_info_.scan_frequency_raw = scan_frequency_raw; + pending_info_.input_status = input_status; + pending_info_.output_status = output_status; + pending_info_.field_status = field_status; + pending_info_.status_flags = status_flags; + + // Magic 0xFEAC corresponds to exactly one model (GS1-5) — no model name + // string in the header like Family B, but recognizing this family is + // already enough to know the model, so auto-detect resolves immediately + // without reading any extra field. + if (auto_detect_ && !model_locked_) { + cfg_.scan_angle_min = MODEL_GS15.scan_angle_min; + cfg_.scan_angle_max = MODEL_GS15.scan_angle_max; + cfg_.range_min_m = MODEL_GS15.range_min_m; + cfg_.range_max_m = MODEL_GS15.range_max_m; + detected_model_name_ = MODEL_GS15.name; + model_locked_ = true; + } + + const float angle_inc = 360.f / static_cast(num_pts_scan); + // raw_angle is used for wrap-around detection: it does NOT have the -180 + // offset that the externally-exposed angle gets, and stays in [0,360), + // monotonically increasing — matching the same convention used by + // Family A/B (last_angle_ >= 0 means "we already have a previous value"); + // subtracting 180 here could go negative and break that sentinel check. + float raw_first_angle = static_cast(first_index) * angle_inc; + + if (last_angle_ >= 0.f && raw_first_angle < last_angle_ - 90.f) { + flush_scan(); + } + + const uint8_t* blk = buf + header_size; + for (int i = 0; i < num_pts; ++i, blk += bytes_per_point) { + uint16_t range_mm; + uint16_t inten_raw = 0; + bool has_inten = false; + + if (types == 0x00) { + range_mm = le16(blk); + } else if (types == 0x01) { + range_mm = le16(blk); + inten_raw = le16(blk + 2); + has_inten = true; + } else { // 0x10 + range_mm = le16(blk + 2); + } + + float angle = to_signed_deg(static_cast(first_index + i) * angle_inc - 180.f); + angle = maybe_invert(angle, inverted_); + if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue; + + push_point(angle, range_mm * 0.001f /* mm → m */, + has_inten ? static_cast(inten_raw > 255 ? 255 : inten_raw) : uint8_t{0}); + } + + last_angle_ = raw_first_angle; + return true; +} + +} // namespace lidarlib diff --git a/src/sick_lidar.cpp b/src/sick_lidar.cpp new file mode 100644 index 0000000..7b8ff9e --- /dev/null +++ b/src/sick_lidar.cpp @@ -0,0 +1,319 @@ +#include "lidarlib/sick_lidar.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace lidarlib { + +namespace { +constexpr float kDeg2Rad = 3.14159265358979323846f / 180.f; +constexpr char kStx = 0x02; +constexpr char kEtx = 0x03; +constexpr int kConnectTimeoutMs = 2000; + +uint32_t hex_to_u32(const std::string& tok) { + return static_cast(std::strtoul(tok.c_str(), nullptr, 16)); +} +int32_t hex_to_i32(const std::string& tok) { + // SICK encodes signed header fields as plain hex of the 2's-complement bits. + return static_cast(hex_to_u32(tok)); +} +float bits_to_float(uint32_t bits) { + float f; + std::memcpy(&f, &bits, sizeof(f)); + return f; +} + +std::vector tokenize(const std::string& s) { + std::vector out; + size_t i = 0, n = s.size(); + while (i < n) { + while (i < n && std::isspace(static_cast(s[i]))) ++i; + size_t start = i; + while (i < n && !std::isspace(static_cast(s[i]))) ++i; + if (i > start) out.push_back(s.substr(start, i - start)); + } + return out; +} +} // namespace + +SickDriver::SickDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port) + : cfg_(cfg), detected_model_name_(cfg.name ? cfg.name : ""), ip_(ip), port_(port) {} + +SickDriver::~SickDriver() { close(); } + +// ─── open() — TCP connect + tell the device to start streaming ──────────── +bool SickDriver::open() { + sock_fd_ = ::socket(AF_INET, SOCK_STREAM, 0); + if (sock_fd_ < 0) return false; + + sockaddr_in addr{}; + addr.sin_family = AF_INET; + addr.sin_port = htons(port_); + addr.sin_addr.s_addr = inet_addr(ip_.c_str()); + + // Non-blocking connect with a bounded timeout: a SICK device that's + // powered off/unreachable leaves the SYN unanswered, and a plain blocking + // connect() would then stall this call — and whatever thread called it, + // e.g. a GUI's "connect" button handler — for the OS's default TCP retry + // timeout (~2 minutes on Linux). + int flags = ::fcntl(sock_fd_, F_GETFL, 0); + ::fcntl(sock_fd_, F_SETFL, flags | O_NONBLOCK); + + int rc = ::connect(sock_fd_, reinterpret_cast(&addr), sizeof(addr)); + if (rc < 0 && errno == EINPROGRESS) { + fd_set wfds; FD_ZERO(&wfds); FD_SET(sock_fd_, &wfds); + timeval tv{ kConnectTimeoutMs / 1000, (kConnectTimeoutMs % 1000) * 1000 }; + rc = ::select(sock_fd_ + 1, nullptr, &wfds, nullptr, &tv); + if (rc > 0) { + int err = 0; socklen_t errlen = sizeof(err); + ::getsockopt(sock_fd_, SOL_SOCKET, SO_ERROR, &err, &errlen); + rc = (err == 0) ? 0 : -1; + } else { + rc = -1; // timeout, or select() itself failed + } + } + ::fcntl(sock_fd_, F_SETFL, flags); // restore blocking mode for send/recv below + + if (rc < 0) { + ::close(sock_fd_); + sock_fd_ = -1; + return false; + } + + int nodelay = 1; + ::setsockopt(sock_fd_, IPPROTO_TCP, TCP_NODELAY, &nodelay, sizeof(nodelay)); + + recv_buf_.clear(); + + // The device stays passive until told otherwise — without this, no + // LMDscandata telegram ever arrives. + if (!send_telegram("sEN LMDscandata 1")) { + close(); + return false; + } + return true; +} + +// ─── close() ──────────────────────────────────────────────────────────────── +void SickDriver::close() { + if (sock_fd_ >= 0) { + send_telegram("sEN LMDscandata 0"); // best-effort, ignore failure + ::close(sock_fd_); + sock_fd_ = -1; + } +} + +// ─── send_telegram() — wrap with STX/ETX and write ───────────────────────── +bool SickDriver::send_telegram(const std::string& body) { + if (sock_fd_ < 0) return false; + std::string framed; + framed.reserve(body.size() + 2); + framed.push_back(kStx); + framed += body; + framed.push_back(kEtx); + + size_t sent = 0; + while (sent < framed.size()) { + ssize_t n = ::send(sock_fd_, framed.data() + sent, framed.size() - sent, 0); + if (n <= 0) return false; + sent += static_cast(n); + } + return true; +} + +// ─── read_telegram() — pull bytes off the TCP stream until one full +// STX..ETX frame is assembled. CoLa-A has no length prefix, so ETX is the +// only frame boundary; recv_buf_ carries any leftover bytes (start of the +// next telegram) across calls. ────────────────────────────────────────────── +bool SickDriver::read_telegram(std::string& out, int timeout_ms) { + if (sock_fd_ < 0) return false; + + for (;;) { + size_t etx_pos = recv_buf_.find(kEtx); + if (etx_pos != std::string::npos) { + size_t stx_pos = recv_buf_.find(kStx); + if (stx_pos == std::string::npos || stx_pos > etx_pos) { + // Stray ETX with no matching STX before it — drop and retry. + recv_buf_.erase(0, etx_pos + 1); + continue; + } + out = recv_buf_.substr(stx_pos + 1, etx_pos - stx_pos - 1); + recv_buf_.erase(0, etx_pos + 1); + return true; + } + + if (timeout_ms > 0) { + fd_set fds; FD_ZERO(&fds); FD_SET(sock_fd_, &fds); + timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 }; + int r = ::select(sock_fd_ + 1, &fds, nullptr, nullptr, &tv); + if (r <= 0) return false; // timeout or error + } + + char buf[4096]; + ssize_t n = ::recv(sock_fd_, buf, sizeof(buf), 0); + if (n <= 0) return false; // closed or error + recv_buf_.append(buf, static_cast(n)); + } +} + +// ─── recv_scan() ──────────────────────────────────────────────────────────── +bool SickDriver::recv_scan(ScanResult& out, int timeout_ms) { + for (;;) { + std::string telegram; + if (!read_telegram(telegram, timeout_ms)) return false; + if (parse_lmdscandata(telegram, out)) return true; + // Non-scan telegram (e.g. an "sEA"/access-mode ack) — keep waiting. + } +} + +// ─── spin_once() ──────────────────────────────────────────────────────────── +bool SickDriver::spin_once() { + std::string telegram; + if (!read_telegram(telegram, 0)) return false; // 0 = block until next telegram + + ScanResult result; + if (!parse_lmdscandata(telegram, result)) return true; // ignore non-scan telegrams + if (cb_) cb_(result); + return true; +} + +// ─── parse_lmdscandata() ──────────────────────────────────────────────────── +// CoLa-A "sSN LMDscandata"/"sRA LMDscandata" telegram, space-separated ASCII +// tokens (mostly hex). UNVERIFIED layout (see header comment) — ported from +// SICK's public Telegram Listing, field order below: +// +// sSN LMDscandata +// +// +// +// +// [ ]* +// +// { +// +// * }* +// { ...same shape, 8-bit data... }* +// (position/name/comment/time/event fields follow — not needed for LaserScan, ignored) +// +// ContentName "DIST1" carries ranges (raw mm × ScalingFactor), "RSSI1" +// carries intensities (raw × ScalingFactor) — any other channel name is +// consumed (to keep the token cursor in sync) but its data discarded. +bool SickDriver::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; + if (tok[1] != "LMDscandata") return false; + + size_t i = 2; + auto next = [&]() -> std::string { return (i < tok.size()) ? tok[i++] : std::string(); }; + + hex_to_u32(next()); // VersionNumber — not exposed + hex_to_u32(next()); // DeviceNumber — not exposed + hex_to_u32(next()); // SerialNumber — not exposed + uint32_t status0 = hex_to_u32(next()); // DeviceStatus: Error + uint32_t status1 = hex_to_u32(next()); // DeviceStatus: Pollution + uint32_t telegram_counter = hex_to_u32(next()); + uint32_t scan_counter = hex_to_u32(next()); + (void)telegram_counter; (void)scan_counter; // not carried by ExtraInfo today + hex_to_u32(next()); // TimeSinceStartup — not exposed + uint32_t time_of_transmission = hex_to_u32(next()); + uint32_t in0 = hex_to_u32(next()); + uint32_t in1 = hex_to_u32(next()); + uint32_t out0 = hex_to_u32(next()); + uint32_t out1 = hex_to_u32(next()); + next(); // Reserved + uint32_t scanning_frequency = hex_to_u32(next()); + hex_to_u32(next()); // MeasurementFrequency — not exposed + + uint32_t num_encoders = hex_to_u32(next()); + for (uint32_t e = 0; e < num_encoders; ++e) { + next(); // EncoderPosition + next(); // EncoderSpeed + } + + LaserScan& scan = out.scan; + scan.ranges.clear(); + scan.intensities.clear(); + float angle_min_deg = 0.f, angle_inc_deg = 0.f; + bool got_dist = false; + + auto parse_channel_block = [&](bool eight_bit) { + std::string content = next(); // e.g. "DIST1", "RSSI1" + uint32_t scale_bits = hex_to_u32(next()); + hex_to_u32(next()); // ScalingOffset — unused + int32_t start_angle = hex_to_i32(next()); // 1/10000 deg + int32_t step_width = hex_to_i32(next()); // 1/10000 deg + uint32_t num_data = hex_to_u32(next()); + + float scale = bits_to_float(scale_bits); + if (scale == 0.f) scale = 1.f; // guard against a zero/garbage scaling factor + + bool is_dist = content.rfind("DIST", 0) == 0; + bool is_rssi = content.rfind("RSSI", 0) == 0; + + if (is_dist) { + angle_min_deg = static_cast(start_angle) * 0.0001f; + angle_inc_deg = static_cast(step_width) * 0.0001f; + scan.ranges.assign(num_data, 0.f); + } else if (is_rssi && scan.intensities.empty()) { + scan.intensities.assign(num_data, 0.f); + } + + for (uint32_t d = 0; d < num_data; ++d) { + uint32_t raw = hex_to_u32(next()); + if (is_dist) { + scan.ranges[d] = static_cast(raw) * scale * 0.001f; // mm -> m + got_dist = true; + } else if (is_rssi && d < scan.intensities.size()) { + scan.intensities[d] = static_cast(raw) * scale; + } + } + (void)eight_bit; + }; + + uint32_t num_16bit_channels = hex_to_u32(next()); + for (uint32_t c = 0; c < num_16bit_channels; ++c) parse_channel_block(false); + + uint32_t num_8bit_channels = hex_to_u32(next()); + for (uint32_t c = 0; c < num_8bit_channels; ++c) parse_channel_block(true); + + if (!got_dist || scan.ranges.empty()) return false; + + scan.timestamp_ms = time_of_transmission; + scan.angle_min = angle_min_deg * kDeg2Rad; + scan.angle_increment = angle_inc_deg * kDeg2Rad; + scan.angle_max = scan.angle_min + + scan.angle_increment * static_cast(scan.ranges.size() - 1); + scan.time_increment = 0.f; // device doesn't expose per-point timing + scan.scan_time = 0.f; // device doesn't expose per-scan timing + scan.range_min = cfg_.range_min_m; + scan.range_max = cfg_.range_max_m; + if (scan.intensities.size() != scan.ranges.size()) + scan.intensities.assign(scan.ranges.size(), 0.f); // RSSI channel wasn't enabled on the device + + ExtraInfo& info = out.info; + info = ExtraInfo{}; + // LMDscandata carries no model-name string (unlike OLEI Family B) — SICK + // doesn't auto-detect, the caller's cfg names the model up front. + info.detected_model = cfg_.name; + info.error_status = static_cast(status0 & 0xFF); + info.status_flags = (status0 << 8) | status1; + info.scan_frequency_raw = static_cast(scanning_frequency); + info.input_status = static_cast((in0 << 8) | in1); + info.output_status = static_cast((out0 << 8) | out1); + + return true; +} + +} // namespace lidarlib