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