diff --git a/CMakeLists.txt b/CMakeLists.txt index 927ce82..81d1f27 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,12 +11,10 @@ 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 + src/lidar_config.cpp ) add_library(lidarlib ${LIDARLIB_SOURCES}) @@ -42,13 +40,14 @@ if(LIDARLIB_BUILD_EXAMPLES) 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(nanoscan_example examples/nanoscan_example.cpp) + target_link_libraries(nanoscan_example PRIVATE lidarlib) + add_executable(lidar_app examples/lidar_app.cpp) target_link_libraries(lidar_app PRIVATE lidarlib) endif() -# ── install + find_package() support ── +# install + find_package() support include(GNUInstallDirs) include(CMakePackageConfigHelpers) diff --git a/README.md b/README.md index 85c1211..c003445 100644 --- a/README.md +++ b/README.md @@ -1,135 +1,14 @@ # 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`). +Thư viện C++17 cho lidar **OLEI** (UDP) và **SICK** (TCP/UDP). Build bằng CMake +ra shared lib, hỗ trợ `find_package(lidarlib)`. Mọi driver dùng chung một +interface `lidarlib::Lidar` và một hàm khởi tạo duy nhất `lidarlib::make_lidar()`. -## 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' -``` +- Tự nhận diện họ giao thức OLEI (Family A/B/C) theo từng gói tin +- Tự dò model (`MODEL_AUTO`) với Family B/C +- Chạy nhiều lidar song song (mỗi instance độc lập, an toàn đa luồng) +- Output chuẩn ROS `sensor_msgs/LaserScan` (radian, mét) +- Không có UI — tự viết giao diện trên API này ## Build @@ -138,209 +17,150 @@ 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`. +Sinh ra `build/liblidarlib.so` (chỉ phụ thuộc pthread) và các binary demo +(`example`, `test_dual`, `sick_example`, `nanoscan_example`, `lidar_app`). -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: +Tùy chọn: `-DLIDARLIB_BUILD_EXAMPLES=OFF` (tắt demo), +`-DBUILD_SHARED_LIBS=OFF` (static lib). + +Cài đặt và dùng từ project khác: ```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 --install build --prefix "$HOME/.local" # hoặc sudo với /usr/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: +## Quick start ```cpp -lidarlib::Driver drv(lidarlib::MODEL_AUTO, "192.168.100.100", 2369); -drv.open(); -lidarlib::ScanResult result; -drv.recv_scan(result, 2000); +#include "lidarlib/lidarlib.hpp" // toàn bộ API trong 1 include + +lidarlib::LidarConfig c{"front", "192.168.1.10", 2368, "AUTO", false, "OLEI"}; +std::unique_ptr lidar = lidarlib::make_lidar(c); +lidar->open(); + +lidarlib::ScanResult r; +lidar->recv_scan(r, 1000); +// r.scan : LaserScan — format sensor_msgs/LaserScan của ROS, chung mọi lidar +// r.info : ExtraInfo — thông tin thêm tuỳ family/model +printf("%zu diem, model=%s\n", r.scan.ranges.size(), r.info.detected_model.c_str()); ``` -### 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ý: +Có thể khởi tạo driver trực tiếp thay vì qua `make_lidar()`: ```cpp -lidarlib::Driver drv(lidarlib::MODEL_AUTO, "192.168.100.100", 2369, /*inverted=*/true); +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); ``` -Đã 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. +Ngoài `recv_scan()` blocking còn có callback: `set_scan_callback()` + +`spin_once()` trong vòng lặp riêng. -## Cấu hình & chạy (lidar_app + config.json) +## Cấu hình (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.). +`lidar_app` là app mẫu headless: đọc `config.json`, mở từng lidar bằng +`make_lidar()`, một thread mỗi con. ```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 +./build/lidar_app [my_config.json] ``` -`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} + {"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"} ] } ``` -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ả. +| Trường | Ý nghĩa | +|---|---| +| `brand` | `"OLEI"` (mặc định) hoặc `"SICK"` | +| `model` | Tên trong bảng model bên dưới; tên lạ → mặc định của hãng (`AUTO` / `SICK-TIM571`). Với SICK, `"SICK-nanoScan3"` → driver UDP, còn lại → driver TCP | +| `inverted` | Chỉ OLEI: `true` nếu lidar lắp úp ngược, driver tự đảo góc về hệ quy chiếu xe | +| `angle_min_deg` / `angle_max_deg` | Tuỳ chọn: remap tuyến tính góc output sang cửa sổ này (độ). Không cắt điểm nào, chỉ ghi lại `angle_min/max/increment`. Bỏ trống (±360) = tắt | -### Sniff packet thô (debug khi nghi ngờ offset header) +Đọc/ghi bằng `lidarlib::load_config(path)` / `lidarlib::save_config(path, cfg)`. -```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 -``` +## Model -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. +### OLEI (`brand = "OLEI"`, UDP, port mặc định 2368) -## Chọn `ModelConfig` - -| Constant | FOV (deg, có dấu) | range_min/max (m) | Khi dùng | +| Constant | FOV (°) | Range (m) | Ghi chú | |---|---|---|---| | `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_LR1F` | -180…180 | 0.05…50 | Family A; 0° thô của máy chỉ về đuôi (offset +180°) | +| `MODEL_LR1FMI` | -180…180 | 0.05…30 | Family B, ~2400 điểm/vòng; 0° thô chỉ về đuôi (offset +180°) | +| `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 | 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 | +| `MODEL_GS15` | -180…180 | 0.05…30 | Family C/V3 — **chưa verify phần cứng** | +| `MODEL_AUTO` | -180…180 | 0.05…30 | Không biết trước model; tự dò với Family B (chuỗi tên) và C (magic). Family A không mang tên model nên giữ FOV rộng | -`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. +Driver nhận diện họ giao thức theo Frame ID mỗi gói: +**Family A** `0xFAF0` (header 20B, 3B/điểm, CRC32) · +**Family B** `0xFEF0` (header 40B kèm tên model ASCII, 8B/điểm) · +**Family C/V3** `0xFEAC` (header 48B, 2/4B/điểm — port từ driver C#, chưa verify). -`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. +Tên model thật đọc từ packet xem qua `detected_model()` hoặc +`result.info.detected_model`. -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. +### SICK (`brand = "SICK"`) -## 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 | +| Constant | FOV (°) | Range (m) | Transport | |---|---|---|---| -| `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° | +| `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 | +| `MODEL_SICK_NANOSCAN3` | -137.5…137.5 | 0.05…40 | UDP safety-data, port 6060 | -**Đã 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. +**TiM (`SickDriver`)** — `open()` tự gửi `sEN LMDscandata 1` để bắt đầu stream. +Hệ góc trên dây đặt 90° = trước mặt nên preset có `angle_offset_deg = -90`, +output ra -135…135° với 0° = phía trước. **Đã verify trên TiM781S thật** +(811 điểm/scan, increment 0.333°, DIST1/RSSI1 đúng layout). Chưa verify: +encoder, kênh 8-bit, thông số TiM5xx/571. -**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. +**nanoScan3 (`NanoScanDriver`)** — UDP receiver thụ động: chỉ bind cổng và +parse datagram; **đích UDP phải cấu hình sẵn trong SICK Safety Designer** +(driver không bắt tay CoLa2). Layout port từ `sick_safetyscanners` (Apache-2.0). +**Chưa verify phần cứng thật** — mới test bằng gói tổng hợp qua loopback. -`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++). +## Output + +`ScanResult { LaserScan scan; ExtraInfo info; }` mỗi vòng quét: + +- **`LaserScan`** — cùng field/đơn vị với ROS: `angle_min/max/increment` (rad, + đã unwrap liên tục, không giới hạn ±π), `ranges[]` (m), `intensities[]` + (0-255), `timestamp_ms` (đồng hồ thiết bị, 0 nếu family không có). + `range_min/max` lấy từ `ModelConfig` (đặt sẵn, không đo mỗi scan); + `time_increment/scan_time` luôn 0. +- **`ExtraInfo`** — field tuỳ family: `detected_model`, `error_status` (Family A), + `distance_scale_mm` (A/B), và các trường raw của Family C (chưa verify). + Field thiết bị không có giữ `std::nullopt`. + +## Cấu trúc source + +| File | Vai trò | +|---|---| +| `include/lidarlib/lidar.hpp` | Data model, interface `Lidar`, driver OLEI, các `MODEL_*` OLEI | +| `include/lidarlib/sick_lidar.hpp` | `SickDriver`, `NanoScanDriver`, các `MODEL_SICK_*` | +| `include/lidarlib/config.hpp` | `LidarConfig`, load/save JSON, `make_lidar()` | +| `src/olei_lidar.cpp` | Parse Family A/B/C, CRC, gom scan | +| `src/sick_lidar.cpp` | Parse CoLa-A (TiM) + safety-data UDP (nanoScan3) | +| `src/lidar_config.cpp` | Bảng model/brand, config JSON, factory | +| `examples/` | Demo: 1 lidar, 2 lidar song song, SICK TiM, nanoScan3, app khung | + +## Ghi chú + +- Nếu port UDP đã bị app khác giữ (không bật `SO_REUSEPORT`), `open()` sẽ thất + bại. Kiểm tra: `ss -lunp | grep 2368`. +- `inverted` đã verify bằng sniff sống: `false` góc tăng dần, `true` góc giảm + dần cùng bước. diff --git a/examples/example.cpp b/examples/example.cpp index c2d5a6f..010ee78 100644 --- a/examples/example.cpp +++ b/examples/example.cpp @@ -1,19 +1,15 @@ -// example.cpp — quick try-out of the OLEI LiDAR driver +// OLEI driver example. #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° + lidarlib::Driver drv(lidarlib::MODEL_VB); 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)) { @@ -26,7 +22,6 @@ int main() { 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", @@ -34,12 +29,6 @@ int main() { } } - // ── 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; } diff --git a/examples/lidar_app.cpp b/examples/lidar_app.cpp index e1f2d2c..6d90fb3 100644 --- a/examples/lidar_app.cpp +++ b/examples/lidar_app.cpp @@ -1,14 +1,4 @@ -// 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. -// +// Headless skeleton app: loads config.json, one reader thread per lidar. // ./lidar_app [config.json] #include "lidarlib/lidarlib.hpp" #include @@ -23,10 +13,8 @@ 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 + std::unique_ptr lidar = lidarlib::make_lidar(cfg); 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); @@ -38,11 +26,9 @@ void run_lidar(lidarlib::LidarConfig cfg) { while (g_running) { lidarlib::ScanResult result; - if (!lidar->recv_scan(result, 1000)) continue; // timeout -> retry + if (!lidar->recv_scan(result, 1000)) continue; - // 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", @@ -57,12 +43,12 @@ void run_lidar(lidarlib::LidarConfig cfg) { } // namespace int main(int argc, char** argv) { - setvbuf(stdout, nullptr, _IOLBF, 0); // line-buffer so logs show promptly + setvbuf(stdout, nullptr, _IOLBF, 0); 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 + lidarlib::save_config(config_path, cfg); // ensure the file exists if (cfg.lidars.empty()) { fprintf(stderr, "Khong co lidar nao trong %s\n", config_path.c_str()); diff --git a/examples/nanoscan_example.cpp b/examples/nanoscan_example.cpp new file mode 100644 index 0000000..0de09bc --- /dev/null +++ b/examples/nanoscan_example.cpp @@ -0,0 +1,37 @@ +// 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); + + if (!drv.open()) { + fprintf(stderr, "Không mở được UDP socket cho nanoScan3\n"); + return 1; + } + + for (int i = 0; i < 10; ++i) { + lidarlib::ScanResult result; + if (!drv.recv_scan(result, 2000)) { + fprintf(stderr, "Timeout hoặc lỗi nhận UDP datagram\n"); + break; + } + const lidarlib::LaserScan& scan = result.scan; + const lidarlib::ExtraInfo& info = result.info; + printf("Scan #%d: %zu điểm, ts=%u, model=%s\n", + i, scan.ranges.size(), scan.timestamp_ms, info.detected_model.c_str()); + + for (size_t j = 0; j < 20 && j < scan.ranges.size(); ++j) { + float angle_deg = (scan.angle_min + j * scan.angle_increment) * 180.f / 3.14159265f; + printf(" [%zu] angle=%.2f° dist=%.3fm intensity=%.0f\n", + j, angle_deg, scan.ranges[j], scan.intensities[j]); + } + } + + drv.close(); + return 0; +} + +// Build: +// g++ -std=c++17 -O2 -Iinclude -o nanoscan_example examples/nanoscan_example.cpp src/sick_lidar.cpp diff --git a/examples/sick_example.cpp b/examples/sick_example.cpp index 20e2e2f..518d223 100644 --- a/examples/sick_example.cpp +++ b/examples/sick_example.cpp @@ -1,7 +1,4 @@ -// 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. +// SICK TiM driver example (SOPAS/CoLa-A, TCP). #include "lidarlib/sick_lidar.hpp" #include diff --git a/examples/test_dual.cpp b/examples/test_dual.cpp index 0cf99db..a96ffa6 100644 --- a/examples/test_dual.cpp +++ b/examples/test_dual.cpp @@ -1,6 +1,4 @@ -// 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 +// Test 2 Olei lidars (front + rear) concurrently. #include "lidarlib/lidar.hpp" #include #include @@ -36,14 +34,7 @@ static void run_lidar(const char* tag, const lidarlib::ModelConfig& cfg, } 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. + // 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, diff --git a/include/lidarlib/config.hpp b/include/lidarlib/config.hpp index 6013cf4..c27f900 100644 --- a/include/lidarlib/config.hpp +++ b/include/lidarlib/config.hpp @@ -6,27 +6,29 @@ 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. +// 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"; // 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" + std::string model = "AUTO"; + bool inverted = false; // OLEI only + std::string brand = "OLEI"; // "OLEI" or "SICK" + + // Output angle window (deg): scan angles are remapped onto + // [angle_min_deg, angle_max_deg] without dropping points. + // Defaults (±360) = off. + float angle_min_deg = -360.f; + float angle_max_deg = 360.f; friend bool operator==(const LidarConfig& a, const LidarConfig& b) { return a.name == b.name && a.ip == b.ip && a.port == b.port && - a.model == b.model && a.inverted == b.inverted && a.brand == b.brand; + a.model == b.model && a.inverted == b.inverted && a.brand == b.brand && + a.angle_min_deg == b.angle_min_deg && a.angle_max_deg == b.angle_max_deg; } friend bool operator!=(const LidarConfig& a, const LidarConfig& b) { return !(a == b); } }; -// 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}, @@ -34,47 +36,25 @@ struct Config { }; }; -// Known model name -> ModelConfig (matches the constants in lidar.hpp/sick_lidar.hpp). -// Returns nullptr if name doesn't match any entry. +// nullptr if `name` doesn't match any known model. 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. +// Subset of model_names() valid for `brand`; empty if unknown. 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). +// Returns defaults if the file doesn't exist (without creating 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 +// Build a ready-to-open lidar from one LidarConfig — the only entry point an +// app needs. brand "SICK" → SICK driver (model "SICK-nanoScan3" → UDP +// NanoScanDriver, others → TCP SickDriver); anything else → OLEI UDP. +// Unknown/cross-brand model falls back to the brand default. Never nullptr. std::unique_ptr make_lidar(const LidarConfig& cfg); } // namespace lidarlib diff --git a/include/lidarlib/lidar.hpp b/include/lidarlib/lidar.hpp index 79a87d2..01792e6 100644 --- a/include/lidarlib/lidar.hpp +++ b/include/lidarlib/lidar.hpp @@ -7,45 +7,31 @@ 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). +// 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 since power-on); 0 if the - // family doesn't expose one (see ExtraInfo) + 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 — 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 + 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 read back as float, like ROS does + std::vector intensities; // 0-255 as float }; -// ─── 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. +// Diagnostic/header fields; fields the family doesn't carry stay std::nullopt. 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) + 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 (0xFAF0) only — raw 16-bit "rotation info" header field, - // meaning not decoded/verified. + // Family A only 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. + // Family C / V3 (GS1-5) only — raw passthroughs, unverified std::optional distance_ratio_raw; std::optional scan_frequency_raw; std::optional input_status; @@ -54,144 +40,90 @@ struct ExtraInfo { 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. +// 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 — VB/LR-16F: -135, 360° models: -180 - float scan_angle_max; // deg — VB/LR-16F: 135, 360° models: 180 + 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 }; -// 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_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° -// 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. +// 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 }; -// 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). +// Unified driver interface returned by make_lidar(); OLEI and SICK drivers +// both derive from it. 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.) + // Block until one full scan; false on error/timeout. timeout_ms = 0 → block + // indefinitely. No default on purpose: drivers differ (OLEI 1000, SICK 2000). virtual bool recv_scan(ScanResult& out, int timeout_ms) = 0; - // 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 ───────────────────────────────────────────────────────────────── +// OLEI UDP 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. + // 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(); - // 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. + // Model name read from the Family B/C header; "AUTO" until one is seen. const char* detected_model() const override { return detected_model_name_.c_str(); } private: - // ── parse Family A packet (ID=0xFAF0): 20B header, 3B block ── - bool parse_family_a(const uint8_t* buf, int len); + 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) - // ── 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_; @@ -201,27 +133,22 @@ private: int sock_fd_ = -1; ScanCallback cb_; - // Per-revolution accumulation buffers (parallel arrays, index-aligned) - std::vector pending_angle_deg_; // unwrapped, continuous + // 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-around (revolution-boundary) detection, device space [0,360) + float last_angle_ = -1.f; // wrap 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). + // Per-instance so two drivers on two threads don't 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"; diff --git a/include/lidarlib/lidarlib.hpp b/include/lidarlib/lidarlib.hpp index dc3e129..766aa38 100644 --- a/include/lidarlib/lidarlib.hpp +++ b/include/lidarlib/lidarlib.hpp @@ -1,13 +1,5 @@ #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(). +// One-include convenience header for the whole public API. #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 index 1045420..320d71e 100644 --- a/include/lidarlib/sick_lidar.hpp +++ b/include/lidarlib/sick_lidar.hpp @@ -1,45 +1,25 @@ #pragma once #include "lidarlib/lidar.hpp" +#include #include +#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 +// 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 -// ─── 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. +// 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; // same callback shape, ScanResult-compatible + using ScanCallback = lidarlib::ScanCallback; - // ip/port: SICK device's TCP endpoint (SOPAS default port 2111). explicit SickDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port = 2111); @@ -50,30 +30,17 @@ public: // 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. + // No model string on the wire — returns the configured name. const char* detected_model() const override { return detected_model_name_.c_str(); } private: - bool send_telegram(const std::string& body); // 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 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_; @@ -83,9 +50,51 @@ private: 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. + // Leftover TCP bytes carried across telegram boundaries; per-instance. std::string recv_buf_; }; +inline constexpr ModelConfig MODEL_SICK_NANOSCAN3 { "SICK-nanoScan3", -137.5f, 137.5f, 0.05f, 40.f }; + +// SICK nanoScan3 / microScan3 safety-scanner binary UDP output. Layout ported +// from SICK's open-source sick_safetyscanners; NOT verified on real hardware. +// Passive UDP receiver: the sensor's UDP target must be configured up front in +// SICK Safety Designer — this class does no CoLa2/TCP handshake. +class NanoScanDriver : public Lidar { +public: + using ScanCallback = lidarlib::ScanCallback; + + // ip: local bind address; port: local UDP port the sensor sends to. + explicit NanoScanDriver(const ModelConfig& cfg, + const std::string& ip = "0.0.0.0", + uint16_t port = 6060); + ~NanoScanDriver(); + + NanoScanDriver(const NanoScanDriver&) = delete; + NanoScanDriver& operator=(const NanoScanDriver&) = delete; + + bool open() override; + void close() override; + bool recv_scan(ScanResult& out, int timeout_ms = 1000) override; + void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); } + bool spin_once() override; + + // No model string on the wire — returns the configured name. + const char* detected_model() const override { return detected_model_name_.c_str(); } + +private: + int recv_datagram(int timeout_ms); + bool parse_packet(const uint8_t* buf, int len, ScanResult& out); + + ModelConfig cfg_; + std::string detected_model_name_; // owned copy of cfg_.name (stable lifetime) + std::string ip_; + uint16_t port_; + int sock_fd_ = -1; + ScanCallback cb_; + + // Per-instance; sized for a full safety-data packet (max ~2751 beams). + std::vector recv_buf_; +}; + } // namespace lidarlib diff --git a/src/json_mini.hpp b/src/json_mini.hpp index f79c20e..4b0a888 100644 --- a/src/json_mini.hpp +++ b/src/json_mini.hpp @@ -1,6 +1,4 @@ -// 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. +// Minimal header-only JSON parse/serialize, just enough for config objects. #pragma once #include #include @@ -104,7 +102,6 @@ private: } }; -// ── Parser ────────────────────────────────────────────────────────────── class ParseError : public std::runtime_error { public: explicit ParseError(const std::string& what) : std::runtime_error(what) {} diff --git a/src/lidar_bytes.hpp b/src/lidar_bytes.hpp new file mode 100644 index 0000000..a3a865b --- /dev/null +++ b/src/lidar_bytes.hpp @@ -0,0 +1,42 @@ +// Internal helpers shared by the driver TUs — not part of the public API. +#pragma once +#include "lidarlib/lidar.hpp" +#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; +} + +// 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/olei_config.cpp b/src/lidar_config.cpp similarity index 78% rename from src/olei_config.cpp rename to src/lidar_config.cpp index 2bcca40..94145dd 100644 --- a/src/olei_config.cpp +++ b/src/lidar_config.cpp @@ -22,9 +22,10 @@ constexpr ModelEntry kModels[] = { { "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-TIM5xx", &MODEL_SICK_TIM5XX, "SICK" }, + { "SICK-TIM571", &MODEL_SICK_TIM571, "SICK" }, + { "SICK-TIM7xx", &MODEL_SICK_TIM7XX, "SICK" }, + { "SICK-nanoScan3", &MODEL_SICK_NANOSCAN3, "SICK" }, }; json::Value to_json(const LidarConfig& c) { @@ -35,6 +36,8 @@ json::Value to_json(const LidarConfig& c) { v.set("brand", json::Value::make_string(c.brand)); v.set("model", json::Value::make_string(c.model)); v.set("inverted", json::Value::make_bool(c.inverted)); + v.set("angle_min_deg", json::Value::make_number(c.angle_min_deg)); + v.set("angle_max_deg", json::Value::make_number(c.angle_max_deg)); return v; } @@ -46,6 +49,8 @@ LidarConfig lidar_from_json(const json::Value& v, const LidarConfig& def) { c.brand = v.get_string("brand", def.brand); c.model = v.get_string("model", def.model); c.inverted = v.get_bool("inverted", def.inverted); + c.angle_min_deg = static_cast(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; } @@ -58,10 +63,6 @@ const ModelConfig* model_by_name(const std::string& name) { } 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; @@ -101,7 +102,7 @@ const std::vector& model_names_for_brand(const std::string& brand) } Config load_config(const std::string& path) { - Config cfg; // defaults + Config cfg; std::ifstream f(path); if (!f) return cfg; @@ -111,7 +112,7 @@ Config load_config(const std::string& path) { try { root = json::parse(ss.str()); } catch (const json::ParseError&) { - return cfg; // malformed file -> fall back to defaults rather than crash + return cfg; // malformed file -> defaults } const json::Value* lidars = root.find("lidars"); @@ -136,21 +137,25 @@ void save_config(const std::string& path, const Config& cfg) { } 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). + // Anything but the exact string "SICK" is OLEI (keeps brand-less configs 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); + 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); + return std::make_unique(mc, cfg.ip, cfg.port); + } + return std::make_unique(mc, cfg.ip, cfg.port, cfg.inverted); } } // namespace lidarlib diff --git a/src/olei_lidar.cpp b/src/olei_lidar.cpp index 403c4af..1425cea 100644 --- a/src/olei_lidar.cpp +++ b/src/olei_lidar.cpp @@ -1,4 +1,5 @@ #include "lidarlib/lidar.hpp" +#include "lidar_bytes.hpp" #include #include @@ -10,40 +11,19 @@ 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. +// Normalize into (-180, 180]: 0 = ahead, + = left, - = right. 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] + if (deg < 0.f) deg += 360.f; + if (deg > 180.f) deg -= 360.f; 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) ────────────────────────────────────── +// 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) { @@ -54,12 +34,10 @@ static uint32_t crc32_olei(const uint8_t* data, size_t len) { 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) { @@ -68,14 +46,10 @@ Driver::Driver(const ModelConfig& cfg, const std::string& ip, uint16_t port, boo 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 @@ -98,7 +72,6 @@ bool Driver::open() { return true; } -// ─── close() ──────────────────────────────────────────────────────────────── void Driver::close() { if (sock_fd_ >= 0) { ::close(sock_fd_); @@ -106,7 +79,6 @@ void Driver::close() { } } -// ─── recv_scan() — blocks until one full revolution is available ────────── bool Driver::recv_scan(ScanResult& out, int timeout_ms) { scan_ready_ = false; @@ -115,7 +87,7 @@ bool Driver::recv_scan(ScanResult& out, int timeout_ms) { fd_set fds; FD_ZERO(&fds); FD_SET(sock_fd_, &fds); timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 }; int r = ::select(sock_fd_ + 1, &fds, nullptr, nullptr, &tv); - if (r <= 0) return false; // timeout or error + if (r <= 0) return false; } if (!spin_once()) return false; } @@ -123,10 +95,7 @@ bool Driver::recv_scan(ScanResult& out, int timeout_ms) { 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); @@ -135,28 +104,19 @@ bool Driver::spin_once() { 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] + // A/C carry the frame id at [0-1]; B has a 0x010F preamble, real id at [2-3]. + if (n < 4) return true; + uint16_t id_at_0 = le16(buf); + uint16_t frame_id_b = le16(buf + 2); 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). +// 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) { float angle = signed_angle_deg; if (!pending_angle_deg_.empty()) { @@ -169,7 +129,6 @@ void Driver::push_point(float signed_angle_deg, float dist_m, uint8_t intensity) pending_intensity_.push_back(intensity); } -// ─── flush_scan() — a revolution is complete ─────────────────────────────── void Driver::flush_scan() { if (pending_angle_deg_.empty()) return; @@ -181,13 +140,16 @@ void Driver::flush_scan() { 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.time_increment = 0.f; + scan.scan_time = 0.f; 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()); + if (cfg_.remap_angles) + remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max); + ExtraInfo& info = ready_result_.info; info = pending_info_; info.detected_model = detected_model_name_; @@ -196,63 +158,42 @@ void Driver::flush_scan() { pending_angle_deg_.clear(); pending_dist_m_.clear(); pending_intensity_.clear(); - pending_info_ = ExtraInfo{}; // reset per-revolution optional fields + pending_info_ = ExtraInfo{}; 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) +// Family A (0xFAF0): 20B header + 3B blocks (u16 dist, u8 intensity). 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 + if (block_bytes < num_pts * BLOCK_LEN) return false; uint32_t crc_calc = crc32_olei(buf + HEADER_LEN, static_cast(num_pts * BLOCK_LEN)); - if (crc_calc != crc_packet) return false; // CRC mismatch + if (crc_calc != crc_packet) return false; - // ── 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]); @@ -261,36 +202,21 @@ bool Driver::parse_family_a(const uint8_t* buf, int 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)); + float angle = to_signed_deg(ang_start + frac * (ang_end - ang_start) + cfg_.angle_offset_deg); 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); + push_point(angle, dist_raw * scale_mm * 0.001f, 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. +// 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) { static constexpr int HEADER_LEN = 40; static constexpr int BLOCK_LEN = 8; @@ -298,7 +224,6 @@ bool Driver::parse_family_b(const uint8_t* buf, int len) { 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_) { @@ -320,10 +245,11 @@ bool Driver::parse_family_b(const uint8_t* buf, int len) { }; 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; + 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; + cfg_.angle_offset_deg = entry.cfg->angle_offset_deg; break; } } @@ -334,26 +260,21 @@ bool Driver::parse_family_b(const uint8_t* buf, int 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. + // A packet is only a ~22° arc and may span >1 rev, so the revolution + // boundary is detected per point: a >90° drop between consecutive angles. 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 + if (angle_raw >= INVALID_ANGLE) continue; - float dev_deg = std::fmod(angle_raw * 0.01f, 360.f); // [0,360) + float dev_deg = std::fmod(angle_raw * 0.01f, 360.f); if (last_angle_ >= 0.f && dev_deg < last_angle_ - 90.f) { - flush_scan(); // revolution complete + flush_scan(); } 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 + float angle = maybe_invert(to_signed_deg(angle_raw * 0.01f + cfg_.angle_offset_deg), inverted_); + float dist_m = le16(blk + 2) * scale_mm * 0.001f; 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; @@ -364,36 +285,9 @@ bool Driver::parse_family_b(const uint8_t* buf, int len) { 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). +// 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) { static constexpr int HEADER_LEN = 48; if (len < HEADER_LEN) return false; @@ -410,13 +304,15 @@ bool Driver::parse_family_c(const uint8_t* buf, int len) { 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 + if (num_pts_scan == 0) return false; int header_size = (header_size_field == 0) ? HEADER_LEN : header_size_field; if (header_size < HEADER_LEN || header_size > len) return false; + // Types: 0x00 = 2B/point (range only), 0x01 = 4B (range+intensity), + // 0x10 = 4B (range at [+2,+4)). int bytes_per_point = (types == 0x00) ? 2 : (types == 0x01 || types == 0x10) ? 4 : 0; - if (bytes_per_point == 0) return false; // unknown Types, layout unclear + if (bytes_per_point == 0) return false; int payload_bytes = len - header_size; int num_pts = num_pts_packet; @@ -432,25 +328,18 @@ bool Driver::parse_family_c(const uint8_t* buf, int len) { 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. + // Magic 0xFEAC == exactly one model (GS1-5). 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; + cfg_.angle_offset_deg = MODEL_GS15.angle_offset_deg; 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) { @@ -473,11 +362,11 @@ bool Driver::parse_family_c(const uint8_t* buf, int len) { range_mm = le16(blk + 2); } - float angle = to_signed_deg(static_cast(first_index + i) * angle_inc - 180.f); + float angle = to_signed_deg(static_cast(first_index + i) * angle_inc - 180.f + cfg_.angle_offset_deg); 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 */, + push_point(angle, range_mm * 0.001f, has_inten ? static_cast(inten_raw > 255 ? 255 : inten_raw) : uint8_t{0}); } diff --git a/src/sick_lidar.cpp b/src/sick_lidar.cpp index 7b8ff9e..675738a 100644 --- a/src/sick_lidar.cpp +++ b/src/sick_lidar.cpp @@ -1,9 +1,12 @@ #include "lidarlib/sick_lidar.hpp" +#include "lidar_bytes.hpp" #include #include +#include #include #include +#include #include #include #include @@ -16,7 +19,6 @@ namespace lidarlib { namespace { -constexpr float kDeg2Rad = 3.14159265358979323846f / 180.f; constexpr char kStx = 0x02; constexpr char kEtx = 0x03; constexpr int kConnectTimeoutMs = 2000; @@ -25,14 +27,9 @@ 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. + // SICK encodes signed 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; @@ -45,6 +42,10 @@ 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) @@ -52,7 +53,6 @@ SickDriver::SickDriver(const ModelConfig& cfg, const std::string& ip, uint16_t p 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; @@ -62,11 +62,8 @@ bool SickDriver::open() { 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). + // Non-blocking connect with a bounded timeout — a blocking connect() to an + // unreachable device would stall for the OS default (~2 min on Linux). int flags = ::fcntl(sock_fd_, F_GETFL, 0); ::fcntl(sock_fd_, F_SETFL, flags | O_NONBLOCK); @@ -80,10 +77,10 @@ bool SickDriver::open() { ::getsockopt(sock_fd_, SOL_SOCKET, SO_ERROR, &err, &errlen); rc = (err == 0) ? 0 : -1; } else { - rc = -1; // timeout, or select() itself failed + rc = -1; } } - ::fcntl(sock_fd_, F_SETFL, flags); // restore blocking mode for send/recv below + ::fcntl(sock_fd_, F_SETFL, flags); if (rc < 0) { ::close(sock_fd_); @@ -96,8 +93,7 @@ bool SickDriver::open() { recv_buf_.clear(); - // The device stays passive until told otherwise — without this, no - // LMDscandata telegram ever arrives. + // Device is passive until told to stream. if (!send_telegram("sEN LMDscandata 1")) { close(); return false; @@ -105,16 +101,14 @@ bool SickDriver::open() { return true; } -// ─── close() ──────────────────────────────────────────────────────────────── void SickDriver::close() { if (sock_fd_ >= 0) { - send_telegram("sEN LMDscandata 0"); // best-effort, ignore failure + send_telegram("sEN LMDscandata 0"); // best-effort ::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; @@ -132,10 +126,8 @@ bool SickDriver::send_telegram(const std::string& body) { 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. ────────────────────────────────────────────── +// CoLa-A has no length prefix, so ETX is the only frame boundary; recv_buf_ +// carries leftover bytes across calls. bool SickDriver::read_telegram(std::string& out, int timeout_ms) { if (sock_fd_ < 0) return false; @@ -144,7 +136,6 @@ bool SickDriver::read_telegram(std::string& out, int timeout_ms) { 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; } @@ -157,58 +148,37 @@ bool SickDriver::read_telegram(std::string& out, int timeout_ms) { fd_set fds; FD_ZERO(&fds); FD_SET(sock_fd_, &fds); timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 }; int r = ::select(sock_fd_ + 1, &fds, nullptr, nullptr, &tv); - if (r <= 0) return false; // timeout or error + if (r <= 0) return false; } char buf[4096]; ssize_t n = ::recv(sock_fd_, buf, sizeof(buf), 0); - if (n <= 0) return false; // closed or error + if (n <= 0) return false; 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. + // Non-scan telegram (e.g. an ack) — keep waiting. } } -// ─── spin_once() ──────────────────────────────────────────────────────────── bool SickDriver::spin_once() { std::string telegram; - if (!read_telegram(telegram, 0)) return false; // 0 = block until next telegram + if (!read_telegram(telegram, 0)) return false; ScanResult result; - if (!parse_lmdscandata(telegram, result)) return true; // ignore non-scan telegrams + if (!parse_lmdscandata(telegram, result)) return true; 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. +// CoLa-A "sSN/sRA LMDscandata": space-separated ASCII hex tokens, field order +// per SICK's Telegram Listing. "DIST1" → ranges, "RSSI1" → intensities. bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out) { std::vector tok = tokenize(telegram); if (tok.size() < 20) return false; @@ -218,15 +188,14 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out) 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 + hex_to_u32(next()); // VersionNumber + hex_to_u32(next()); // DeviceNumber + hex_to_u32(next()); // SerialNumber + uint32_t status0 = hex_to_u32(next()); + uint32_t status1 = hex_to_u32(next()); + hex_to_u32(next()); // TelegramCounter + hex_to_u32(next()); // ScanCounter + hex_to_u32(next()); // TimeSinceStartup uint32_t time_of_transmission = hex_to_u32(next()); uint32_t in0 = hex_to_u32(next()); uint32_t in1 = hex_to_u32(next()); @@ -234,7 +203,7 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out) uint32_t out1 = hex_to_u32(next()); next(); // Reserved uint32_t scanning_frequency = hex_to_u32(next()); - hex_to_u32(next()); // MeasurementFrequency — not exposed + hex_to_u32(next()); // MeasurementFrequency uint32_t num_encoders = hex_to_u32(next()); for (uint32_t e = 0; e < num_encoders; ++e) { @@ -248,22 +217,23 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out) 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" + // 16-bit and 8-bit channel blocks share the same ASCII layout. + auto parse_channel_block = [&]() { + std::string content = next(); uint32_t scale_bits = hex_to_u32(next()); - hex_to_u32(next()); // ScalingOffset — unused + hex_to_u32(next()); // ScalingOffset int32_t start_angle = hex_to_i32(next()); // 1/10000 deg int32_t step_width = hex_to_i32(next()); // 1/10000 deg uint32_t num_data = hex_to_u32(next()); float scale = bits_to_float(scale_bits); - if (scale == 0.f) scale = 1.f; // guard against a zero/garbage scaling factor + if (scale == 0.f) scale = 1.f; bool is_dist = content.rfind("DIST", 0) == 0; bool is_rssi = content.rfind("RSSI", 0) == 0; if (is_dist) { - angle_min_deg = static_cast(start_angle) * 0.0001f; + angle_min_deg = static_cast(start_angle) * 0.0001f + cfg_.angle_offset_deg; angle_inc_deg = static_cast(step_width) * 0.0001f; scan.ranges.assign(num_data, 0.f); } else if (is_rssi && scan.intensities.empty()) { @@ -279,14 +249,13 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out) 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); + for (uint32_t c = 0; c < num_16bit_channels; ++c) parse_channel_block(); uint32_t num_8bit_channels = hex_to_u32(next()); - for (uint32_t c = 0; c < num_8bit_channels; ++c) parse_channel_block(true); + for (uint32_t c = 0; c < num_8bit_channels; ++c) parse_channel_block(); if (!got_dist || scan.ranges.empty()) return false; @@ -295,17 +264,18 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out) 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.time_increment = 0.f; + scan.scan_time = 0.f; scan.range_min = cfg_.range_min_m; scan.range_max = cfg_.range_max_m; if (scan.intensities.size() != scan.ranges.size()) - scan.intensities.assign(scan.ranges.size(), 0.f); // RSSI channel wasn't enabled on the device + scan.intensities.assign(scan.ranges.size(), 0.f); + + if (cfg_.remap_angles) + remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max); ExtraInfo& info = out.info; info = ExtraInfo{}; - // 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; @@ -316,4 +286,181 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out) return true; } +// ── NanoScanDriver — SICK nanoScan3/microScan3 safety-scanner UDP output ──── + +NanoScanDriver::NanoScanDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port) + : cfg_(cfg), detected_model_name_(cfg.name ? cfg.name : ""), ip_(ip), port_(port), + recv_buf_(kNanoRecvBufSize) {} + +NanoScanDriver::~NanoScanDriver() { close(); } + +bool NanoScanDriver::open() { + sock_fd_ = ::socket(AF_INET, SOCK_DGRAM, 0); + if (sock_fd_ < 0) return false; + + int reuse = 1; + ::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)); + + sockaddr_in addr{}; + addr.sin_family = AF_INET; + addr.sin_port = htons(port_); + addr.sin_addr.s_addr = (ip_ == "0.0.0.0" || ip_.empty()) ? INADDR_ANY + : inet_addr(ip_.c_str()); + + if (::bind(sock_fd_, reinterpret_cast(&addr), sizeof(addr)) < 0) { + ::close(sock_fd_); + sock_fd_ = -1; + return false; + } + return true; +} + +void NanoScanDriver::close() { + if (sock_fd_ >= 0) { + ::close(sock_fd_); + sock_fd_ = -1; + } +} + +int NanoScanDriver::recv_datagram(int timeout_ms) { + if (sock_fd_ < 0) return -1; + + if (timeout_ms > 0) { + fd_set fds; FD_ZERO(&fds); FD_SET(sock_fd_, &fds); + timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 }; + int r = ::select(sock_fd_ + 1, &fds, nullptr, nullptr, &tv); + if (r <= 0) return -1; + } + + ssize_t n = ::recv(sock_fd_, recv_buf_.data(), recv_buf_.size(), 0); + return (n <= 0) ? -1 : static_cast(n); +} + +// A scan is split across datagrams at the application layer. Each starts with +// a 24-byte fragment header: "MS3 " @0, u32 totalLength @8, u32 scanNumber @12, +// u32 fragmentOffset @16. Reassemble until totalLength bytes; a lost fragment +// drops that scan and we resync on the next scanNumber. +bool NanoScanDriver::recv_scan(ScanResult& out, int timeout_ms) { + std::vector tele; + uint32_t cur_scan = 0, total = 0, got = 0; + bool assembling = false; + + for (;;) { + int n = recv_datagram(timeout_ms); + if (n < 0) return false; + const uint8_t* d = recv_buf_.data(); + + if (n < 24 || std::memcmp(d, "MS3 ", 4) != 0) { + if (parse_packet(d, n, out)) return true; + continue; + } + + uint32_t tl = le32(d + 8); + uint32_t scan = le32(d + 12); + uint32_t foff = le32(d + 16); + const uint8_t* pl = d + 24; + uint32_t pl_len = static_cast(n) - 24; + if (tl == 0 || tl > kNanoRecvBufSize) continue; + + if (!assembling || scan != cur_scan || tl != total) { + cur_scan = scan; total = tl; got = 0; + tele.assign(total, 0); + assembling = true; + } + + if (static_cast(foff) + pl_len <= total) { + std::memcpy(tele.data() + foff, pl, pl_len); + got += pl_len; + } + + if (got >= total) { + assembling = false; + if (parse_packet(tele.data(), static_cast(total), out)) return true; + } + } +} + +bool NanoScanDriver::spin_once() { + int n = recv_datagram(0); + if (n < 0) return false; + + ScanResult result; + if (!parse_packet(recv_buf_.data(), n, result)) return true; + if (cb_) cb_(result); + return true; +} + +// SICK safety-scanner data packet (LE), layout ported from sick_safetyscanners: +// DataHeader offset table at fixed offsets (derivedValues @36, measurementData +// @40); DerivedValues holds multiplicationFactor/startAngle/resolution; +// MeasurementData is u32 numBeams then 4 B/beam (u16 dist, u8 reflect, u8 status). +bool NanoScanDriver::parse_packet(const uint8_t* buf, int len, ScanResult& out) { + if (len < 52) return false; + + uint16_t dv_off = le16(buf + 36); + uint16_t dv_size = le16(buf + 38); + uint16_t md_off = le16(buf + 40); + uint16_t md_size = le16(buf + 42); + + if (dv_off == 0 || dv_size == 0 || md_off == 0 || md_size == 0) return false; + if (static_cast(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 (cfg_.remap_angles) + remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max); + + ExtraInfo& info = out.info; + info = ExtraInfo{}; + info.detected_model = cfg_.name; + + return true; +} + } // namespace lidarlib