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

update brand ESPE

Browse Source
This commit is contained in:
QUYVN
2026-07-07 10:38:42 +07:00
parent 1c347a4918
commit 917b4fe4c5
15 changed files with 741 additions and 263 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

@@ -14,6 +14,7 @@ option(BUILD_SHARED_LIBS "Build shared (.so) libraries instead of static" ON)
set(LIDARLIB_SOURCES set(LIDARLIB_SOURCES
src/olei_lidar.cpp src/olei_lidar.cpp
src/sick_lidar.cpp src/sick_lidar.cpp
src/espe_lidar.cpp
src/lidar_config.cpp src/lidar_config.cpp
) )
@@ -43,6 +44,9 @@ if(LIDARLIB_BUILD_EXAMPLES)
add_executable(nanoscan_example examples/nanoscan_example.cpp) add_executable(nanoscan_example examples/nanoscan_example.cpp)
target_link_libraries(nanoscan_example PRIVATE lidarlib) target_link_libraries(nanoscan_example PRIVATE lidarlib)
add_executable(espe_example examples/espe_example.cpp)
target_link_libraries(espe_example PRIVATE lidarlib)
add_executable(lidar_app examples/lidar_app.cpp) add_executable(lidar_app examples/lidar_app.cpp)
target_link_libraries(lidar_app PRIVATE lidarlib) target_link_libraries(lidar_app PRIVATE lidarlib)
endif() endif()

405
README.md
View File

@@ -1,143 +1,145 @@
# Lidarlib # Lidarlib
Thư viện C++17 cho lidar **OLEI** (UDP) và **SICK** (TCP/UDP). Build bằng CMake Thư viện C++17 thu nhận dữ liệu lidar 2D cho **OLEI** (UDP), **SICK**
ra shared lib, hỗ trợ `find_package(lidarlib)`. Mọi driver dùng chung một (TCP/UDP) và **ESPE** (TCP/UDP)
interface `lidarlib::Lidar` và một hàm khởi tạo duy nhất `lidarlib::make_lidar()`.
- Tự nhận diện họ giao thức OLEI (Family A/B/C) theo từng gói tin Mọi driver cùng implement một interface `lidarlib::Lidar`, khởi tạo qua một
- Tự dò model (`MODEL_AUTO`) với Family B/C factory duy nhất `lidarlib::make_lidar()`, output thống nhất theo định dạng
- Chạy nhiều lidar song song (mỗi instance độc lập, an toàn đa luồng) ROS `sensor_msgs/LaserScan`.
- Output chuẩn ROS `sensor_msgs/LaserScan` (radian, mét)
- Không có UI — tự viết giao diện trên API này
## Build ## Tính năng
- **Đa hãng, một API** — OLEI (Family A/B/C), SICK (TiM 5xx/7xx, nanoScan3)
và ESPE (LGA60) dùng chung interface: `open()` / `recv_scan()` / callback /
`close()`.
- **Tự nhận diện giao thức** — phân biệt họ giao thức OLEI theo frame ID từng
gói; chế độ `AUTO` tự dò model từ dữ liệu (Family B/C).
- **Chẩn đoán thiết bị** — đọc trạng thái tự chẩn đoán nhúng trong stream:
lỗi motor/điện áp/nhiệt độ (OLEI), kính bẩn/pollution (SICK TiM),
contamination/manipulation (nanoScan3).
- **Xử lý lỗi tường minh** — `ErrorCode` phân loại từ `errno` thật;
lifecycle an toàn với mọi thứ tự gọi `open()`/`close()`.
- **Đa luồng an toàn** — mỗi instance độc lập hoàn toàn, chạy mỗi lidar một
thread không cần khóa.
- **Cấu hình JSON** — khai báo danh sách lidar trong `config.json`,
load/save bằng API kèm sẵn.
## Cài đặt
Yêu cầu: Linux, CMake ≥ 3.10, trình dịch C++17. Không có dependency ngoài
(chỉ pthread).
```bash ```bash
cmake -S . -B build -DCMAKE_BUILD_TYPE=Release cmake -S . -B build -DCMAKE_BUILD_TYPE=Release
cmake --build build -j"$(nproc)" cmake --build build -j"$(nproc)"
```
Sinh ra `build/liblidarlib.so` (chỉ phụ thuộc pthread) và các binary demo
(`example`, `test_dual`, `sick_example`, `nanoscan_example`, `lidar_app`).
Tùy chọn: `-DLIDARLIB_BUILD_EXAMPLES=OFF` (tắt demo),
`-DBUILD_SHARED_LIBS=OFF` (static lib).
Cài đặt và dùng từ project khác:
```bash
cmake --install build --prefix "$HOME/.local" # hoặc sudo với /usr/local cmake --install build --prefix "$HOME/.local" # hoặc sudo với /usr/local
``` ```
Tùy chọn CMake: `-DLIDARLIB_BUILD_EXAMPLES=OFF` (tắt binary demo),
`-DBUILD_SHARED_LIBS=OFF` (build static).
Dùng từ project khác:
```cmake ```cmake
find_package(lidarlib REQUIRED) find_package(lidarlib REQUIRED)
target_link_libraries(my_app PRIVATE lidarlib::lidarlib) target_link_libraries(my_app PRIVATE lidarlib::lidarlib)
``` ```
## Quick start ## Sử dụng
### Đọc scan
```cpp ```cpp
#include "lidarlib/lidarlib.hpp" // toàn bộ API trong 1 include #include "lidarlib/lidarlib.hpp" // toàn bộ API trong một include
lidarlib::LidarConfig c{"front", "192.168.1.10", 2368, "AUTO", false, "OLEI"}; lidarlib::LidarConfig c{"front", "192.168.1.10", 2368, "AUTO", false, "OLEI"};
std::unique_ptr<lidarlib::Lidar> lidar = lidarlib::make_lidar(c); std::unique_ptr<lidarlib::Lidar> lidar = lidarlib::make_lidar(c);
lidarlib::ErrorCode err = lidar->open(); if (lidar->open() != lidarlib::ErrorCode::Ok) {
if (err != lidarlib::ErrorCode::Ok) { fprintf(stderr, "open: %s\n", lidarlib::to_string(lidar->last_error()));
fprintf(stderr, "open that bai: %s\n", lidarlib::to_string(err));
return 1; return 1;
} }
lidarlib::ScanResult r; lidarlib::ScanResult r;
if (!lidar->recv_scan(r, 1000)) { if (lidar->recv_scan(r, 1000)) {
// Timeout / DeviceDisconnected / NotOpen — xem lidar->last_error() // r.scan : LaserScan — điểm đo, format ROS
// r.info : ExtraInfo — metadata tuỳ model
} else {
// Timeout / DeviceDisconnected — xem lidar->last_error()
} }
// r.scan : LaserScan — format sensor_msgs/LaserScan của ROS, chung mọi lidar
// r.info : ExtraInfo — thông tin thêm tuỳ family/model
printf("%zu diem, model=%s\n", r.scan.ranges.size(), r.info.detected_model.c_str());
``` ```
Có thể khởi tạo driver trực tiếp thay vì qua `make_lidar()`: Có thể khởi tạo driver trực tiếp không qua factory:
```cpp ```cpp
lidarlib::Driver olei(lidarlib::MODEL_AUTO, "192.168.100.100", 2368); lidarlib::Driver olei(lidarlib::MODEL_AUTO, "192.168.100.100", 2368);
lidarlib::SickDriver tim (lidarlib::MODEL_SICK_TIM571, "192.168.0.1", 2111); lidarlib::SickDriver tim (lidarlib::MODEL_SICK_TIM571, "192.168.0.1", 2111);
lidarlib::NanoScanDriver nano(lidarlib::MODEL_SICK_NANOSCAN3, "0.0.0.0", 6060); lidarlib::NanoScanDriver nano(lidarlib::MODEL_SICK_NANOSCAN3, "0.0.0.0", 6060);
lidarlib::EspeDriver espe(lidarlib::MODEL_ESPE_LGA60, "192.168.1.88", 8080);
``` ```
Ngoài `recv_scan()` blocking còn có callback: `set_scan_callback()` + ### Chế độ callback
`spin_once()` trong vòng lặp riêng.
### Error handling & lifecycle Thay cho `recv_scan()` blocking:
`open()` trả về `lidarlib::ErrorCode` (header `lidarlib/error.hpp`, ```cpp
`to_string()` để log). Các code chính: lidar->set_scan_callback([](const lidarlib::ScanResult& r) { /* mỗi vòng quét */ });
while (running) lidar->spin_once();
```
| ErrorCode | Khi nào | ### Chẩn đoán thiết bị
|---|---|
| `Ok` | Thành công |
| `AlreadyOpen` | Gọi `open()` khi đang mở — kết nối cũ giữ nguyên |
| `NotOpen` | Gọi `recv_scan()`/`spin_once()` khi chưa `open()` |
| `InvalidAddress` | Chuỗi IP không hợp lệ |
| `PortInUse` | Port local đã bị chiếm (bind `EADDRINUSE`/`EACCES`) |
| `BindFailed` / `SocketError` | Lỗi bind khác / không tạo được socket |
| `ConnectionRefused` / `ConnectionFailed` / `Timeout` | TCP connect (SICK TiM) bị từ chối / không tới được / quá 2s |
| `HandshakeFailed` | TCP nối được nhưng gửi `sEN LMDscandata 1` thất bại |
| `DeviceDisconnected` | Thiết bị đóng kết nối / lỗi recv giữa chừng |
Trạng thái instance: Thiết bị nhúng thông tin tự chẩn đoán trong stream dữ liệu; driver decode
sẵn qua `get_diagnostics()` (chi tiết layout từng giao thức:
- `is_open()` — socket đang mở hay không. [docs/diagnostics.md](docs/diagnostics.md)):
- `last_error()` — kết quả của lần `open()`/`recv_scan()`/`spin_once()` gần
nhất (`recv_scan()` trả `false` thì gọi hàm này để biết `Timeout` hay
`DeviceDisconnected`).
- Lifecycle chịu lỗi mọi thứ tự gọi: `close()` trước `open()` hoặc `close()`
hai lần là no-op; `open()` hai lần trả `AlreadyOpen` và không đụng kết nối
đang chạy; sau `close()` có thể `open()` lại (state scan dở được reset).
### Chẩn đoán thiết bị (diagnostics)
Lidar OLEI nhúng thông tin tự chẩn đoán trong header gói dữ liệu (không có
kênh query riêng). API đọc ra qua `lidarlib/diagnostics.hpp` — nghiên cứu
chi tiết layout từng family xem [docs/diagnostics.md](docs/diagnostics.md).
```cpp ```cpp
lidarlib::Diagnostics d = lidar->get_diagnostics(); lidarlib::Diagnostics d = lidar->get_diagnostics();
if (!d.valid) { if (!d.valid) {
// chưa decode được scan nào // chưa decode được vòng quét nào
} else if (d.has_fault()) { } else if (d.has_fault()) {
// OLEI Family A // OLEI Family A
d.monitor_fault(); // motor/giám sát bất thường d.monitor_fault(); // motor/giám sát bất thường
d.voltage_fault(); // điện áp ngoài dải d.voltage_fault(); // điện áp ngoài dải
d.temperature_fault(); // nhiệt độ bất thường d.temperature_fault(); // nhiệt độ bất thường
// SICK // SICK
d.sick_error(); // TiM: device error d.sick_error(); // TiM: device error
d.pollution_error(); // TiM: kính bẩn nặng d.pollution_error(); // TiM: kính bẩn nặng
d.contamination_error(); // nanoScan3: kính bẩn nặng d.contamination_error(); // nanoScan3: kính bẩn nặng
d.manipulation(); // nanoScan3: nghi bị che/can thiệp d.manipulation(); // nanoScan3: nghi bị che/can thiệp
printf("lidar fault: %s\n", lidarlib::to_string(d).c_str()); printf("fault: %s\n", lidarlib::to_string(d).c_str());
} else if (d.has_warning()) { } else if (d.has_warning()) {
// pollution_warning() / contamination_warning() — kính bẩn nhẹ, nên lau // pollution_warning() / contamination_warning() — kính bẩn nhẹ, nên lau
} }
``` ```
- `get_diagnostics()` — snapshot từ vòng quét gần nhất; gọi cùng thread với `d.healthy()` = đã có dữ liệu và không có fault. Khuyến nghị giám sát: cảm
`recv_scan()`/`spin_once()`. `d.healthy()` = có data và không có fault. biến khỏe khi và chỉ khi `recv_scan()` thành công đều đặn **và**
- `decode_diagnostics(result.info)` — decode gắn với một scan cụ thể. `get_diagnostics().has_fault() == false`.
- Nguồn dữ liệu: OLEI Family A byte lỗi + `rotation_raw` + timestamp;
Family B không có gì trên wire; Family C `input/output/field_status`,
`status_flags` (raw, chưa verify); SICK TiM cặp device status trong
`LMDscandata`; nanoScan3 block General System State (cần bật trong
Safety Designer).
## Cấu hình (config.json) ### Xử lý lỗi & lifecycle
`lidar_app` là app mẫu headless: đọc `config.json`, mở từng lidar bằng `open()` trả về `ErrorCode`; `last_error()` giữ kết quả của lần gọi gần nhất.
`make_lidar()`, một thread mỗi con.
```bash | ErrorCode | Ý nghĩa |
./build/lidar_app [my_config.json] |---|---|
``` | `Ok` | Thành công |
| `AlreadyOpen` | `open()` khi đang mở — kết nối cũ giữ nguyên |
| `NotOpen` | `recv_scan()`/`spin_once()` khi chưa `open()` |
| `InvalidAddress` | Chuỗi IP không hợp lệ |
| `PortInUse` | Port local đã bị chiếm |
| `BindFailed` / `SocketError` | Lỗi bind khác / không tạo được socket |
| `ConnectionRefused` / `ConnectionFailed` / `Timeout` | TCP connect bị từ chối / không tới được / quá thời hạn |
| `HandshakeFailed` | TCP nối được nhưng lệnh start-stream thất bại |
| `DeviceDisconnected` | Thiết bị đóng kết nối / lỗi recv giữa chừng |
Lifecycle an toàn với mọi thứ tự gọi: `close()` là idempotent, `open()` lặp
trả `AlreadyOpen` và không đụng kết nối đang chạy, sau `close()` có thể
`open()` lại (state được reset).
### Cấu hình JSON
`lidar_app` (binary demo) đọc `config.json`, mở từng lidar một thread:
```json ```json
{ {
@@ -145,157 +147,146 @@ if (!d.valid) {
{"name":"front", "ip":"192.168.100.100", "port":2368, "brand":"OLEI", "model":"AUTO", "inverted":false}, {"name":"front", "ip":"192.168.100.100", "port":2368, "brand":"OLEI", "model":"AUTO", "inverted":false},
{"name":"rear", "ip":"192.168.100.100", "port":2369, "brand":"OLEI", "model":"AUTO", "inverted":true}, {"name":"rear", "ip":"192.168.100.100", "port":2369, "brand":"OLEI", "model":"AUTO", "inverted":true},
{"name":"sick1", "ip":"192.168.0.1", "port":2111, "brand":"SICK", "model":"SICK-TIM571"}, {"name":"sick1", "ip":"192.168.0.1", "port":2111, "brand":"SICK", "model":"SICK-TIM571"},
{"name":"nano1", "ip":"0.0.0.0", "port":6060, "brand":"SICK", "model":"SICK-nanoScan3"} {"name":"nano1", "ip":"0.0.0.0", "port":6060, "brand":"SICK", "model":"SICK-nanoScan3"},
{"name":"espe1", "ip":"192.168.1.88", "port":8080, "brand":"ESPE", "model":"ESPE-LGA60"}
] ]
} }
``` ```
| Trường | Ý nghĩa | | Trường | Ý nghĩa |
|---|---| |---|---|
| `brand` | `"OLEI"` (mặc định) hoặc `"SICK"` | | `brand` | `"OLEI"` (mặc định), `"SICK"` hoặc `"ESPE"` |
| `model` | Tên trong bảng model bên dưới; tên lạ → mặc định của hãng (`AUTO` / `SICK-TIM571`). Với SICK, `"SICK-nanoScan3"` → driver UDP, còn lại → driver TCP | | `model` | Tên trong bảng model bên dưới; tên lạ → mặc định của hãng |
| `inverted` | Chỉ OLEI: `true` nếu lidar lắp úp ngược, driver tự đảo góc về hệ quy chiếu xe | | `inverted` | `true` nếu lidar lắp úp ngược — driver tự đảo góc (mọi hãng) |
| `angle_min_deg` / `angle_max_deg` | Tuỳ chọn: remap tuyến tính góc output sang cửa sổ này (độ). Không cắt điểm nào, chỉ ghi lại `angle_min/max/increment`. Bỏ trống (±360) = tắt | | `use_udp` | Chỉ ESPE: `true` để dùng transport UDP thay vì TCP |
| `angle_min_deg` / `angle_max_deg` | Tuỳ chọn: remap tuyến tính góc output sang cửa sổ này (không cắt điểm). Bỏ trống = tắt |
Đọc/ghi bằng `lidarlib::load_config(path)` / `lidarlib::save_config(path, cfg)`. Đọc/ghi bằng `lidarlib::load_config(path)` / `lidarlib::save_config(path, cfg)`.
## Model ## Model hỗ trợ
### OLEI (`brand = "OLEI"`, UDP, port mặc định 2368) ### OLEI (UDP, port mặc định 2368)
| Constant | FOV (°) | Range (m) | Ghi chú | | Constant | FOV (°) | Range (m) | Giao thức |
|---|---|---|---| |---|---|---|---|
| `MODEL_VB` | -135…135 | 0.05…30 | 2D 270°, Family A | | `MODEL_VB` | −135…135 | 0.05…30 | Family A |
| `MODEL_VF` | -180…180 | 0.05…30 | 2D 360°, Family A | | `MODEL_VF` | −180…180 | 0.05…30 | Family A |
| `MODEL_LR1F` | -180…180 | 0.05…50 | Family A; 0° thô của máy chỉ về đuôi (offset +180°) | | `MODEL_LR1F` | −180…180 | 0.05…50 | Family A (0° thiết bị hướng đuôi, offset +180°) |
| `MODEL_LR1FMI` | -180…180 | 0.05…30 | Family B, ~2400 điểm/vòng; 0° thô chỉ về đuôi (offset +180°) | | `MODEL_LR1FMI` | −180…180 | 0.05…30 | Family B, ~2400 điểm/vòng (0° hướng đuôi) |
| `MODEL_LR1BS5` | -180…180 | 0.05…30 | Family B | | `MODEL_LR1BS5` | −180…180 | 0.05…30 | Family B |
| `MODEL_LR16F` | -135…135 | 0.05…30 | 3D 16-line | | `MODEL_LR16F` | −135…135 | 0.05…30 | 3D 16-line |
| `MODEL_GS15` | -180…180 | 0.05…30 | Family C/V3 — **chưa verify phần cứng** | | `MODEL_GS15` | −180…180 | 0.05…30 | Family C/V3 — chưa verify phần cứng |
| `MODEL_AUTO` | -180…180 | 0.05…30 | Không biết trước model; tự dò với Family B (chuỗi tên) và C (magic). Family A không mang tên model nên giữ FOV rộng | | `MODEL_AUTO` | −180…180 | 0.05…30 | Tự dò model (Family B/C) |
Driver nhận diện họ giao thức theo Frame ID mỗi gói: Driver nhận diện họ giao thức theo frame ID từng gói: **Family A** `0xFAF0`
**Family A** `0xFAF0` (header 20B, 3B/điểm, CRC32) · (header 20 B, 3 B/điểm, CRC32) · **Family B** `0xFEF0` (header 40 B kèm tên
**Family B** `0xFEF0` (header 40B kèm tên model ASCII, 8B/điểm) · model ASCII, 8 B/điểm) · **Family C/V3** `0xFEAC` (header 48 B, 2–4 B/điểm).
**Family C/V3** `0xFEAC` (header 48B, 2/4B/điểm — port từ driver C#, chưa verify). Tên model đọc từ packet: `detected_model()` hoặc `result.info.detected_model`.
Tên model thật đọc từ packet xem qua `detected_model()` hoặc ### SICK
`result.info.detected_model`.
### SICK (`brand = "SICK"`)
| Constant | FOV (°) | Range (m) | Transport | | Constant | FOV (°) | Range (m) | Transport |
|---|---|---|---| |---|---|---|---|
| `MODEL_SICK_TIM5XX` | -135…135 | 0.05…10 | TCP/SOPAS (CoLa-A), port 2111 | | `MODEL_SICK_TIM5XX` | −135…135 | 0.05…10 | TCP/SOPAS (CoLa-A), port 2111 |
| `MODEL_SICK_TIM571` | -135…135 | 0.05…25 | TCP/SOPAS, port 2111 | | `MODEL_SICK_TIM571` | −135…135 | 0.05…25 | TCP/SOPAS, port 2111 |
| `MODEL_SICK_TIM7XX` | -135…135 | 0.05…25 | TCP/SOPAS, port 2111 | | `MODEL_SICK_TIM7XX` | −135…135 | 0.05…25 | TCP/SOPAS, port 2111 — verify trên TiM781S thật |
| `MODEL_SICK_NANOSCAN3` | -137.5…137.5 | 0.05…40 | UDP safety-data, port 6060 | | `MODEL_SICK_NANOSCAN3` | −137.5…137.5 | 0.05…40 | UDP safety-data, port 6060 |
**TiM (`SickDriver`)** — `open()` tự gửi `sEN LMDscandata 1` để bắt đầu stream. - **TiM (`SickDriver`)** — `open()` tự gửi lệnh start-stream; góc output đã
Hệ góc trên dây đặt 90° = trước mặt nên preset có `angle_offset_deg = -90`, quy về 0° = phía trước.
output ra -135…135° với 0° = phía trước. **Đã verify trên TiM781S thật** - **nanoScan3 (`NanoScanDriver`)** — receiver UDP thụ động; đích UDP phải
(811 điểm/scan, increment 0.333°, DIST1/RSSI1 đúng layout). Chưa verify: cấu hình sẵn trong SICK Safety Designer. Chưa verify phần cứng thật.
encoder, kênh 8-bit, thông số TiM5xx/571.
**nanoScan3 (`NanoScanDriver`)** — UDP receiver thụ động: chỉ bind cổng và ### ESPE
parse datagram; **đích UDP phải cấu hình sẵn trong SICK Safety Designer**
(driver không bắt tay CoLa2). Layout port từ `sick_safetyscanners` (Apache-2.0).
**Chưa verify phần cứng thật** — mới test bằng gói tổng hợp qua loopback.
## Output | Constant | FOV (°) | Range (m) | Transport |
|---|---|---|---|
| `MODEL_ESPE_LGA60` | −160…160 | 0.05…50 | TCP (mặc định) hoặc UDP, port 8080 |
`ScanResult { LaserScan scan; ExtraInfo info; }` mỗi vòng quét: - **LGA60 (`EspeDriver`)** — laser scanner FOV 320°, thiết bị quét
20°→340° với 0° hướng đuôi (offset −180° để output 0° = phía trước).
`open()` tự gửi lệnh start-capture `RAuto`; các tham số thiết bị (tốc độ
quay, độ phân giải 0.025–0.5°, mức lọc nhiễu) lấy theo cấu hình đã nạp
bằng phần mềm Windows của hãng — driver không tự đổi. Frame dữ liệu
`HISN` (header big-endian, điểm đo little-endian: distance mm +
intensity); frame vùng `WSimu` (nếu thiết bị gửi) được đọc lấy mã lỗi.
Chuyển transport UDP qua tham số `use_udp` của constructor hoặc trường
`use_udp` trong config JSON. Mặc định của hãng: IP 192.168.1.88, port
8080. Port từ driver ROS gốc của hãng — chưa verify trên phần cứng thật.
- **`LaserScan`** — cùng field/đơn vị với ROS: `angle_min/max/increment` (rad, ## Kiểu dữ liệu
đã unwrap liên tục, không giới hạn ±π), `ranges[]` (m), `intensities[]`
(0-255), `timestamp_ms` (đồng hồ thiết bị, 0 nếu family không có). ### `ScanResult`
`range_min/max` lấy từ `ModelConfig` (đặt sẵn, không đo mỗi scan);
`time_increment/scan_time` luôn 0. Kết quả một vòng quét: `{ LaserScan scan; ExtraInfo info; }`.
- **`ExtraInfo`** — field tuỳ family: `detected_model`, `error_status` (Family A),
`distance_scale_mm` (A/B), và các trường raw của Family C (chưa verify). ### `LaserScan`
Field thiết bị không có giữ `std::nullopt`.
Cùng field và đơn vị với ROS `sensor_msgs/LaserScan`:
| Field | Kiểu | Ý nghĩa |
|---|---|---|
| `angle_min` / `angle_max` | `float` | Góc điểm đầu/cuối (rad), unwrap liên tục |
| `angle_increment` | `float` | Bước góc (rad); góc điểm *i* = `angle_min + i·increment` |
| `ranges` | `vector<float>` | Khoảng cách (m), theo thứ tự quét |
| `intensities` | `vector<float>` | Cường độ phản xạ 0–255 |
| `range_min` / `range_max` | `float` | Dải đo hợp lệ (m), lấy từ `ModelConfig` |
| `timestamp_ms` | `uint32_t` | Đồng hồ thiết bị (ms); 0 nếu giao thức không có |
| `time_increment` / `scan_time` | `float` | Luôn 0 (thiết bị không cung cấp) |
### `ExtraInfo`
Metadata tuỳ giao thức; trường thiết bị không có giữ `std::nullopt`:
| Field | Nguồn | Ý nghĩa |
|---|---|---|
| `detected_model` | mọi driver | Tên model thực đọc từ dữ liệu (hoặc tên cấu hình) |
| `error_status` | OLEI Family A | Byte lỗi thiết bị (xem Diagnostics) |
| `distance_scale_mm` | OLEI A/B | Hệ số mm/count của khoảng cách |
| `rotation_raw` | OLEI Family A | Tốc độ motor (raw) |
| `scan_frequency_raw`, `input_status`, `output_status`, `field_status`, `status_flags` | OLEI Family C, SICK TiM | Trạng thái I/O, field an toàn, cờ trạng thái (raw) |
| `sick_device_status` | SICK TiM | Cặp Device Status: 0 ok · 1 error · 2 pollution warning · 4 pollution error |
| `nano_general_state` | nanoScan3 | Byte 0 block General System State (bit `kNanoState*`) |
| `espe_error_status` | ESPE LGA60 | Từ lỗi thiết bị trong frame vùng `WSimu` (chỉ có khi host poll area data) |
### `Diagnostics`
Trạng thái tự chẩn đoán đã decode (`lidarlib/diagnostics.hpp`), trả về từ
`get_diagnostics()` hoặc `decode_diagnostics(result.info)`:
| API | Ý nghĩa |
|---|---|
| `valid` | Đã decode được ít nhất một vòng quét |
| `monitor_fault()` / `voltage_fault()` / `temperature_fault()` | OLEI Family A: motor / điện áp / nhiệt độ bất thường |
| `sick_error()` / `pollution_warning()` / `pollution_error()` | SICK TiM: lỗi thiết bị / kính bẩn nhẹ / kính bẩn nặng |
| `contamination_warning()` / `contamination_error()` / `manipulation()` | nanoScan3: kính bẩn / nghi bị can thiệp |
| `espe_fault()` | ESPE LGA60: từ lỗi thiết bị khác 0 (ý nghĩa bit chưa verify) |
| `has_fault()` | Gộp mọi nguồn lỗi |
| `has_warning()` | Gộp các cảnh báo kính bẩn (vẫn đo được) |
| `healthy()` | `valid && !has_fault()` |
| `to_string(d)` | Chuỗi log một dòng: `no data` / `ok` / `WARN: …` / `FAULT: …` |
### `ModelConfig` & `LidarConfig`
- `ModelConfig` — thông số một model: tên, FOV, dải đo, offset góc, cửa sổ
remap. Các preset `MODEL_*` khai báo sẵn trong header.
- `LidarConfig` — một entry cấu hình runtime: `{name, ip, port, model,
inverted, brand}`, dùng với `make_lidar()` và file JSON.
## Cấu trúc source ## Cấu trúc source
| File | Vai trò | | File | Vai trò |
|---|---| |---|---|
| `include/lidarlib/lidarlib.hpp` | Include tổng hợp toàn bộ API |
| `include/lidarlib/lidar.hpp` | Kiểu dữ liệu, interface `Lidar`, driver OLEI, preset `MODEL_*` |
| `include/lidarlib/sick_lidar.hpp` | `SickDriver`, `NanoScanDriver`, preset `MODEL_SICK_*` |
| `include/lidarlib/espe_lidar.hpp` | `EspeDriver`, preset `MODEL_ESPE_LGA60` |
| `include/lidarlib/diagnostics.hpp` | `Diagnostics`, bit lỗi, `decode_diagnostics()` |
| `include/lidarlib/error.hpp` | `enum class ErrorCode` + `to_string()` | | `include/lidarlib/error.hpp` | `enum class ErrorCode` + `to_string()` |
| `include/lidarlib/diagnostics.hpp` | `Diagnostics`, bit lỗi Family A, `decode_diagnostics()` |
| `include/lidarlib/lidar.hpp` | Data model, interface `Lidar`, driver OLEI, các `MODEL_*` OLEI |
| `include/lidarlib/sick_lidar.hpp` | `SickDriver`, `NanoScanDriver`, các `MODEL_SICK_*` |
| `include/lidarlib/config.hpp` | `LidarConfig`, load/save JSON, `make_lidar()` | | `include/lidarlib/config.hpp` | `LidarConfig`, load/save JSON, `make_lidar()` |
| `src/olei_lidar.cpp` | Parse Family A/B/C, CRC, gom scan | | `src/olei_lidar.cpp` | Parse Family A/B/C, CRC, gom vòng quét |
| `src/sick_lidar.cpp` | Parse CoLa-A (TiM) + safety-data UDP (nanoScan3) | | `src/sick_lidar.cpp` | Parse CoLa-A (TiM) + safety-data UDP (nanoScan3) |
| `src/espe_lidar.cpp` | Parse frame `HISN`/`WSimu` (LGA60), gom vòng quét |
| `src/lidar_config.cpp` | Bảng model/brand, config JSON, factory | | `src/lidar_config.cpp` | Bảng model/brand, config JSON, factory |
| `examples/` | Demo: 1 lidar, 2 lidar song song, SICK TiM, nanoScan3, app khung | | `docs/diagnostics.md` | Nghiên cứu layout dữ liệu chẩn đoán từng giao thức |
| `examples/` | Demo: một lidar, hai lidar song song, SICK TiM, nanoScan3, app khung |
## Ghi chú
- Nếu port UDP đã bị app khác giữ (không bật `SO_REUSEPORT`), `open()` trả
`ErrorCode::PortInUse`. Kiểm tra: `ss -lunp | grep 2368`.
- `inverted` đã verify bằng sniff sống: `false` góc tăng dần, `true` góc giảm
dần cùng bước.
## Changelog
### 2026-07-07 — Diagnostics API cho SICK
- **`SickDriver` (TiM)**: decode cặp Device Status của `LMDscandata`
(0 ok / 1 error / 2 pollution warning / 4 pollution error) vào
`Diagnostics` — `sick_error()`, `pollution_warning()`, `pollution_error()`.
`info.sick_device_status` là trường mới; `info.error_status` **không còn**
nhận `status0` (trường đó thuộc Family A OLEI).
- **`NanoScanDriver`**: parse block General System State (offset header
`[32]/[34]`, byte 0) → `contamination_warning()`, `contamination_error()`,
`manipulation()`. Layout theo `sick_safetyscanners`, chưa verify phần cứng;
block phải được bật trong Safety Designer.
- **`Diagnostics` thêm `has_warning()`** (kính bẩn mức cảnh báo) và
`to_string()` in thêm `WARN: pollution` / `FAULT: ... contamination`.
- Test loopback: TiM server TCP giả (status `0 4` → pollution error) và gói
nanoScan3 tổng hợp (state `0x09` → contamination error, gói sạch →
`healthy()`); tài liệu tại [docs/diagnostics.md](docs/diagnostics.md) §4-5.
### 2026-07-07 — Diagnostics API
- **Header mới `lidarlib/diagnostics.hpp`**: struct `Diagnostics` (bit lỗi
Family A decode sẵn: `monitor/voltage/temperature_fault()`, `has_fault()`,
`healthy()`; trường raw Family C), hằng `kFaultMonitor/Voltage/Temperature`,
`to_string()` để log.
- **`Lidar::get_diagnostics()`**: snapshot chẩn đoán từ vòng quét decode gần
nhất; `valid = false` khi chưa có scan. Driver OLEI implement đầy đủ;
SICK tạm trả mặc định.
- **`decode_diagnostics(const ExtraInfo&)`**: decode gắn với một
`ScanResult` cụ thể.
- **Tài liệu nghiên cứu [docs/diagnostics.md](docs/diagnostics.md)**: layout
byte chẩn đoán từng family (A: byte lỗi `[5]` + rotation + timestamp;
B: không có; C: input/output/field/status raw), chẩn đoán tầng transport,
hướng mở rộng SICK.
- Đã test bằng gói Family A tổng hợp qua loopback (fault set → decode đúng
từng bit, vòng sạch → `healthy()`); bit map lấy theo tài liệu OLEI, chưa
tái tạo fault trên phần cứng thật.
### 2026-07-06 — Error handling & lifecycle API
- **Header mới `lidarlib/error.hpp`**: `enum class ErrorCode` — `Ok`,
`AlreadyOpen`, `NotOpen`, `SocketError`, `InvalidAddress`, `PortInUse`,
`BindFailed`, `ConnectionRefused`, `ConnectionFailed`, `HandshakeFailed`,
`Timeout`, `DeviceDisconnected` — kèm `to_string()` để log.
- **`open()` đổi chữ ký `bool` → `ErrorCode`** trên cả 3 driver (`Driver`,
`SickDriver`, `NanoScanDriver`). Lỗi phân loại từ `errno` thật: bind
`EADDRINUSE` → `PortInUse`, connect TCP bị từ chối → `ConnectionRefused`,
quá 2s → `Timeout`, gửi lệnh start-stream fail → `HandshakeFailed`,
IP sai format → `InvalidAddress`.
- **API trạng thái mới trên interface `Lidar`**:
- `is_open()` — socket đang mở hay không (cả 3 driver implement);
- `last_error()` — kết quả lần `open()`/`recv_scan()`/`spin_once()` gần
nhất; `recv_scan()` trả `false` thì gọi hàm này để biết `Timeout` hay
`DeviceDisconnected`.
- **Lifecycle chịu lỗi mọi thứ tự gọi**: `close()` trước `open()` hoặc gọi
hai lần là no-op; `open()` khi đang mở trả `AlreadyOpen` và không đụng kết
nối đang chạy; `recv_scan()`/`spin_once()` khi chưa mở trả `false` +
`NotOpen`; sau `close()` có thể `open()` lại (state scan dở được reset).
- **`NanoScanDriver` bỏ `SO_REUSEADDR`**: với UDP không có tác dụng (không có
TIME_WAIT) mà còn che mất lỗi trùng port — giờ `PortInUse` báo được thật.
- ⚠️ **Breaking change**: code cũ viết `if (!lidar->open())` bị đảo ngược
logic vì `ErrorCode::Ok == 0` — phải đổi thành
`if (lidar->open() != lidarlib::ErrorCode::Ok)`.

39
examples/espe_example.cpp Normal file
View File

@@ -0,0 +1,39 @@
// ESPE LGA60 driver example (TCP, port 8080 theo cấu hình mặc định của hãng).
#include "lidarlib/espe_lidar.hpp"
#include <cstdio>
int main() {
lidarlib::EspeDriver drv(lidarlib::MODEL_ESPE_LGA60, "192.168.1.88", 8080);
lidarlib::ErrorCode err = drv.open();
if (err != lidarlib::ErrorCode::Ok) {
fprintf(stderr, "Không kết nối được tới lidar ESPE: %s\n",
lidarlib::to_string(err));
return 1;
}
for (int i = 0; i < 10; ++i) {
lidarlib::ScanResult result;
if (!drv.recv_scan(result, 1000)) {
fprintf(stderr, "Timeout hoặc lỗi nhận dữ liệu: %s\n",
lidarlib::to_string(drv.last_error()));
continue;
}
const lidarlib::LaserScan& scan = result.scan;
printf("Scan #%d: %zu điểm, model=%s, diag=%s\n",
i, scan.ranges.size(), result.info.detected_model.c_str(),
lidarlib::to_string(drv.get_diagnostics()).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 espe_example examples/espe_example.cpp src/espe_lidar.cpp src/olei_lidar.cpp

9
include/lidarlib/config.hpp
View File

@@ -12,8 +12,9 @@ struct LidarConfig {
std::string ip = "0.0.0.0"; std::string ip = "0.0.0.0";
uint16_t port = 2368; uint16_t port = 2368;
std::string model = "AUTO"; std::string model = "AUTO";
bool inverted = false; // OLEI only bool inverted = false; // unit mounted upside-down → mirror the scan
std::string brand = "OLEI"; // "OLEI" or "SICK" std::string brand = "OLEI"; // "OLEI", "SICK" or "ESPE"
bool use_udp = false; // ESPE only: UDP instead of TCP transport
// Output angle window (deg): scan angles are remapped onto // Output angle window (deg): scan angles are remapped onto
// [angle_min_deg, angle_max_deg] without dropping points. // [angle_min_deg, angle_max_deg] without dropping points.
@@ -24,6 +25,7 @@ struct LidarConfig {
friend bool operator==(const LidarConfig& a, const LidarConfig& b) { friend bool operator==(const LidarConfig& a, const LidarConfig& b) {
return a.name == b.name && a.ip == b.ip && a.port == b.port && return a.name == b.name && a.ip == b.ip && a.port == b.port &&
a.model == b.model && a.inverted == b.inverted && a.brand == b.brand && a.model == b.model && a.inverted == b.inverted && a.brand == b.brand &&
a.use_udp == b.use_udp &&
a.angle_min_deg == b.angle_min_deg && a.angle_max_deg == b.angle_max_deg; a.angle_min_deg == b.angle_min_deg && a.angle_max_deg == b.angle_max_deg;
} }
friend bool operator!=(const LidarConfig& a, const LidarConfig& b) { return !(a == b); } friend bool operator!=(const LidarConfig& a, const LidarConfig& b) { return !(a == b); }
@@ -53,7 +55,8 @@ void save_config(const std::string& path, const Config& cfg);
// Build a ready-to-open lidar from one LidarConfig — the only entry point an // Build a ready-to-open lidar from one LidarConfig — the only entry point an
// app needs. brand "SICK" → SICK driver (model "SICK-nanoScan3" → UDP // app needs. brand "SICK" → SICK driver (model "SICK-nanoScan3" → UDP
// NanoScanDriver, others → TCP SickDriver); anything else → OLEI UDP. // NanoScanDriver, others → TCP SickDriver); brand "ESPE" → TCP EspeDriver;
// anything else → OLEI UDP.
// Unknown/cross-brand model falls back to the brand default. Never nullptr. // Unknown/cross-brand model falls back to the brand default. Never nullptr.
std::unique_ptr<Lidar> make_lidar(const LidarConfig& cfg); std::unique_ptr<Lidar> make_lidar(const LidarConfig& cfg);

13
include/lidarlib/diagnostics.hpp
View File

@@ -67,11 +67,17 @@ struct Diagnostics {
bool contamination_error() const { return nano_general_state && (*nano_general_state & kNanoStateContaminationError); } bool contamination_error() const { return nano_general_state && (*nano_general_state & kNanoStateContaminationError); }
bool manipulation() const { return nano_general_state && (*nano_general_state & kNanoStateManipulation); } bool manipulation() const { return nano_general_state && (*nano_general_state & kNanoStateManipulation); }
// ESPE LGA60 — raw fault word from area frames (bit meanings unverified);
// only present when the host polls area data.
std::optional<uint16_t> espe_error_status;
bool espe_fault() const { return espe_error_status && *espe_error_status != 0; }
// Fault = device says something is wrong now; warning = degraded but // Fault = device says something is wrong now; warning = degraded but
// still measuring (dirty optics) — schedule cleaning/service. // still measuring (dirty optics) — schedule cleaning/service.
bool has_fault() const { bool has_fault() const {
return error_status != 0 || sick_error() || pollution_error() return error_status != 0 || sick_error() || pollution_error()
|| contamination_error() || manipulation(); || contamination_error() || manipulation() || espe_fault();
} }
bool has_warning() const { return pollution_warning() || contamination_warning(); } bool has_warning() const { return pollution_warning() || contamination_warning(); }
bool healthy() const { return valid && !has_fault(); } bool healthy() const { return valid && !has_fault(); }
@@ -97,6 +103,11 @@ inline std::string to_string(const Diagnostics& d) {
if (d.pollution_error()) s += " pollution"; if (d.pollution_error()) s += " pollution";
if (d.contamination_error()) s += " contamination"; if (d.contamination_error()) s += " contamination";
if (d.manipulation()) s += " manipulation"; if (d.manipulation()) s += " manipulation";
if (d.espe_fault()) {
char buf[24];
std::snprintf(buf, sizeof(buf), " espe(0x%04X)", *d.espe_error_status);
s += buf;
}
if (uint8_t rest = d.error_status & ~(kFaultMonitor | kFaultVoltage | kFaultTemperature)) { if (uint8_t rest = d.error_status & ~(kFaultMonitor | kFaultVoltage | kFaultTemperature)) {
char buf[24]; char buf[24];
std::snprintf(buf, sizeof(buf), " reserved(0x%02X)", rest); std::snprintf(buf, sizeof(buf), " reserved(0x%02X)", rest);

83
include/lidarlib/espe_lidar.hpp Normal file
View File

@@ -0,0 +1,83 @@
#pragma once
#include "lidarlib/lidar.hpp"
#include <cstdint>
#include <string>
namespace lidarlib {
// ESPE LGA60-320: 320° FOV, device sweeps 20°..340° with 0° at the rear
// (angle_offset_deg = -180 so output 0° = ahead). Range per datasheet page;
// the wire caps distance at 50000 mm.
inline constexpr ModelConfig MODEL_ESPE_LGA60 { "ESPE-LGA60", -160.f, 160.f, 0.05f, 50.f, -180.f };
// ESPE LGA60 over TCP (default port 8080) or UDP, ported from the vendor's
// ROS driver; NOT verified on real hardware. open() sends the "RAuto" start
// command; device parameters (spin rate, resolution, filters) are whatever
// the vendor Windows config tool programmed — this driver does not set them.
class EspeDriver : public Lidar {
public:
using ScanCallback = lidarlib::ScanCallback;
// ip: device address; use_udp selects the transport the device is
// configured for (vendor default is TCP); inverted: unit mounted
// upside-down → mirror the scan.
explicit EspeDriver(const ModelConfig& cfg,
const std::string& ip,
uint16_t port = 8080,
bool use_udp = false,
bool inverted = false);
~EspeDriver();
EspeDriver(const EspeDriver&) = delete;
EspeDriver& operator=(const EspeDriver&) = delete;
// Connect + send the start-capture command.
ErrorCode open() override;
void close() override;
bool recv_scan(ScanResult& out, int timeout_ms = 1000) override;
void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
bool spin_once() override;
bool is_open() const override { return sock_fd_ >= 0; }
// No model string on the wire — returns the configured name.
const char* detected_model() const override { return detected_model_name_.c_str(); }
Diagnostics get_diagnostics() const override { return latest_diag_; }
private:
bool fill_buffer(int timeout_ms); // one recv() into recv_buf_
bool parse_buffer(); // consume frames; true when a scan completed
void handle_range_frame(const uint8_t* frame, uint16_t data_size);
void finish_scan();
ModelConfig cfg_;
std::string detected_model_name_; // owned copy of cfg_.name (stable lifetime)
std::string ip_;
uint16_t port_;
bool use_udp_ = false;
bool inverted_ = false;
int sock_fd_ = -1;
ScanCallback cb_;
// Stream bytes carried across frame boundaries; per-instance.
std::string recv_buf_;
// Per-revolution accumulation
std::vector<float> pending_ranges_;
std::vector<float> pending_intensities_;
float rev_start_deg_ = 0.f; // device angle of the revolution's first point
float angle_inc_deg_ = 0.f;
uint32_t points_total_ = 0; // measure_size from the header; 0 = no rev open
uint16_t pending_time_ = 0; // header "time" field, unit unverified
// Latched from the newest "WSimu" area frame, if the device sends any.
std::optional<uint16_t> espe_error_status_;
// Snapshot for get_diagnostics(); refreshed by finish_scan().
Diagnostics latest_diag_;
ScanResult ready_result_;
bool scan_ready_ = false;
};
} // namespace lidarlib

9
include/lidarlib/lidar.hpp
View File

@@ -47,6 +47,10 @@ struct ExtraInfo {
// SICK nanoScan3 only — General System State byte 0 (see kNanoState* bits). // SICK nanoScan3 only — General System State byte 0 (see kNanoState* bits).
std::optional<uint8_t> nano_general_state; std::optional<uint8_t> nano_general_state;
// ESPE LGA60 only — fault word from the newest "WSimu" area frame; the
// device only sends those when area data is polled, so usually nullopt.
std::optional<uint16_t> espe_error_status;
}; };
struct ScanResult { struct ScanResult {
@@ -101,7 +105,11 @@ public:
// timeout_ms = 0 → block indefinitely. No default on purpose: drivers // timeout_ms = 0 → block indefinitely. No default on purpose: drivers
// differ (OLEI 1000, SICK 2000). // differ (OLEI 1000, SICK 2000).
virtual bool recv_scan(ScanResult& out, int timeout_ms) = 0; virtual bool recv_scan(ScanResult& out, int timeout_ms) = 0;
// The callback fires only from spin_once() — recv_scan() never invokes
// it. Pick one pump style: recv_scan() to poll, or callback + spin_once().
virtual void set_scan_callback(ScanCallback cb) = 0; virtual void set_scan_callback(ScanCallback cb) = 0;
// Process one unit of input (may block on the socket while the device is
// silent); fires the scan callback when a scan completed. False on error.
virtual bool spin_once() = 0; virtual bool spin_once() = 0;
virtual const char* detected_model() const = 0; virtual const char* detected_model() const = 0;
@@ -151,6 +159,7 @@ public:
Diagnostics get_diagnostics() const override { return latest_diag_; } Diagnostics get_diagnostics() const override { return latest_diag_; }
private: private:
bool poll_packet(); // one recvfrom() + dispatch to the family parser
bool parse_family_a(const uint8_t* buf, int len); // ID=0xFAF0 bool parse_family_a(const uint8_t* buf, int len); // ID=0xFAF0
bool parse_family_b(const uint8_t* buf, int len); // ID=0xFEF0 bool parse_family_b(const uint8_t* buf, int len); // ID=0xFEF0
bool parse_family_c(const uint8_t* buf, int len); // Magic=0xFEAC (GS1-5) bool parse_family_c(const uint8_t* buf, int len); // Magic=0xFEAC (GS1-5)

1
include/lidarlib/lidarlib.hpp
View File

@@ -3,4 +3,5 @@
#include "lidarlib/error.hpp" #include "lidarlib/error.hpp"
#include "lidarlib/lidar.hpp" #include "lidarlib/lidar.hpp"
#include "lidarlib/sick_lidar.hpp" #include "lidarlib/sick_lidar.hpp"
#include "lidarlib/espe_lidar.hpp"
#include "lidarlib/config.hpp" #include "lidarlib/config.hpp"

14
include/lidarlib/sick_lidar.hpp
View File

@@ -20,9 +20,11 @@ class SickDriver : public Lidar {
public: public:
using ScanCallback = lidarlib::ScanCallback; using ScanCallback = lidarlib::ScanCallback;
// inverted: unit mounted upside-down → mirror the scan.
explicit SickDriver(const ModelConfig& cfg, explicit SickDriver(const ModelConfig& cfg,
const std::string& ip, const std::string& ip,
uint16_t port = 2111); uint16_t port = 2111,
bool inverted = false);
~SickDriver(); ~SickDriver();
SickDriver(const SickDriver&) = delete; SickDriver(const SickDriver&) = delete;
@@ -50,6 +52,7 @@ private:
std::string detected_model_name_; // owned copy of cfg_.name (stable lifetime) std::string detected_model_name_; // owned copy of cfg_.name (stable lifetime)
std::string ip_; std::string ip_;
uint16_t port_; uint16_t port_;
bool inverted_ = false;
int sock_fd_ = -1; int sock_fd_ = -1;
ScanCallback cb_; ScanCallback cb_;
@@ -70,10 +73,12 @@ class NanoScanDriver : public Lidar {
public: public:
using ScanCallback = lidarlib::ScanCallback; using ScanCallback = lidarlib::ScanCallback;
// ip: local bind address; port: local UDP port the sensor sends to. // ip: local bind address; port: local UDP port the sensor sends to;
// inverted: unit mounted upside-down → mirror the scan.
explicit NanoScanDriver(const ModelConfig& cfg, explicit NanoScanDriver(const ModelConfig& cfg,
const std::string& ip = "0.0.0.0", const std::string& ip = "0.0.0.0",
uint16_t port = 6060); uint16_t port = 6060,
bool inverted = false);
~NanoScanDriver(); ~NanoScanDriver();
NanoScanDriver(const NanoScanDriver&) = delete; NanoScanDriver(const NanoScanDriver&) = delete;
@@ -99,6 +104,7 @@ private:
std::string detected_model_name_; // owned copy of cfg_.name (stable lifetime) std::string detected_model_name_; // owned copy of cfg_.name (stable lifetime)
std::string ip_; std::string ip_;
uint16_t port_; uint16_t port_;
bool inverted_ = false;
int sock_fd_ = -1; int sock_fd_ = -1;
ScanCallback cb_; ScanCallback cb_;

265
src/espe_lidar.cpp Normal file
View File

@@ -0,0 +1,265 @@
#include "lidarlib/espe_lidar.hpp"
#include "lidar_bytes.hpp"
#include "lidar_net.hpp"
#include <algorithm>
#include <cerrno>
#include <cmath>
#include <cstring>
#include <limits>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <sys/select.h>
#include <netinet/in.h>
namespace lidarlib {
namespace {
// "RAuto" + fixed tail — puts the device into continuous measurement output.
constexpr uint8_t kStartCapture[8] = {0x52, 0x41, 0x75, 0x74, 0x6F, 0x01, 0x87, 0x80};
constexpr char kRangeMagic[4] = {'H', 'I', 'S', 'N'};
constexpr char kAreaMagic[5] = {'W', 'S', 'i', 'm', 'u'};
constexpr size_t kRangeHeaderSize = 16; // magic + 6 big-endian u16 fields
constexpr size_t kAreaFrameSize = 13; // magic + 4 status bytes + err u16 + crc u16
constexpr uint16_t kMaxDistanceMm = 50000; // wire sentinel: beyond = no return
constexpr uint16_t kMaxIntensity = 30000;
constexpr uint32_t kMaxPointsPerRev = 12800; // 320° at the finest 0.025° step
constexpr int kConnectTimeoutMs = 2000;
uint16_t be16(const uint8_t* p) {
return static_cast<uint16_t>((p[0] << 8) | p[1]);
}
} // namespace
EspeDriver::EspeDriver(const ModelConfig& cfg, const std::string& ip,
uint16_t port, bool use_udp, bool inverted)
: cfg_(cfg), detected_model_name_(cfg.name ? cfg.name : ""), ip_(ip),
port_(port), use_udp_(use_udp), inverted_(inverted) {}
EspeDriver::~EspeDriver() { close(); }
ErrorCode EspeDriver::open() {
if (is_open()) return set_error(ErrorCode::AlreadyOpen);
sockaddr_in addr{};
addr.sin_family = AF_INET;
addr.sin_port = htons(port_);
if (::inet_pton(AF_INET, ip_.c_str(), &addr.sin_addr) != 1)
return set_error(ErrorCode::InvalidAddress);
sock_fd_ = ::socket(AF_INET, use_udp_ ? SOCK_DGRAM : SOCK_STREAM, 0);
if (sock_fd_ < 0) return set_error(ErrorCode::SocketError);
ErrorCode conn_err = ErrorCode::Ok;
if (use_udp_) {
// connect() on UDP just fixes the peer; replies come to our port.
if (::connect(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0)
conn_err = ErrorCode::ConnectionFailed;
} else {
conn_err = connect_tcp_with_timeout(sock_fd_, addr, kConnectTimeoutMs);
}
if (conn_err != ErrorCode::Ok) {
::close(sock_fd_);
sock_fd_ = -1;
return set_error(conn_err);
}
recv_buf_.clear();
points_total_ = 0;
pending_time_ = 0;
scan_ready_ = false;
espe_error_status_.reset();
latest_diag_ = Diagnostics{};
// Device is passive until told to stream.
ssize_t n = ::send(sock_fd_, kStartCapture, sizeof(kStartCapture), 0);
if (n != static_cast<ssize_t>(sizeof(kStartCapture))) {
close();
return set_error(ErrorCode::HandshakeFailed);
}
return set_error(ErrorCode::Ok);
}
void EspeDriver::close() {
if (sock_fd_ >= 0) {
::close(sock_fd_);
sock_fd_ = -1;
}
}
bool EspeDriver::fill_buffer(int timeout_ms) {
if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }
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) {
set_error(r == 0 ? ErrorCode::Timeout : ErrorCode::DeviceDisconnected);
return false;
}
}
char buf[4096];
ssize_t n = ::recv(sock_fd_, buf, sizeof(buf), 0);
if (n <= 0) { set_error(ErrorCode::DeviceDisconnected); return false; }
recv_buf_.append(buf, static_cast<size_t>(n));
return true;
}
// Consume complete frames from recv_buf_; returns true once a full revolution
// has been assembled (ready_result_/scan_ready_ set by finish_scan()).
bool EspeDriver::parse_buffer() {
for (;;) {
size_t range_pos = recv_buf_.find(kRangeMagic, 0, sizeof(kRangeMagic));
size_t area_pos = recv_buf_.find(kAreaMagic, 0, sizeof(kAreaMagic));
size_t pos = std::min(range_pos, area_pos);
if (pos == std::string::npos) {
// No magic in sight: keep only a possible partial magic at the tail.
if (recv_buf_.size() > sizeof(kAreaMagic) - 1)
recv_buf_.erase(0, recv_buf_.size() - (sizeof(kAreaMagic) - 1));
return scan_ready_;
}
if (pos > 0) recv_buf_.erase(0, pos);
const uint8_t* d = reinterpret_cast<const uint8_t*>(recv_buf_.data());
if (area_pos < range_pos) {
if (recv_buf_.size() < kAreaFrameSize) return scan_ready_;
// Zone/obstacle frame — only sent when the host polls areas, but
// it carries the device fault word, so latch it if it appears.
// Byte order unverified on hardware: the protocol is mixed-endian
// (header fields big-endian, point payload little-endian) and no
// spec covers this field; little-endian assumed like the payload.
espe_error_status_ = le16(d + 9);
recv_buf_.erase(0, kAreaFrameSize);
continue;
}
if (recv_buf_.size() < kRangeHeaderSize) return scan_ready_;
uint16_t data_size = be16(d + 8);
uint16_t measure_size = be16(d + 12);
if (measure_size == 0 || measure_size > kMaxPointsPerRev) {
recv_buf_.erase(0, sizeof(kRangeMagic)); // bogus header — resync
continue;
}
if (data_size > measure_size) data_size = measure_size;
size_t frame_size = kRangeHeaderSize + static_cast<size_t>(data_size) * 4;
if (recv_buf_.size() < frame_size) return scan_ready_;
handle_range_frame(d, data_size);
recv_buf_.erase(0, frame_size);
// Stop as soon as a revolution completes — draining further frames
// could finish a second revolution and overwrite ready_result_ before
// the caller consumes it. Leftover bytes wait for the next call.
if (scan_ready_) return true;
}
}
// Range frame: "HISN", then big-endian u16 start_angle, end_angle (deg),
// data_size (points in this frame), data_position (cumulative points incl.
// this frame), measure_size (points per revolution), time; then data_size ×
// 4 B little-endian (u16 distance mm, u16 intensity).
void EspeDriver::handle_range_frame(const uint8_t* frame, uint16_t data_size) {
uint16_t start_angle = be16(frame + 4);
uint16_t end_angle = be16(frame + 6);
uint16_t data_position = be16(frame + 10);
uint16_t measure_size = be16(frame + 12);
pending_time_ = be16(frame + 14);
// First frame of a revolution (or geometry changed) → start a new one.
if (points_total_ != measure_size || data_position <= data_size) {
points_total_ = measure_size;
rev_start_deg_ = static_cast<float>(start_angle);
angle_inc_deg_ = static_cast<float>(end_angle - start_angle) / measure_size;
pending_ranges_.assign(points_total_, 0.f);
pending_intensities_.assign(points_total_, 0.f);
}
if (angle_inc_deg_ <= 0.f) { points_total_ = 0; return; }
// start_angle is normally constant across the revolution, so this is just
// the cumulative position; the angle term covers firmware that advances it.
int32_t begin = static_cast<int32_t>(std::lround(
(static_cast<float>(start_angle) - rev_start_deg_) / angle_inc_deg_))
+ static_cast<int32_t>(data_position) - static_cast<int32_t>(data_size);
const uint8_t* p = frame + kRangeHeaderSize;
for (uint16_t i = 0; i < data_size; ++i, p += 4) {
int32_t idx = begin + i;
if (idx < 0 || idx >= static_cast<int32_t>(points_total_)) continue;
uint16_t dist = le16(p + 0);
uint16_t inten = le16(p + 2);
pending_ranges_[idx] = (dist > kMaxDistanceMm)
? std::numeric_limits<float>::infinity()
: static_cast<float>(dist) * 1e-3f; // mm -> m
// Wire intensity is 0..30000 — rescale to the 0-255 LaserScan contract.
pending_intensities_[idx] =
static_cast<float>(inten > kMaxIntensity ? kMaxIntensity : inten)
* (255.f / kMaxIntensity);
}
if (data_position >= points_total_) finish_scan();
}
void EspeDriver::finish_scan() {
LaserScan& scan = ready_result_.scan;
scan = LaserScan{};
scan.timestamp_ms = pending_time_; // header "time" field, unit unverified
scan.ranges = std::move(pending_ranges_);
scan.intensities = std::move(pending_intensities_);
scan.angle_min = (rev_start_deg_ + cfg_.angle_offset_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.range_min = cfg_.range_min_m;
scan.range_max = cfg_.range_max_m;
if (inverted_)
invert_scan(scan);
if (cfg_.remap_angles)
remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max);
ExtraInfo& info = ready_result_.info;
info = ExtraInfo{};
info.detected_model = cfg_.name;
info.espe_error_status = espe_error_status_;
latest_diag_ = decode_diagnostics(info);
latest_diag_.device_timestamp_ms = scan.timestamp_ms;
pending_ranges_.clear();
pending_intensities_.clear();
points_total_ = 0;
scan_ready_ = true;
}
bool EspeDriver::recv_scan(ScanResult& out, int timeout_ms) {
for (;;) {
if (parse_buffer()) {
scan_ready_ = false;
out = std::move(ready_result_);
set_error(ErrorCode::Ok);
return true;
}
if (!fill_buffer(timeout_ms)) return false;
}
}
bool EspeDriver::spin_once() {
if (!parse_buffer()) {
if (!fill_buffer(0)) return false;
parse_buffer();
}
if (scan_ready_) {
scan_ready_ = false;
if (cb_) cb_(ready_result_);
}
return true;
}
} // namespace lidarlib

11
src/lidar_bytes.hpp
View File

@@ -1,6 +1,7 @@
// Internal helpers shared by the driver TUs — not part of the public API. // Internal helpers shared by the driver TUs — not part of the public API.
#pragma once #pragma once
#include "lidarlib/lidar.hpp" #include "lidarlib/lidar.hpp"
#include <algorithm>
#include <cstdint> #include <cstdint>
#include <cstring> #include <cstring>
@@ -20,6 +21,16 @@ inline void remap_scan_window(LaserScan& scan, float min_deg, float max_deg) {
scan.angle_max = new_max; scan.angle_max = new_max;
} }
// Mirror a finished scan for a unit mounted upside-down: reverse the point
// order and negate the angular window. Apply before remap_scan_window().
inline void invert_scan(LaserScan& scan) {
std::reverse(scan.ranges.begin(), scan.ranges.end());
std::reverse(scan.intensities.begin(), scan.intensities.end());
const float new_min = -scan.angle_max;
scan.angle_max = -scan.angle_min;
scan.angle_min = new_min;
}
// Little-endian readers (bounds are the caller's responsibility). // Little-endian readers (bounds are the caller's responsibility).
inline uint8_t le_u8 (const uint8_t* p) { return p[0]; } inline uint8_t le_u8 (const uint8_t* p) { return p[0]; }
inline uint16_t le16(const uint8_t* p) { inline uint16_t le16(const uint8_t* p) {

27
src/lidar_config.cpp
View File

@@ -1,5 +1,6 @@
#include "lidarlib/config.hpp" #include "lidarlib/config.hpp"
#include "lidarlib/sick_lidar.hpp" #include "lidarlib/sick_lidar.hpp"
#include "lidarlib/espe_lidar.hpp"
#include "json_mini.hpp" #include "json_mini.hpp"
#include <algorithm> #include <algorithm>
#include <fstream> #include <fstream>
@@ -26,6 +27,7 @@ constexpr ModelEntry kModels[] = {
{ "SICK-TIM571", &MODEL_SICK_TIM571, "SICK" }, { "SICK-TIM571", &MODEL_SICK_TIM571, "SICK" },
{ "SICK-TIM7xx", &MODEL_SICK_TIM7XX, "SICK" }, { "SICK-TIM7xx", &MODEL_SICK_TIM7XX, "SICK" },
{ "SICK-nanoScan3", &MODEL_SICK_NANOSCAN3, "SICK" }, { "SICK-nanoScan3", &MODEL_SICK_NANOSCAN3, "SICK" },
{ "ESPE-LGA60", &MODEL_ESPE_LGA60, "ESPE" },
}; };
json::Value to_json(const LidarConfig& c) { json::Value to_json(const LidarConfig& c) {
@@ -36,6 +38,7 @@ json::Value to_json(const LidarConfig& c) {
v.set("brand", json::Value::make_string(c.brand)); v.set("brand", json::Value::make_string(c.brand));
v.set("model", json::Value::make_string(c.model)); v.set("model", json::Value::make_string(c.model));
v.set("inverted", json::Value::make_bool(c.inverted)); v.set("inverted", json::Value::make_bool(c.inverted));
v.set("use_udp", json::Value::make_bool(c.use_udp));
v.set("angle_min_deg", json::Value::make_number(c.angle_min_deg)); v.set("angle_min_deg", json::Value::make_number(c.angle_min_deg));
v.set("angle_max_deg", json::Value::make_number(c.angle_max_deg)); v.set("angle_max_deg", json::Value::make_number(c.angle_max_deg));
return v; return v;
@@ -49,6 +52,7 @@ LidarConfig lidar_from_json(const json::Value& v, const LidarConfig& def) {
c.brand = v.get_string("brand", def.brand); c.brand = v.get_string("brand", def.brand);
c.model = v.get_string("model", def.model); c.model = v.get_string("model", def.model);
c.inverted = v.get_bool("inverted", def.inverted); c.inverted = v.get_bool("inverted", def.inverted);
c.use_udp = v.get_bool("use_udp", def.use_udp);
c.angle_min_deg = static_cast<float>(v.get_number("angle_min_deg", def.angle_min_deg)); c.angle_min_deg = static_cast<float>(v.get_number("angle_min_deg", def.angle_min_deg));
c.angle_max_deg = static_cast<float>(v.get_number("angle_max_deg", def.angle_max_deg)); c.angle_max_deg = static_cast<float>(v.get_number("angle_max_deg", def.angle_max_deg));
return c; return c;
@@ -80,7 +84,7 @@ const std::vector<std::string>& model_names() {
} }
const std::vector<std::string>& brand_names() { const std::vector<std::string>& brand_names() {
static const std::vector<std::string> names = {"OLEI", "SICK"}; static const std::vector<std::string> names = {"OLEI", "SICK", "ESPE"};
return names; return names;
} }
@@ -137,11 +141,20 @@ void save_config(const std::string& path, const Config& cfg) {
} }
std::unique_ptr<Lidar> make_lidar(const LidarConfig& cfg) { std::unique_ptr<Lidar> make_lidar(const LidarConfig& cfg) {
// Anything but the exact string "SICK" is OLEI (keeps brand-less configs working). // Anything but the exact strings "SICK"/"ESPE" is OLEI (keeps brand-less
// configs working). Brand name and fallback model are decided together so
// a new brand adds exactly one branch here plus one construction case.
const bool is_sick = (cfg.brand == "SICK"); const bool is_sick = (cfg.brand == "SICK");
const bool is_espe = (cfg.brand == "ESPE");
const ModelConfig* model = model_by_name_for_brand(cfg.model, is_sick ? "SICK" : "OLEI"); const char* brand;
if (!model) model = is_sick ? &MODEL_SICK_TIM571 : &MODEL_AUTO; const ModelConfig* fallback;
if (is_sick) { brand = "SICK"; fallback = &MODEL_SICK_TIM571; }
else if (is_espe) { brand = "ESPE"; fallback = &MODEL_ESPE_LGA60; }
else { brand = "OLEI"; fallback = &MODEL_AUTO; }
const ModelConfig* model = model_by_name_for_brand(cfg.model, brand);
if (!model) model = fallback;
ModelConfig mc = *model; ModelConfig mc = *model;
if (cfg.angle_min_deg > -360.f || cfg.angle_max_deg < 360.f) { if (cfg.angle_min_deg > -360.f || cfg.angle_max_deg < 360.f) {
@@ -152,9 +165,11 @@ std::unique_ptr<Lidar> make_lidar(const LidarConfig& cfg) {
if (is_sick) { if (is_sick) {
if (model == &MODEL_SICK_NANOSCAN3) if (model == &MODEL_SICK_NANOSCAN3)
return std::make_unique<NanoScanDriver>(mc, cfg.ip, cfg.port); return std::make_unique<NanoScanDriver>(mc, cfg.ip, cfg.port, cfg.inverted);
return std::make_unique<SickDriver>(mc, cfg.ip, cfg.port); return std::make_unique<SickDriver>(mc, cfg.ip, cfg.port, cfg.inverted);
} }
if (is_espe)
return std::make_unique<EspeDriver>(mc, cfg.ip, cfg.port, cfg.use_udp, cfg.inverted);
return std::make_unique<Driver>(mc, cfg.ip, cfg.port, cfg.inverted); return std::make_unique<Driver>(mc, cfg.ip, cfg.port, cfg.inverted);
} }

51
src/lidar_net.hpp Normal file
View File

@@ -0,0 +1,51 @@
// Internal socket helpers shared by the TCP driver TUs — not part of the public API.
#pragma once
#include "lidarlib/error.hpp"
#include <cerrno>
#include <fcntl.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <sys/select.h>
#include <sys/socket.h>
namespace lidarlib {
// Non-blocking connect with a bounded timeout — a blocking connect() to an
// unreachable device would stall for the OS default (~2 min on Linux).
// Enables TCP_NODELAY on success; the fd is returned to blocking mode either
// way. The caller owns the fd and closes it on failure.
inline ErrorCode connect_tcp_with_timeout(int fd, const sockaddr_in& addr, int timeout_ms) {
int flags = ::fcntl(fd, F_GETFL, 0);
::fcntl(fd, F_SETFL, flags | O_NONBLOCK);
ErrorCode conn_err = ErrorCode::Ok;
int rc = ::connect(fd, reinterpret_cast<const sockaddr*>(&addr), sizeof(addr));
if (rc < 0 && errno != EINPROGRESS) {
conn_err = (errno == ECONNREFUSED) ? ErrorCode::ConnectionRefused
: ErrorCode::ConnectionFailed;
} else if (rc < 0) {
fd_set wfds; FD_ZERO(&wfds); FD_SET(fd, &wfds);
timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 };
rc = ::select(fd + 1, nullptr, &wfds, nullptr, &tv);
if (rc == 0) {
conn_err = ErrorCode::Timeout;
} else if (rc < 0) {
conn_err = ErrorCode::ConnectionFailed;
} else {
int err = 0; socklen_t errlen = sizeof(err);
::getsockopt(fd, SOL_SOCKET, SO_ERROR, &err, &errlen);
if (err != 0)
conn_err = (err == ECONNREFUSED) ? ErrorCode::ConnectionRefused
: ErrorCode::ConnectionFailed;
}
}
::fcntl(fd, F_SETFL, flags);
if (conn_err == ErrorCode::Ok) {
int nodelay = 1;
::setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, &nodelay, sizeof(nodelay));
}
return conn_err;
}
} // namespace lidarlib

14
src/olei_lidar.cpp
View File

@@ -52,6 +52,7 @@ Diagnostics decode_diagnostics(const ExtraInfo& info) {
d.status_flags = info.status_flags; d.status_flags = info.status_flags;
d.sick_device_status = info.sick_device_status; d.sick_device_status = info.sick_device_status;
d.nano_general_state = info.nano_general_state; d.nano_general_state = info.nano_general_state;
d.espe_error_status = info.espe_error_status;
return d; return d;
} }
@@ -125,7 +126,7 @@ bool Driver::recv_scan(ScanResult& out, int timeout_ms) {
return false; return false;
} }
} }
if (!spin_once()) return false; if (!poll_packet()) return false;
} }
out = std::move(ready_result_); out = std::move(ready_result_);
set_error(ErrorCode::Ok); set_error(ErrorCode::Ok);
@@ -133,6 +134,15 @@ bool Driver::recv_scan(ScanResult& out, int timeout_ms) {
} }
bool Driver::spin_once() { bool Driver::spin_once() {
if (!poll_packet()) return false;
if (scan_ready_) {
scan_ready_ = false;
if (cb_) cb_(ready_result_);
}
return true;
}
bool Driver::poll_packet() {
if (!is_open()) { set_error(ErrorCode::NotOpen); return false; } if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }
uint8_t* buf = recv_buf_; uint8_t* buf = recv_buf_;
sockaddr_in from{}; sockaddr_in from{};
@@ -201,8 +211,6 @@ void Driver::flush_scan() {
pending_intensity_.clear(); pending_intensity_.clear();
pending_info_ = ExtraInfo{}; pending_info_ = ExtraInfo{};
scan_ready_ = true; scan_ready_ = true;
if (cb_) cb_(ready_result_);
} }
// Family A (0xFAF0): 20B header + 3B blocks (u16 dist, u8 intensity). // Family A (0xFAF0): 20B header + 3B blocks (u16 dist, u8 intensity).

59
src/sick_lidar.cpp
View File

@@ -1,5 +1,6 @@
#include "lidarlib/sick_lidar.hpp" #include "lidarlib/sick_lidar.hpp"
#include "lidar_bytes.hpp" #include "lidar_bytes.hpp"
#include "lidar_net.hpp"
#include <cctype> #include <cctype>
#include <cerrno> #include <cerrno>
@@ -8,13 +9,11 @@
#include <cstring> #include <cstring>
#include <limits> #include <limits>
#include <vector> #include <vector>
#include <fcntl.h>
#include <sys/socket.h> #include <sys/socket.h>
#include <arpa/inet.h> #include <arpa/inet.h>
#include <unistd.h> #include <unistd.h>
#include <sys/select.h> #include <sys/select.h>
#include <netinet/in.h> #include <netinet/in.h>
#include <netinet/tcp.h>
namespace lidarlib { namespace lidarlib {
@@ -48,8 +47,10 @@ constexpr size_t kNanoRecvBufSize = 65536;
constexpr double kNanoAngleResolution = 4194304.0; constexpr double kNanoAngleResolution = 4194304.0;
} // namespace } // namespace
SickDriver::SickDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port) SickDriver::SickDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port,
: cfg_(cfg), detected_model_name_(cfg.name ? cfg.name : ""), ip_(ip), port_(port) {} bool inverted)
: cfg_(cfg), detected_model_name_(cfg.name ? cfg.name : ""), ip_(ip), port_(port),
inverted_(inverted) {}
SickDriver::~SickDriver() { close(); } SickDriver::~SickDriver() { close(); }
@@ -65,43 +66,13 @@ ErrorCode SickDriver::open() {
sock_fd_ = ::socket(AF_INET, SOCK_STREAM, 0); sock_fd_ = ::socket(AF_INET, SOCK_STREAM, 0);
if (sock_fd_ < 0) return set_error(ErrorCode::SocketError); if (sock_fd_ < 0) return set_error(ErrorCode::SocketError);
// Non-blocking connect with a bounded timeout — a blocking connect() to an ErrorCode conn_err = connect_tcp_with_timeout(sock_fd_, addr, kConnectTimeoutMs);
// unreachable device would stall for the OS default (~2 min on Linux).
int flags = ::fcntl(sock_fd_, F_GETFL, 0);
::fcntl(sock_fd_, F_SETFL, flags | O_NONBLOCK);
ErrorCode conn_err = ErrorCode::Ok;
int rc = ::connect(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr));
if (rc < 0 && errno != EINPROGRESS) {
conn_err = (errno == ECONNREFUSED) ? ErrorCode::ConnectionRefused
: ErrorCode::ConnectionFailed;
} else if (rc < 0) {
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) {
conn_err = ErrorCode::Timeout;
} else if (rc < 0) {
conn_err = ErrorCode::ConnectionFailed;
} else {
int err = 0; socklen_t errlen = sizeof(err);
::getsockopt(sock_fd_, SOL_SOCKET, SO_ERROR, &err, &errlen);
if (err != 0)
conn_err = (err == ECONNREFUSED) ? ErrorCode::ConnectionRefused
: ErrorCode::ConnectionFailed;
}
}
::fcntl(sock_fd_, F_SETFL, flags);
if (conn_err != ErrorCode::Ok) { if (conn_err != ErrorCode::Ok) {
::close(sock_fd_); ::close(sock_fd_);
sock_fd_ = -1; sock_fd_ = -1;
return set_error(conn_err); return set_error(conn_err);
} }
int nodelay = 1;
::setsockopt(sock_fd_, IPPROTO_TCP, TCP_NODELAY, &nodelay, sizeof(nodelay));
recv_buf_.clear(); recv_buf_.clear();
latest_diag_ = Diagnostics{}; latest_diag_ = Diagnostics{};
@@ -261,7 +232,9 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out)
scan.ranges[d] = static_cast<float>(raw) * scale * 0.001f; // mm -> m scan.ranges[d] = static_cast<float>(raw) * scale * 0.001f; // mm -> m
got_dist = true; got_dist = true;
} else if (is_rssi && d < scan.intensities.size()) { } else if (is_rssi && d < scan.intensities.size()) {
scan.intensities[d] = static_cast<float>(raw) * scale; // Clamp to the 0-255 LaserScan contract (16-bit RSSI can exceed it).
float v = static_cast<float>(raw) * scale;
scan.intensities[d] = v > 255.f ? 255.f : v;
} }
} }
}; };
@@ -286,6 +259,8 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out)
if (scan.intensities.size() != scan.ranges.size()) if (scan.intensities.size() != scan.ranges.size())
scan.intensities.assign(scan.ranges.size(), 0.f); scan.intensities.assign(scan.ranges.size(), 0.f);
if (inverted_)
invert_scan(scan);
if (cfg_.remap_angles) if (cfg_.remap_angles)
remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max); remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max);
@@ -306,9 +281,10 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out)
// ── NanoScanDriver — SICK nanoScan3/microScan3 safety-scanner UDP output ──── // ── NanoScanDriver — SICK nanoScan3/microScan3 safety-scanner UDP output ────
NanoScanDriver::NanoScanDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port) NanoScanDriver::NanoScanDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port,
bool inverted)
: cfg_(cfg), detected_model_name_(cfg.name ? cfg.name : ""), ip_(ip), port_(port), : cfg_(cfg), detected_model_name_(cfg.name ? cfg.name : ""), ip_(ip), port_(port),
recv_buf_(kNanoRecvBufSize) {} inverted_(inverted), recv_buf_(kNanoRecvBufSize) {}
NanoScanDriver::~NanoScanDriver() { close(); } NanoScanDriver::~NanoScanDriver() { close(); }
@@ -371,6 +347,7 @@ int NanoScanDriver::recv_datagram(int timeout_ms) {
// drops that scan and we resync on the next scanNumber. // drops that scan and we resync on the next scanNumber.
bool NanoScanDriver::recv_scan(ScanResult& out, int timeout_ms) { bool NanoScanDriver::recv_scan(ScanResult& out, int timeout_ms) {
std::vector<uint8_t> tele; std::vector<uint8_t> tele;
std::vector<uint8_t> have; // per-byte coverage so duplicate fragments don't count twice
uint32_t cur_scan = 0, total = 0, got = 0; uint32_t cur_scan = 0, total = 0, got = 0;
bool assembling = false; bool assembling = false;
@@ -394,12 +371,14 @@ bool NanoScanDriver::recv_scan(ScanResult& out, int timeout_ms) {
if (!assembling || scan != cur_scan || tl != total) { if (!assembling || scan != cur_scan || tl != total) {
cur_scan = scan; total = tl; got = 0; cur_scan = scan; total = tl; got = 0;
tele.assign(total, 0); tele.assign(total, 0);
have.assign(total, 0);
assembling = true; assembling = true;
} }
if (static_cast<uint64_t>(foff) + pl_len <= total) { if (static_cast<uint64_t>(foff) + pl_len <= total) {
std::memcpy(tele.data() + foff, pl, pl_len); std::memcpy(tele.data() + foff, pl, pl_len);
got += pl_len; for (uint32_t b = 0; b < pl_len; ++b)
if (!have[foff + b]) { have[foff + b] = 1; ++got; }
} }
if (got >= total) { if (got >= total) {
@@ -487,6 +466,8 @@ bool NanoScanDriver::parse_packet(const uint8_t* buf, int len, ScanResult& out)
// Raw device time from the DataHeader — an opaque tag, not ms since power-on. // Raw device time from the DataHeader — an opaque tag, not ms since power-on.
scan.timestamp_ms = le32(buf + 28); scan.timestamp_ms = le32(buf + 28);
if (inverted_)
invert_scan(scan);
if (cfg_.remap_angles) if (cfg_.remap_angles)
remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max); remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max);