Refactor lidar library: rename olei_config to lidar_config, add nanoscan example and shared byte helpers
Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
This commit is contained in:
@@ -11,12 +11,10 @@ find_package(Threads REQUIRED)
|
||||
|
||||
option(BUILD_SHARED_LIBS "Build shared (.so) libraries instead of static" ON)
|
||||
|
||||
# ── lidarlib: OLEI (UDP) + SICK (TCP) drivers, the unified make_lidar()
|
||||
# factory, and config.json load/save. No web UI. ──
|
||||
set(LIDARLIB_SOURCES
|
||||
src/olei_lidar.cpp
|
||||
src/sick_lidar.cpp
|
||||
src/olei_config.cpp
|
||||
src/lidar_config.cpp
|
||||
)
|
||||
|
||||
add_library(lidarlib ${LIDARLIB_SOURCES})
|
||||
@@ -42,13 +40,14 @@ if(LIDARLIB_BUILD_EXAMPLES)
|
||||
add_executable(sick_example examples/sick_example.cpp)
|
||||
target_link_libraries(sick_example PRIVATE lidarlib)
|
||||
|
||||
# Headless skeleton app: load config.json -> make_lidar() each -> print scans.
|
||||
# Replace its print loop with your own GUI; this is the integration template.
|
||||
add_executable(nanoscan_example examples/nanoscan_example.cpp)
|
||||
target_link_libraries(nanoscan_example PRIVATE lidarlib)
|
||||
|
||||
add_executable(lidar_app examples/lidar_app.cpp)
|
||||
target_link_libraries(lidar_app PRIVATE lidarlib)
|
||||
endif()
|
||||
|
||||
# ── install + find_package() support ──
|
||||
# install + find_package() support
|
||||
include(GNUInstallDirs)
|
||||
include(CMakePackageConfigHelpers)
|
||||
|
||||
|
||||
392
README.md
392
README.md
@@ -1,135 +1,14 @@
|
||||
# Lidarlib
|
||||
|
||||
Thư viện C++17 cho lidar (OLEI + SICK), build bằng CMake ra **shared lib `.so`**
|
||||
(`lidarlib::lidarlib`), hỗ trợ `find_package()` để link vào project khác. Tự nhận
|
||||
diện họ giao thức (Family A/B/C) theo từng gói, hỗ trợ chạy nhiều lidar song
|
||||
song, gộp cả OLEI (UDP) và SICK (TCP) sau **một hàm config duy nhất**
|
||||
`lidarlib::make_lidar()`. **Không có Web UI** — người dùng tự viết giao diện trên
|
||||
API C++ này (include header + link `.so`).
|
||||
Thư viện C++17 cho lidar **OLEI** (UDP) và **SICK** (TCP/UDP). Build bằng CMake
|
||||
ra shared lib, hỗ trợ `find_package(lidarlib)`. Mọi driver dùng chung một
|
||||
interface `lidarlib::Lidar` và một hàm khởi tạo duy nhất `lidarlib::make_lidar()`.
|
||||
|
||||
## Tóm tắt API (cho người viết giao diện)
|
||||
|
||||
```cpp
|
||||
#include "lidarlib/lidarlib.hpp" // gộp toàn bộ API trong 1 include
|
||||
|
||||
// 1) MỘT hàm config: từ LidarConfig -> handle chung (OLEI hoặc SICK)
|
||||
lidarlib::LidarConfig c{"front", "192.168.1.10", 2368, "AUTO", false, "OLEI"};
|
||||
std::unique_ptr<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'
|
||||
```
|
||||
- Tự nhận diện họ giao thức OLEI (Family A/B/C) theo từng gói tin
|
||||
- Tự dò model (`MODEL_AUTO`) với Family B/C
|
||||
- Chạy nhiều lidar song song (mỗi instance độc lập, an toàn đa luồng)
|
||||
- Output chuẩn ROS `sensor_msgs/LaserScan` (radian, mét)
|
||||
- Không có UI — tự viết giao diện trên API này
|
||||
|
||||
## Build
|
||||
|
||||
@@ -138,209 +17,150 @@ cmake -S . -B build -DCMAKE_BUILD_TYPE=Release
|
||||
cmake --build build -j"$(nproc)"
|
||||
```
|
||||
|
||||
Sinh ra `build/liblidarlib.so` (driver OLEI + SICK + factory + config, không
|
||||
phụ thuộc gì ngoài pthread) và 4 binary demo (`example`, `test_dual`,
|
||||
`sick_example`, `lidar_app`). Tắt build demo bằng `-DLIDARLIB_BUILD_EXAMPLES=OFF`.
|
||||
Muốn ra static lib `.a` thay vì `.so` thì thêm `-DBUILD_SHARED_LIBS=OFF`.
|
||||
Sinh ra `build/liblidarlib.so` (chỉ phụ thuộc pthread) và các binary demo
|
||||
(`example`, `test_dual`, `sick_example`, `nanoscan_example`, `lidar_app`).
|
||||
|
||||
Cài vào hệ thống để dùng `find_package(lidarlib)` từ project khác — `/usr/local`
|
||||
chỉ root mới ghi được nên cần `sudo`, không thì `cmake --install` báo lỗi
|
||||
permission denied:
|
||||
Tùy chọn: `-DLIDARLIB_BUILD_EXAMPLES=OFF` (tắt demo),
|
||||
`-DBUILD_SHARED_LIBS=OFF` (static lib).
|
||||
|
||||
Cài đặt và dùng từ project khác:
|
||||
|
||||
```bash
|
||||
sudo cmake --install build --prefix /usr/local
|
||||
```
|
||||
|
||||
Muốn cài không cần `sudo` thì đổi prefix sang thư mục riêng trong `$HOME` (vd
|
||||
`~/.local`), rồi thêm `-DCMAKE_PREFIX_PATH=~/.local` khi configure project nào
|
||||
gọi `find_package(lidarlib)`:
|
||||
|
||||
```bash
|
||||
cmake --install build --prefix "$HOME/.local"
|
||||
cmake --install build --prefix "$HOME/.local" # hoặc sudo với /usr/local
|
||||
```
|
||||
|
||||
```cmake
|
||||
# trong CMakeLists.txt của project dùng thư viện này
|
||||
find_package(lidarlib REQUIRED)
|
||||
target_link_libraries(my_app PRIVATE lidarlib::lidarlib)
|
||||
```
|
||||
|
||||
Sau khi link, giao diện chỉ cần `#include "lidarlib/lidarlib.hpp"` rồi gọi
|
||||
`lidarlib::make_lidar()` — xem "Tóm tắt API" ở đầu README.
|
||||
|
||||
Vẫn có thể build từng file bằng g++ thuần nếu không muốn dùng CMake:
|
||||
|
||||
```bash
|
||||
g++ -std=c++17 -O2 -pthread -Wall -Wextra -Iinclude -o test_dual examples/test_dual.cpp src/olei_lidar.cpp
|
||||
g++ -std=c++17 -O2 -pthread -Wall -Wextra -Iinclude -o example examples/example.cpp src/olei_lidar.cpp
|
||||
g++ -std=c++17 -O2 -pthread -Wall -Wextra -Iinclude -o lidar_app examples/lidar_app.cpp src/olei_lidar.cpp src/sick_lidar.cpp src/olei_config.cpp
|
||||
g++ -std=c++17 -O2 -pthread -Wall -Wextra -Iinclude -o sick_example examples/sick_example.cpp src/sick_lidar.cpp
|
||||
```
|
||||
|
||||
## Test thử
|
||||
|
||||
### 2 lidar song song
|
||||
|
||||
```bash
|
||||
./build/test_dual
|
||||
```
|
||||
|
||||
In ra 5 scan mỗi bên, kèm số điểm, timestamp, error status, và **model đã tự
|
||||
dò được** (`model=...`).
|
||||
|
||||
### 1 lidar
|
||||
|
||||
Sửa model/IP/port trong `examples/example.cpp` rồi build lại, hoặc gọi trực tiếp:
|
||||
## Quick start
|
||||
|
||||
```cpp
|
||||
lidarlib::Driver drv(lidarlib::MODEL_AUTO, "192.168.100.100", 2369);
|
||||
drv.open();
|
||||
lidarlib::ScanResult result;
|
||||
drv.recv_scan(result, 2000);
|
||||
#include "lidarlib/lidarlib.hpp" // toàn bộ API trong 1 include
|
||||
|
||||
lidarlib::LidarConfig c{"front", "192.168.1.10", 2368, "AUTO", false, "OLEI"};
|
||||
std::unique_ptr<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
|
||||
|
||||
Constructor có tham số thứ 4 `inverted` (mặc định `false`). Đặt `true` nếu
|
||||
thiết bị bị lắp lật 180° quanh trục hướng về phía trước — driver tự đảo dấu
|
||||
góc từng điểm (`angle = -angle`, chuẩn hóa lại về `-180..180`) để output luôn
|
||||
đúng theo hệ quy chiếu xe, không phụ thuộc hướng lắp vật lý:
|
||||
Có thể khởi tạo driver trực tiếp thay vì qua `make_lidar()`:
|
||||
|
||||
```cpp
|
||||
lidarlib::Driver drv(lidarlib::MODEL_AUTO, "192.168.100.100", 2369, /*inverted=*/true);
|
||||
lidarlib::Driver olei(lidarlib::MODEL_AUTO, "192.168.100.100", 2368);
|
||||
lidarlib::SickDriver tim (lidarlib::MODEL_SICK_TIM571, "192.168.0.1", 2111);
|
||||
lidarlib::NanoScanDriver nano(lidarlib::MODEL_SICK_NANOSCAN3, "0.0.0.0", 6060);
|
||||
```
|
||||
|
||||
Đã verify bằng sniff sống: chạy `inverted=false` góc tăng dần theo thời gian,
|
||||
chạy `inverted=true` góc giảm dần với cùng bước góc — đúng chữ ký của đảo dấu.
|
||||
Ngoài `recv_scan()` blocking còn có callback: `set_scan_callback()` +
|
||||
`spin_once()` trong vòng lặp riêng.
|
||||
|
||||
## Cấu hình & chạy (lidar_app + config.json)
|
||||
## Cấu hình (config.json)
|
||||
|
||||
Không còn Web UI. Cấu hình là một file JSON đơn giản — `examples/lidar_app.cpp`
|
||||
đọc nó, mở từng lidar qua đúng **một hàm** `lidarlib::make_lidar()`, rồi đọc scan
|
||||
trên mỗi thread. Đây là khung mẫu để bạn thay vòng `printf` bằng giao diện
|
||||
riêng (Qt, ImGui, ROS node, v.v.).
|
||||
`lidar_app` là app mẫu headless: đọc `config.json`, mở từng lidar bằng
|
||||
`make_lidar()`, một thread mỗi con.
|
||||
|
||||
```bash
|
||||
./build/lidar_app # đọc/tạo config.json cạnh chỗ chạy
|
||||
./build/lidar_app my_config.json # đường dẫn config khác
|
||||
./build/lidar_app [my_config.json]
|
||||
```
|
||||
|
||||
`config.json` — danh sách lidar, không cố định số lượng. `brand` chọn loại
|
||||
driver (`"OLEI"` = UDP, `"SICK"` = TCP/SOPAS); `model` tra trong bảng
|
||||
`ModelConfig` (tên lạ → tự lùi về mặc định của hãng: `AUTO` cho OLEI,
|
||||
`SICK-TIM571` cho SICK); `inverted` chỉ có tác dụng với OLEI. Bỏ trống `brand`
|
||||
thì mặc định `"OLEI"` (tương thích file cũ).
|
||||
|
||||
```json
|
||||
{
|
||||
"lidars": [
|
||||
{"name":"front", "ip":"192.168.100.100", "port":2368, "brand":"OLEI", "model":"AUTO", "inverted":false},
|
||||
{"name":"rear", "ip":"192.168.100.100", "port":2369, "brand":"OLEI", "model":"AUTO", "inverted":true},
|
||||
{"name":"sick1", "ip":"192.168.0.1", "port":2111,"brand":"SICK","model":"SICK-TIM571", "inverted":false}
|
||||
{"name":"sick1", "ip":"192.168.0.1", "port":2111, "brand":"SICK", "model":"SICK-TIM571"},
|
||||
{"name":"nano1", "ip":"0.0.0.0", "port":6060, "brand":"SICK", "model":"SICK-nanoScan3"}
|
||||
]
|
||||
}
|
||||
```
|
||||
|
||||
Trong code, đọc/ghi file bằng `lidarlib::load_config(path)` / `lidarlib::save_config(path, cfg)`
|
||||
(file hỏng → trả về mặc định, không crash). Giao diện của bạn tự quyết khi nào
|
||||
lưu — thư viện không tự bind port hay phục vụ HTTP gì cả.
|
||||
| Trường | Ý nghĩa |
|
||||
|---|---|
|
||||
| `brand` | `"OLEI"` (mặc định) hoặc `"SICK"` |
|
||||
| `model` | Tên trong bảng model bên dưới; tên lạ → mặc định của hãng (`AUTO` / `SICK-TIM571`). Với SICK, `"SICK-nanoScan3"` → driver UDP, còn lại → driver TCP |
|
||||
| `inverted` | Chỉ OLEI: `true` nếu lidar lắp úp ngược, driver tự đảo góc về hệ quy chiếu xe |
|
||||
| `angle_min_deg` / `angle_max_deg` | Tuỳ chọn: remap tuyến tính góc output sang cửa sổ này (độ). Không cắt điểm nào, chỉ ghi lại `angle_min/max/increment`. Bỏ trống (±360) = tắt |
|
||||
|
||||
### Sniff packet thô (debug khi nghi ngờ offset header)
|
||||
Đọc/ghi bằng `lidarlib::load_config(path)` / `lidarlib::save_config(path, cfg)`.
|
||||
|
||||
```bash
|
||||
python3 - <<'EOF'
|
||||
import socket
|
||||
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
s.bind(("192.168.100.100", 2369)) # đổi port theo lidar cần xem
|
||||
data, addr = s.recvfrom(4096)
|
||||
print("from", addr, "len", len(data))
|
||||
print(data[:40].hex(' '))
|
||||
EOF
|
||||
```
|
||||
## Model
|
||||
|
||||
Luôn ưu tiên capture thật hơn là tin comment trong code — comment mô tả ý
|
||||
định lúc viết, không phải offset đã verify trên thiết bị thật.
|
||||
### OLEI (`brand = "OLEI"`, UDP, port mặc định 2368)
|
||||
|
||||
## Chọn `ModelConfig`
|
||||
|
||||
| Constant | FOV (deg, có dấu) | range_min/max (m) | Khi dùng |
|
||||
| Constant | FOV (°) | Range (m) | Ghi chú |
|
||||
|---|---|---|---|
|
||||
| `MODEL_VB` | -135…135 | 0.05…30 | 2D 270°, Family A |
|
||||
| `MODEL_VF` | -180…180 | 0.05…30 | 2D 360°, Family A |
|
||||
| `MODEL_LR1F` | -180…180 | 0.05…50 | 2D 360° 50m, Family A |
|
||||
| `MODEL_LR1BS5` | -180…180 | 0.05…30 | 2D 360°, Family B |
|
||||
| `MODEL_LR1F` | -180…180 | 0.05…50 | Family A; 0° thô của máy chỉ về đuôi (offset +180°) |
|
||||
| `MODEL_LR1FMI` | -180…180 | 0.05…30 | Family B, ~2400 điểm/vòng; 0° thô chỉ về đuôi (offset +180°) |
|
||||
| `MODEL_LR1BS5` | -180…180 | 0.05…30 | Family B |
|
||||
| `MODEL_LR16F` | -135…135 | 0.05…30 | 3D 16-line |
|
||||
| `MODEL_GS15` | -180…180 | 0.05…30 | 2D 360°, Family C/V3 — **chưa verify hardware thật** |
|
||||
| `MODEL_AUTO` | -180…180 (mặc định, có thể tự thu hẹp) | 0.05…30 | Không biết trước model |
|
||||
| `MODEL_GS15` | -180…180 | 0.05…30 | Family C/V3 — **chưa verify phần cứng** |
|
||||
| `MODEL_AUTO` | -180…180 | 0.05…30 | Không biết trước model; tự dò với Family B (chuỗi tên) và C (magic). Family A không mang tên model nên giữ FOV rộng |
|
||||
|
||||
`range_min_m`/`range_max_m` chỉ là giá trị đặt sẵn để điền vào
|
||||
`LaserScan::range_min/range_max` (không đọc từ packet) — chỉnh trong
|
||||
`include/lidarlib/lidar.hpp` theo datasheet thật của từng model nếu cần chính xác.
|
||||
Driver nhận diện họ giao thức theo Frame ID mỗi gói:
|
||||
**Family A** `0xFAF0` (header 20B, 3B/điểm, CRC32) ·
|
||||
**Family B** `0xFEF0` (header 40B kèm tên model ASCII, 8B/điểm) ·
|
||||
**Family C/V3** `0xFEAC` (header 48B, 2/4B/điểm — port từ driver C#, chưa verify).
|
||||
|
||||
`MODEL_AUTO`: chỉ có tác dụng tự-dò với **Family B** (đọc chuỗi tên model
|
||||
trong header). Driver luôn lưu lại **tên thật** đọc từ packet (vd
|
||||
`"OLELR-1BS5"`, `"OLELR-1BS2"`) — gọi `drv.detected_model()` hoặc
|
||||
`result.info.detected_model` để xem (trả về `"AUTO"` nếu chưa nhận gói Family B
|
||||
nào). FOV chỉ tự thu hẹp khi tên đó khớp một entry trong `kModelTable`
|
||||
(`src/olei_lidar.cpp`); nếu không khớp, FOV giữ nguyên mặc định 360°
|
||||
(`-180..180`, không mất điểm) — an toàn nhưng có thể giữ lại điểm ngoài FOV
|
||||
thật của thiết bị nếu thiết bị đó không quét tròn.
|
||||
Tên model thật đọc từ packet xem qua `detected_model()` hoặc
|
||||
`result.info.detected_model`.
|
||||
|
||||
Family A không mang chuỗi tên model trong packet, nên `MODEL_AUTO` trên thiết
|
||||
bị Family A cũng giữ nguyên FOV rộng — phải chỉ định model cụ thể (VD
|
||||
`MODEL_VB`) nếu muốn thu hẹp FOV cho thiết bị góc hẹp.
|
||||
### SICK (`brand = "SICK"`)
|
||||
|
||||
## Lidar SICK (TiM5xx/7xx) — driver riêng
|
||||
|
||||
`lidarlib::SickDriver` (`include/lidarlib/sick_lidar.hpp` + `src/sick_lidar.cpp`) là
|
||||
driver **độc lập** với `lidarlib::Driver` ở trên — không phải thêm 1 family vào
|
||||
driver OLEI, vì giao thức khác hẳn:
|
||||
|
||||
- Kết nối **TCP** (SOPAS, port mặc định 2111) tới lidar, không phải UDP
|
||||
broadcast như OLEI.
|
||||
- Telegram là **ASCII** (CoLa-A), đóng khung bằng `STX`(0x02)/`ETX`(0x03),
|
||||
không có CRC32 như Family A.
|
||||
- Thiết bị đứng im cho tới khi driver gửi lệnh `sEN LMDscandata 1` — `open()`
|
||||
tự làm việc này; `close()` gửi `sEN LMDscandata 0` trước khi đóng socket.
|
||||
|
||||
Output vẫn dùng chung `ScanResult`/`LaserScan`/`ExtraInfo` như driver OLEI nên
|
||||
gọi giống hệt:
|
||||
|
||||
```cpp
|
||||
lidarlib::SickDriver drv(lidarlib::MODEL_SICK_TIM571, "192.168.0.1", 2111);
|
||||
drv.open();
|
||||
lidarlib::ScanResult result;
|
||||
drv.recv_scan(result, 2000);
|
||||
```
|
||||
|
||||
| Constant | FOV (deg, có dấu) | range_min/max (m) | Khi dùng |
|
||||
| Constant | FOV (°) | Range (m) | Transport |
|
||||
|---|---|---|---|
|
||||
| `MODEL_SICK_TIM5XX` | -135…135 | 0.05…10 | TiM551/561, 270° |
|
||||
| `MODEL_SICK_TIM571` | -135…135 | 0.05…25 | TiM571, 270° |
|
||||
| `MODEL_SICK_TIM7XX` | -135…135 | 0.05…25 | TiM781, 270° |
|
||||
| `MODEL_SICK_TIM5XX` | -135…135 | 0.05…10 | TCP/SOPAS (CoLa-A), port 2111 |
|
||||
| `MODEL_SICK_TIM571` | -135…135 | 0.05…25 | TCP/SOPAS, port 2111 |
|
||||
| `MODEL_SICK_TIM7XX` | -135…135 | 0.05…25 | TCP/SOPAS, port 2111 |
|
||||
| `MODEL_SICK_NANOSCAN3` | -137.5…137.5 | 0.05…40 | UDP safety-data, port 6060 |
|
||||
|
||||
**Đã verify bằng phần cứng thật**: chạy trực tiếp với 1 con **SICK TiM781S**
|
||||
(FW `V5.11-14.10.24`, DeviceIdent đọc qua `sRN DeviceIdent` trên cổng 2111)
|
||||
tại `192.168.100.22:2111` — `MODEL_SICK_TIM7XX`. Kết quả khớp đúng datasheet
|
||||
TiM781S: 811 điểm/scan trải từ -45°…225° (270° FOV), `angle_increment` =
|
||||
0.333° (1/3°, đúng độ phân giải góc của dòng 781), khoảng cách 0.3-1.5m ổn
|
||||
định qua nhiều scan liên tiếp, kênh `RSSI1` có giá trị intensity hợp lý
|
||||
(không phải toàn 0), `error_status` = 0x00. Điều này xác nhận layout
|
||||
`LMDscandata` trong `parse_lmdscandata()` (`src/sick_lidar.cpp`) — kênh
|
||||
`DIST1`/`RSSI1`, scaling factor IEEE-754, start angle/step width — đọc đúng
|
||||
trên hardware thật, không chỉ đúng theo tài liệu nữa.
|
||||
**TiM (`SickDriver`)** — `open()` tự gửi `sEN LMDscandata 1` để bắt đầu stream.
|
||||
Hệ góc trên dây đặt 90° = trước mặt nên preset có `angle_offset_deg = -90`,
|
||||
output ra -135…135° với 0° = phía trước. **Đã verify trên TiM781S thật**
|
||||
(811 điểm/scan, increment 0.333°, DIST1/RSSI1 đúng layout). Chưa verify:
|
||||
encoder, kênh 8-bit, thông số TiM5xx/571.
|
||||
|
||||
**Vẫn chưa verify**: chiều quy ước góc 0° (thẳng phía trước thiết bị hay
|
||||
hướng khác — chưa đối chiếu với hướng lắp vật lý thật), thiết bị có encoder
|
||||
(`NumEncoders > 0`, nhánh `next()`×2 chưa từng chạy qua vì test thực tế không
|
||||
có encoder), và nhánh 8-bit channel (`Num8BitChannels`, thiết bị test chỉ
|
||||
dùng kênh 16-bit). `MODEL_SICK_TIM5XX`/`MODEL_SICK_TIM571` (FOV/range theo
|
||||
datasheet) cũng chưa test trên phần cứng — chỉ `MODEL_SICK_TIM7XX` đã chạy
|
||||
thật.
|
||||
**nanoScan3 (`NanoScanDriver`)** — UDP receiver thụ động: chỉ bind cổng và
|
||||
parse datagram; **đích UDP phải cấu hình sẵn trong SICK Safety Designer**
|
||||
(driver không bắt tay CoLa2). Layout port từ `sick_safetyscanners` (Apache-2.0).
|
||||
**Chưa verify phần cứng thật** — mới test bằng gói tổng hợp qua loopback.
|
||||
|
||||
`SickDriver` nối vào `config.json` qua field `LidarConfig::brand` (`"OLEI"`
|
||||
hoặc `"SICK"`, mặc định `"OLEI"`): `lidarlib::make_lidar()` thấy `brand=="SICK"` thì
|
||||
trả về `lidarlib::SickDriver` (TCP/SOPAS) thay cho `lidarlib::Driver` (UDP), cùng kiểu
|
||||
trả về `std::unique_ptr<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`
|
||||
(dùng thẳng `SickDriver` qua API C++).
|
||||
## Output
|
||||
|
||||
`ScanResult { LaserScan scan; ExtraInfo info; }` mỗi vòng quét:
|
||||
|
||||
- **`LaserScan`** — cùng field/đơn vị với ROS: `angle_min/max/increment` (rad,
|
||||
đã unwrap liên tục, không giới hạn ±π), `ranges[]` (m), `intensities[]`
|
||||
(0-255), `timestamp_ms` (đồng hồ thiết bị, 0 nếu family không có).
|
||||
`range_min/max` lấy từ `ModelConfig` (đặt sẵn, không đo mỗi scan);
|
||||
`time_increment/scan_time` luôn 0.
|
||||
- **`ExtraInfo`** — field tuỳ family: `detected_model`, `error_status` (Family A),
|
||||
`distance_scale_mm` (A/B), và các trường raw của Family C (chưa verify).
|
||||
Field thiết bị không có giữ `std::nullopt`.
|
||||
|
||||
## Cấu trúc source
|
||||
|
||||
| File | Vai trò |
|
||||
|---|---|
|
||||
| `include/lidarlib/lidar.hpp` | Data model, interface `Lidar`, driver OLEI, các `MODEL_*` OLEI |
|
||||
| `include/lidarlib/sick_lidar.hpp` | `SickDriver`, `NanoScanDriver`, các `MODEL_SICK_*` |
|
||||
| `include/lidarlib/config.hpp` | `LidarConfig`, load/save JSON, `make_lidar()` |
|
||||
| `src/olei_lidar.cpp` | Parse Family A/B/C, CRC, gom scan |
|
||||
| `src/sick_lidar.cpp` | Parse CoLa-A (TiM) + safety-data UDP (nanoScan3) |
|
||||
| `src/lidar_config.cpp` | Bảng model/brand, config JSON, factory |
|
||||
| `examples/` | Demo: 1 lidar, 2 lidar song song, SICK TiM, nanoScan3, app khung |
|
||||
|
||||
## Ghi chú
|
||||
|
||||
- Nếu port UDP đã bị app khác giữ (không bật `SO_REUSEPORT`), `open()` sẽ thất
|
||||
bại. Kiểm tra: `ss -lunp | grep 2368`.
|
||||
- `inverted` đã verify bằng sniff sống: `false` góc tăng dần, `true` góc giảm
|
||||
dần cùng bước.
|
||||
|
||||
@@ -1,19 +1,15 @@
|
||||
// example.cpp — quick try-out of the OLEI LiDAR driver
|
||||
// OLEI driver example.
|
||||
#include "lidarlib/lidar.hpp"
|
||||
#include <cstdio>
|
||||
|
||||
int main() {
|
||||
// ── pick a model ────────────────────────────────────────────────────────
|
||||
// lidarlib::Driver drv(lidarlib::MODEL_VF); // 2D 360°
|
||||
// lidarlib::Driver drv(lidarlib::MODEL_LR1F); // 2D 360°, 50m
|
||||
lidarlib::Driver drv(lidarlib::MODEL_VB); // 2D 270°
|
||||
lidarlib::Driver drv(lidarlib::MODEL_VB);
|
||||
|
||||
if (!drv.open()) {
|
||||
fprintf(stderr, "Không mở được socket\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
// ── option 1: blocking recv ─────────────────────────────────────────────
|
||||
for (int i = 0; i < 10; ++i) {
|
||||
lidarlib::ScanResult result;
|
||||
if (!drv.recv_scan(result, 2000)) {
|
||||
@@ -26,7 +22,6 @@ int main() {
|
||||
i, scan.ranges.size(), scan.timestamp_ms, info.error_status,
|
||||
info.detected_model.c_str());
|
||||
|
||||
// Print the first few points
|
||||
for (size_t j = 0; j < 20 && j < scan.ranges.size(); ++j) {
|
||||
float angle_deg = (scan.angle_min + j * scan.angle_increment) * 180.f / 3.14159265f;
|
||||
printf(" [%zu] angle=%.2f° dist=%.3fm intensity=%.0f\n",
|
||||
@@ -34,12 +29,6 @@ int main() {
|
||||
}
|
||||
}
|
||||
|
||||
// ── option 2: callback (your own loop) ──────────────────────────────────
|
||||
// drv.set_scan_callback([](const lidarlib::ScanResult& result) {
|
||||
// printf("Got scan: %zu pts\n", result.scan.ranges.size());
|
||||
// });
|
||||
// while (true) drv.spin_once();
|
||||
|
||||
drv.close();
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -1,14 +1,4 @@
|
||||
// lidar_app.cpp — headless skeleton app and integration template.
|
||||
//
|
||||
// Loads the lidar list from config.json, opens each one through the SINGLE
|
||||
// config function lidarlib::make_lidar() (no per-brand branching), then reads scans
|
||||
// on one thread per lidar and prints a one-line summary. There is no web UI:
|
||||
// edit config.json directly, or build your own GUI on top of this same API.
|
||||
//
|
||||
// What a GUI author keeps: load_config() + make_lidar() + the recv_scan() loop.
|
||||
// What a GUI author replaces: the printf() with their own rendering/persistence,
|
||||
// and save_config() to write edits back.
|
||||
//
|
||||
// Headless skeleton app: loads config.json, one reader thread per lidar.
|
||||
// ./lidar_app [config.json]
|
||||
#include "lidarlib/lidarlib.hpp"
|
||||
#include <atomic>
|
||||
@@ -23,10 +13,8 @@ namespace {
|
||||
std::atomic<bool> g_running{true};
|
||||
void on_signal(int) { g_running = false; }
|
||||
|
||||
// One reader thread per lidar. Owns the handle for its whole lifetime so the
|
||||
// per-instance receive buffers never race another thread.
|
||||
void run_lidar(lidarlib::LidarConfig cfg) {
|
||||
std::unique_ptr<lidarlib::Lidar> lidar = lidarlib::make_lidar(cfg); // the one config call
|
||||
std::unique_ptr<lidarlib::Lidar> lidar = lidarlib::make_lidar(cfg);
|
||||
if (!lidar->open()) {
|
||||
fprintf(stderr, "[%s] khong mo duoc %s %s:%u\n",
|
||||
cfg.name.c_str(), cfg.brand.c_str(), cfg.ip.c_str(), cfg.port);
|
||||
@@ -38,11 +26,9 @@ void run_lidar(lidarlib::LidarConfig cfg) {
|
||||
|
||||
while (g_running) {
|
||||
lidarlib::ScanResult result;
|
||||
if (!lidar->recv_scan(result, 1000)) continue; // timeout -> retry
|
||||
if (!lidar->recv_scan(result, 1000)) continue;
|
||||
|
||||
// Output #1: ROS-shaped LaserScan (same for every lidar)
|
||||
const lidarlib::LaserScan& scan = result.scan;
|
||||
// Output #2: ExtraInfo (fields vary by model/family)
|
||||
const lidarlib::ExtraInfo& info = result.info;
|
||||
|
||||
printf("[%s] %zu diem | ts=%u ms | model=%s | err=0x%02X\n",
|
||||
@@ -57,12 +43,12 @@ void run_lidar(lidarlib::LidarConfig cfg) {
|
||||
} // namespace
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
setvbuf(stdout, nullptr, _IOLBF, 0); // line-buffer so logs show promptly
|
||||
setvbuf(stdout, nullptr, _IOLBF, 0);
|
||||
|
||||
const std::string config_path = (argc > 1) ? argv[1] : "config.json";
|
||||
|
||||
lidarlib::Config cfg = lidarlib::load_config(config_path);
|
||||
lidarlib::save_config(config_path, cfg); // ensure the file exists & is editable
|
||||
lidarlib::save_config(config_path, cfg); // ensure the file exists
|
||||
|
||||
if (cfg.lidars.empty()) {
|
||||
fprintf(stderr, "Khong co lidar nao trong %s\n", config_path.c_str());
|
||||
|
||||
37
examples/nanoscan_example.cpp
Normal file
37
examples/nanoscan_example.cpp
Normal 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
|
||||
@@ -1,7 +1,4 @@
|
||||
// sick_example.cpp — quick try-out of the SICK TiM driver (SOPAS/CoLa-A, TCP)
|
||||
//
|
||||
// Verified against a real SICK TiM781S — see the caveat in
|
||||
// include/lidarlib/sick_lidar.hpp for exactly what was (and wasn't) confirmed.
|
||||
// SICK TiM driver example (SOPAS/CoLa-A, TCP).
|
||||
#include "lidarlib/sick_lidar.hpp"
|
||||
#include <cstdio>
|
||||
|
||||
|
||||
@@ -1,6 +1,4 @@
|
||||
// test_dual.cpp — test 2 Olei lidars (front + rear) concurrently, per appsettings.json
|
||||
// Olei-front: scan_1, DeviceIp 192.168.100.11, LocalIp 192.168.100.100, DevicePort 2368
|
||||
// Olei-rear : scan_2, DeviceIp 192.168.100.12, LocalIp 192.168.100.100, DevicePort 2369
|
||||
// Test 2 Olei lidars (front + rear) concurrently.
|
||||
#include "lidarlib/lidar.hpp"
|
||||
#include <cstdio>
|
||||
#include <thread>
|
||||
@@ -36,14 +34,7 @@ static void run_lidar(const char* tag, const lidarlib::ModelConfig& cfg,
|
||||
}
|
||||
|
||||
int main() {
|
||||
// Both front and rear are Family B in practice — front's real header
|
||||
// string is "OLELR-1BS2", rear's is "OLELR-1BS5" (verified via live UDP
|
||||
// sniff), NOT the VB (Family A) model the config name suggested. With
|
||||
// MODEL_AUTO, the driver reads the real model name from the header and
|
||||
// narrows the FOV when it matches a known entry in kModelTable
|
||||
// (olei_lidar.cpp); "1BS5" matches (→ full 360°), but "1BS2" doesn't, so
|
||||
// front currently stays at the unfiltered 360° default. Call
|
||||
// drv.detected_model() to see which name was actually read.
|
||||
// Real headers (UDP sniff): front = "OLELR-1BS2", rear = "OLELR-1BS5".
|
||||
std::thread t_front(run_lidar, "front/scan_1", lidarlib::MODEL_AUTO,
|
||||
"192.168.100.100", 2368, false, 5);
|
||||
std::thread t_rear(run_lidar, "rear/scan_2", lidarlib::MODEL_AUTO,
|
||||
|
||||
@@ -6,27 +6,29 @@
|
||||
|
||||
namespace lidarlib {
|
||||
|
||||
// ─── Settings for one lidar ─────────────────────────────────────────────────
|
||||
// `name` is the unique key used to match entries across saves (rename = old
|
||||
// key removed, new key added) — useful if your GUI reconciles a running set of
|
||||
// lidars against an edited config list.
|
||||
// Settings for one lidar. `name` is the unique key across saves.
|
||||
struct LidarConfig {
|
||||
std::string name = "lidar";
|
||||
std::string ip = "0.0.0.0";
|
||||
uint16_t port = 2368;
|
||||
std::string model = "AUTO"; // must match an entry in model_names_for_brand(brand)
|
||||
bool inverted = false; // only applies to brand "OLEI" — SickDriver has no equivalent
|
||||
std::string brand = "OLEI"; // must match an entry in brand_names() — "OLEI" or "SICK"
|
||||
std::string model = "AUTO";
|
||||
bool inverted = false; // OLEI only
|
||||
std::string brand = "OLEI"; // "OLEI" or "SICK"
|
||||
|
||||
// Output angle window (deg): scan angles are remapped onto
|
||||
// [angle_min_deg, angle_max_deg] without dropping points.
|
||||
// Defaults (±360) = off.
|
||||
float angle_min_deg = -360.f;
|
||||
float angle_max_deg = 360.f;
|
||||
|
||||
friend bool operator==(const LidarConfig& a, const LidarConfig& b) {
|
||||
return a.name == b.name && a.ip == b.ip && a.port == b.port &&
|
||||
a.model == b.model && a.inverted == b.inverted && a.brand == b.brand;
|
||||
a.model == b.model && a.inverted == b.inverted && a.brand == b.brand &&
|
||||
a.angle_min_deg == b.angle_min_deg && a.angle_max_deg == b.angle_max_deg;
|
||||
}
|
||||
friend bool operator!=(const LidarConfig& a, const LidarConfig& b) { return !(a == b); }
|
||||
};
|
||||
|
||||
// Any number of lidars — managed as a list so a consuming app/GUI can
|
||||
// add/remove entries freely instead of being locked to a fixed front/rear pair.
|
||||
struct Config {
|
||||
std::vector<LidarConfig> lidars = {
|
||||
{"front", "0.0.0.0", 2368, "AUTO", false},
|
||||
@@ -34,47 +36,25 @@ struct Config {
|
||||
};
|
||||
};
|
||||
|
||||
// Known model name -> ModelConfig (matches the constants in lidar.hpp/sick_lidar.hpp).
|
||||
// Returns nullptr if name doesn't match any entry.
|
||||
// nullptr if `name` doesn't match any known model.
|
||||
const ModelConfig* model_by_name(const std::string& name);
|
||||
|
||||
// Names accepted by model_by_name(), for populating a UI dropdown.
|
||||
const std::vector<std::string>& model_names();
|
||||
|
||||
// Brand names accepted in LidarConfig::brand ("OLEI", "SICK"), for populating
|
||||
// a UI dropdown.
|
||||
const std::vector<std::string>& brand_names();
|
||||
|
||||
// Subset of model_names() valid for a given brand (e.g. "SICK" -> the
|
||||
// MODEL_SICK_* names) — empty if `brand` doesn't match any entry in
|
||||
// brand_names(). Used to filter the model dropdown once a brand is picked,
|
||||
// and to validate that LidarConfig::model actually belongs to its brand.
|
||||
// Subset of model_names() valid for `brand`; empty if unknown.
|
||||
const std::vector<std::string>& model_names_for_brand(const std::string& brand);
|
||||
|
||||
// Load config.json at `path`. If the file doesn't exist, returns defaults
|
||||
// (and does NOT create the file — caller decides whether to save it).
|
||||
// Returns defaults if the file doesn't exist (without creating it).
|
||||
Config load_config(const std::string& path);
|
||||
|
||||
// Overwrite `path` with `cfg` serialized as JSON.
|
||||
void save_config(const std::string& path, const Config& cfg);
|
||||
|
||||
// ─── THE single config function ─────────────────────────────────────────────
|
||||
// Build a ready-to-open lidar from one LidarConfig. This is the one entry point
|
||||
// a GUI/app needs: `brand` selects the transport — exactly "SICK" → TCP, any
|
||||
// other value (incl. "OLEI", empty, or an old config without the field) → OLEI
|
||||
// UDP. `model` is resolved *for that brand*: a name that is unknown OR belongs
|
||||
// to the other brand falls back to the brand's sensible default (MODEL_AUTO for
|
||||
// OLEI, MODEL_SICK_TIM571 for SICK), so a mis-paired brand+model can't silently
|
||||
// configure the wrong driver. `inverted` applies to OLEI only. Returns a unique
|
||||
// handle to the unified Lidar interface — call ->open() then
|
||||
// ->recv_scan(out, timeout_ms) to get ScanResult { LaserScan scan; ExtraInfo
|
||||
// info; }. Never returns nullptr.
|
||||
//
|
||||
// lidarlib::LidarConfig c{"front", "192.168.1.10", 2368, "AUTO", false, "OLEI"};
|
||||
// auto lidar = lidarlib::make_lidar(c);
|
||||
// lidar->open();
|
||||
// lidarlib::ScanResult r;
|
||||
// lidar->recv_scan(r, 1000); // r.scan = LaserScan, r.info = ExtraInfo
|
||||
// Build a ready-to-open lidar from one LidarConfig — the only entry point an
|
||||
// app needs. brand "SICK" → SICK driver (model "SICK-nanoScan3" → UDP
|
||||
// NanoScanDriver, others → TCP SickDriver); anything else → OLEI UDP.
|
||||
// Unknown/cross-brand model falls back to the brand default. Never nullptr.
|
||||
std::unique_ptr<Lidar> make_lidar(const LidarConfig& cfg);
|
||||
|
||||
} // namespace lidarlib
|
||||
|
||||
@@ -7,45 +7,31 @@
|
||||
|
||||
namespace lidarlib {
|
||||
|
||||
// ─── Default output: ROS sensor_msgs/LaserScan-shaped ──────────────────────
|
||||
// Same field names/semantics as ROS's LaserScan message (radians, meters,
|
||||
// seconds) so this can be bridged into a ROS node with a near-1:1 field copy.
|
||||
// ranges[i]/intensities[i] correspond to angle = angle_min + i*angle_increment;
|
||||
// the array spans exactly one revolution (or the model's FOV window) in the
|
||||
// order the device actually swept it — angle_min/angle_max are NOT clamped to
|
||||
// [-pi,pi], they just describe whatever contiguous window this revolution
|
||||
// covered (matches how continuously-rotating lidars without a phase reset
|
||||
// behave: the starting angle drifts slightly scan to scan).
|
||||
// ROS sensor_msgs/LaserScan-shaped output (radians, meters, seconds).
|
||||
// ranges[i] is at angle_min + i*angle_increment, in sweep order.
|
||||
struct LaserScan {
|
||||
uint32_t timestamp_ms = 0; // device clock (ms since power-on); 0 if the
|
||||
// family doesn't expose one (see ExtraInfo)
|
||||
uint32_t timestamp_ms = 0; // device clock (ms); 0 if not on the wire
|
||||
float angle_min = 0.f; // rad
|
||||
float angle_max = 0.f; // rad
|
||||
float angle_increment = 0.f; // rad
|
||||
float time_increment = 0.f; // sec — device doesn't expose per-point timing, always 0
|
||||
float scan_time = 0.f; // sec — device doesn't expose per-scan timing, always 0
|
||||
float range_min = 0.f; // m — from ModelConfig, NOT measured per-scan
|
||||
float range_max = 0.f; // m — from ModelConfig, NOT measured per-scan
|
||||
float time_increment = 0.f; // sec — not exposed by devices, always 0
|
||||
float scan_time = 0.f; // sec — not exposed by devices, always 0
|
||||
float range_min = 0.f; // m — from ModelConfig, not measured
|
||||
float range_max = 0.f; // m — from ModelConfig, not measured
|
||||
std::vector<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 ─
|
||||
// Fields the protocol family doesn't carry stay unset (std::nullopt). Several
|
||||
// of these are raw, undecoded passthroughs of header bytes whose exact
|
||||
// meaning hasn't been verified against real hardware/datasheet — see comments
|
||||
// in olei_lidar.cpp next to where each is read.
|
||||
// Diagnostic/header fields; fields the family doesn't carry stay std::nullopt.
|
||||
struct ExtraInfo {
|
||||
std::string detected_model = "AUTO"; // real model name read from the packet, or "AUTO"
|
||||
uint8_t error_status = 0; // Family A only; BIT0=Monitor, BIT1=Voltage, BIT2=Temp
|
||||
uint8_t distance_scale_mm = 0; // mm/count used to decode ranges this scan (0 = not reported)
|
||||
std::string detected_model = "AUTO";
|
||||
uint8_t error_status = 0; // Family A: BIT0=Monitor, BIT1=Voltage, BIT2=Temp
|
||||
uint8_t distance_scale_mm = 0; // 0 = not reported
|
||||
|
||||
// Family A (0xFAF0) only — raw 16-bit "rotation info" header field,
|
||||
// meaning not decoded/verified.
|
||||
// Family A only
|
||||
std::optional<uint16_t> rotation_raw;
|
||||
|
||||
// Family C / protocol V3 (0xFEAC, GS1-5) only — ported from the C# driver
|
||||
// header layout, NOT cross-checked against real GS1-5 hardware.
|
||||
// Family C / V3 (GS1-5) only — raw passthroughs, unverified
|
||||
std::optional<uint8_t> distance_ratio_raw;
|
||||
std::optional<uint16_t> scan_frequency_raw;
|
||||
std::optional<uint16_t> input_status;
|
||||
@@ -54,144 +40,90 @@ struct ExtraInfo {
|
||||
std::optional<uint32_t> status_flags;
|
||||
};
|
||||
|
||||
// One complete revolution, in both forms at once.
|
||||
struct ScanResult {
|
||||
LaserScan scan;
|
||||
ExtraInfo info;
|
||||
};
|
||||
|
||||
// ─── Per-model configuration ───────────────────────────────────────────────
|
||||
// scan_angle_* use the SIGNED system [-180,180]: 0 = straight ahead, + = left, - = right.
|
||||
// 360° lidars keep the full circle [-180,180]; narrow-FOV lidars (VB 270°) shrink it.
|
||||
// range_min_m/range_max_m are sensor-spec placeholders (NOT read from any
|
||||
// packet) used to fill LaserScan::range_min/range_max — adjust to the real
|
||||
// datasheet values for each model if precision matters to your consumer.
|
||||
// Per-model configuration. scan_angle_* use the signed system [-180,180]:
|
||||
// 0 = ahead, + = left, - = right. range_min/max are datasheet placeholders.
|
||||
struct ModelConfig {
|
||||
const char* name;
|
||||
float scan_angle_min; // deg — VB/LR-16F: -135, 360° models: -180
|
||||
float scan_angle_max; // deg — VB/LR-16F: 135, 360° models: 180
|
||||
float scan_angle_min; // deg
|
||||
float scan_angle_max; // deg
|
||||
float range_min_m = 0.05f;
|
||||
float range_max_m = 30.f;
|
||||
// Added to the raw device angle so output 0° = ahead (LR-1F/1FMI report 0°
|
||||
// at the back: +180; SICK TiM puts the front at 90°: -90).
|
||||
float angle_offset_deg = 0.f;
|
||||
// Output remap window (see make_lidar / remap_scan_window): shifts the
|
||||
// scan's angles onto [out_angle_min, out_angle_max] without dropping points.
|
||||
bool remap_angles = false;
|
||||
float out_angle_min = 0.f; // deg
|
||||
float out_angle_max = 0.f; // deg
|
||||
};
|
||||
|
||||
// Table of known models — the driver auto-detects the packet family (A=0xFAF0 /
|
||||
// B=0xFEF0 / C=0xFEAC) per packet, so this config mainly decides the angular
|
||||
// window (FOV) that gets kept.
|
||||
inline constexpr ModelConfig MODEL_VB { "VB", -135.f, 135.f, 0.05f, 30.f }; // 2D 270°
|
||||
inline constexpr ModelConfig MODEL_VF { "VF", -180.f, 180.f, 0.05f, 30.f }; // 2D 360°
|
||||
inline constexpr ModelConfig MODEL_LR1F { "LR-1F", -180.f, 180.f, 0.05f, 50.f }; // 2D 360° 50m
|
||||
inline constexpr ModelConfig MODEL_LR1FMI { "LR-1FMI", -180.f, 180.f, 0.05f, 30.f }; // 2D 360°, 0.01°/LSB ~2400 pts/rev (Family B)
|
||||
inline constexpr ModelConfig MODEL_LR1F { "LR-1F", -180.f, 180.f, 0.05f, 50.f, 180.f }; // 2D 360° 50m; device 0° = rear
|
||||
inline constexpr ModelConfig MODEL_LR1FMI { "LR-1FMI", -180.f, 180.f, 0.05f, 30.f, 180.f }; // 2D 360° (Family B); device 0° = rear
|
||||
inline constexpr ModelConfig MODEL_LR1BS5 { "LR-1BS5", -180.f, 180.f, 0.05f, 30.f }; // 2D 360° (Family B)
|
||||
inline constexpr ModelConfig MODEL_LR16F { "LR-16F", -135.f, 135.f, 0.05f, 30.f }; // 3D 16 line
|
||||
inline constexpr ModelConfig MODEL_GS15 { "GS1-5", -180.f, 180.f, 0.05f, 30.f }; // 2D 360°
|
||||
|
||||
// Sentinel: model unknown ahead of time. Family B (0xFEF0) carries an ASCII
|
||||
// model name string in its header (e.g. "OLELR-1BS5", verified via live UDP
|
||||
// sniff) → the driver auto-detects it and narrows the FOV per the table
|
||||
// above. Family C (0xFEAC, GS1-5) is identified by magic alone. Family A has
|
||||
// no such string, so on a Family-A device MODEL_AUTO keeps the wide default
|
||||
// FOV (-180..180, no points dropped) until the user specifies a concrete model.
|
||||
// Model unknown ahead of time: Family B/C packets carry enough to auto-detect;
|
||||
// Family A doesn't, so the wide default FOV is kept.
|
||||
inline constexpr ModelConfig MODEL_AUTO { "AUTO", -180.f, 180.f, 0.05f, 30.f };
|
||||
|
||||
// callback invoked whenever a complete scan is ready — shared by every driver
|
||||
// (lidarlib::Driver, lidarlib::SickDriver) and the unified Lidar interface below.
|
||||
using ScanCallback = std::function<void(const ScanResult&)>;
|
||||
|
||||
// ─── Unified driver interface ───────────────────────────────────────────────
|
||||
// Common handle returned by lidarlib::make_lidar() (the single config function in
|
||||
// config.hpp). Both the OLEI Driver (UDP) and the SICK SickDriver (TCP) derive
|
||||
// from this, so a GUI/app can drive any supported lidar through one type and
|
||||
// never branch on brand. Every call yields the same ScanResult { LaserScan
|
||||
// scan; ExtraInfo info; } — output #1 (ROS-shaped LaserScan, identical across
|
||||
// all models) and output #2 (ExtraInfo, model-specific extra fields).
|
||||
// Unified driver interface returned by make_lidar(); OLEI and SICK drivers
|
||||
// both derive from it.
|
||||
class Lidar {
|
||||
public:
|
||||
virtual ~Lidar() = default;
|
||||
|
||||
// Open the transport (UDP socket / TCP connection) and start receiving.
|
||||
virtual bool open() = 0;
|
||||
// Close the transport.
|
||||
virtual void close() = 0;
|
||||
// Block until one full scan is received; false on error/timeout.
|
||||
// timeout_ms = 0 → block indefinitely. (No default here on purpose: the
|
||||
// concrete drivers differ — OLEI 1000 ms, SICK 2000 ms — so callers using
|
||||
// the interface must state the timeout they want.)
|
||||
// Block until one full scan; false on error/timeout. timeout_ms = 0 → block
|
||||
// indefinitely. No default on purpose: drivers differ (OLEI 1000, SICK 2000).
|
||||
virtual bool recv_scan(ScanResult& out, int timeout_ms) = 0;
|
||||
// Or set a callback and drive it from your own loop via spin_once().
|
||||
virtual void set_scan_callback(ScanCallback cb) = 0;
|
||||
// Receive + dispatch the callback once (non-owning loop step).
|
||||
virtual bool spin_once() = 0;
|
||||
// Real model name read from the packet, or the configured name if the
|
||||
// family carries none. See Driver::detected_model() for OLEI specifics.
|
||||
virtual const char* detected_model() const = 0;
|
||||
};
|
||||
|
||||
// ─── Driver ─────────────────────────────────────────────────────────────────
|
||||
// OLEI UDP driver.
|
||||
class Driver : public Lidar {
|
||||
public:
|
||||
// callback invoked whenever a complete scan is ready
|
||||
using ScanCallback = lidarlib::ScanCallback;
|
||||
|
||||
// ip : receiving host's bind address, usually "0.0.0.0"
|
||||
// port : UDP port the lidar sends to (default 2368)
|
||||
// cfg : model config
|
||||
// inverted : set true if this physical unit is mounted upside-down
|
||||
// (flipped 180° about its forward-facing axis). Mirrors every
|
||||
// point's angle (angle = -angle) so output stays in the
|
||||
// vehicle's frame regardless of mounting orientation — useful
|
||||
// when e.g. front is mounted normally but rear is flipped.
|
||||
// ip: local bind address; port: UDP port the lidar sends to;
|
||||
// inverted: unit mounted upside-down → mirror every angle.
|
||||
explicit Driver(const ModelConfig& cfg,
|
||||
const std::string& ip = "0.0.0.0",
|
||||
uint16_t port = 2368,
|
||||
bool inverted = false);
|
||||
~Driver();
|
||||
|
||||
// Non-copyable
|
||||
Driver(const Driver&) = delete;
|
||||
Driver& operator=(const Driver&) = delete;
|
||||
|
||||
// Open the socket and start receiving
|
||||
bool open() override;
|
||||
|
||||
// Close the socket
|
||||
void close() override;
|
||||
|
||||
// Blocks until a full revolution has been received; returns false on error/timeout
|
||||
// timeout_ms = 0 → block indefinitely
|
||||
bool recv_scan(ScanResult& out, int timeout_ms = 1000) override;
|
||||
|
||||
// Or use the callback (drive it from your own non-blocking loop)
|
||||
void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
|
||||
|
||||
// Receive + dispatch callback (call from your own loop)
|
||||
bool spin_once() override;
|
||||
|
||||
// The REAL model name read from the Family B/C header (only meaningful
|
||||
// when the Driver was constructed with MODEL_AUTO). Always the actual
|
||||
// string found in the packet (e.g. "OLELR-1BS2"), even when that model
|
||||
// has no specific FOV entry in the table (FOV then stays at the 360°
|
||||
// default). Returns "AUTO" if no Family B/C packet has been seen yet.
|
||||
// Mirrored per-scan in ScanResult::info::detected_model.
|
||||
// Model name read from the Family B/C header; "AUTO" until one is seen.
|
||||
const char* detected_model() const override { return detected_model_name_.c_str(); }
|
||||
|
||||
private:
|
||||
// ── parse Family A packet (ID=0xFAF0): 20B header, 3B block ──
|
||||
bool parse_family_a(const uint8_t* buf, int len);
|
||||
bool parse_family_a(const uint8_t* buf, int len); // ID=0xFAF0
|
||||
bool parse_family_b(const uint8_t* buf, int len); // ID=0xFEF0
|
||||
bool parse_family_c(const uint8_t* buf, int len); // Magic=0xFEAC (GS1-5)
|
||||
|
||||
// ── parse Family B packet (ID=0xFEF0): 40B header, 8B block ──
|
||||
bool parse_family_b(const uint8_t* buf, int len);
|
||||
|
||||
// ── parse Family C / protocol V3 packet (Magic=0xFEAC, GS1-5): 48B header ──
|
||||
bool parse_family_c(const uint8_t* buf, int len);
|
||||
|
||||
// Appends one point's angle (already signed+inverted+FOV-filtered by the
|
||||
// caller), unwrapping it against the previous point in this revolution so
|
||||
// the accumulated sequence stays continuous across the ±180° seam instead
|
||||
// of jumping — required for LaserScan::angle_min/angle_max/ranges to stay
|
||||
// monotonic for 360° devices.
|
||||
void push_point(float signed_angle_deg, float dist_m, uint8_t intensity);
|
||||
|
||||
// Once a full revolution is ready → flush into ready_result_ and fire the callback
|
||||
void flush_scan();
|
||||
|
||||
ModelConfig cfg_;
|
||||
@@ -201,27 +133,22 @@ private:
|
||||
int sock_fd_ = -1;
|
||||
ScanCallback cb_;
|
||||
|
||||
// Per-revolution accumulation buffers (parallel arrays, index-aligned)
|
||||
std::vector<float> pending_angle_deg_; // unwrapped, continuous
|
||||
// Per-revolution accumulation buffers (index-aligned)
|
||||
std::vector<float> pending_angle_deg_;
|
||||
std::vector<float> pending_dist_m_;
|
||||
std::vector<uint8_t> pending_intensity_;
|
||||
uint32_t pending_ts_ = 0;
|
||||
uint8_t pending_err_ = 0;
|
||||
float last_angle_ = -1.f; // wrap-around (revolution-boundary) detection, device space [0,360)
|
||||
float last_angle_ = -1.f; // wrap detection, device space [0,360)
|
||||
|
||||
// Per-revolution ExtraInfo accumulation — overwritten as packets for the
|
||||
// in-progress revolution are parsed, then copied into ready_result_ on flush.
|
||||
ExtraInfo pending_info_;
|
||||
|
||||
// recv_scan()'s output, gated by a simple ready flag
|
||||
ScanResult ready_result_;
|
||||
bool scan_ready_ = false;
|
||||
|
||||
// Per-instance receive buffer — NOT static, so that 2 lidars running on 2
|
||||
// threads don't overwrite each other's data (data race).
|
||||
// Per-instance so two drivers on two threads don't race.
|
||||
uint8_t recv_buf_[4096];
|
||||
|
||||
// Model auto-detection from the Family B/C header (see MODEL_AUTO)
|
||||
bool auto_detect_ = false;
|
||||
bool model_locked_ = false;
|
||||
std::string detected_model_name_ = "AUTO";
|
||||
|
||||
@@ -1,13 +1,5 @@
|
||||
#pragma once
|
||||
// ─── One-include convenience header ─────────────────────────────────────────
|
||||
// Pull in the whole public API in a single line:
|
||||
//
|
||||
// #include "lidarlib/lidarlib.hpp"
|
||||
//
|
||||
// Gives you the data model (LaserScan / ExtraInfo / ScanResult), the unified
|
||||
// Lidar interface, both concrete drivers (Driver = OLEI/UDP, SickDriver =
|
||||
// SICK/TCP), the model/brand tables, config.json load/save, and the single
|
||||
// config function lidarlib::make_lidar().
|
||||
// One-include convenience header for the whole public API.
|
||||
#include "lidarlib/lidar.hpp"
|
||||
#include "lidarlib/sick_lidar.hpp"
|
||||
#include "lidarlib/config.hpp"
|
||||
|
||||
@@ -1,45 +1,25 @@
|
||||
#pragma once
|
||||
#include "lidarlib/lidar.hpp"
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace lidarlib {
|
||||
|
||||
// ─── SICK TiM5xx/7xx model presets ──────────────────────────────────────────
|
||||
// FOV/range taken from SICK's public datasheets (NOT read from any packet —
|
||||
// same placeholder convention as the OLEI ModelConfig constants in lidar.hpp).
|
||||
// Scanning angle is 270° on every TiM5xx/7xx variant; only the rated range
|
||||
// differs by model.
|
||||
inline constexpr ModelConfig MODEL_SICK_TIM5XX { "SICK-TIM5xx", -135.f, 135.f, 0.05f, 10.f }; // TiM551/561, 270°, 10m
|
||||
inline constexpr ModelConfig MODEL_SICK_TIM571 { "SICK-TIM571", -135.f, 135.f, 0.05f, 25.f }; // TiM571, 270°, 25m
|
||||
inline constexpr ModelConfig MODEL_SICK_TIM7XX { "SICK-TIM7xx", -135.f, 135.f, 0.05f, 25.f }; // TiM781, 270°, 25m
|
||||
// SICK TiM presets. FOV/range from datasheets; scan_angle_* are informational
|
||||
// only and do NOT filter points. angle_offset_deg = -90 because the TiM wire
|
||||
// frame puts 90° at the device front.
|
||||
inline constexpr ModelConfig MODEL_SICK_TIM5XX { "SICK-TIM5xx", -135.f, 135.f, 0.05f, 10.f, -90.f }; // TiM551/561, 270°, 10m
|
||||
inline constexpr ModelConfig MODEL_SICK_TIM571 { "SICK-TIM571", -135.f, 135.f, 0.05f, 25.f, -90.f }; // TiM571, 270°, 25m
|
||||
inline constexpr ModelConfig MODEL_SICK_TIM7XX { "SICK-TIM7xx", -135.f, 135.f, 0.05f, 25.f, -90.f }; // TiM781, 270°, 25m
|
||||
|
||||
// ─── SickDriver — SICK TiM5xx/7xx over SOPAS/CoLa-A (TCP, default port 2111) ─
|
||||
//
|
||||
// VERIFIED against a real SICK TiM781S (FW V5.11-14.10.24, MODEL_SICK_TIM7XX)
|
||||
// on port 2111: 811 pts/scan over -45..225° (270° FOV), angle_increment =
|
||||
// 0.333° (matches the 781's rated angular resolution), stable ranges, and a
|
||||
// non-zero RSSI1 channel — confirms the DIST1/RSSI1 channel layout, the
|
||||
// IEEE-754 scaling factor, and the start-angle/step-width decode in
|
||||
// parse_lmdscandata() (sick_lidar.cpp) are correct on real hardware, not just
|
||||
// per SICK's "Telegram Listing" doc.
|
||||
// NOT yet verified: the angle-zero reference vs physical mounting direction,
|
||||
// devices reporting NumEncoders > 0, the 8-bit-channel branch (the test unit
|
||||
// only emitted 16-bit channels), and MODEL_SICK_TIM5XX/MODEL_SICK_TIM571's
|
||||
// FOV/range numbers (only TiM7xx was tested).
|
||||
//
|
||||
// Protocol differences from lidarlib::Driver that justify a separate class
|
||||
// instead of extending Driver:
|
||||
// - Transport is TCP (a connection, request/response + streamed telegrams),
|
||||
// not connectionless UDP broadcast.
|
||||
// - Telegrams are ASCII (CoLa-A), framed by STX(0x02)/ETX(0x03) instead of
|
||||
// the OLEI binary header+block layout — no CRC32 like Family A.
|
||||
// - The device is passive until told to start: must send "sEN LMDscandata 1"
|
||||
// before any scan telegram arrives.
|
||||
// SICK TiM5xx/7xx over SOPAS/CoLa-A (TCP, default port 2111).
|
||||
// Verified against a real TiM781S (FW V5.11). NOT verified: NumEncoders > 0,
|
||||
// the 8-bit channel branch, and the TIM5xx/TIM571 FOV/range numbers.
|
||||
class SickDriver : public Lidar {
|
||||
public:
|
||||
using ScanCallback = lidarlib::ScanCallback; // same callback shape, ScanResult-compatible
|
||||
using ScanCallback = lidarlib::ScanCallback;
|
||||
|
||||
// ip/port: SICK device's TCP endpoint (SOPAS default port 2111).
|
||||
explicit SickDriver(const ModelConfig& cfg,
|
||||
const std::string& ip,
|
||||
uint16_t port = 2111);
|
||||
@@ -50,30 +30,17 @@ public:
|
||||
|
||||
// Connect + send "sEN LMDscandata 1" to start continuous scan output.
|
||||
bool open() override;
|
||||
|
||||
// Best-effort "sEN LMDscandata 0" then close the socket.
|
||||
void close() override;
|
||||
|
||||
// Blocks until one LMDscandata telegram has been parsed; returns false on
|
||||
// error/timeout. timeout_ms = 0 → block indefinitely.
|
||||
bool recv_scan(ScanResult& out, int timeout_ms = 2000) override;
|
||||
|
||||
// Or use the callback (drive it from your own non-blocking loop)
|
||||
void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
|
||||
|
||||
// Receive + parse + dispatch callback (call from your own loop)
|
||||
bool spin_once() override;
|
||||
|
||||
// SICK telegrams carry no model string — returns the configured model name
|
||||
// (e.g. "SICK-TIM7xx") so the unified Lidar interface stays consistent.
|
||||
// Backed by an owned std::string (not cfg_.name, a borrowed const char*) so
|
||||
// the pointer stays valid even if the ModelConfig was built from temporary
|
||||
// storage — matching Driver::detected_model()'s ownership.
|
||||
// No model string on the wire — returns the configured name.
|
||||
const char* detected_model() const override { return detected_model_name_.c_str(); }
|
||||
|
||||
private:
|
||||
bool send_telegram(const std::string& body); // wraps body with STX/ETX, writes to socket
|
||||
bool read_telegram(std::string& out, int timeout_ms); // returns next STX..ETX frame, STX/ETX stripped
|
||||
bool send_telegram(const std::string& body);
|
||||
bool read_telegram(std::string& out, int timeout_ms);
|
||||
bool parse_lmdscandata(const std::string& telegram, ScanResult& out);
|
||||
|
||||
ModelConfig cfg_;
|
||||
@@ -83,9 +50,51 @@ private:
|
||||
int sock_fd_ = -1;
|
||||
ScanCallback cb_;
|
||||
|
||||
// Accumulates bytes read from the TCP stream between telegram boundaries —
|
||||
// per-instance (not static) so 2 SickDrivers on 2 threads don't race.
|
||||
// Leftover TCP bytes carried across telegram boundaries; per-instance.
|
||||
std::string recv_buf_;
|
||||
};
|
||||
|
||||
inline constexpr ModelConfig MODEL_SICK_NANOSCAN3 { "SICK-nanoScan3", -137.5f, 137.5f, 0.05f, 40.f };
|
||||
|
||||
// SICK nanoScan3 / microScan3 safety-scanner binary UDP output. Layout ported
|
||||
// from SICK's open-source sick_safetyscanners; NOT verified on real hardware.
|
||||
// Passive UDP receiver: the sensor's UDP target must be configured up front in
|
||||
// SICK Safety Designer — this class does no CoLa2/TCP handshake.
|
||||
class NanoScanDriver : public Lidar {
|
||||
public:
|
||||
using ScanCallback = lidarlib::ScanCallback;
|
||||
|
||||
// ip: local bind address; port: local UDP port the sensor sends to.
|
||||
explicit NanoScanDriver(const ModelConfig& cfg,
|
||||
const std::string& ip = "0.0.0.0",
|
||||
uint16_t port = 6060);
|
||||
~NanoScanDriver();
|
||||
|
||||
NanoScanDriver(const NanoScanDriver&) = delete;
|
||||
NanoScanDriver& operator=(const NanoScanDriver&) = delete;
|
||||
|
||||
bool open() override;
|
||||
void close() override;
|
||||
bool recv_scan(ScanResult& out, int timeout_ms = 1000) override;
|
||||
void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
|
||||
bool spin_once() override;
|
||||
|
||||
// No model string on the wire — returns the configured name.
|
||||
const char* detected_model() const override { return detected_model_name_.c_str(); }
|
||||
|
||||
private:
|
||||
int recv_datagram(int timeout_ms);
|
||||
bool parse_packet(const uint8_t* buf, int len, ScanResult& out);
|
||||
|
||||
ModelConfig cfg_;
|
||||
std::string detected_model_name_; // owned copy of cfg_.name (stable lifetime)
|
||||
std::string ip_;
|
||||
uint16_t port_;
|
||||
int sock_fd_ = -1;
|
||||
ScanCallback cb_;
|
||||
|
||||
// Per-instance; sized for a full safety-data packet (max ~2751 beams).
|
||||
std::vector<uint8_t> recv_buf_;
|
||||
};
|
||||
|
||||
} // namespace lidarlib
|
||||
|
||||
@@ -1,6 +1,4 @@
|
||||
// json_mini.hpp — minimal header-only JSON parse/serialize, just enough for
|
||||
// flat-ish config objects (no comments, no streaming, no error recovery).
|
||||
// Not a general-purpose JSON library — kept tiny on purpose.
|
||||
// Minimal header-only JSON parse/serialize, just enough for config objects.
|
||||
#pragma once
|
||||
#include <string>
|
||||
#include <vector>
|
||||
@@ -104,7 +102,6 @@ private:
|
||||
}
|
||||
};
|
||||
|
||||
// ── Parser ──────────────────────────────────────────────────────────────
|
||||
class ParseError : public std::runtime_error {
|
||||
public:
|
||||
explicit ParseError(const std::string& what) : std::runtime_error(what) {}
|
||||
|
||||
42
src/lidar_bytes.hpp
Normal file
42
src/lidar_bytes.hpp
Normal 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
|
||||
@@ -25,6 +25,7 @@ constexpr ModelEntry kModels[] = {
|
||||
{ "SICK-TIM5xx", &MODEL_SICK_TIM5XX, "SICK" },
|
||||
{ "SICK-TIM571", &MODEL_SICK_TIM571, "SICK" },
|
||||
{ "SICK-TIM7xx", &MODEL_SICK_TIM7XX, "SICK" },
|
||||
{ "SICK-nanoScan3", &MODEL_SICK_NANOSCAN3, "SICK" },
|
||||
};
|
||||
|
||||
json::Value to_json(const LidarConfig& c) {
|
||||
@@ -35,6 +36,8 @@ json::Value to_json(const LidarConfig& c) {
|
||||
v.set("brand", json::Value::make_string(c.brand));
|
||||
v.set("model", json::Value::make_string(c.model));
|
||||
v.set("inverted", json::Value::make_bool(c.inverted));
|
||||
v.set("angle_min_deg", json::Value::make_number(c.angle_min_deg));
|
||||
v.set("angle_max_deg", json::Value::make_number(c.angle_max_deg));
|
||||
return v;
|
||||
}
|
||||
|
||||
@@ -46,6 +49,8 @@ LidarConfig lidar_from_json(const json::Value& v, const LidarConfig& def) {
|
||||
c.brand = v.get_string("brand", def.brand);
|
||||
c.model = v.get_string("model", def.model);
|
||||
c.inverted = v.get_bool("inverted", def.inverted);
|
||||
c.angle_min_deg = static_cast<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;
|
||||
}
|
||||
|
||||
@@ -58,10 +63,6 @@ const ModelConfig* model_by_name(const std::string& name) {
|
||||
}
|
||||
|
||||
namespace {
|
||||
// Like model_by_name() but only accepts a model that actually belongs to
|
||||
// `brand` — so a mis-paired brand+model (e.g. brand="OLEI", model="SICK-TIM571")
|
||||
// doesn't resolve to the other brand's preset. Returns nullptr if the name
|
||||
// isn't a valid model for that brand.
|
||||
const ModelConfig* model_by_name_for_brand(const std::string& name, const std::string& brand) {
|
||||
for (const auto& e : kModels)
|
||||
if (name == e.name && brand == e.brand) return e.cfg;
|
||||
@@ -101,7 +102,7 @@ const std::vector<std::string>& model_names_for_brand(const std::string& brand)
|
||||
}
|
||||
|
||||
Config load_config(const std::string& path) {
|
||||
Config cfg; // defaults
|
||||
Config cfg;
|
||||
std::ifstream f(path);
|
||||
if (!f) return cfg;
|
||||
|
||||
@@ -111,7 +112,7 @@ Config load_config(const std::string& path) {
|
||||
try {
|
||||
root = json::parse(ss.str());
|
||||
} catch (const json::ParseError&) {
|
||||
return cfg; // malformed file -> fall back to defaults rather than crash
|
||||
return cfg; // malformed file -> defaults
|
||||
}
|
||||
|
||||
const json::Value* lidars = root.find("lidars");
|
||||
@@ -136,21 +137,25 @@ void save_config(const std::string& path, const Config& cfg) {
|
||||
}
|
||||
|
||||
std::unique_ptr<Lidar> make_lidar(const LidarConfig& cfg) {
|
||||
// Anything other than the exact string "SICK" is treated as OLEI (this also
|
||||
// keeps old config.json files without a `brand` field working).
|
||||
// Anything but the exact string "SICK" is OLEI (keeps brand-less configs working).
|
||||
const bool is_sick = (cfg.brand == "SICK");
|
||||
|
||||
// Resolve the model *for this brand*: an unknown/empty name, OR a name that
|
||||
// belongs to the other brand, falls back to the brand default (OLEI
|
||||
// auto-detects from the packet; SICK has no model string in the wire
|
||||
// protocol so we pick a mid-range preset). This prevents a mis-paired
|
||||
// brand+model from silently configuring the wrong driver/FOV.
|
||||
const ModelConfig* model = model_by_name_for_brand(cfg.model, is_sick ? "SICK" : "OLEI");
|
||||
if (!model) model = is_sick ? &MODEL_SICK_TIM571 : &MODEL_AUTO;
|
||||
|
||||
if (is_sick)
|
||||
return std::make_unique<SickDriver>(*model, cfg.ip, cfg.port);
|
||||
return std::make_unique<Driver>(*model, cfg.ip, cfg.port, cfg.inverted);
|
||||
ModelConfig mc = *model;
|
||||
if (cfg.angle_min_deg > -360.f || cfg.angle_max_deg < 360.f) {
|
||||
mc.remap_angles = true;
|
||||
mc.out_angle_min = cfg.angle_min_deg;
|
||||
mc.out_angle_max = cfg.angle_max_deg;
|
||||
}
|
||||
|
||||
if (is_sick) {
|
||||
if (model == &MODEL_SICK_NANOSCAN3)
|
||||
return std::make_unique<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
|
||||
@@ -1,4 +1,5 @@
|
||||
#include "lidarlib/lidar.hpp"
|
||||
#include "lidar_bytes.hpp"
|
||||
|
||||
#include <cstring>
|
||||
#include <cmath>
|
||||
@@ -10,40 +11,19 @@
|
||||
|
||||
namespace lidarlib {
|
||||
|
||||
namespace {
|
||||
constexpr float kDeg2Rad = 3.14159265358979323846f / 180.f;
|
||||
}
|
||||
|
||||
// ── Little-endian helpers ────────────────────────────────────────────────────
|
||||
static inline uint16_t le16(const uint8_t* p) {
|
||||
return static_cast<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.
|
||||
// Normalize into (-180, 180]: 0 = ahead, + = left, - = right.
|
||||
static inline float to_signed_deg(float deg) {
|
||||
deg = std::fmod(deg, 360.f);
|
||||
if (deg < 0.f) deg += 360.f; // → [0,360)
|
||||
if (deg > 180.f) deg -= 360.f; // → (-180,180]
|
||||
if (deg < 0.f) deg += 360.f;
|
||||
if (deg > 180.f) deg -= 360.f;
|
||||
return deg;
|
||||
}
|
||||
|
||||
// Mirror the angle when the unit is mounted upside-down (flipped 180° about
|
||||
// its forward axis), so output angle stays correct relative to the vehicle
|
||||
// frame regardless of physical mounting. Must run AFTER to_signed_deg() and
|
||||
// BEFORE the FOV filter, since the FOV window is defined in vehicle frame.
|
||||
static inline float maybe_invert(float signed_deg, bool inverted) {
|
||||
return inverted ? to_signed_deg(-signed_deg) : signed_deg;
|
||||
}
|
||||
|
||||
// ── CRC32 (poly 0x04C11DB7, MSB-first) ──────────────────────────────────────
|
||||
// CRC32 poly 0x04C11DB7, MSB-first
|
||||
static uint32_t crc32_olei(const uint8_t* data, size_t len) {
|
||||
uint32_t crc = 0xFFFFFFFF;
|
||||
for (size_t i = 0; i < len; ++i) {
|
||||
@@ -54,12 +34,10 @@ static uint32_t crc32_olei(const uint8_t* data, size_t len) {
|
||||
return crc;
|
||||
}
|
||||
|
||||
// ── Frame IDs ────────────────────────────────────────────────────────────────
|
||||
static constexpr uint16_t FRAME_ID_A = 0xFAF0; // 2D Ethernet (VB, VF, LR-1F)
|
||||
static constexpr uint16_t FRAME_ID_B = 0xFEF0; // LR-1BS5 / LR-1BS2 Ethernet variant
|
||||
static constexpr uint16_t FRAME_ID_C = 0xFEAC; // Protocol V3 (GS1-5)
|
||||
|
||||
// ─── Constructor / Destructor ────────────────────────────────────────────────
|
||||
Driver::Driver(const ModelConfig& cfg, const std::string& ip, uint16_t port, bool inverted)
|
||||
: cfg_(cfg), ip_(ip), port_(port), inverted_(inverted)
|
||||
{
|
||||
@@ -68,14 +46,10 @@ Driver::Driver(const ModelConfig& cfg, const std::string& ip, uint16_t port, boo
|
||||
|
||||
Driver::~Driver() { close(); }
|
||||
|
||||
// ─── open() ─────────────────────────────────────────────────────────────────
|
||||
bool Driver::open() {
|
||||
sock_fd_ = ::socket(AF_INET, SOCK_DGRAM, 0);
|
||||
if (sock_fd_ < 0) return false;
|
||||
|
||||
// Allow multiple sockets to bind the same port (run alongside another
|
||||
// app / debugging). SO_REUSEPORT lets several listeners receive the same
|
||||
// UDP stream — only works if EVERY socket on that port sets this flag.
|
||||
int reuse = 1;
|
||||
::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));
|
||||
#ifdef SO_REUSEPORT
|
||||
@@ -98,7 +72,6 @@ bool Driver::open() {
|
||||
return true;
|
||||
}
|
||||
|
||||
// ─── close() ────────────────────────────────────────────────────────────────
|
||||
void Driver::close() {
|
||||
if (sock_fd_ >= 0) {
|
||||
::close(sock_fd_);
|
||||
@@ -106,7 +79,6 @@ void Driver::close() {
|
||||
}
|
||||
}
|
||||
|
||||
// ─── recv_scan() — blocks until one full revolution is available ──────────
|
||||
bool Driver::recv_scan(ScanResult& out, int timeout_ms) {
|
||||
scan_ready_ = false;
|
||||
|
||||
@@ -115,7 +87,7 @@ bool Driver::recv_scan(ScanResult& out, int timeout_ms) {
|
||||
fd_set fds; FD_ZERO(&fds); FD_SET(sock_fd_, &fds);
|
||||
timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 };
|
||||
int r = ::select(sock_fd_ + 1, &fds, nullptr, nullptr, &tv);
|
||||
if (r <= 0) return false; // timeout or error
|
||||
if (r <= 0) return false;
|
||||
}
|
||||
if (!spin_once()) return false;
|
||||
}
|
||||
@@ -123,10 +95,7 @@ bool Driver::recv_scan(ScanResult& out, int timeout_ms) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// ─── spin_once() ────────────────────────────────────────────────────────────
|
||||
bool Driver::spin_once() {
|
||||
// buf is the recv_buf_ member, NOT static → each Driver has its own
|
||||
// memory, safe when 2 lidars receive concurrently on 2 threads.
|
||||
uint8_t* buf = recv_buf_;
|
||||
sockaddr_in from{};
|
||||
socklen_t fromlen = sizeof(from);
|
||||
@@ -135,28 +104,19 @@ bool Driver::spin_once() {
|
||||
reinterpret_cast<sockaddr*>(&from), &fromlen);
|
||||
if (n < 0) return false;
|
||||
|
||||
// Distinguish protocol family by Frame ID (little-endian)
|
||||
// Family A / C: Frame ID / magic sits right at bytes [0-1]
|
||||
// Family B: has a 0x010F preamble at bytes [0-1], real Frame ID at bytes [2-3]
|
||||
if (n < 4) return true; // too short, skip
|
||||
uint16_t id_at_0 = le16(buf); // Family A (0xFAF0) or Family C (0xFEAC)
|
||||
uint16_t frame_id_b = le16(buf + 2); // Family B: preamble 0x010F + real id at [2-3]
|
||||
// A/C carry the frame id at [0-1]; B has a 0x010F preamble, real id at [2-3].
|
||||
if (n < 4) return true;
|
||||
uint16_t id_at_0 = le16(buf);
|
||||
uint16_t frame_id_b = le16(buf + 2);
|
||||
|
||||
if (id_at_0 == FRAME_ID_A) parse_family_a(buf, static_cast<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: unknown family (3D LR-16F uses a different format, extend later)
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// ─── push_point() — append with angle-unwrapping ───────────────────────────
|
||||
// `signed_angle_deg` is already signed+inverted+FOV-filtered by the caller.
|
||||
// Unwrapping against the previous point (rather than re-deriving from device
|
||||
// raw angle) keeps this identical for all 3 families and survives the ±180°
|
||||
// seam: a 360° device's points cross from +179.x to -179.x mid-revolution in
|
||||
// the signed system, which push_point() turns back into a continuous ramp so
|
||||
// LaserScan::angle_min/angle_max/ranges stay meaningful (monotonic, ROS-style).
|
||||
// Append with angle-unwrapping so the ±180° seam stays a continuous ramp.
|
||||
void Driver::push_point(float signed_angle_deg, float dist_m, uint8_t intensity) {
|
||||
float angle = signed_angle_deg;
|
||||
if (!pending_angle_deg_.empty()) {
|
||||
@@ -169,7 +129,6 @@ void Driver::push_point(float signed_angle_deg, float dist_m, uint8_t intensity)
|
||||
pending_intensity_.push_back(intensity);
|
||||
}
|
||||
|
||||
// ─── flush_scan() — a revolution is complete ───────────────────────────────
|
||||
void Driver::flush_scan() {
|
||||
if (pending_angle_deg_.empty()) return;
|
||||
|
||||
@@ -181,13 +140,16 @@ void Driver::flush_scan() {
|
||||
scan.angle_max = pending_angle_deg_.back() * kDeg2Rad;
|
||||
scan.angle_increment = (n > 1)
|
||||
? (scan.angle_max - scan.angle_min) / static_cast<float>(n - 1) : 0.f;
|
||||
scan.time_increment = 0.f; // device doesn't expose per-point timing
|
||||
scan.scan_time = 0.f; // device doesn't expose per-scan timing
|
||||
scan.time_increment = 0.f;
|
||||
scan.scan_time = 0.f;
|
||||
scan.range_min = cfg_.range_min_m;
|
||||
scan.range_max = cfg_.range_max_m;
|
||||
scan.ranges.assign(pending_dist_m_.begin(), pending_dist_m_.end());
|
||||
scan.intensities.assign(pending_intensity_.begin(), pending_intensity_.end());
|
||||
|
||||
if (cfg_.remap_angles)
|
||||
remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max);
|
||||
|
||||
ExtraInfo& info = ready_result_.info;
|
||||
info = pending_info_;
|
||||
info.detected_model = detected_model_name_;
|
||||
@@ -196,63 +158,42 @@ void Driver::flush_scan() {
|
||||
pending_angle_deg_.clear();
|
||||
pending_dist_m_.clear();
|
||||
pending_intensity_.clear();
|
||||
pending_info_ = ExtraInfo{}; // reset per-revolution optional fields
|
||||
pending_info_ = ExtraInfo{};
|
||||
scan_ready_ = true;
|
||||
|
||||
if (cb_) cb_(ready_result_);
|
||||
}
|
||||
|
||||
// ─── parse_family_a() ───────────────────────────────────────────────────────
|
||||
// 20-byte header:
|
||||
// [0-1] Frame ID = 0xFAF0
|
||||
// [2-3] Protocol = 0x0200
|
||||
// [4] Distance scale (mm/count)
|
||||
// [5] Error status
|
||||
// [6] Start angle (deg, uint8)
|
||||
// [7] End angle (deg, uint8, exclusive)
|
||||
// [8-9] Num points (uint16 LE)
|
||||
// [10-11] Rotation info — raw, undecoded (exposed as ExtraInfo::rotation_raw)
|
||||
// [12-15] Timestamp (uint32 LE, ms)
|
||||
// [16-19] CRC32 of the block data
|
||||
// 3-byte block × N:
|
||||
// [0-1] Distance readout (uint16 LE)
|
||||
// [2] Intensity (uint8)
|
||||
// Family A (0xFAF0): 20B header + 3B blocks (u16 dist, u8 intensity).
|
||||
bool Driver::parse_family_a(const uint8_t* buf, int len) {
|
||||
static constexpr int HEADER_LEN = 20;
|
||||
static constexpr int BLOCK_LEN = 3;
|
||||
|
||||
if (len < HEADER_LEN) return false;
|
||||
|
||||
// ── read header ──
|
||||
// uint16_t protocol = le16(buf + 2); // 0x0200
|
||||
uint8_t dist_scale = buf[4]; // mm per count
|
||||
uint8_t err_status = buf[5];
|
||||
float ang_start = static_cast<float>(buf[6]);
|
||||
// float ang_end = static_cast<float>(buf[7]); // exclusive
|
||||
uint16_t num_pts = le16(buf + 8);
|
||||
uint16_t rotation_raw = le16(buf + 10);
|
||||
uint32_t timestamp = le32(buf + 12);
|
||||
uint32_t crc_packet = le32(buf + 16);
|
||||
|
||||
// ── verify CRC (optional but recommended) ──
|
||||
int block_bytes = len - HEADER_LEN;
|
||||
if (block_bytes < num_pts * BLOCK_LEN) return false; // truncated packet
|
||||
if (block_bytes < num_pts * BLOCK_LEN) return false;
|
||||
|
||||
uint32_t crc_calc = crc32_olei(buf + HEADER_LEN, static_cast<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) {
|
||||
flush_scan();
|
||||
}
|
||||
|
||||
// ── decode points ──
|
||||
pending_ts_ = timestamp;
|
||||
pending_err_ = err_status;
|
||||
pending_info_.distance_scale_mm = dist_scale;
|
||||
pending_info_.rotation_raw = rotation_raw;
|
||||
|
||||
// scale=0 means the firmware didn't report it → default to 1 mm/count to avoid dist=0.
|
||||
const float scale_mm = (dist_scale ? static_cast<float>(dist_scale) : 1.f);
|
||||
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);
|
||||
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 angle = to_signed_deg(ang_start + frac * (ang_end - ang_start));
|
||||
float angle = to_signed_deg(ang_start + frac * (ang_end - ang_start) + cfg_.angle_offset_deg);
|
||||
angle = maybe_invert(angle, inverted_);
|
||||
|
||||
// Filter out anything outside the model's FOV (already in the signed -180…180 system)
|
||||
if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue;
|
||||
|
||||
push_point(angle, dist_raw * scale_mm * 0.001f /* mm → m */, intensity);
|
||||
push_point(angle, dist_raw * scale_mm * 0.001f, intensity);
|
||||
}
|
||||
|
||||
last_angle_ = ang_start;
|
||||
return true;
|
||||
}
|
||||
|
||||
// ─── parse_family_b() ───────────────────────────────────────────────────────
|
||||
// 40-byte header:
|
||||
// [0-1] 0x010F
|
||||
// [2-3] 0xFEF0 (Frame ID)
|
||||
// [4-5] 0x0200 (Protocol)
|
||||
// [6] Distance scale
|
||||
// [7-16] Model identifier string (e.g. "OLELR-1BS5")
|
||||
// [17-39] Reserved
|
||||
// 8-byte block × N:
|
||||
// [0-1] AngleRaw (uint16 LE, × 0.01° → deg, 0–359.99); >= 0xFF00 = invalid point
|
||||
// [2-3] Distance readout (uint16 LE); meters = value × DistanceScale / 1000
|
||||
// [4-5] Signal strength (uint16 LE)
|
||||
// [6-7] Reserved
|
||||
// NOTE: this header carries no timestamp/error field, so ScanResult::scan's
|
||||
// timestamp_ms and info.error_status stay at their defaults (0) for Family B.
|
||||
// Family B (0xFEF0): 40B header (model string at [7-16]) + 8B blocks
|
||||
// (u16 angle ×0.01°, u16 dist, u16 signal). No timestamp/error on the wire.
|
||||
bool Driver::parse_family_b(const uint8_t* buf, int len) {
|
||||
static constexpr int HEADER_LEN = 40;
|
||||
static constexpr int BLOCK_LEN = 8;
|
||||
@@ -298,7 +224,6 @@ bool Driver::parse_family_b(const uint8_t* buf, int len) {
|
||||
if (len < HEADER_LEN) return false;
|
||||
|
||||
uint8_t dist_scale = buf[6];
|
||||
// scale=0 → default to 1 mm/count so distances don't collapse to zero.
|
||||
const float scale_mm = (dist_scale ? static_cast<float>(dist_scale) : 1.f);
|
||||
pending_info_.distance_scale_mm = dist_scale;
|
||||
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_.range_min_m = entry.cfg->range_min_m;
|
||||
cfg_.range_max_m = entry.cfg->range_max_m;
|
||||
cfg_.angle_offset_deg = entry.cfg->angle_offset_deg;
|
||||
break;
|
||||
}
|
||||
}
|
||||
@@ -334,26 +260,21 @@ bool Driver::parse_family_b(const uint8_t* buf, int len) {
|
||||
if (num_pts <= 0) return false;
|
||||
|
||||
const uint8_t* blk = buf + HEADER_LEN;
|
||||
// AngleRaw is 0.01°/LSB (0–359.99°), per the official Olei block spec —
|
||||
// verified against real OLELR-1FMI geometry (a 0.25° scale smears a room
|
||||
// into a circle). AngleRaw >= 0xFF00 marks an invalid point → skip it.
|
||||
// The counter resets to 0 each revolution, but one packet is only a ~22°
|
||||
// arc and the device can pack >1 revolution across packets, so the
|
||||
// revolution boundary is detected PER POINT: a >90° drop between
|
||||
// consecutive [0,360) angles ends the current revolution.
|
||||
// A packet is only a ~22° arc and may span >1 rev, so the revolution
|
||||
// boundary is detected per point: a >90° drop between consecutive angles.
|
||||
static constexpr uint16_t INVALID_ANGLE = 0xFF00;
|
||||
for (int i = 0; i < num_pts; ++i, blk += BLOCK_LEN) {
|
||||
uint16_t angle_raw = le16(blk);
|
||||
if (angle_raw >= INVALID_ANGLE) continue; // invalid point
|
||||
if (angle_raw >= INVALID_ANGLE) continue;
|
||||
|
||||
float dev_deg = std::fmod(angle_raw * 0.01f, 360.f); // [0,360)
|
||||
float dev_deg = std::fmod(angle_raw * 0.01f, 360.f);
|
||||
if (last_angle_ >= 0.f && dev_deg < last_angle_ - 90.f) {
|
||||
flush_scan(); // revolution complete
|
||||
flush_scan();
|
||||
}
|
||||
last_angle_ = dev_deg;
|
||||
|
||||
float angle = maybe_invert(to_signed_deg(angle_raw * 0.01f), inverted_); // -180…180
|
||||
float dist_m = le16(blk + 2) * scale_mm * 0.001f; // readout × scale → m
|
||||
float angle = maybe_invert(to_signed_deg(angle_raw * 0.01f + cfg_.angle_offset_deg), inverted_);
|
||||
float dist_m = le16(blk + 2) * scale_mm * 0.001f;
|
||||
uint8_t intensity = static_cast<uint8_t>(le16(blk + 4) >> 2); // 10-bit → 8-bit
|
||||
|
||||
if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue;
|
||||
@@ -364,36 +285,9 @@ bool Driver::parse_family_b(const uint8_t* buf, int len) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// ─── parse_family_c() ───────────────────────────────────────────────────────
|
||||
// Protocol V3 (Olei GS1-5, magic 0xFEAC) — ported from the existing C#
|
||||
// production driver OleiGS15Driver.cs (RobotNet10.RobotApp); NOT independently
|
||||
// sniffed/verified against real GS1-5 hardware (no device was available to
|
||||
// test this while writing the code).
|
||||
// 48-byte header:
|
||||
// [0-1] Magic = 0xFEAC
|
||||
// [2-3] Version
|
||||
// [4-7] PacketSize (uint32 LE)
|
||||
// [8-9] HeaderSize (uint16 LE, usually = 48)
|
||||
// [10] Distance ratio — read by the original C# driver but NOT applied
|
||||
// (distance is always raw mm / 1000); same behavior kept here.
|
||||
// Exposed raw as ExtraInfo::distance_ratio_raw.
|
||||
// [11] Types: 0x00=2B/point (range only), 0x01=4B/point (range+intensity),
|
||||
// 0x10=4B/point (first 2 bytes unused, range at [+2,+4))
|
||||
// [12-13] Scan number [14-15] Packet number
|
||||
// [16-19] Timestamp decimal [20-23] Timestamp integer
|
||||
// [24-25] Scan frequency raw [26-27] NumPointsScan (total points per revolution)
|
||||
// [28-29] Input status [30-31] Output status
|
||||
// [32-35] Field status
|
||||
// [36-37] StartIndex [38-39] EndIndex
|
||||
// [40-41] FirstIndex — index of this packet's first point within the full revolution
|
||||
// [42-43] NumPointsPacket — number of points in this packet
|
||||
// [44-47] Status flags
|
||||
// All of [10], [24-25], [28-35], [44-47] are read and passed through raw in
|
||||
// ExtraInfo — none of these are cross-verified against real hardware, same
|
||||
// caveat as the rest of this family.
|
||||
// Angle: angle = (FirstIndex + i) * (360 / NumPointsScan) - 180 → already in
|
||||
// the signed system (-180..180); no fmod needed like Family B since the
|
||||
// index always stays within [0, NumPointsScan).
|
||||
// Family C / Protocol V3 (0xFEAC, GS1-5): 48B header + 2 or 4B points depending
|
||||
// on Types. Ported from the C# driver OleiGS15Driver.cs; NOT verified on real
|
||||
// hardware. Angle = (FirstIndex + i) * (360 / NumPointsScan) - 180.
|
||||
bool Driver::parse_family_c(const uint8_t* buf, int len) {
|
||||
static constexpr int HEADER_LEN = 48;
|
||||
if (len < HEADER_LEN) return false;
|
||||
@@ -410,13 +304,15 @@ bool Driver::parse_family_c(const uint8_t* buf, int len) {
|
||||
uint16_t num_pts_packet = le16(buf + 42);
|
||||
uint32_t status_flags = le32(buf + 44);
|
||||
|
||||
if (num_pts_scan == 0) return false; // avoid divide-by-zero
|
||||
if (num_pts_scan == 0) return false;
|
||||
|
||||
int header_size = (header_size_field == 0) ? HEADER_LEN : header_size_field;
|
||||
if (header_size < HEADER_LEN || header_size > len) return false;
|
||||
|
||||
// Types: 0x00 = 2B/point (range only), 0x01 = 4B (range+intensity),
|
||||
// 0x10 = 4B (range at [+2,+4)).
|
||||
int bytes_per_point = (types == 0x00) ? 2 : (types == 0x01 || types == 0x10) ? 4 : 0;
|
||||
if (bytes_per_point == 0) return false; // unknown Types, layout unclear
|
||||
if (bytes_per_point == 0) return false;
|
||||
|
||||
int payload_bytes = len - header_size;
|
||||
int num_pts = num_pts_packet;
|
||||
@@ -432,25 +328,18 @@ bool Driver::parse_family_c(const uint8_t* buf, int len) {
|
||||
pending_info_.field_status = field_status;
|
||||
pending_info_.status_flags = status_flags;
|
||||
|
||||
// Magic 0xFEAC corresponds to exactly one model (GS1-5) — no model name
|
||||
// string in the header like Family B, but recognizing this family is
|
||||
// already enough to know the model, so auto-detect resolves immediately
|
||||
// without reading any extra field.
|
||||
// Magic 0xFEAC == exactly one model (GS1-5).
|
||||
if (auto_detect_ && !model_locked_) {
|
||||
cfg_.scan_angle_min = MODEL_GS15.scan_angle_min;
|
||||
cfg_.scan_angle_max = MODEL_GS15.scan_angle_max;
|
||||
cfg_.range_min_m = MODEL_GS15.range_min_m;
|
||||
cfg_.range_max_m = MODEL_GS15.range_max_m;
|
||||
cfg_.angle_offset_deg = MODEL_GS15.angle_offset_deg;
|
||||
detected_model_name_ = MODEL_GS15.name;
|
||||
model_locked_ = true;
|
||||
}
|
||||
|
||||
const float angle_inc = 360.f / static_cast<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;
|
||||
|
||||
if (last_angle_ >= 0.f && raw_first_angle < last_angle_ - 90.f) {
|
||||
@@ -473,11 +362,11 @@ bool Driver::parse_family_c(const uint8_t* buf, int len) {
|
||||
range_mm = le16(blk + 2);
|
||||
}
|
||||
|
||||
float angle = to_signed_deg(static_cast<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_);
|
||||
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});
|
||||
}
|
||||
|
||||
|
||||
@@ -1,9 +1,12 @@
|
||||
#include "lidarlib/sick_lidar.hpp"
|
||||
#include "lidar_bytes.hpp"
|
||||
|
||||
#include <cctype>
|
||||
#include <cerrno>
|
||||
#include <cmath>
|
||||
#include <cstdlib>
|
||||
#include <cstring>
|
||||
#include <limits>
|
||||
#include <vector>
|
||||
#include <fcntl.h>
|
||||
#include <sys/socket.h>
|
||||
@@ -16,7 +19,6 @@
|
||||
namespace lidarlib {
|
||||
|
||||
namespace {
|
||||
constexpr float kDeg2Rad = 3.14159265358979323846f / 180.f;
|
||||
constexpr char kStx = 0x02;
|
||||
constexpr char kEtx = 0x03;
|
||||
constexpr int kConnectTimeoutMs = 2000;
|
||||
@@ -25,14 +27,9 @@ uint32_t hex_to_u32(const std::string& tok) {
|
||||
return static_cast<uint32_t>(std::strtoul(tok.c_str(), nullptr, 16));
|
||||
}
|
||||
int32_t hex_to_i32(const std::string& tok) {
|
||||
// SICK encodes signed header fields as plain hex of the 2's-complement bits.
|
||||
// SICK encodes signed fields as plain hex of the 2's-complement bits.
|
||||
return static_cast<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> out;
|
||||
@@ -45,6 +42,10 @@ std::vector<std::string> tokenize(const std::string& s) {
|
||||
}
|
||||
return out;
|
||||
}
|
||||
|
||||
constexpr size_t kNanoRecvBufSize = 65536;
|
||||
// nanoScan3 DerivedValues store angles as int32 in 1/4194304 degree.
|
||||
constexpr double kNanoAngleResolution = 4194304.0;
|
||||
} // namespace
|
||||
|
||||
SickDriver::SickDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port)
|
||||
@@ -52,7 +53,6 @@ SickDriver::SickDriver(const ModelConfig& cfg, const std::string& ip, uint16_t p
|
||||
|
||||
SickDriver::~SickDriver() { close(); }
|
||||
|
||||
// ─── open() — TCP connect + tell the device to start streaming ────────────
|
||||
bool SickDriver::open() {
|
||||
sock_fd_ = ::socket(AF_INET, SOCK_STREAM, 0);
|
||||
if (sock_fd_ < 0) return false;
|
||||
@@ -62,11 +62,8 @@ bool SickDriver::open() {
|
||||
addr.sin_port = htons(port_);
|
||||
addr.sin_addr.s_addr = inet_addr(ip_.c_str());
|
||||
|
||||
// Non-blocking connect with a bounded timeout: a SICK device that's
|
||||
// powered off/unreachable leaves the SYN unanswered, and a plain blocking
|
||||
// connect() would then stall this call — and whatever thread called it,
|
||||
// e.g. a GUI's "connect" button handler — for the OS's default TCP retry
|
||||
// timeout (~2 minutes on Linux).
|
||||
// Non-blocking connect with a bounded timeout — a blocking connect() to an
|
||||
// unreachable device would stall for the OS default (~2 min on Linux).
|
||||
int flags = ::fcntl(sock_fd_, F_GETFL, 0);
|
||||
::fcntl(sock_fd_, F_SETFL, flags | O_NONBLOCK);
|
||||
|
||||
@@ -80,10 +77,10 @@ bool SickDriver::open() {
|
||||
::getsockopt(sock_fd_, SOL_SOCKET, SO_ERROR, &err, &errlen);
|
||||
rc = (err == 0) ? 0 : -1;
|
||||
} else {
|
||||
rc = -1; // timeout, or select() itself failed
|
||||
rc = -1;
|
||||
}
|
||||
}
|
||||
::fcntl(sock_fd_, F_SETFL, flags); // restore blocking mode for send/recv below
|
||||
::fcntl(sock_fd_, F_SETFL, flags);
|
||||
|
||||
if (rc < 0) {
|
||||
::close(sock_fd_);
|
||||
@@ -96,8 +93,7 @@ bool SickDriver::open() {
|
||||
|
||||
recv_buf_.clear();
|
||||
|
||||
// The device stays passive until told otherwise — without this, no
|
||||
// LMDscandata telegram ever arrives.
|
||||
// Device is passive until told to stream.
|
||||
if (!send_telegram("sEN LMDscandata 1")) {
|
||||
close();
|
||||
return false;
|
||||
@@ -105,16 +101,14 @@ bool SickDriver::open() {
|
||||
return true;
|
||||
}
|
||||
|
||||
// ─── close() ────────────────────────────────────────────────────────────────
|
||||
void SickDriver::close() {
|
||||
if (sock_fd_ >= 0) {
|
||||
send_telegram("sEN LMDscandata 0"); // best-effort, ignore failure
|
||||
send_telegram("sEN LMDscandata 0"); // best-effort
|
||||
::close(sock_fd_);
|
||||
sock_fd_ = -1;
|
||||
}
|
||||
}
|
||||
|
||||
// ─── send_telegram() — wrap with STX/ETX and write ─────────────────────────
|
||||
bool SickDriver::send_telegram(const std::string& body) {
|
||||
if (sock_fd_ < 0) return false;
|
||||
std::string framed;
|
||||
@@ -132,10 +126,8 @@ bool SickDriver::send_telegram(const std::string& body) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// ─── read_telegram() — pull bytes off the TCP stream until one full
|
||||
// STX..ETX frame is assembled. CoLa-A has no length prefix, so ETX is the
|
||||
// only frame boundary; recv_buf_ carries any leftover bytes (start of the
|
||||
// next telegram) across calls. ──────────────────────────────────────────────
|
||||
// CoLa-A has no length prefix, so ETX is the only frame boundary; recv_buf_
|
||||
// carries leftover bytes across calls.
|
||||
bool SickDriver::read_telegram(std::string& out, int timeout_ms) {
|
||||
if (sock_fd_ < 0) return false;
|
||||
|
||||
@@ -144,7 +136,6 @@ bool SickDriver::read_telegram(std::string& out, int timeout_ms) {
|
||||
if (etx_pos != std::string::npos) {
|
||||
size_t stx_pos = recv_buf_.find(kStx);
|
||||
if (stx_pos == std::string::npos || stx_pos > etx_pos) {
|
||||
// Stray ETX with no matching STX before it — drop and retry.
|
||||
recv_buf_.erase(0, etx_pos + 1);
|
||||
continue;
|
||||
}
|
||||
@@ -157,58 +148,37 @@ bool SickDriver::read_telegram(std::string& out, int timeout_ms) {
|
||||
fd_set fds; FD_ZERO(&fds); FD_SET(sock_fd_, &fds);
|
||||
timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 };
|
||||
int r = ::select(sock_fd_ + 1, &fds, nullptr, nullptr, &tv);
|
||||
if (r <= 0) return false; // timeout or error
|
||||
if (r <= 0) return false;
|
||||
}
|
||||
|
||||
char buf[4096];
|
||||
ssize_t n = ::recv(sock_fd_, buf, sizeof(buf), 0);
|
||||
if (n <= 0) return false; // closed or error
|
||||
if (n <= 0) return false;
|
||||
recv_buf_.append(buf, static_cast<size_t>(n));
|
||||
}
|
||||
}
|
||||
|
||||
// ─── recv_scan() ────────────────────────────────────────────────────────────
|
||||
bool SickDriver::recv_scan(ScanResult& out, int timeout_ms) {
|
||||
for (;;) {
|
||||
std::string telegram;
|
||||
if (!read_telegram(telegram, timeout_ms)) return false;
|
||||
if (parse_lmdscandata(telegram, out)) return true;
|
||||
// Non-scan telegram (e.g. an "sEA"/access-mode ack) — keep waiting.
|
||||
// Non-scan telegram (e.g. an ack) — keep waiting.
|
||||
}
|
||||
}
|
||||
|
||||
// ─── spin_once() ────────────────────────────────────────────────────────────
|
||||
bool SickDriver::spin_once() {
|
||||
std::string telegram;
|
||||
if (!read_telegram(telegram, 0)) return false; // 0 = block until next telegram
|
||||
if (!read_telegram(telegram, 0)) return false;
|
||||
|
||||
ScanResult result;
|
||||
if (!parse_lmdscandata(telegram, result)) return true; // ignore non-scan telegrams
|
||||
if (!parse_lmdscandata(telegram, result)) return true;
|
||||
if (cb_) cb_(result);
|
||||
return true;
|
||||
}
|
||||
|
||||
// ─── parse_lmdscandata() ────────────────────────────────────────────────────
|
||||
// CoLa-A "sSN LMDscandata"/"sRA LMDscandata" telegram, space-separated ASCII
|
||||
// tokens (mostly hex). UNVERIFIED layout (see header comment) — ported from
|
||||
// SICK's public Telegram Listing, field order below:
|
||||
//
|
||||
// sSN LMDscandata <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.
|
||||
// CoLa-A "sSN/sRA LMDscandata": space-separated ASCII hex tokens, field order
|
||||
// per SICK's Telegram Listing. "DIST1" → ranges, "RSSI1" → intensities.
|
||||
bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out) {
|
||||
std::vector<std::string> tok = tokenize(telegram);
|
||||
if (tok.size() < 20) return false;
|
||||
@@ -218,15 +188,14 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out)
|
||||
size_t i = 2;
|
||||
auto next = [&]() -> std::string { return (i < tok.size()) ? tok[i++] : std::string(); };
|
||||
|
||||
hex_to_u32(next()); // VersionNumber — not exposed
|
||||
hex_to_u32(next()); // DeviceNumber — not exposed
|
||||
hex_to_u32(next()); // SerialNumber — not exposed
|
||||
uint32_t status0 = hex_to_u32(next()); // DeviceStatus: Error
|
||||
uint32_t status1 = hex_to_u32(next()); // DeviceStatus: Pollution
|
||||
uint32_t telegram_counter = hex_to_u32(next());
|
||||
uint32_t scan_counter = hex_to_u32(next());
|
||||
(void)telegram_counter; (void)scan_counter; // not carried by ExtraInfo today
|
||||
hex_to_u32(next()); // TimeSinceStartup — not exposed
|
||||
hex_to_u32(next()); // VersionNumber
|
||||
hex_to_u32(next()); // DeviceNumber
|
||||
hex_to_u32(next()); // SerialNumber
|
||||
uint32_t status0 = hex_to_u32(next());
|
||||
uint32_t status1 = hex_to_u32(next());
|
||||
hex_to_u32(next()); // TelegramCounter
|
||||
hex_to_u32(next()); // ScanCounter
|
||||
hex_to_u32(next()); // TimeSinceStartup
|
||||
uint32_t time_of_transmission = hex_to_u32(next());
|
||||
uint32_t in0 = hex_to_u32(next());
|
||||
uint32_t in1 = hex_to_u32(next());
|
||||
@@ -234,7 +203,7 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out)
|
||||
uint32_t out1 = hex_to_u32(next());
|
||||
next(); // Reserved
|
||||
uint32_t scanning_frequency = hex_to_u32(next());
|
||||
hex_to_u32(next()); // MeasurementFrequency — not exposed
|
||||
hex_to_u32(next()); // MeasurementFrequency
|
||||
|
||||
uint32_t num_encoders = hex_to_u32(next());
|
||||
for (uint32_t e = 0; e < num_encoders; ++e) {
|
||||
@@ -248,22 +217,23 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out)
|
||||
float angle_min_deg = 0.f, angle_inc_deg = 0.f;
|
||||
bool got_dist = false;
|
||||
|
||||
auto parse_channel_block = [&](bool eight_bit) {
|
||||
std::string content = next(); // e.g. "DIST1", "RSSI1"
|
||||
// 16-bit and 8-bit channel blocks share the same ASCII layout.
|
||||
auto parse_channel_block = [&]() {
|
||||
std::string content = next();
|
||||
uint32_t scale_bits = hex_to_u32(next());
|
||||
hex_to_u32(next()); // ScalingOffset — unused
|
||||
hex_to_u32(next()); // ScalingOffset
|
||||
int32_t start_angle = hex_to_i32(next()); // 1/10000 deg
|
||||
int32_t step_width = hex_to_i32(next()); // 1/10000 deg
|
||||
uint32_t num_data = hex_to_u32(next());
|
||||
|
||||
float scale = bits_to_float(scale_bits);
|
||||
if (scale == 0.f) scale = 1.f; // guard against a zero/garbage scaling factor
|
||||
if (scale == 0.f) scale = 1.f;
|
||||
|
||||
bool is_dist = content.rfind("DIST", 0) == 0;
|
||||
bool is_rssi = content.rfind("RSSI", 0) == 0;
|
||||
|
||||
if (is_dist) {
|
||||
angle_min_deg = static_cast<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;
|
||||
scan.ranges.assign(num_data, 0.f);
|
||||
} else if (is_rssi && scan.intensities.empty()) {
|
||||
@@ -279,14 +249,13 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out)
|
||||
scan.intensities[d] = static_cast<float>(raw) * scale;
|
||||
}
|
||||
}
|
||||
(void)eight_bit;
|
||||
};
|
||||
|
||||
uint32_t num_16bit_channels = hex_to_u32(next());
|
||||
for (uint32_t c = 0; c < num_16bit_channels; ++c) parse_channel_block(false);
|
||||
for (uint32_t c = 0; c < num_16bit_channels; ++c) parse_channel_block();
|
||||
|
||||
uint32_t num_8bit_channels = hex_to_u32(next());
|
||||
for (uint32_t c = 0; c < num_8bit_channels; ++c) parse_channel_block(true);
|
||||
for (uint32_t c = 0; c < num_8bit_channels; ++c) parse_channel_block();
|
||||
|
||||
if (!got_dist || scan.ranges.empty()) return false;
|
||||
|
||||
@@ -295,17 +264,18 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out)
|
||||
scan.angle_increment = angle_inc_deg * kDeg2Rad;
|
||||
scan.angle_max = scan.angle_min +
|
||||
scan.angle_increment * static_cast<float>(scan.ranges.size() - 1);
|
||||
scan.time_increment = 0.f; // device doesn't expose per-point timing
|
||||
scan.scan_time = 0.f; // device doesn't expose per-scan timing
|
||||
scan.time_increment = 0.f;
|
||||
scan.scan_time = 0.f;
|
||||
scan.range_min = cfg_.range_min_m;
|
||||
scan.range_max = cfg_.range_max_m;
|
||||
if (scan.intensities.size() != scan.ranges.size())
|
||||
scan.intensities.assign(scan.ranges.size(), 0.f); // RSSI channel wasn't enabled on the device
|
||||
scan.intensities.assign(scan.ranges.size(), 0.f);
|
||||
|
||||
if (cfg_.remap_angles)
|
||||
remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max);
|
||||
|
||||
ExtraInfo& info = out.info;
|
||||
info = ExtraInfo{};
|
||||
// LMDscandata carries no model-name string (unlike OLEI Family B) — SICK
|
||||
// doesn't auto-detect, the caller's cfg names the model up front.
|
||||
info.detected_model = cfg_.name;
|
||||
info.error_status = static_cast<uint8_t>(status0 & 0xFF);
|
||||
info.status_flags = (status0 << 8) | status1;
|
||||
@@ -316,4 +286,181 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out)
|
||||
return true;
|
||||
}
|
||||
|
||||
// ── NanoScanDriver — SICK nanoScan3/microScan3 safety-scanner UDP output ────
|
||||
|
||||
NanoScanDriver::NanoScanDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port)
|
||||
: cfg_(cfg), detected_model_name_(cfg.name ? cfg.name : ""), ip_(ip), port_(port),
|
||||
recv_buf_(kNanoRecvBufSize) {}
|
||||
|
||||
NanoScanDriver::~NanoScanDriver() { close(); }
|
||||
|
||||
bool NanoScanDriver::open() {
|
||||
sock_fd_ = ::socket(AF_INET, SOCK_DGRAM, 0);
|
||||
if (sock_fd_ < 0) return false;
|
||||
|
||||
int reuse = 1;
|
||||
::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));
|
||||
|
||||
sockaddr_in addr{};
|
||||
addr.sin_family = AF_INET;
|
||||
addr.sin_port = htons(port_);
|
||||
addr.sin_addr.s_addr = (ip_ == "0.0.0.0" || ip_.empty()) ? INADDR_ANY
|
||||
: inet_addr(ip_.c_str());
|
||||
|
||||
if (::bind(sock_fd_, reinterpret_cast<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
|
||||
|
||||
Reference in New Issue
Block a user