QUYVN/DriverLIdar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Refactor lidar library: rename olei_config to lidar_config, add nanoscan example and shared byte helpers

Browse Source 
Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
This commit is contained in:
QUYVN
2026-07-03 09:27:43 +07:00
parent 59880871b0
commit 323715eab0
16 changed files with 621 additions and 814 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
CMakeLists.txt
View File

@@ -11,12 +11,10 @@ find_package(Threads REQUIRED)
option(BUILD_SHARED_LIBS "Build shared (.so) libraries instead of static" ON) 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 set(LIDARLIB_SOURCES
src/olei_lidar.cpp src/olei_lidar.cpp
src/sick_lidar.cpp src/sick_lidar.cpp
src/olei_config.cpp src/lidar_config.cpp
) )
add_library(lidarlib ${LIDARLIB_SOURCES}) add_library(lidarlib ${LIDARLIB_SOURCES})
@@ -42,13 +40,14 @@ if(LIDARLIB_BUILD_EXAMPLES)
add_executable(sick_example examples/sick_example.cpp) add_executable(sick_example examples/sick_example.cpp)
target_link_libraries(sick_example PRIVATE lidarlib) target_link_libraries(sick_example PRIVATE lidarlib)
# Headless skeleton app: load config.json -> make_lidar() each -> print scans. add_executable(nanoscan_example examples/nanoscan_example.cpp)
# Replace its print loop with your own GUI; this is the integration template. target_link_libraries(nanoscan_example PRIVATE lidarlib)
add_executable(lidar_app examples/lidar_app.cpp) add_executable(lidar_app examples/lidar_app.cpp)
target_link_libraries(lidar_app PRIVATE lidarlib) target_link_libraries(lidar_app PRIVATE lidarlib)
endif() endif()
# ── install + find_package() support ── # install + find_package() support
include(GNUInstallDirs) include(GNUInstallDirs)
include(CMakePackageConfigHelpers) include(CMakePackageConfigHelpers)

396
README.md
View File

@@ -1,135 +1,14 @@
# Lidarlib # Lidarlib
Thư viện C++17 cho lidar (OLEI + SICK), build bằng CMake ra **shared lib `.so`** Thư viện C++17 cho lidar **OLEI** (UDP) và **SICK** (TCP/UDP). Build bằng CMake
(`lidarlib::lidarlib`), hỗ trợ `find_package()` để link vào project khác. Tự nhận ra shared lib, hỗ trợ `find_package(lidarlib)`. Mọi driver dùng chung một
diện họ giao thức (Family A/B/C) theo từng gói, hỗ trợ chạy nhiều lidar song interface `lidarlib::Lidar` và một hàm khởi tạo duy nhất `lidarlib::make_lidar()`.
song, gộp cả OLEI (UDP) và SICK (TCP) sau **một hàm config duy nhất**
`lidarlib::make_lidar()`. **Không có Web UI** — người dùng tự viết giao diện trên
API C++ này (include header + link `.so`).
## Tóm tắt API (cho người viết giao diện) - 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
```cpp - Chạy nhiều lidar song song (mỗi instance độc lập, an toàn đa luồng)
#include "lidarlib/lidarlib.hpp" // gộp toàn bộ API trong 1 include - 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
// 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<lidarlib::Lidar> lidar = lidarlib::make_lidar(c);
lidar->open();
// 2) HAI loại dữ liệu mỗi vòng quét
lidarlib::ScanResult r;
lidar->recv_scan(r, 1000);
// r.scan : lidarlib::LaserScan — chung cho mọi lidar, đúng format sensor_msgs/LaserScan của ROS
// r.info : lidarlib::ExtraInfo — thông tin thêm, tuỳ family/model thực tế
// (tuỳ chọn) lưu/đọc danh sách lidar ra file:
lidarlib::Config cfg = lidarlib::load_config("config.json");
lidarlib::save_config("config.json", cfg);
```
`lidarlib::Lidar` là interface chung; `lidarlib::Driver` (OLEI/UDP) và `lidarlib::SickDriver`
(SICK/TCP) đều kế thừa nó, nên giao diện chỉ cần thao tác qua `lidarlib::Lidar*` mà
không phải phân biệt hãng. Vẫn có thể `new` thẳng `Driver`/`SickDriver` nếu muốn.
## Kiến trúc
| File | Vai trò |
|------|---------|
| `include/lidarlib/lidarlib.hpp` | Header tổng hợp — include 1 dòng là có cả data model + 2 driver + config + factory |
| `include/lidarlib/lidar.hpp` | API public: `LaserScan`, `ExtraInfo`, `ScanResult`, `ModelConfig`, interface `Lidar`, class `Driver` |
| `src/olei_lidar.cpp` | Parse Family A (0xFAF0), Family B (0xFEF0), Family C/V3 (0xFEAC), CRC, gom scan |
| `include/lidarlib/config.hpp` + `src/olei_config.cpp` | `Config`/`LidarConfig` (gồm `brand`: `"OLEI"`/`"SICK"`), load/save `config.json`, tra cứu `ModelConfig` theo tên/theo hãng, và **hàm config `make_lidar()`** |
| `src/json_mini.hpp` | Parser/serializer JSON tối giản, chỉ dùng nội bộ cho `olei_config` (load/save `config.json`) |
| `include/lidarlib/sick_lidar.hpp` + `src/sick_lidar.cpp` | Driver riêng cho lidar **SICK TiM5xx/7xx** — giao thức SOPAS/CoLa-A qua TCP (port 2111), khác hoàn toàn UDP binary của OLEI. **Đã verify bằng TiM781S thật** (xem mục riêng bên dưới) |
| `examples/example.cpp` | Demo 1 lidar, `recv_scan()` blocking |
| `examples/test_dual.cpp` | Demo 2 lidar song song (2 thread) |
| `examples/sick_example.cpp` | Demo driver SICK TiM, `recv_scan()` blocking qua TCP |
| `examples/lidar_app.cpp` | Khung app headless: đọc `config.json` → `make_lidar()` từng con → in scan. Thay vòng `printf` bằng giao diện của bạn |
| `CMakeLists.txt` | Build `lidarlib` (OLEI + SICK + factory + config, chỉ phụ thuộc pthread) thành `.so`, cài `install()`/`find_package()` |
Ba họ giao thức được driver tự nhận diện theo Frame ID/magic trong từng gói:
- **Family A** (`0xFAF0`) — VB/VF/LR-1F. Header 20B + block 3B/điểm. Có CRC32.
- **Family B** (`0xFEF0`) — LR-1BS5/LR-1BS2. Header 40B (preamble `0x010F` +
frame id ở offset [2-3], chuỗi tên model ASCII ở offset [7-17)) + block
8B/điểm.
- **Family C / protocol V3** (`0xFEAC`) — GS1-5. Header 48B, block 2/4B/điểm
tùy byte `Types`. **Port từ driver C# `OleiGS15Driver.cs`
(RobotNet10.RobotApp), CHƯA verify bằng phần cứng GS1-5 thật** (không có
thiết bị để sniff) — chỉ test bằng packet giả lập tự dựng theo đúng cấu trúc
header.
## Output: 2 loại
`Driver::recv_scan()` (và callback `set_scan_callback`) trả về
`ScanResult { LaserScan scan; ExtraInfo info; }` mỗi khi gom đủ 1 vòng quay:
```cpp
lidarlib::Driver drv(lidarlib::MODEL_AUTO, "192.168.100.100", 2369);
drv.open();
lidarlib::ScanResult result;
drv.recv_scan(result, 2000);
printf("%zu diem, model=%s\n", result.scan.ranges.size(), result.info.detected_model.c_str());
```
**`LaserScan`** — cùng tên field/đơn vị với `sensor_msgs/LaserScan` của ROS
(radian, mét, giây):
| Field | Ý nghĩa |
|---|---|
| `timestamp_ms` | Đồng hồ thiết bị (ms từ lúc bật nguồn); = 0 nếu family không có (xem `ExtraInfo`) |
| `angle_min`/`angle_max`/`angle_increment` | rad — đã unwrap liên tục, KHÔNG bị giới hạn `[-π,π]` |
| `time_increment`/`scan_time` | Luôn = 0 — thiết bị không báo timing chi tiết đó |
| `range_min`/`range_max` | m — lấy từ `ModelConfig` (giá trị đặt sẵn, **không đo được mỗi scan**) |
| `ranges[]`/`intensities[]` | m / 0-255 (đọc lại thành float như ROS) |
**`ExtraInfo`** — thông tin thêm tuỳ family/model thực tế của packet, field
nào thiết bị không có thì giữ `std::nullopt`:
| Field | Family | Verify hardware? |
|---|---|---|
| `detected_model` | mọi family (qua `MODEL_AUTO`) | Family B verify bằng sniff sống |
| `error_status` | Family A | Verify |
| `distance_scale_mm` | Family A/B | Verify |
| `rotation_raw` | Family A | Raw, chưa decode ý nghĩa |
| `distance_ratio_raw`, `scan_frequency_raw`, `input_status`, `output_status`, `field_status`, `status_flags` | Family C/GS1-5 | Raw, **chưa verify hardware thật** |
Vì sao cần "unwrap": góc từng điểm được lọc theo FOV ở hệ **có dấu**
`[-180, 180]` (0 = phía trước, dương = bên trái) — nhưng hệ này gãy ở biên
±180° đối với lidar quét 360°. Trước khi đưa vào `LaserScan`, driver unwrap
lại thành một dải góc liên tục trong từng vòng quay (`Driver::push_point()`
trong `src/olei_lidar.cpp`), nên `angle_min`/`angle_max`/`ranges[]` luôn đơn
điệu — đúng kiểu mảng mà `sensor_msgs/LaserScan` kỳ vọng.
## Kết nối lidar
Mạng tham chiếu trên host này (`/home/robotics`):
```
eth0: 192.168.100.100/24
├─ front (scan_1): DeviceIp 192.168.100.11, DevicePort 2368
└─ rear (scan_2): DeviceIp 192.168.100.12, DevicePort 2369
```
(Khớp với `RobotApp/RobotNet10.RobotApp/appsettings.json`, các mục
`Olei-front`/`Olei-rear`.)
Kiểm tra kết nối trước khi test:
```bash
ip -4 addr show eth0 # phải thấy inet 192.168.100.100/24
ping -c1 192.168.100.11 # front
ping -c1 192.168.100.12 # rear
```
**Lưu ý quan trọng:** nếu `RobotNet10.RobotApp` đang chạy, nó bind sẵn port
2368/2369 (không bật `SO_REUSEPORT`) → driver standalone sẽ bind lỗi
(`Khong mo duoc socket... interface khong ton tai?`). Kiểm tra ai đang giữ port:
```bash
ss -lunp | grep -E '2368|2369'
```
## Build ## Build
@@ -138,209 +17,150 @@ cmake -S . -B build -DCMAKE_BUILD_TYPE=Release
cmake --build build -j"$(nproc)" cmake --build build -j"$(nproc)"
``` ```
Sinh ra `build/liblidarlib.so` (driver OLEI + SICK + factory + config, không Sinh ra `build/liblidarlib.so` (chỉ phụ thuộc pthread) và các binary demo
phụ thuộc gì ngoài pthread) và 4 binary demo (`example`, `test_dual`, (`example`, `test_dual`, `sick_example`, `nanoscan_example`, `lidar_app`).
`sick_example`, `lidar_app`). Tắt build demo bằng `-DLIDARLIB_BUILD_EXAMPLES=OFF`.
Muốn ra static lib `.a` thay vì `.so` thì thêm `-DBUILD_SHARED_LIBS=OFF`.
Cài vào hệ thống để dùng `find_package(lidarlib)` từ project khác — `/usr/local` Tùy chọn: `-DLIDARLIB_BUILD_EXAMPLES=OFF` (tắt demo),
chỉ root mới ghi được nên cần `sudo`, không thì `cmake --install` báo lỗi `-DBUILD_SHARED_LIBS=OFF` (static lib).
permission denied:
Cài đặt và dùng từ project khác:
```bash ```bash
sudo cmake --install build --prefix /usr/local cmake --install build --prefix "$HOME/.local" # hoặc sudo với /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 ```cmake
# trong CMakeLists.txt của project dùng thư viện này
find_package(lidarlib REQUIRED) find_package(lidarlib REQUIRED)
target_link_libraries(my_app PRIVATE lidarlib::lidarlib) 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 ## Quick start
`lidarlib::make_lidar()` — xem "Tóm tắt API" ở đầu README.
Vẫn có thể build từng file bằng g++ thuần nếu không muốn dùng CMake:
```bash
g++ -std=c++17 -O2 -pthread -Wall -Wextra -Iinclude -o test_dual examples/test_dual.cpp src/olei_lidar.cpp
g++ -std=c++17 -O2 -pthread -Wall -Wextra -Iinclude -o example examples/example.cpp src/olei_lidar.cpp
g++ -std=c++17 -O2 -pthread -Wall -Wextra -Iinclude -o lidar_app examples/lidar_app.cpp src/olei_lidar.cpp src/sick_lidar.cpp src/olei_config.cpp
g++ -std=c++17 -O2 -pthread -Wall -Wextra -Iinclude -o sick_example examples/sick_example.cpp src/sick_lidar.cpp
```
## Test thử
### 2 lidar song song
```bash
./build/test_dual
```
In ra 5 scan mỗi bên, kèm số điểm, timestamp, error status, và **model đã tự
dò được** (`model=...`).
### 1 lidar
Sửa model/IP/port trong `examples/example.cpp` rồi build lại, hoặc gọi trực tiếp:
```cpp ```cpp
lidarlib::Driver drv(lidarlib::MODEL_AUTO, "192.168.100.100", 2369); #include "lidarlib/lidarlib.hpp" // toàn bộ API trong 1 include
drv.open();
lidarlib::ScanResult result; lidarlib::LidarConfig c{"front", "192.168.1.10", 2368, "AUTO", false, "OLEI"};
drv.recv_scan(result, 2000); std::unique_ptr<lidarlib::Lidar> 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 Có thể khởi tạo driver trực tiếp thay vì qua `make_lidar()`:
Constructor có tham số thứ 4 `inverted` (mặc định `false`). Đặt `true` nếu
thiết bị bị lắp lật 180° quanh trục hướng về phía trước — driver tự đảo dấu
góc từng điểm (`angle = -angle`, chuẩn hóa lại về `-180..180`) để output luôn
đúng theo hệ quy chiếu xe, không phụ thuộc hướng lắp vật lý:
```cpp ```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, Ngoài `recv_scan()` blocking còn có callback: `set_scan_callback()` +
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. `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` `lidar_app` là app mẫu headless: đọc `config.json`, mở từng lidar bằng
đọc nó, mở từng lidar qua đúng **một hàm** `lidarlib::make_lidar()`, rồi đọc scan `make_lidar()`, một thread mỗi con.
trên mỗi thread. Đây là khung mẫu để bạn thay vòng `printf` bằng giao diện
riêng (Qt, ImGui, ROS node, v.v.).
```bash ```bash
./build/lidar_app # đọc/tạo config.json cạnh chỗ chạy ./build/lidar_app [my_config.json]
./build/lidar_app my_config.json # đường dẫn config khác
``` ```
`config.json` — danh sách lidar, không cố định số lượng. `brand` chọn loại
driver (`"OLEI"` = UDP, `"SICK"` = TCP/SOPAS); `model` tra trong bảng
`ModelConfig` (tên lạ → tự lùi về mặc định của hãng: `AUTO` cho OLEI,
`SICK-TIM571` cho SICK); `inverted` chỉ có tác dụng với OLEI. Bỏ trống `brand`
thì mặc định `"OLEI"` (tương thích file cũ).
```json ```json
{ {
"lidars": [ "lidars": [
{"name":"front", "ip":"192.168.100.100","port":2368,"brand":"OLEI","model":"AUTO", "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":"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":"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)` | Trường | Ý nghĩa |
(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ả. | `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 ## Model
python3 - <<'EOF'
import socket
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
s.bind(("192.168.100.100", 2369)) # đổi port theo lidar cần xem
data, addr = s.recvfrom(4096)
print("from", addr, "len", len(data))
print(data[:40].hex(' '))
EOF
```
Luôn ưu tiên capture thật hơn là tin comment trong code — comment mô tả ý ### OLEI (`brand = "OLEI"`, UDP, port mặc định 2368)
định lúc viết, không phải offset đã verify trên thiết bị thật.
## Chọn `ModelConfig` | Constant | FOV (°) | Range (m) | Ghi chú |
| Constant | FOV (deg, có dấu) | range_min/max (m) | Khi dùng |
|---|---|---|---| |---|---|---|---|
| `MODEL_VB` | -135…135 | 0.05…30 | 2D 270°, Family A | | `MODEL_VB` | -135…135 | 0.05…30 | 2D 270°, Family A |
| `MODEL_VF` | -180…180 | 0.05…30 | 2D 360°, 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_LR1F` | -180…180 | 0.05…50 | Family A; 0° thô của máy chỉ về đuôi (offset +180°) |
| `MODEL_LR1BS5` | -180…180 | 0.05…30 | 2D 360°, Family B | | `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_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_GS15` | -180…180 | 0.05…30 | Family C/V3 — **chưa verify phần cứng** |
| `MODEL_AUTO` | -180…180 (mặc định, có thể tự thu hẹp) | 0.05…30 | Không biết trước model | | `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 Driver nhận diện họ giao thức theo Frame ID mỗi gói:
`LaserScan::range_min/range_max` (không đọc từ packet) — chỉnh trong **Family A** `0xFAF0` (header 20B, 3B/điểm, CRC32) ·
`include/lidarlib/lidar.hpp` theo datasheet thật của từng model nếu cần chính xác. **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 Tên model thật đọc từ packet xem qua `detected_model()` hoặc
trong header). Driver luôn lưu lại **tên thật** đọc từ packet (vd `result.info.detected_model`.
`"OLELR-1BS5"`, `"OLELR-1BS2"`) — gọi `drv.detected_model()` hoặc
`result.info.detected_model` để xem (trả về `"AUTO"` nếu chưa nhận gói Family B
nào). FOV chỉ tự thu hẹp khi tên đó khớp một entry trong `kModelTable`
(`src/olei_lidar.cpp`); nếu không khớp, FOV giữ nguyên mặc định 360°
(`-180..180`, không mất điểm) — an toàn nhưng có thể giữ lại điểm ngoài FOV
thật của thiết bị nếu thiết bị đó không quét tròn.
Family A không mang chuỗi tên model trong packet, nên `MODEL_AUTO` trên thiết ### SICK (`brand = "SICK"`)
bị Family A cũng giữ nguyên FOV rộng — phải chỉ định model cụ thể (VD
`MODEL_VB`) nếu muốn thu hẹp FOV cho thiết bị góc hẹp.
## Lidar SICK (TiM5xx/7xx) — driver riêng | Constant | FOV (°) | Range (m) | Transport |
`lidarlib::SickDriver` (`include/lidarlib/sick_lidar.hpp` + `src/sick_lidar.cpp`) là
driver **độc lập** với `lidarlib::Driver` ở trên — không phải thêm 1 family vào
driver OLEI, vì giao thức khác hẳn:
- Kết nối **TCP** (SOPAS, port mặc định 2111) tới lidar, không phải UDP
broadcast như OLEI.
- Telegram là **ASCII** (CoLa-A), đóng khung bằng `STX`(0x02)/`ETX`(0x03),
không có CRC32 như Family A.
- Thiết bị đứng im cho tới khi driver gửi lệnh `sEN LMDscandata 1` — `open()`
tự làm việc này; `close()` gửi `sEN LMDscandata 0` trước khi đóng socket.
Output vẫn dùng chung `ScanResult`/`LaserScan`/`ExtraInfo` như driver OLEI nên
gọi giống hệt:
```cpp
lidarlib::SickDriver drv(lidarlib::MODEL_SICK_TIM571, "192.168.0.1", 2111);
drv.open();
lidarlib::ScanResult result;
drv.recv_scan(result, 2000);
```
| Constant | FOV (deg, có dấu) | range_min/max (m) | Khi dùng |
|---|---|---|---| |---|---|---|---|
| `MODEL_SICK_TIM5XX` | -135…135 | 0.05…10 | TiM551/561, 270° | | `MODEL_SICK_TIM5XX` | -135…135 | 0.05…10 | TCP/SOPAS (CoLa-A), port 2111 |
| `MODEL_SICK_TIM571` | -135…135 | 0.05…25 | TiM571, 270° | | `MODEL_SICK_TIM571` | -135…135 | 0.05…25 | TCP/SOPAS, port 2111 |
| `MODEL_SICK_TIM7XX` | -135…135 | 0.05…25 | TiM781, 270° | | `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** **TiM (`SickDriver`)** — `open()` tự gửi `sEN LMDscandata 1` để bắt đầu stream.
(FW `V5.11-14.10.24`, DeviceIdent đọc qua `sRN DeviceIdent` trên cổng 2111) Hệ góc trên dây đặt 90° = trước mặt nên preset có `angle_offset_deg = -90`,
tại `192.168.100.22:2111` — `MODEL_SICK_TIM7XX`. Kết quả khớp đúng datasheet output ra -135…135° với 0° = phía trước. **Đã verify trên TiM781S thật**
TiM781S: 811 điểm/scan trải từ -45°…225° (270° FOV), `angle_increment` = (811 điểm/scan, increment 0.333°, DIST1/RSSI1 đúng layout). Chưa verify:
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 encoder, kênh 8-bit, thông số TiM5xx/571.
định qua nhiều scan liên tiếp, kênh `RSSI1` có giá trị intensity hợp lý
(không phải toàn 0), `error_status` = 0x00. Điều này xác nhận layout
`LMDscandata` trong `parse_lmdscandata()` (`src/sick_lidar.cpp`) — kênh
`DIST1`/`RSSI1`, scaling factor IEEE-754, start angle/step width — đọc đúng
trên hardware thật, không chỉ đúng theo tài liệu nữa.
**Vẫn chưa verify**: chiều quy ước góc 0° (thẳng phía trước thiết bị hay **nanoScan3 (`NanoScanDriver`)** — UDP receiver thụ động: chỉ bind cổng và
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 parse datagram; **đích UDP phải cấu hình sẵn trong SICK Safety Designer**
(`NumEncoders > 0`, nhánh `next()`×2 chưa từng chạy qua vì test thực tế không (driver không bắt tay CoLa2). Layout port từ `sick_safetyscanners` (Apache-2.0).
có encoder), và nhánh 8-bit channel (`Num8BitChannels`, thiết bị test chỉ **Chưa verify phần cứng thật** — mới test bằng gói tổng hợp qua loopback.
dùng kênh 16-bit). `MODEL_SICK_TIM5XX`/`MODEL_SICK_TIM571` (FOV/range theo
datasheet) cũng chưa test trên phần cứng — chỉ `MODEL_SICK_TIM7XX` đã chạy
thật.
`SickDriver` nối vào `config.json` qua field `LidarConfig::brand` (`"OLEI"` ## Output
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 `ScanResult { LaserScan scan; ExtraInfo info; }` mỗi vòng quét:
trả về `std::unique_ptr<lidarlib::Lidar>` 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` - **`LaserScan`** — cùng field/đơn vị với ROS: `angle_min/max/increment` (rad,
(dùng thẳng `SickDriver` qua API C++). đã 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.

15
examples/example.cpp
View File

@@ -1,19 +1,15 @@
// example.cpp — quick try-out of the OLEI LiDAR driver // OLEI driver example.
#include "lidarlib/lidar.hpp" #include "lidarlib/lidar.hpp"
#include <cstdio> #include <cstdio>
int main() { int main() {
// ── pick a model ──────────────────────────────────────────────────────── lidarlib::Driver drv(lidarlib::MODEL_VB);
// lidarlib::Driver drv(lidarlib::MODEL_VF); // 2D 360°
// lidarlib::Driver drv(lidarlib::MODEL_LR1F); // 2D 360°, 50m
lidarlib::Driver drv(lidarlib::MODEL_VB); // 2D 270°
if (!drv.open()) { if (!drv.open()) {
fprintf(stderr, "Không mở được socket\n"); fprintf(stderr, "Không mở được socket\n");
return 1; return 1;
} }
// ── option 1: blocking recv ─────────────────────────────────────────────
for (int i = 0; i < 10; ++i) { for (int i = 0; i < 10; ++i) {
lidarlib::ScanResult result; lidarlib::ScanResult result;
if (!drv.recv_scan(result, 2000)) { if (!drv.recv_scan(result, 2000)) {
@@ -26,7 +22,6 @@ int main() {
i, scan.ranges.size(), scan.timestamp_ms, info.error_status, i, scan.ranges.size(), scan.timestamp_ms, info.error_status,
info.detected_model.c_str()); info.detected_model.c_str());
// Print the first few points
for (size_t j = 0; j < 20 && j < scan.ranges.size(); ++j) { 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; float angle_deg = (scan.angle_min + j * scan.angle_increment) * 180.f / 3.14159265f;
printf(" [%zu] angle=%.2f° dist=%.3fm intensity=%.0f\n", 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(); drv.close();
return 0; return 0;
} }

24
examples/lidar_app.cpp
View File

@@ -1,14 +1,4 @@
// lidar_app.cpp — headless skeleton app and integration template. // Headless skeleton app: loads config.json, one reader thread per lidar.
//
// Loads the lidar list from config.json, opens each one through the SINGLE
// config function lidarlib::make_lidar() (no per-brand branching), then reads scans
// on one thread per lidar and prints a one-line summary. There is no web UI:
// edit config.json directly, or build your own GUI on top of this same API.
//
// What a GUI author keeps: load_config() + make_lidar() + the recv_scan() loop.
// What a GUI author replaces: the printf() with their own rendering/persistence,
// and save_config() to write edits back.
//
// ./lidar_app [config.json] // ./lidar_app [config.json]
#include "lidarlib/lidarlib.hpp" #include "lidarlib/lidarlib.hpp"
#include <atomic> #include <atomic>
@@ -23,10 +13,8 @@ namespace {
std::atomic<bool> g_running{true}; std::atomic<bool> g_running{true};
void on_signal(int) { g_running = false; } 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) { void run_lidar(lidarlib::LidarConfig cfg) {
std::unique_ptr<lidarlib::Lidar> lidar = lidarlib::make_lidar(cfg); // the one config call std::unique_ptr<lidarlib::Lidar> lidar = lidarlib::make_lidar(cfg);
if (!lidar->open()) { if (!lidar->open()) {
fprintf(stderr, "[%s] khong mo duoc %s %s:%u\n", fprintf(stderr, "[%s] khong mo duoc %s %s:%u\n",
cfg.name.c_str(), cfg.brand.c_str(), cfg.ip.c_str(), cfg.port); 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) { while (g_running) {
lidarlib::ScanResult result; 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; const lidarlib::LaserScan& scan = result.scan;
// Output #2: ExtraInfo (fields vary by model/family)
const lidarlib::ExtraInfo& info = result.info; const lidarlib::ExtraInfo& info = result.info;
printf("[%s] %zu diem | ts=%u ms | model=%s | err=0x%02X\n", printf("[%s] %zu diem | ts=%u ms | model=%s | err=0x%02X\n",
@@ -57,12 +43,12 @@ void run_lidar(lidarlib::LidarConfig cfg) {
} // namespace } // namespace
int main(int argc, char** argv) { 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"; const std::string config_path = (argc > 1) ? argv[1] : "config.json";
lidarlib::Config cfg = lidarlib::load_config(config_path); 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()) { if (cfg.lidars.empty()) {
fprintf(stderr, "Khong co lidar nao trong %s\n", config_path.c_str()); fprintf(stderr, "Khong co lidar nao trong %s\n", config_path.c_str());

37
examples/nanoscan_example.cpp Normal file
View File

@@ -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 <cstdio>
int main() {
lidarlib::NanoScanDriver drv(lidarlib::MODEL_SICK_NANOSCAN3, "0.0.0.0", 6060);
if (!drv.open()) {
fprintf(stderr, "Không mở được UDP socket cho nanoScan3\n");
return 1;
}
for (int i = 0; i < 10; ++i) {
lidarlib::ScanResult result;
if (!drv.recv_scan(result, 2000)) {
fprintf(stderr, "Timeout hoặc lỗi nhận UDP datagram\n");
break;
}
const lidarlib::LaserScan& scan = result.scan;
const lidarlib::ExtraInfo& info = result.info;
printf("Scan #%d: %zu điểm, ts=%u, model=%s\n",
i, scan.ranges.size(), scan.timestamp_ms, info.detected_model.c_str());
for (size_t j = 0; j < 20 && j < scan.ranges.size(); ++j) {
float angle_deg = (scan.angle_min + j * scan.angle_increment) * 180.f / 3.14159265f;
printf(" [%zu] angle=%.2f° dist=%.3fm intensity=%.0f\n",
j, angle_deg, scan.ranges[j], scan.intensities[j]);
}
}
drv.close();
return 0;
}
// Build:
// g++ -std=c++17 -O2 -Iinclude -o nanoscan_example examples/nanoscan_example.cpp src/sick_lidar.cpp

5
examples/sick_example.cpp
View File

@@ -1,7 +1,4 @@
// sick_example.cpp — quick try-out of the SICK TiM driver (SOPAS/CoLa-A, TCP) // SICK TiM driver example (SOPAS/CoLa-A, TCP).
//
// Verified against a real SICK TiM781S — see the caveat in
// include/lidarlib/sick_lidar.hpp for exactly what was (and wasn't) confirmed.
#include "lidarlib/sick_lidar.hpp" #include "lidarlib/sick_lidar.hpp"
#include <cstdio> #include <cstdio>

13
examples/test_dual.cpp
View File

@@ -1,6 +1,4 @@
// test_dual.cpp — test 2 Olei lidars (front + rear) concurrently, per appsettings.json // Test 2 Olei lidars (front + rear) concurrently.
// Olei-front: scan_1, DeviceIp 192.168.100.11, LocalIp 192.168.100.100, DevicePort 2368
// Olei-rear : scan_2, DeviceIp 192.168.100.12, LocalIp 192.168.100.100, DevicePort 2369
#include "lidarlib/lidar.hpp" #include "lidarlib/lidar.hpp"
#include <cstdio> #include <cstdio>
#include <thread> #include <thread>
@@ -36,14 +34,7 @@ static void run_lidar(const char* tag, const lidarlib::ModelConfig& cfg,
} }
int main() { int main() {
// Both front and rear are Family B in practice — front's real header // Real headers (UDP sniff): front = "OLELR-1BS2", rear = "OLELR-1BS5".
// string is "OLELR-1BS2", rear's is "OLELR-1BS5" (verified via live UDP
// sniff), NOT the VB (Family A) model the config name suggested. With
// MODEL_AUTO, the driver reads the real model name from the header and
// narrows the FOV when it matches a known entry in kModelTable
// (olei_lidar.cpp); "1BS5" matches (→ full 360°), but "1BS2" doesn't, so
// front currently stays at the unfiltered 360° default. Call
// drv.detected_model() to see which name was actually read.
std::thread t_front(run_lidar, "front/scan_1", lidarlib::MODEL_AUTO, std::thread t_front(run_lidar, "front/scan_1", lidarlib::MODEL_AUTO,
"192.168.100.100", 2368, false, 5); "192.168.100.100", 2368, false, 5);
std::thread t_rear(run_lidar, "rear/scan_2", lidarlib::MODEL_AUTO, std::thread t_rear(run_lidar, "rear/scan_2", lidarlib::MODEL_AUTO,

58
include/lidarlib/config.hpp
View File

@@ -6,27 +6,29 @@
namespace lidarlib { namespace lidarlib {
// ─── Settings for one lidar ───────────────────────────────────────────────── // Settings for one lidar. `name` is the unique key across saves.
// `name` is the unique key used to match entries across saves (rename = old
// key removed, new key added) — useful if your GUI reconciles a running set of
// lidars against an edited config list.
struct LidarConfig { struct LidarConfig {
std::string name = "lidar"; std::string name = "lidar";
std::string ip = "0.0.0.0"; std::string ip = "0.0.0.0";
uint16_t port = 2368; uint16_t port = 2368;
std::string model = "AUTO"; // must match an entry in model_names_for_brand(brand) std::string model = "AUTO";
bool inverted = false; // only applies to brand "OLEI" — SickDriver has no equivalent bool inverted = false; // OLEI only
std::string brand = "OLEI"; // must match an entry in brand_names() — "OLEI" or "SICK" 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) { friend bool operator==(const LidarConfig& a, const LidarConfig& b) {
return a.name == b.name && a.ip == b.ip && a.port == b.port && 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); } 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 { struct Config {
std::vector<LidarConfig> lidars = { std::vector<LidarConfig> lidars = {
{"front", "0.0.0.0", 2368, "AUTO", false}, {"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). // nullptr if `name` doesn't match any known model.
// Returns nullptr if name doesn't match any entry.
const ModelConfig* model_by_name(const std::string& name); const ModelConfig* model_by_name(const std::string& name);
// Names accepted by model_by_name(), for populating a UI dropdown.
const std::vector<std::string>& model_names(); const std::vector<std::string>& model_names();
// Brand names accepted in LidarConfig::brand ("OLEI", "SICK"), for populating
// a UI dropdown.
const std::vector<std::string>& brand_names(); const std::vector<std::string>& brand_names();
// Subset of model_names() valid for a given brand (e.g. "SICK" -> the // Subset of model_names() valid for `brand`; empty if unknown.
// MODEL_SICK_* names) — empty if `brand` doesn't match any entry in
// brand_names(). Used to filter the model dropdown once a brand is picked,
// and to validate that LidarConfig::model actually belongs to its brand.
const std::vector<std::string>& model_names_for_brand(const std::string& brand); const std::vector<std::string>& model_names_for_brand(const std::string& brand);
// Load config.json at `path`. If the file doesn't exist, returns defaults // Returns defaults if the file doesn't exist (without creating it).
// (and does NOT create the file — caller decides whether to save it).
Config load_config(const std::string& path); Config load_config(const std::string& path);
// Overwrite `path` with `cfg` serialized as JSON.
void save_config(const std::string& path, const Config& cfg); void save_config(const std::string& path, const Config& cfg);
// ─── THE single config function ───────────────────────────────────────────── // Build a ready-to-open lidar from one LidarConfig — the only entry point an
// Build a ready-to-open lidar from one LidarConfig. This is the one entry point // app needs. brand "SICK" → SICK driver (model "SICK-nanoScan3" → UDP
// a GUI/app needs: `brand` selects the transport — exactly "SICK" → TCP, any // NanoScanDriver, others → TCP SickDriver); anything else → OLEI UDP.
// other value (incl. "OLEI", empty, or an old config without the field) → OLEI // Unknown/cross-brand model falls back to the brand default. Never nullptr.
// UDP. `model` is resolved *for that brand*: a name that is unknown OR belongs
// to the other brand falls back to the brand's sensible default (MODEL_AUTO for
// OLEI, MODEL_SICK_TIM571 for SICK), so a mis-paired brand+model can't silently
// configure the wrong driver. `inverted` applies to OLEI only. Returns a unique
// handle to the unified Lidar interface — call ->open() then
// ->recv_scan(out, timeout_ms) to get ScanResult { LaserScan scan; ExtraInfo
// info; }. Never returns nullptr.
//
// lidarlib::LidarConfig c{"front", "192.168.1.10", 2368, "AUTO", false, "OLEI"};
// auto lidar = lidarlib::make_lidar(c);
// lidar->open();
// lidarlib::ScanResult r;
// lidar->recv_scan(r, 1000); // r.scan = LaserScan, r.info = ExtraInfo
std::unique_ptr<Lidar> make_lidar(const LidarConfig& cfg); std::unique_ptr<Lidar> make_lidar(const LidarConfig& cfg);
} // namespace lidarlib } // namespace lidarlib

163
include/lidarlib/lidar.hpp
View File

@@ -7,45 +7,31 @@
namespace lidarlib { namespace lidarlib {
// ─── Default output: ROS sensor_msgs/LaserScan-shaped ────────────────────── // ROS sensor_msgs/LaserScan-shaped output (radians, meters, seconds).
// Same field names/semantics as ROS's LaserScan message (radians, meters, // ranges[i] is at angle_min + i*angle_increment, in sweep order.
// seconds) so this can be bridged into a ROS node with a near-1:1 field copy.
// ranges[i]/intensities[i] correspond to angle = angle_min + i*angle_increment;
// the array spans exactly one revolution (or the model's FOV window) in the
// order the device actually swept it — angle_min/angle_max are NOT clamped to
// [-pi,pi], they just describe whatever contiguous window this revolution
// covered (matches how continuously-rotating lidars without a phase reset
// behave: the starting angle drifts slightly scan to scan).
struct LaserScan { struct LaserScan {
uint32_t timestamp_ms = 0; // device clock (ms since power-on); 0 if the uint32_t timestamp_ms = 0; // device clock (ms); 0 if not on the wire
// family doesn't expose one (see ExtraInfo)
float angle_min = 0.f; // rad float angle_min = 0.f; // rad
float angle_max = 0.f; // rad float angle_max = 0.f; // rad
float angle_increment = 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 time_increment = 0.f; // sec — not exposed by devices, always 0
float scan_time = 0.f; // sec — device doesn't expose per-scan timing, always 0 float scan_time = 0.f; // sec — not exposed by devices, always 0
float range_min = 0.f; // m — from ModelConfig, NOT measured per-scan float range_min = 0.f; // m — from ModelConfig, not measured
float range_max = 0.f; // m — from ModelConfig, NOT measured per-scan float range_max = 0.f; // m — from ModelConfig, not measured
std::vector<float> ranges; // m std::vector<float> ranges; // m
std::vector<float> intensities; // 0-255 read back as float, like ROS does std::vector<float> intensities; // 0-255 as float
}; };
// ─── Extra info: whatever diagnostic/header fields THIS family/model exposes ─ // Diagnostic/header fields; fields the family doesn't carry stay std::nullopt.
// Fields the protocol family doesn't carry stay unset (std::nullopt). Several
// of these are raw, undecoded passthroughs of header bytes whose exact
// meaning hasn't been verified against real hardware/datasheet — see comments
// in olei_lidar.cpp next to where each is read.
struct ExtraInfo { struct ExtraInfo {
std::string detected_model = "AUTO"; // real model name read from the packet, or "AUTO" std::string detected_model = "AUTO";
uint8_t error_status = 0; // Family A only; BIT0=Monitor, BIT1=Voltage, BIT2=Temp uint8_t error_status = 0; // Family A: BIT0=Monitor, BIT1=Voltage, BIT2=Temp
uint8_t distance_scale_mm = 0; // mm/count used to decode ranges this scan (0 = not reported) uint8_t distance_scale_mm = 0; // 0 = not reported
// Family A (0xFAF0) only — raw 16-bit "rotation info" header field, // Family A only
// meaning not decoded/verified.
std::optional<uint16_t> rotation_raw; std::optional<uint16_t> rotation_raw;
// Family C / protocol V3 (0xFEAC, GS1-5) only — ported from the C# driver // Family C / V3 (GS1-5) only — raw passthroughs, unverified
// header layout, NOT cross-checked against real GS1-5 hardware.
std::optional<uint8_t> distance_ratio_raw; std::optional<uint8_t> distance_ratio_raw;
std::optional<uint16_t> scan_frequency_raw; std::optional<uint16_t> scan_frequency_raw;
std::optional<uint16_t> input_status; std::optional<uint16_t> input_status;
@@ -54,144 +40,90 @@ struct ExtraInfo {
std::optional<uint32_t> status_flags; std::optional<uint32_t> status_flags;
}; };
// One complete revolution, in both forms at once.
struct ScanResult { struct ScanResult {
LaserScan scan; LaserScan scan;
ExtraInfo info; ExtraInfo info;
}; };
// ─── Per-model configuration ─────────────────────────────────────────────── // Per-model configuration. scan_angle_* use the signed system [-180,180]:
// scan_angle_* use the SIGNED system [-180,180]: 0 = straight ahead, + = left, - = right. // 0 = ahead, + = left, - = right. range_min/max are datasheet placeholders.
// 360° lidars keep the full circle [-180,180]; narrow-FOV lidars (VB 270°) shrink it.
// range_min_m/range_max_m are sensor-spec placeholders (NOT read from any
// packet) used to fill LaserScan::range_min/range_max — adjust to the real
// datasheet values for each model if precision matters to your consumer.
struct ModelConfig { struct ModelConfig {
const char* name; const char* name;
float scan_angle_min; // deg — VB/LR-16F: -135, 360° models: -180 float scan_angle_min; // deg
float scan_angle_max; // deg — VB/LR-16F: 135, 360° models: 180 float scan_angle_max; // deg
float range_min_m = 0.05f; float range_min_m = 0.05f;
float range_max_m = 30.f; 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_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_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_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 }; // 2D 360°, 0.01°/LSB ~2400 pts/rev (Family B) 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_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_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° 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 unknown ahead of time: Family B/C packets carry enough to auto-detect;
// model name string in its header (e.g. "OLELR-1BS5", verified via live UDP // Family A doesn't, so the wide default FOV is kept.
// sniff) → the driver auto-detects it and narrows the FOV per the table
// above. Family C (0xFEAC, GS1-5) is identified by magic alone. Family A has
// no such string, so on a Family-A device MODEL_AUTO keeps the wide default
// FOV (-180..180, no points dropped) until the user specifies a concrete model.
inline constexpr ModelConfig MODEL_AUTO { "AUTO", -180.f, 180.f, 0.05f, 30.f }; 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<void(const ScanResult&)>; using ScanCallback = std::function<void(const ScanResult&)>;
// ─── Unified driver interface ─────────────────────────────────────────────── // Unified driver interface returned by make_lidar(); OLEI and SICK drivers
// Common handle returned by lidarlib::make_lidar() (the single config function in // both derive from it.
// config.hpp). Both the OLEI Driver (UDP) and the SICK SickDriver (TCP) derive
// from this, so a GUI/app can drive any supported lidar through one type and
// never branch on brand. Every call yields the same ScanResult { LaserScan
// scan; ExtraInfo info; } — output #1 (ROS-shaped LaserScan, identical across
// all models) and output #2 (ExtraInfo, model-specific extra fields).
class Lidar { class Lidar {
public: public:
virtual ~Lidar() = default; virtual ~Lidar() = default;
// Open the transport (UDP socket / TCP connection) and start receiving.
virtual bool open() = 0; virtual bool open() = 0;
// Close the transport.
virtual void close() = 0; virtual void close() = 0;
// Block until one full scan is received; false on error/timeout. // Block until one full scan; false on error/timeout. timeout_ms = 0 → block
// timeout_ms = 0 → block indefinitely. (No default here on purpose: the // indefinitely. No default on purpose: drivers differ (OLEI 1000, SICK 2000).
// concrete drivers differ — OLEI 1000 ms, SICK 2000 ms — so callers using
// the interface must state the timeout they want.)
virtual bool recv_scan(ScanResult& out, int timeout_ms) = 0; 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; virtual void set_scan_callback(ScanCallback cb) = 0;
// Receive + dispatch the callback once (non-owning loop step).
virtual bool spin_once() = 0; 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; virtual const char* detected_model() const = 0;
}; };
// ─── Driver ───────────────────────────────────────────────────────────────── // OLEI UDP driver.
class Driver : public Lidar { class Driver : public Lidar {
public: public:
// callback invoked whenever a complete scan is ready
using ScanCallback = lidarlib::ScanCallback; using ScanCallback = lidarlib::ScanCallback;
// ip : receiving host's bind address, usually "0.0.0.0" // ip: local bind address; port: UDP port the lidar sends to;
// port : UDP port the lidar sends to (default 2368) // inverted: unit mounted upside-down → mirror every angle.
// cfg : model config
// inverted : set true if this physical unit is mounted upside-down
// (flipped 180° about its forward-facing axis). Mirrors every
// point's angle (angle = -angle) so output stays in the
// vehicle's frame regardless of mounting orientation — useful
// when e.g. front is mounted normally but rear is flipped.
explicit Driver(const ModelConfig& cfg, explicit Driver(const ModelConfig& cfg,
const std::string& ip = "0.0.0.0", const std::string& ip = "0.0.0.0",
uint16_t port = 2368, uint16_t port = 2368,
bool inverted = false); bool inverted = false);
~Driver(); ~Driver();
// Non-copyable
Driver(const Driver&) = delete; Driver(const Driver&) = delete;
Driver& operator=(const Driver&) = delete; Driver& operator=(const Driver&) = delete;
// Open the socket and start receiving
bool open() override; bool open() override;
// Close the socket
void close() override; 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; 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); } void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
// Receive + dispatch callback (call from your own loop)
bool spin_once() override; bool spin_once() override;
// The REAL model name read from the Family B/C header (only meaningful // Model name read from the Family B/C header; "AUTO" until one is seen.
// when the Driver was constructed with MODEL_AUTO). Always the actual
// string found in the packet (e.g. "OLELR-1BS2"), even when that model
// has no specific FOV entry in the table (FOV then stays at the 360°
// default). Returns "AUTO" if no Family B/C packet has been seen yet.
// Mirrored per-scan in ScanResult::info::detected_model.
const char* detected_model() const override { return detected_model_name_.c_str(); } const char* detected_model() const override { return detected_model_name_.c_str(); }
private: private:
// ── parse Family A packet (ID=0xFAF0): 20B header, 3B block ── bool parse_family_a(const uint8_t* buf, int len); // ID=0xFAF0
bool parse_family_a(const uint8_t* buf, int len); 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); 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(); void flush_scan();
ModelConfig cfg_; ModelConfig cfg_;
@@ -201,27 +133,22 @@ private:
int sock_fd_ = -1; int sock_fd_ = -1;
ScanCallback cb_; ScanCallback cb_;
// Per-revolution accumulation buffers (parallel arrays, index-aligned) // Per-revolution accumulation buffers (index-aligned)
std::vector<float> pending_angle_deg_; // unwrapped, continuous std::vector<float> pending_angle_deg_;
std::vector<float> pending_dist_m_; std::vector<float> pending_dist_m_;
std::vector<uint8_t> pending_intensity_; std::vector<uint8_t> pending_intensity_;
uint32_t pending_ts_ = 0; uint32_t pending_ts_ = 0;
uint8_t pending_err_ = 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_; ExtraInfo pending_info_;
// recv_scan()'s output, gated by a simple ready flag
ScanResult ready_result_; ScanResult ready_result_;
bool scan_ready_ = false; bool scan_ready_ = false;
// Per-instance receive buffer — NOT static, so that 2 lidars running on 2 // Per-instance so two drivers on two threads don't race.
// threads don't overwrite each other's data (data race).
uint8_t recv_buf_[4096]; uint8_t recv_buf_[4096];
// Model auto-detection from the Family B/C header (see MODEL_AUTO)
bool auto_detect_ = false; bool auto_detect_ = false;
bool model_locked_ = false; bool model_locked_ = false;
std::string detected_model_name_ = "AUTO"; std::string detected_model_name_ = "AUTO";

10
include/lidarlib/lidarlib.hpp
View File

@@ -1,13 +1,5 @@
#pragma once #pragma once
// ─── One-include convenience header ───────────────────────────────────────── // One-include convenience header for the whole public API.
// Pull in the whole public API in a single line:
//
// #include "lidarlib/lidarlib.hpp"
//
// Gives you the data model (LaserScan / ExtraInfo / ScanResult), the unified
// Lidar interface, both concrete drivers (Driver = OLEI/UDP, SickDriver =
// SICK/TCP), the model/brand tables, config.json load/save, and the single
// config function lidarlib::make_lidar().
#include "lidarlib/lidar.hpp" #include "lidarlib/lidar.hpp"
#include "lidarlib/sick_lidar.hpp" #include "lidarlib/sick_lidar.hpp"
#include "lidarlib/config.hpp" #include "lidarlib/config.hpp"

109
include/lidarlib/sick_lidar.hpp
View File

@@ -1,45 +1,25 @@
#pragma once #pragma once
#include "lidarlib/lidar.hpp" #include "lidarlib/lidar.hpp"
#include <cstdint>
#include <string> #include <string>
#include <vector>
namespace lidarlib { namespace lidarlib {
// ─── SICK TiM5xx/7xx model presets ────────────────────────────────────────── // SICK TiM presets. FOV/range from datasheets; scan_angle_* are informational
// FOV/range taken from SICK's public datasheets (NOT read from any packet — // only and do NOT filter points. angle_offset_deg = -90 because the TiM wire
// same placeholder convention as the OLEI ModelConfig constants in lidar.hpp). // frame puts 90° at the device front.
// Scanning angle is 270° on every TiM5xx/7xx variant; only the rated range inline constexpr ModelConfig MODEL_SICK_TIM5XX { "SICK-TIM5xx", -135.f, 135.f, 0.05f, 10.f, -90.f }; // TiM551/561, 270°, 10m
// differs by model. 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_TIM5XX { "SICK-TIM5xx", -135.f, 135.f, 0.05f, 10.f }; // TiM551/561, 270°, 10m inline constexpr ModelConfig MODEL_SICK_TIM7XX { "SICK-TIM7xx", -135.f, 135.f, 0.05f, 25.f, -90.f }; // TiM781, 270°, 25m
inline constexpr ModelConfig MODEL_SICK_TIM571 { "SICK-TIM571", -135.f, 135.f, 0.05f, 25.f }; // TiM571, 270°, 25m
inline constexpr ModelConfig MODEL_SICK_TIM7XX { "SICK-TIM7xx", -135.f, 135.f, 0.05f, 25.f }; // TiM781, 270°, 25m
// ─── SickDriver — SICK TiM5xx/7xx over SOPAS/CoLa-A (TCP, default port 2111) ─ // SICK TiM5xx/7xx over SOPAS/CoLa-A (TCP, default port 2111).
// // Verified against a real TiM781S (FW V5.11). NOT verified: NumEncoders > 0,
// VERIFIED against a real SICK TiM781S (FW V5.11-14.10.24, MODEL_SICK_TIM7XX) // the 8-bit channel branch, and the TIM5xx/TIM571 FOV/range numbers.
// on port 2111: 811 pts/scan over -45..225° (270° FOV), angle_increment =
// 0.333° (matches the 781's rated angular resolution), stable ranges, and a
// non-zero RSSI1 channel — confirms the DIST1/RSSI1 channel layout, the
// IEEE-754 scaling factor, and the start-angle/step-width decode in
// parse_lmdscandata() (sick_lidar.cpp) are correct on real hardware, not just
// per SICK's "Telegram Listing" doc.
// NOT yet verified: the angle-zero reference vs physical mounting direction,
// devices reporting NumEncoders > 0, the 8-bit-channel branch (the test unit
// only emitted 16-bit channels), and MODEL_SICK_TIM5XX/MODEL_SICK_TIM571's
// FOV/range numbers (only TiM7xx was tested).
//
// Protocol differences from lidarlib::Driver that justify a separate class
// instead of extending Driver:
// - Transport is TCP (a connection, request/response + streamed telegrams),
// not connectionless UDP broadcast.
// - Telegrams are ASCII (CoLa-A), framed by STX(0x02)/ETX(0x03) instead of
// the OLEI binary header+block layout — no CRC32 like Family A.
// - The device is passive until told to start: must send "sEN LMDscandata 1"
// before any scan telegram arrives.
class SickDriver : public Lidar { class SickDriver : public Lidar {
public: 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, explicit SickDriver(const ModelConfig& cfg,
const std::string& ip, const std::string& ip,
uint16_t port = 2111); uint16_t port = 2111);
@@ -50,30 +30,17 @@ public:
// Connect + send "sEN LMDscandata 1" to start continuous scan output. // Connect + send "sEN LMDscandata 1" to start continuous scan output.
bool open() override; bool open() override;
// Best-effort "sEN LMDscandata 0" then close the socket.
void close() override; 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; 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); } void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
// Receive + parse + dispatch callback (call from your own loop)
bool spin_once() override; bool spin_once() override;
// SICK telegrams carry no model string — returns the configured model name // No model string on the wire — returns the configured name.
// (e.g. "SICK-TIM7xx") so the unified Lidar interface stays consistent.
// Backed by an owned std::string (not cfg_.name, a borrowed const char*) so
// the pointer stays valid even if the ModelConfig was built from temporary
// storage — matching Driver::detected_model()'s ownership.
const char* detected_model() const override { return detected_model_name_.c_str(); } const char* detected_model() const override { return detected_model_name_.c_str(); }
private: private:
bool send_telegram(const std::string& body); // wraps body with STX/ETX, writes to socket bool send_telegram(const std::string& body);
bool read_telegram(std::string& out, int timeout_ms); // returns next STX..ETX frame, STX/ETX stripped bool read_telegram(std::string& out, int timeout_ms);
bool parse_lmdscandata(const std::string& telegram, ScanResult& out); bool parse_lmdscandata(const std::string& telegram, ScanResult& out);
ModelConfig cfg_; ModelConfig cfg_;
@@ -83,9 +50,51 @@ private:
int sock_fd_ = -1; int sock_fd_ = -1;
ScanCallback cb_; ScanCallback cb_;
// Accumulates bytes read from the TCP stream between telegram boundaries — // Leftover TCP bytes carried across telegram boundaries; per-instance.
// per-instance (not static) so 2 SickDrivers on 2 threads don't race.
std::string recv_buf_; std::string recv_buf_;
}; };
inline constexpr ModelConfig MODEL_SICK_NANOSCAN3 { "SICK-nanoScan3", -137.5f, 137.5f, 0.05f, 40.f };
// SICK nanoScan3 / microScan3 safety-scanner binary UDP output. Layout ported
// from SICK's open-source sick_safetyscanners; NOT verified on real hardware.
// Passive UDP receiver: the sensor's UDP target must be configured up front in
// SICK Safety Designer — this class does no CoLa2/TCP handshake.
class NanoScanDriver : public Lidar {
public:
using ScanCallback = lidarlib::ScanCallback;
// ip: local bind address; port: local UDP port the sensor sends to.
explicit NanoScanDriver(const ModelConfig& cfg,
const std::string& ip = "0.0.0.0",
uint16_t port = 6060);
~NanoScanDriver();
NanoScanDriver(const NanoScanDriver&) = delete;
NanoScanDriver& operator=(const NanoScanDriver&) = delete;
bool open() override;
void close() override;
bool recv_scan(ScanResult& out, int timeout_ms = 1000) override;
void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
bool spin_once() override;
// No model string on the wire — returns the configured name.
const char* detected_model() const override { return detected_model_name_.c_str(); }
private:
int recv_datagram(int timeout_ms);
bool parse_packet(const uint8_t* buf, int len, ScanResult& out);
ModelConfig cfg_;
std::string detected_model_name_; // owned copy of cfg_.name (stable lifetime)
std::string ip_;
uint16_t port_;
int sock_fd_ = -1;
ScanCallback cb_;
// Per-instance; sized for a full safety-data packet (max ~2751 beams).
std::vector<uint8_t> recv_buf_;
};
} // namespace lidarlib } // namespace lidarlib

5
src/json_mini.hpp
View File

@@ -1,6 +1,4 @@
// json_mini.hpp — minimal header-only JSON parse/serialize, just enough for // Minimal header-only JSON parse/serialize, just enough for config objects.
// flat-ish config objects (no comments, no streaming, no error recovery).
// Not a general-purpose JSON library — kept tiny on purpose.
#pragma once #pragma once
#include <string> #include <string>
#include <vector> #include <vector>
@@ -104,7 +102,6 @@ private:
} }
}; };
// ── Parser ──────────────────────────────────────────────────────────────
class ParseError : public std::runtime_error { class ParseError : public std::runtime_error {
public: public:
explicit ParseError(const std::string& what) : std::runtime_error(what) {} explicit ParseError(const std::string& what) : std::runtime_error(what) {}

42
src/lidar_bytes.hpp Normal file
View File

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

37
src/olei_config.cpp → src/lidar_config.cpp
View File

@@ -25,6 +25,7 @@ constexpr ModelEntry kModels[] = {
{ "SICK-TIM5xx", &MODEL_SICK_TIM5XX, "SICK" }, { "SICK-TIM5xx", &MODEL_SICK_TIM5XX, "SICK" },
{ "SICK-TIM571", &MODEL_SICK_TIM571, "SICK" }, { "SICK-TIM571", &MODEL_SICK_TIM571, "SICK" },
{ "SICK-TIM7xx", &MODEL_SICK_TIM7XX, "SICK" }, { "SICK-TIM7xx", &MODEL_SICK_TIM7XX, "SICK" },
{ "SICK-nanoScan3", &MODEL_SICK_NANOSCAN3, "SICK" },
}; };
json::Value to_json(const LidarConfig& c) { 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("brand", json::Value::make_string(c.brand));
v.set("model", json::Value::make_string(c.model)); v.set("model", json::Value::make_string(c.model));
v.set("inverted", json::Value::make_bool(c.inverted)); 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; 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.brand = v.get_string("brand", def.brand);
c.model = v.get_string("model", def.model); c.model = v.get_string("model", def.model);
c.inverted = v.get_bool("inverted", def.inverted); c.inverted = v.get_bool("inverted", def.inverted);
c.angle_min_deg = static_cast<float>(v.get_number("angle_min_deg", def.angle_min_deg));
c.angle_max_deg = static_cast<float>(v.get_number("angle_max_deg", def.angle_max_deg));
return c; return c;
} }
@@ -58,10 +63,6 @@ const ModelConfig* model_by_name(const std::string& name) {
} }
namespace { 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) { const ModelConfig* model_by_name_for_brand(const std::string& name, const std::string& brand) {
for (const auto& e : kModels) for (const auto& e : kModels)
if (name == e.name && brand == e.brand) return e.cfg; if (name == e.name && brand == e.brand) return e.cfg;
@@ -101,7 +102,7 @@ const std::vector<std::string>& model_names_for_brand(const std::string& brand)
} }
Config load_config(const std::string& path) { Config load_config(const std::string& path) {
Config cfg; // defaults Config cfg;
std::ifstream f(path); std::ifstream f(path);
if (!f) return cfg; if (!f) return cfg;
@@ -111,7 +112,7 @@ Config load_config(const std::string& path) {
try { try {
root = json::parse(ss.str()); root = json::parse(ss.str());
} catch (const json::ParseError&) { } 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"); const json::Value* lidars = root.find("lidars");
@@ -136,21 +137,25 @@ void save_config(const std::string& path, const Config& cfg) {
} }
std::unique_ptr<Lidar> make_lidar(const LidarConfig& cfg) { std::unique_ptr<Lidar> make_lidar(const LidarConfig& cfg) {
// Anything other than the exact string "SICK" is treated as OLEI (this also // Anything but the exact string "SICK" is OLEI (keeps brand-less configs working).
// keeps old config.json files without a `brand` field working).
const bool is_sick = (cfg.brand == "SICK"); 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"); 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 (!model) model = is_sick ? &MODEL_SICK_TIM571 : &MODEL_AUTO;
if (is_sick) ModelConfig mc = *model;
return std::make_unique<SickDriver>(*model, cfg.ip, cfg.port); if (cfg.angle_min_deg > -360.f || cfg.angle_max_deg < 360.f) {
return std::make_unique<Driver>(*model, cfg.ip, cfg.port, cfg.inverted); mc.remap_angles = true;
mc.out_angle_min = cfg.angle_min_deg;
mc.out_angle_max = cfg.angle_max_deg;
}
if (is_sick) {
if (model == &MODEL_SICK_NANOSCAN3)
return std::make_unique<NanoScanDriver>(mc, cfg.ip, cfg.port);
return std::make_unique<SickDriver>(mc, cfg.ip, cfg.port);
}
return std::make_unique<Driver>(mc, cfg.ip, cfg.port, cfg.inverted);
} }
} // namespace lidarlib } // namespace lidarlib

197
src/olei_lidar.cpp
View File

@@ -1,4 +1,5 @@
#include "lidarlib/lidar.hpp" #include "lidarlib/lidar.hpp"
#include "lidar_bytes.hpp"
#include <cstring> #include <cstring>
#include <cmath> #include <cmath>
@@ -10,40 +11,19 @@
namespace lidarlib { namespace lidarlib {
namespace { // Normalize into (-180, 180]: 0 = ahead, + = left, - = right.
constexpr float kDeg2Rad = 3.14159265358979323846f / 180.f;
}
// ── Little-endian helpers ────────────────────────────────────────────────────
static inline uint16_t le16(const uint8_t* p) {
return static_cast<uint16_t>(p[0]) | (static_cast<uint16_t>(p[1]) << 8);
}
static inline uint32_t le32(const uint8_t* p) {
return static_cast<uint32_t>(p[0])
| (static_cast<uint32_t>(p[1]) << 8)
| (static_cast<uint32_t>(p[2]) << 16)
| (static_cast<uint32_t>(p[3]) << 24);
}
// Normalize any angle into the SIGNED system (-180, 180]: 0 = straight ahead,
// + = left, - = right. This lets a model's FOV (e.g. VB -135…135) correctly
// filter lidars that report angles in 0–360 too.
static inline float to_signed_deg(float deg) { static inline float to_signed_deg(float deg) {
deg = std::fmod(deg, 360.f); deg = std::fmod(deg, 360.f);
if (deg < 0.f) deg += 360.f; // → [0,360) if (deg < 0.f) deg += 360.f;
if (deg > 180.f) deg -= 360.f; // → (-180,180] if (deg > 180.f) deg -= 360.f;
return deg; 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) { static inline float maybe_invert(float signed_deg, bool inverted) {
return inverted ? to_signed_deg(-signed_deg) : signed_deg; 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) { static uint32_t crc32_olei(const uint8_t* data, size_t len) {
uint32_t crc = 0xFFFFFFFF; uint32_t crc = 0xFFFFFFFF;
for (size_t i = 0; i < len; ++i) { 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; return crc;
} }
// ── Frame IDs ────────────────────────────────────────────────────────────────
static constexpr uint16_t FRAME_ID_A = 0xFAF0; // 2D Ethernet (VB, VF, LR-1F) 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_B = 0xFEF0; // LR-1BS5 / LR-1BS2 Ethernet variant
static constexpr uint16_t FRAME_ID_C = 0xFEAC; // Protocol V3 (GS1-5) 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) Driver::Driver(const ModelConfig& cfg, const std::string& ip, uint16_t port, bool inverted)
: cfg_(cfg), ip_(ip), port_(port), inverted_(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(); } Driver::~Driver() { close(); }
// ─── open() ─────────────────────────────────────────────────────────────────
bool Driver::open() { bool Driver::open() {
sock_fd_ = ::socket(AF_INET, SOCK_DGRAM, 0); sock_fd_ = ::socket(AF_INET, SOCK_DGRAM, 0);
if (sock_fd_ < 0) return false; 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; int reuse = 1;
::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)); ::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));
#ifdef SO_REUSEPORT #ifdef SO_REUSEPORT
@@ -98,7 +72,6 @@ bool Driver::open() {
return true; return true;
} }
// ─── close() ────────────────────────────────────────────────────────────────
void Driver::close() { void Driver::close() {
if (sock_fd_ >= 0) { if (sock_fd_ >= 0) {
::close(sock_fd_); ::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) { bool Driver::recv_scan(ScanResult& out, int timeout_ms) {
scan_ready_ = false; 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); fd_set fds; FD_ZERO(&fds); FD_SET(sock_fd_, &fds);
timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 }; timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 };
int r = ::select(sock_fd_ + 1, &fds, nullptr, nullptr, &tv); 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; if (!spin_once()) return false;
} }
@@ -123,10 +95,7 @@ bool Driver::recv_scan(ScanResult& out, int timeout_ms) {
return true; return true;
} }
// ─── spin_once() ────────────────────────────────────────────────────────────
bool Driver::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_; uint8_t* buf = recv_buf_;
sockaddr_in from{}; sockaddr_in from{};
socklen_t fromlen = sizeof(from); socklen_t fromlen = sizeof(from);
@@ -135,28 +104,19 @@ bool Driver::spin_once() {
reinterpret_cast<sockaddr*>(&from), &fromlen); reinterpret_cast<sockaddr*>(&from), &fromlen);
if (n < 0) return false; if (n < 0) return false;
// Distinguish protocol family by Frame ID (little-endian) // A/C carry the frame id at [0-1]; B has a 0x010F preamble, real id at [2-3].
// Family A / C: Frame ID / magic sits right at bytes [0-1] if (n < 4) return true;
// Family B: has a 0x010F preamble at bytes [0-1], real Frame ID at bytes [2-3] uint16_t id_at_0 = le16(buf);
if (n < 4) return true; // too short, skip uint16_t frame_id_b = le16(buf + 2);
uint16_t id_at_0 = le16(buf); // Family A (0xFAF0) or Family C (0xFEAC)
uint16_t frame_id_b = le16(buf + 2); // Family B: preamble 0x010F + real id at [2-3]
if (id_at_0 == FRAME_ID_A) parse_family_a(buf, static_cast<int>(n)); if (id_at_0 == FRAME_ID_A) parse_family_a(buf, static_cast<int>(n));
else if (id_at_0 == FRAME_ID_C) parse_family_c(buf, static_cast<int>(n)); else if (id_at_0 == FRAME_ID_C) parse_family_c(buf, static_cast<int>(n));
else if (frame_id_b == FRAME_ID_B) parse_family_b(buf, static_cast<int>(n)); else if (frame_id_b == FRAME_ID_B) parse_family_b(buf, static_cast<int>(n));
// else: unknown family (3D LR-16F uses a different format, extend later)
return true; return true;
} }
// ─── push_point() — append with angle-unwrapping ─────────────────────────── // Append with angle-unwrapping so the ±180° seam stays a continuous ramp.
// `signed_angle_deg` is already signed+inverted+FOV-filtered by the caller.
// Unwrapping against the previous point (rather than re-deriving from device
// raw angle) keeps this identical for all 3 families and survives the ±180°
// seam: a 360° device's points cross from +179.x to -179.x mid-revolution in
// the signed system, which push_point() turns back into a continuous ramp so
// LaserScan::angle_min/angle_max/ranges stay meaningful (monotonic, ROS-style).
void Driver::push_point(float signed_angle_deg, float dist_m, uint8_t intensity) { void Driver::push_point(float signed_angle_deg, float dist_m, uint8_t intensity) {
float angle = signed_angle_deg; float angle = signed_angle_deg;
if (!pending_angle_deg_.empty()) { 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); pending_intensity_.push_back(intensity);
} }
// ─── flush_scan() — a revolution is complete ───────────────────────────────
void Driver::flush_scan() { void Driver::flush_scan() {
if (pending_angle_deg_.empty()) return; if (pending_angle_deg_.empty()) return;
@@ -181,13 +140,16 @@ void Driver::flush_scan() {
scan.angle_max = pending_angle_deg_.back() * kDeg2Rad; scan.angle_max = pending_angle_deg_.back() * kDeg2Rad;
scan.angle_increment = (n > 1) scan.angle_increment = (n > 1)
? (scan.angle_max - scan.angle_min) / static_cast<float>(n - 1) : 0.f; ? (scan.angle_max - scan.angle_min) / static_cast<float>(n - 1) : 0.f;
scan.time_increment = 0.f; // device doesn't expose per-point timing scan.time_increment = 0.f;
scan.scan_time = 0.f; // device doesn't expose per-scan timing scan.scan_time = 0.f;
scan.range_min = cfg_.range_min_m; scan.range_min = cfg_.range_min_m;
scan.range_max = cfg_.range_max_m; scan.range_max = cfg_.range_max_m;
scan.ranges.assign(pending_dist_m_.begin(), pending_dist_m_.end()); scan.ranges.assign(pending_dist_m_.begin(), pending_dist_m_.end());
scan.intensities.assign(pending_intensity_.begin(), pending_intensity_.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; ExtraInfo& info = ready_result_.info;
info = pending_info_; info = pending_info_;
info.detected_model = detected_model_name_; info.detected_model = detected_model_name_;
@@ -196,63 +158,42 @@ void Driver::flush_scan() {
pending_angle_deg_.clear(); pending_angle_deg_.clear();
pending_dist_m_.clear(); pending_dist_m_.clear();
pending_intensity_.clear(); pending_intensity_.clear();
pending_info_ = ExtraInfo{}; // reset per-revolution optional fields pending_info_ = ExtraInfo{};
scan_ready_ = true; scan_ready_ = true;
if (cb_) cb_(ready_result_); if (cb_) cb_(ready_result_);
} }
// ─── parse_family_a() ─────────────────────────────────────────────────────── // Family A (0xFAF0): 20B header + 3B blocks (u16 dist, u8 intensity).
// 20-byte header:
// [0-1] Frame ID = 0xFAF0
// [2-3] Protocol = 0x0200
// [4] Distance scale (mm/count)
// [5] Error status
// [6] Start angle (deg, uint8)
// [7] End angle (deg, uint8, exclusive)
// [8-9] Num points (uint16 LE)
// [10-11] Rotation info — raw, undecoded (exposed as ExtraInfo::rotation_raw)
// [12-15] Timestamp (uint32 LE, ms)
// [16-19] CRC32 of the block data
// 3-byte block × N:
// [0-1] Distance readout (uint16 LE)
// [2] Intensity (uint8)
bool Driver::parse_family_a(const uint8_t* buf, int len) { bool Driver::parse_family_a(const uint8_t* buf, int len) {
static constexpr int HEADER_LEN = 20; static constexpr int HEADER_LEN = 20;
static constexpr int BLOCK_LEN = 3; static constexpr int BLOCK_LEN = 3;
if (len < HEADER_LEN) return false; 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 dist_scale = buf[4]; // mm per count
uint8_t err_status = buf[5]; uint8_t err_status = buf[5];
float ang_start = static_cast<float>(buf[6]); float ang_start = static_cast<float>(buf[6]);
// float ang_end = static_cast<float>(buf[7]); // exclusive
uint16_t num_pts = le16(buf + 8); uint16_t num_pts = le16(buf + 8);
uint16_t rotation_raw = le16(buf + 10); uint16_t rotation_raw = le16(buf + 10);
uint32_t timestamp = le32(buf + 12); uint32_t timestamp = le32(buf + 12);
uint32_t crc_packet = le32(buf + 16); uint32_t crc_packet = le32(buf + 16);
// ── verify CRC (optional but recommended) ──
int block_bytes = len - HEADER_LEN; 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<size_t>(num_pts * BLOCK_LEN)); uint32_t crc_calc = crc32_olei(buf + HEADER_LEN, static_cast<size_t>(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) { if (last_angle_ >= 0.f && ang_start < last_angle_ - 90.f) {
flush_scan(); flush_scan();
} }
// ── decode points ──
pending_ts_ = timestamp; pending_ts_ = timestamp;
pending_err_ = err_status; pending_err_ = err_status;
pending_info_.distance_scale_mm = dist_scale; pending_info_.distance_scale_mm = dist_scale;
pending_info_.rotation_raw = rotation_raw; 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<float>(dist_scale) : 1.f); const float scale_mm = (dist_scale ? static_cast<float>(dist_scale) : 1.f);
const float ang_end = static_cast<float>(buf[7]); const float ang_end = static_cast<float>(buf[7]);
@@ -261,36 +202,21 @@ bool Driver::parse_family_a(const uint8_t* buf, int len) {
uint16_t dist_raw = le16(blk); uint16_t dist_raw = le16(blk);
uint8_t intensity = blk[2]; uint8_t intensity = blk[2];
// Compute angle: linear interpolation within the packet's range (device-space)
float frac = (num_pts > 1) ? static_cast<float>(i) / (num_pts - 1) : 0.f; float frac = (num_pts > 1) ? static_cast<float>(i) / (num_pts - 1) : 0.f;
float angle = to_signed_deg(ang_start + frac * (ang_end - ang_start)); float angle = to_signed_deg(ang_start + frac * (ang_end - ang_start) + cfg_.angle_offset_deg);
angle = maybe_invert(angle, inverted_); 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; 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; last_angle_ = ang_start;
return true; return true;
} }
// ─── parse_family_b() ─────────────────────────────────────────────────────── // Family B (0xFEF0): 40B header (model string at [7-16]) + 8B blocks
// 40-byte header: // (u16 angle ×0.01°, u16 dist, u16 signal). No timestamp/error on the wire.
// [0-1] 0x010F
// [2-3] 0xFEF0 (Frame ID)
// [4-5] 0x0200 (Protocol)
// [6] Distance scale
// [7-16] Model identifier string (e.g. "OLELR-1BS5")
// [17-39] Reserved
// 8-byte block × N:
// [0-1] AngleRaw (uint16 LE, × 0.01° → deg, 0–359.99); >= 0xFF00 = invalid point
// [2-3] Distance readout (uint16 LE); meters = value × DistanceScale / 1000
// [4-5] Signal strength (uint16 LE)
// [6-7] Reserved
// NOTE: this header carries no timestamp/error field, so ScanResult::scan's
// timestamp_ms and info.error_status stay at their defaults (0) for Family B.
bool Driver::parse_family_b(const uint8_t* buf, int len) { bool Driver::parse_family_b(const uint8_t* buf, int len) {
static constexpr int HEADER_LEN = 40; static constexpr int HEADER_LEN = 40;
static constexpr int BLOCK_LEN = 8; 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; if (len < HEADER_LEN) return false;
uint8_t dist_scale = buf[6]; 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<float>(dist_scale) : 1.f); const float scale_mm = (dist_scale ? static_cast<float>(dist_scale) : 1.f);
pending_info_.distance_scale_mm = dist_scale; pending_info_.distance_scale_mm = dist_scale;
if (auto_detect_ && !model_locked_) { if (auto_detect_ && !model_locked_) {
@@ -324,6 +249,7 @@ bool Driver::parse_family_b(const uint8_t* buf, int len) {
cfg_.scan_angle_max = entry.cfg->scan_angle_max; cfg_.scan_angle_max = entry.cfg->scan_angle_max;
cfg_.range_min_m = entry.cfg->range_min_m; cfg_.range_min_m = entry.cfg->range_min_m;
cfg_.range_max_m = entry.cfg->range_max_m; cfg_.range_max_m = entry.cfg->range_max_m;
cfg_.angle_offset_deg = entry.cfg->angle_offset_deg;
break; break;
} }
} }
@@ -334,26 +260,21 @@ bool Driver::parse_family_b(const uint8_t* buf, int len) {
if (num_pts <= 0) return false; if (num_pts <= 0) return false;
const uint8_t* blk = buf + HEADER_LEN; const uint8_t* blk = buf + HEADER_LEN;
// AngleRaw is 0.01°/LSB (0–359.99°), per the official Olei block spec — // A packet is only a ~22° arc and may span >1 rev, so the revolution
// verified against real OLELR-1FMI geometry (a 0.25° scale smears a room // boundary is detected per point: a >90° drop between consecutive angles.
// into a circle). AngleRaw >= 0xFF00 marks an invalid point → skip it.
// The counter resets to 0 each revolution, but one packet is only a ~22°
// arc and the device can pack >1 revolution across packets, so the
// revolution boundary is detected PER POINT: a >90° drop between
// consecutive [0,360) angles ends the current revolution.
static constexpr uint16_t INVALID_ANGLE = 0xFF00; static constexpr uint16_t INVALID_ANGLE = 0xFF00;
for (int i = 0; i < num_pts; ++i, blk += BLOCK_LEN) { for (int i = 0; i < num_pts; ++i, blk += BLOCK_LEN) {
uint16_t angle_raw = le16(blk); 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) { if (last_angle_ >= 0.f && dev_deg < last_angle_ - 90.f) {
flush_scan(); // revolution complete flush_scan();
} }
last_angle_ = dev_deg; last_angle_ = dev_deg;
float angle = maybe_invert(to_signed_deg(angle_raw * 0.01f), inverted_); // -180…180 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; // readout × scale → m float dist_m = le16(blk + 2) * scale_mm * 0.001f;
uint8_t intensity = static_cast<uint8_t>(le16(blk + 4) >> 2); // 10-bit → 8-bit uint8_t intensity = static_cast<uint8_t>(le16(blk + 4) >> 2); // 10-bit → 8-bit
if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue; 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; return true;
} }
// ─── parse_family_c() ─────────────────────────────────────────────────────── // Family C / Protocol V3 (0xFEAC, GS1-5): 48B header + 2 or 4B points depending
// Protocol V3 (Olei GS1-5, magic 0xFEAC) — ported from the existing C# // on Types. Ported from the C# driver OleiGS15Driver.cs; NOT verified on real
// production driver OleiGS15Driver.cs (RobotNet10.RobotApp); NOT independently // hardware. Angle = (FirstIndex + i) * (360 / NumPointsScan) - 180.
// sniffed/verified against real GS1-5 hardware (no device was available to
// test this while writing the code).
// 48-byte header:
// [0-1] Magic = 0xFEAC
// [2-3] Version
// [4-7] PacketSize (uint32 LE)
// [8-9] HeaderSize (uint16 LE, usually = 48)
// [10] Distance ratio — read by the original C# driver but NOT applied
// (distance is always raw mm / 1000); same behavior kept here.
// Exposed raw as ExtraInfo::distance_ratio_raw.
// [11] Types: 0x00=2B/point (range only), 0x01=4B/point (range+intensity),
// 0x10=4B/point (first 2 bytes unused, range at [+2,+4))
// [12-13] Scan number [14-15] Packet number
// [16-19] Timestamp decimal [20-23] Timestamp integer
// [24-25] Scan frequency raw [26-27] NumPointsScan (total points per revolution)
// [28-29] Input status [30-31] Output status
// [32-35] Field status
// [36-37] StartIndex [38-39] EndIndex
// [40-41] FirstIndex — index of this packet's first point within the full revolution
// [42-43] NumPointsPacket — number of points in this packet
// [44-47] Status flags
// All of [10], [24-25], [28-35], [44-47] are read and passed through raw in
// ExtraInfo — none of these are cross-verified against real hardware, same
// caveat as the rest of this family.
// Angle: angle = (FirstIndex + i) * (360 / NumPointsScan) - 180 → already in
// the signed system (-180..180); no fmod needed like Family B since the
// index always stays within [0, NumPointsScan).
bool Driver::parse_family_c(const uint8_t* buf, int len) { bool Driver::parse_family_c(const uint8_t* buf, int len) {
static constexpr int HEADER_LEN = 48; static constexpr int HEADER_LEN = 48;
if (len < HEADER_LEN) return false; 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); uint16_t num_pts_packet = le16(buf + 42);
uint32_t status_flags = le32(buf + 44); 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; int header_size = (header_size_field == 0) ? HEADER_LEN : header_size_field;
if (header_size < HEADER_LEN || header_size > len) return false; 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; 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 payload_bytes = len - header_size;
int num_pts = num_pts_packet; 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_.field_status = field_status;
pending_info_.status_flags = status_flags; pending_info_.status_flags = status_flags;
// Magic 0xFEAC corresponds to exactly one model (GS1-5) — no model name // Magic 0xFEAC == exactly one model (GS1-5).
// string in the header like Family B, but recognizing this family is
// already enough to know the model, so auto-detect resolves immediately
// without reading any extra field.
if (auto_detect_ && !model_locked_) { if (auto_detect_ && !model_locked_) {
cfg_.scan_angle_min = MODEL_GS15.scan_angle_min; cfg_.scan_angle_min = MODEL_GS15.scan_angle_min;
cfg_.scan_angle_max = MODEL_GS15.scan_angle_max; cfg_.scan_angle_max = MODEL_GS15.scan_angle_max;
cfg_.range_min_m = MODEL_GS15.range_min_m; cfg_.range_min_m = MODEL_GS15.range_min_m;
cfg_.range_max_m = MODEL_GS15.range_max_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; detected_model_name_ = MODEL_GS15.name;
model_locked_ = true; model_locked_ = true;
} }
const float angle_inc = 360.f / static_cast<float>(num_pts_scan); const float angle_inc = 360.f / static_cast<float>(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<float>(first_index) * angle_inc; float raw_first_angle = static_cast<float>(first_index) * angle_inc;
if (last_angle_ >= 0.f && raw_first_angle < last_angle_ - 90.f) { 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); range_mm = le16(blk + 2);
} }
float angle = to_signed_deg(static_cast<float>(first_index + i) * angle_inc - 180.f); float angle = to_signed_deg(static_cast<float>(first_index + i) * angle_inc - 180.f + cfg_.angle_offset_deg);
angle = maybe_invert(angle, inverted_); angle = maybe_invert(angle, inverted_);
if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue; 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<uint8_t>(inten_raw > 255 ? 255 : inten_raw) : uint8_t{0}); has_inten ? static_cast<uint8_t>(inten_raw > 255 ? 255 : inten_raw) : uint8_t{0});
} }

299
src/sick_lidar.cpp
View File

@@ -1,9 +1,12 @@
#include "lidarlib/sick_lidar.hpp" #include "lidarlib/sick_lidar.hpp"
#include "lidar_bytes.hpp"
#include <cctype> #include <cctype>
#include <cerrno> #include <cerrno>
#include <cmath>
#include <cstdlib> #include <cstdlib>
#include <cstring> #include <cstring>
#include <limits>
#include <vector> #include <vector>
#include <fcntl.h> #include <fcntl.h>
#include <sys/socket.h> #include <sys/socket.h>
@@ -16,7 +19,6 @@
namespace lidarlib { namespace lidarlib {
namespace { namespace {
constexpr float kDeg2Rad = 3.14159265358979323846f / 180.f;
constexpr char kStx = 0x02; constexpr char kStx = 0x02;
constexpr char kEtx = 0x03; constexpr char kEtx = 0x03;
constexpr int kConnectTimeoutMs = 2000; constexpr int kConnectTimeoutMs = 2000;
@@ -25,14 +27,9 @@ uint32_t hex_to_u32(const std::string& tok) {
return static_cast<uint32_t>(std::strtoul(tok.c_str(), nullptr, 16)); return static_cast<uint32_t>(std::strtoul(tok.c_str(), nullptr, 16));
} }
int32_t hex_to_i32(const std::string& tok) { 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<int32_t>(hex_to_u32(tok)); return static_cast<int32_t>(hex_to_u32(tok));
} }
float bits_to_float(uint32_t bits) {
float f;
std::memcpy(&f, &bits, sizeof(f));
return f;
}
std::vector<std::string> tokenize(const std::string& s) { std::vector<std::string> tokenize(const std::string& s) {
std::vector<std::string> out; std::vector<std::string> out;
@@ -45,6 +42,10 @@ std::vector<std::string> tokenize(const std::string& s) {
} }
return out; return out;
} }
constexpr size_t kNanoRecvBufSize = 65536;
// nanoScan3 DerivedValues store angles as int32 in 1/4194304 degree.
constexpr double kNanoAngleResolution = 4194304.0;
} // namespace } // namespace
SickDriver::SickDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port) 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(); } SickDriver::~SickDriver() { close(); }
// ─── open() — TCP connect + tell the device to start streaming ────────────
bool SickDriver::open() { bool SickDriver::open() {
sock_fd_ = ::socket(AF_INET, SOCK_STREAM, 0); sock_fd_ = ::socket(AF_INET, SOCK_STREAM, 0);
if (sock_fd_ < 0) return false; if (sock_fd_ < 0) return false;
@@ -62,11 +62,8 @@ bool SickDriver::open() {
addr.sin_port = htons(port_); addr.sin_port = htons(port_);
addr.sin_addr.s_addr = inet_addr(ip_.c_str()); addr.sin_addr.s_addr = inet_addr(ip_.c_str());
// Non-blocking connect with a bounded timeout: a SICK device that's // Non-blocking connect with a bounded timeout — a blocking connect() to an
// powered off/unreachable leaves the SYN unanswered, and a plain blocking // unreachable device would stall for the OS default (~2 min on Linux).
// connect() would then stall this call — and whatever thread called it,
// e.g. a GUI's "connect" button handler — for the OS's default TCP retry
// timeout (~2 minutes on Linux).
int flags = ::fcntl(sock_fd_, F_GETFL, 0); int flags = ::fcntl(sock_fd_, F_GETFL, 0);
::fcntl(sock_fd_, F_SETFL, flags | O_NONBLOCK); ::fcntl(sock_fd_, F_SETFL, flags | O_NONBLOCK);
@@ -80,10 +77,10 @@ bool SickDriver::open() {
::getsockopt(sock_fd_, SOL_SOCKET, SO_ERROR, &err, &errlen); ::getsockopt(sock_fd_, SOL_SOCKET, SO_ERROR, &err, &errlen);
rc = (err == 0) ? 0 : -1; rc = (err == 0) ? 0 : -1;
} else { } 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) { if (rc < 0) {
::close(sock_fd_); ::close(sock_fd_);
@@ -96,8 +93,7 @@ bool SickDriver::open() {
recv_buf_.clear(); recv_buf_.clear();
// The device stays passive until told otherwise — without this, no // Device is passive until told to stream.
// LMDscandata telegram ever arrives.
if (!send_telegram("sEN LMDscandata 1")) { if (!send_telegram("sEN LMDscandata 1")) {
close(); close();
return false; return false;
@@ -105,16 +101,14 @@ bool SickDriver::open() {
return true; return true;
} }
// ─── close() ────────────────────────────────────────────────────────────────
void SickDriver::close() { void SickDriver::close() {
if (sock_fd_ >= 0) { if (sock_fd_ >= 0) {
send_telegram("sEN LMDscandata 0"); // best-effort, ignore failure send_telegram("sEN LMDscandata 0"); // best-effort
::close(sock_fd_); ::close(sock_fd_);
sock_fd_ = -1; sock_fd_ = -1;
} }
} }
// ─── send_telegram() — wrap with STX/ETX and write ─────────────────────────
bool SickDriver::send_telegram(const std::string& body) { bool SickDriver::send_telegram(const std::string& body) {
if (sock_fd_ < 0) return false; if (sock_fd_ < 0) return false;
std::string framed; std::string framed;
@@ -132,10 +126,8 @@ bool SickDriver::send_telegram(const std::string& body) {
return true; return true;
} }
// ─── read_telegram() — pull bytes off the TCP stream until one full // CoLa-A has no length prefix, so ETX is the only frame boundary; recv_buf_
// STX..ETX frame is assembled. CoLa-A has no length prefix, so ETX is the // carries leftover bytes across calls.
// only frame boundary; recv_buf_ carries any leftover bytes (start of the
// next telegram) across calls. ──────────────────────────────────────────────
bool SickDriver::read_telegram(std::string& out, int timeout_ms) { bool SickDriver::read_telegram(std::string& out, int timeout_ms) {
if (sock_fd_ < 0) return false; 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) { if (etx_pos != std::string::npos) {
size_t stx_pos = recv_buf_.find(kStx); size_t stx_pos = recv_buf_.find(kStx);
if (stx_pos == std::string::npos || stx_pos > etx_pos) { 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); recv_buf_.erase(0, etx_pos + 1);
continue; 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); fd_set fds; FD_ZERO(&fds); FD_SET(sock_fd_, &fds);
timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 }; timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 };
int r = ::select(sock_fd_ + 1, &fds, nullptr, nullptr, &tv); 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]; char buf[4096];
ssize_t n = ::recv(sock_fd_, buf, sizeof(buf), 0); 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<size_t>(n)); recv_buf_.append(buf, static_cast<size_t>(n));
} }
} }
// ─── recv_scan() ────────────────────────────────────────────────────────────
bool SickDriver::recv_scan(ScanResult& out, int timeout_ms) { bool SickDriver::recv_scan(ScanResult& out, int timeout_ms) {
for (;;) { for (;;) {
std::string telegram; std::string telegram;
if (!read_telegram(telegram, timeout_ms)) return false; if (!read_telegram(telegram, timeout_ms)) return false;
if (parse_lmdscandata(telegram, out)) return true; 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() { bool SickDriver::spin_once() {
std::string telegram; std::string telegram;
if (!read_telegram(telegram, 0)) return false; // 0 = block until next telegram if (!read_telegram(telegram, 0)) return false;
ScanResult result; ScanResult result;
if (!parse_lmdscandata(telegram, result)) return true; // ignore non-scan telegrams if (!parse_lmdscandata(telegram, result)) return true;
if (cb_) cb_(result); if (cb_) cb_(result);
return true; return true;
} }
// ─── parse_lmdscandata() ──────────────────────────────────────────────────── // CoLa-A "sSN/sRA LMDscandata": space-separated ASCII hex tokens, field order
// CoLa-A "sSN LMDscandata"/"sRA LMDscandata" telegram, space-separated ASCII // per SICK's Telegram Listing. "DIST1" → ranges, "RSSI1" → intensities.
// tokens (mostly hex). UNVERIFIED layout (see header comment) — ported from
// SICK's public Telegram Listing, field order below:
//
// sSN LMDscandata <Version> <DeviceNumber> <SerialNumber>
// <Status0> <Status1> <TelegramCounter> <ScanCounter>
// <TimeSinceStartup> <TimeOfTransmission>
// <In0> <In1> <Out0> <Out1> <Reserved>
// <ScanningFrequency> <MeasurementFrequency>
// <NumEncoders> [<EncoderPosition> <EncoderSpeed>]*
// <Num16BitChannels>
// { <ContentName> <ScalingFactor(IEEE754 hex)> <ScalingOffset(hex)>
// <StartAngle(1/10000 deg, signed hex)> <StepWidth(1/10000 deg, signed hex)>
// <NumData> <Data>* }*
// <Num8BitChannels> { ...same shape, 8-bit data... }*
// (position/name/comment/time/event fields follow — not needed for LaserScan, ignored)
//
// ContentName "DIST1" carries ranges (raw mm × ScalingFactor), "RSSI1"
// carries intensities (raw × ScalingFactor) — any other channel name is
// consumed (to keep the token cursor in sync) but its data discarded.
bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out) { bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out) {
std::vector<std::string> tok = tokenize(telegram); std::vector<std::string> tok = tokenize(telegram);
if (tok.size() < 20) return false; if (tok.size() < 20) return false;
@@ -218,15 +188,14 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out)
size_t i = 2; size_t i = 2;
auto next = [&]() -> std::string { return (i < tok.size()) ? tok[i++] : std::string(); }; auto next = [&]() -> std::string { return (i < tok.size()) ? tok[i++] : std::string(); };
hex_to_u32(next()); // VersionNumber — not exposed hex_to_u32(next()); // VersionNumber
hex_to_u32(next()); // DeviceNumber — not exposed hex_to_u32(next()); // DeviceNumber
hex_to_u32(next()); // SerialNumber — not exposed hex_to_u32(next()); // SerialNumber
uint32_t status0 = hex_to_u32(next()); // DeviceStatus: Error uint32_t status0 = hex_to_u32(next());
uint32_t status1 = hex_to_u32(next()); // DeviceStatus: Pollution uint32_t status1 = hex_to_u32(next());
uint32_t telegram_counter = hex_to_u32(next()); hex_to_u32(next()); // TelegramCounter
uint32_t scan_counter = hex_to_u32(next()); hex_to_u32(next()); // ScanCounter
(void)telegram_counter; (void)scan_counter; // not carried by ExtraInfo today hex_to_u32(next()); // TimeSinceStartup
hex_to_u32(next()); // TimeSinceStartup — not exposed
uint32_t time_of_transmission = hex_to_u32(next()); uint32_t time_of_transmission = hex_to_u32(next());
uint32_t in0 = hex_to_u32(next()); uint32_t in0 = hex_to_u32(next());
uint32_t in1 = 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()); uint32_t out1 = hex_to_u32(next());
next(); // Reserved next(); // Reserved
uint32_t scanning_frequency = hex_to_u32(next()); 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()); uint32_t num_encoders = hex_to_u32(next());
for (uint32_t e = 0; e < num_encoders; ++e) { 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; float angle_min_deg = 0.f, angle_inc_deg = 0.f;
bool got_dist = false; bool got_dist = false;
auto parse_channel_block = [&](bool eight_bit) { // 16-bit and 8-bit channel blocks share the same ASCII layout.
std::string content = next(); // e.g. "DIST1", "RSSI1" auto parse_channel_block = [&]() {
std::string content = next();
uint32_t scale_bits = hex_to_u32(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 start_angle = hex_to_i32(next()); // 1/10000 deg
int32_t step_width = 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()); uint32_t num_data = hex_to_u32(next());
float scale = bits_to_float(scale_bits); 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_dist = content.rfind("DIST", 0) == 0;
bool is_rssi = content.rfind("RSSI", 0) == 0; bool is_rssi = content.rfind("RSSI", 0) == 0;
if (is_dist) { if (is_dist) {
angle_min_deg = static_cast<float>(start_angle) * 0.0001f; angle_min_deg = static_cast<float>(start_angle) * 0.0001f + cfg_.angle_offset_deg;
angle_inc_deg = static_cast<float>(step_width) * 0.0001f; angle_inc_deg = static_cast<float>(step_width) * 0.0001f;
scan.ranges.assign(num_data, 0.f); scan.ranges.assign(num_data, 0.f);
} else if (is_rssi && scan.intensities.empty()) { } 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<float>(raw) * scale; scan.intensities[d] = static_cast<float>(raw) * scale;
} }
} }
(void)eight_bit;
}; };
uint32_t num_16bit_channels = hex_to_u32(next()); 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()); 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; 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_increment = angle_inc_deg * kDeg2Rad;
scan.angle_max = scan.angle_min + scan.angle_max = scan.angle_min +
scan.angle_increment * static_cast<float>(scan.ranges.size() - 1); scan.angle_increment * static_cast<float>(scan.ranges.size() - 1);
scan.time_increment = 0.f; // device doesn't expose per-point timing scan.time_increment = 0.f;
scan.scan_time = 0.f; // device doesn't expose per-scan timing scan.scan_time = 0.f;
scan.range_min = cfg_.range_min_m; scan.range_min = cfg_.range_min_m;
scan.range_max = cfg_.range_max_m; scan.range_max = cfg_.range_max_m;
if (scan.intensities.size() != scan.ranges.size()) 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; ExtraInfo& info = out.info;
info = ExtraInfo{}; 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.detected_model = cfg_.name;
info.error_status = static_cast<uint8_t>(status0 & 0xFF); info.error_status = static_cast<uint8_t>(status0 & 0xFF);
info.status_flags = (status0 << 8) | status1; info.status_flags = (status0 << 8) | status1;
@@ -316,4 +286,181 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out)
return true; return true;
} }
// ── NanoScanDriver — SICK nanoScan3/microScan3 safety-scanner UDP output ────
NanoScanDriver::NanoScanDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port)
: cfg_(cfg), detected_model_name_(cfg.name ? cfg.name : ""), ip_(ip), port_(port),
recv_buf_(kNanoRecvBufSize) {}
NanoScanDriver::~NanoScanDriver() { close(); }
bool NanoScanDriver::open() {
sock_fd_ = ::socket(AF_INET, SOCK_DGRAM, 0);
if (sock_fd_ < 0) return false;
int reuse = 1;
::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));
sockaddr_in addr{};
addr.sin_family = AF_INET;
addr.sin_port = htons(port_);
addr.sin_addr.s_addr = (ip_ == "0.0.0.0" || ip_.empty()) ? INADDR_ANY
: inet_addr(ip_.c_str());
if (::bind(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0) {
::close(sock_fd_);
sock_fd_ = -1;
return false;
}
return true;
}
void NanoScanDriver::close() {
if (sock_fd_ >= 0) {
::close(sock_fd_);
sock_fd_ = -1;
}
}
int NanoScanDriver::recv_datagram(int timeout_ms) {
if (sock_fd_ < 0) return -1;
if (timeout_ms > 0) {
fd_set fds; FD_ZERO(&fds); FD_SET(sock_fd_, &fds);
timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 };
int r = ::select(sock_fd_ + 1, &fds, nullptr, nullptr, &tv);
if (r <= 0) return -1;
}
ssize_t n = ::recv(sock_fd_, recv_buf_.data(), recv_buf_.size(), 0);
return (n <= 0) ? -1 : static_cast<int>(n);
}
// A scan is split across datagrams at the application layer. Each starts with
// a 24-byte fragment header: "MS3 " @0, u32 totalLength @8, u32 scanNumber @12,
// u32 fragmentOffset @16. Reassemble until totalLength bytes; a lost fragment
// drops that scan and we resync on the next scanNumber.
bool NanoScanDriver::recv_scan(ScanResult& out, int timeout_ms) {
std::vector<uint8_t> tele;
uint32_t cur_scan = 0, total = 0, got = 0;
bool assembling = false;
for (;;) {
int n = recv_datagram(timeout_ms);
if (n < 0) return false;
const uint8_t* d = recv_buf_.data();
if (n < 24 || std::memcmp(d, "MS3 ", 4) != 0) {
if (parse_packet(d, n, out)) return true;
continue;
}
uint32_t tl = le32(d + 8);
uint32_t scan = le32(d + 12);
uint32_t foff = le32(d + 16);
const uint8_t* pl = d + 24;
uint32_t pl_len = static_cast<uint32_t>(n) - 24;
if (tl == 0 || tl > kNanoRecvBufSize) continue;
if (!assembling || scan != cur_scan || tl != total) {
cur_scan = scan; total = tl; got = 0;
tele.assign(total, 0);
assembling = true;
}
if (static_cast<uint64_t>(foff) + pl_len <= total) {
std::memcpy(tele.data() + foff, pl, pl_len);
got += pl_len;
}
if (got >= total) {
assembling = false;
if (parse_packet(tele.data(), static_cast<int>(total), out)) return true;
}
}
}
bool NanoScanDriver::spin_once() {
int n = recv_datagram(0);
if (n < 0) return false;
ScanResult result;
if (!parse_packet(recv_buf_.data(), n, result)) return true;
if (cb_) cb_(result);
return true;
}
// SICK safety-scanner data packet (LE), layout ported from sick_safetyscanners:
// DataHeader offset table at fixed offsets (derivedValues @36, measurementData
// @40); DerivedValues holds multiplicationFactor/startAngle/resolution;
// MeasurementData is u32 numBeams then 4 B/beam (u16 dist, u8 reflect, u8 status).
bool NanoScanDriver::parse_packet(const uint8_t* buf, int len, ScanResult& out) {
if (len < 52) return false;
uint16_t dv_off = le16(buf + 36);
uint16_t dv_size = le16(buf + 38);
uint16_t md_off = le16(buf + 40);
uint16_t md_size = le16(buf + 42);
if (dv_off == 0 || dv_size == 0 || md_off == 0 || md_size == 0) return false;
if (static_cast<int>(dv_off) + 20 > len) return false;
if (static_cast<int>(md_off) + 4 > len) return false;
const uint8_t* dv = buf + dv_off;
uint16_t mult_factor = le16(dv + 0);
int32_t start_raw = le_i32(dv + 8);
int32_t res_raw = le_i32(dv + 12);
if (mult_factor == 0) mult_factor = 1;
double start_deg = static_cast<double>(start_raw) / kNanoAngleResolution;
double res_deg = static_cast<double>(res_raw) / kNanoAngleResolution;
const uint8_t* md = buf + md_off;
uint32_t num_beams = le32(md + 0);
if (num_beams == 0 || num_beams > 2751) return false; // 2751 = sensor max
if (static_cast<int64_t>(md_off) + 4 + static_cast<int64_t>(num_beams) * 4 > len)
return false;
LaserScan& scan = out.scan;
scan.ranges.assign(num_beams, 0.f);
scan.intensities.assign(num_beams, 0.f);
for (uint32_t i = 0; i < num_beams; ++i) {
const uint8_t* p = md + 4 + i * 4;
uint16_t distance = le16(p + 0);
uint8_t reflect = le_u8(p + 2);
uint8_t status = le_u8(p + 3);
bool valid = (status & 0x01) != 0;
bool infinite = (status & 0x02) != 0;
if (!valid || infinite) {
scan.ranges[i] = std::numeric_limits<float>::infinity();
} else {
scan.ranges[i] = static_cast<float>(distance) *
static_cast<float>(mult_factor) * 1e-3f; // mm -> m
}
scan.intensities[i] = static_cast<float>(reflect);
}
scan.angle_min = (static_cast<float>(start_deg) + cfg_.angle_offset_deg) * kDeg2Rad;
scan.angle_increment = static_cast<float>(res_deg * kDeg2Rad);
scan.angle_max = scan.angle_min +
scan.angle_increment * static_cast<float>(num_beams - 1);
scan.time_increment = 0.f;
scan.scan_time = 0.f;
scan.range_min = cfg_.range_min_m;
scan.range_max = cfg_.range_max_m;
// Raw device time from the DataHeader — an opaque tag, not ms since power-on.
scan.timestamp_ms = le32(buf + 28);
if (cfg_.remap_angles)
remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max);
ExtraInfo& info = out.info;
info = ExtraInfo{};
info.detected_model = cfg_.name;
return true;
}
} // namespace lidarlib } // namespace lidarlib