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

Compare commits

base: QUYVN:323715eab098c557b287d494f94394aac3328fdf
Branches Tags
QUYVN:main QUYVN:xlidar-driver QUYVN:QUYVN
...
compare: QUYVN:QUYVN
Branches Tags
QUYVN:xlidar-driver QUYVN:QUYVN QUYVN:main

4 Commits
323715eab0 ... QUYVN

Author SHA1 Message Date
QUYVN
49d4e04530 logic ready 2026-07-07 13:46:20 +07:00
QUYVN
917b4fe4c5 update brand ESPE 2026-07-07 10:38:42 +07:00
QUYVN
1c347a4918 update Diagnostics 2026-07-07 08:59:36 +07:00
QUYVN
397b9ab3c5 update ErrorCode, status, lifecycle 2026-07-06 15:55:14 +07:00
22 changed files with 1386 additions and 205 deletions
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()

320
README.md
View File

@@ -1,166 +1,292 @@
# 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);
lidar->open();
if (lidar->open() != lidarlib::ErrorCode::Ok) {
fprintf(stderr, "open: %s\n", lidarlib::to_string(lidar->last_error()));
return 1;
}
lidarlib::ScanResult r; lidarlib::ScanResult r;
lidar->recv_scan(r, 1000); if (lidar->recv_scan(r, 1000)) {
// r.scan : LaserScan — format sensor_msgs/LaserScan của ROS, chung mọi lidar // r.scan : LaserScan — điểm đo, format ROS
// r.info : ExtraInfo — thông tin thêm tuỳ family/model // r.info : ExtraInfo — metadata tuỳ model
printf("%zu diem, model=%s\n", r.scan.ranges.size(), r.info.detected_model.c_str()); } else {
// Timeout / DeviceDisconnected — xem lidar->last_error()
}
``` ```
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.
## Cấu hình (config.json) Thay cho `recv_scan()` blocking:
`lidar_app` là app mẫu headless: đọc `config.json`, mở từng lidar bằng ```cpp
`make_lidar()`, một thread mỗi con. lidar->set_scan_callback([](const lidarlib::ScanResult& r) { /* mỗi vòng quét */ });
while (running) lidar->spin_once();
```bash
./build/lidar_app [my_config.json]
``` ```
### Chẩn đoán thiết bị
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:
[docs/diagnostics.md](docs/diagnostics.md)):
```cpp
lidarlib::Diagnostics d = lidar->get_diagnostics();
if (!d.valid) {
// chưa decode được vòng quét nào
} else if (d.has_fault()) {
// OLEI Family A
d.monitor_fault(); // motor/giám sát bất thường
d.voltage_fault(); // điện áp ngoài dải
d.temperature_fault(); // nhiệt độ bất thường
// SICK
d.sick_error(); // TiM: device error
d.pollution_error(); // TiM: 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
printf("fault: %s\n", lidarlib::to_string(d).c_str());
} else if (d.has_warning()) {
// pollution_warning() / contamination_warning() — kính bẩn nhẹ, nên lau
}
```
`d.healthy()` = đã có dữ liệu và không có fault. Khuyến nghị giám sát: cảm
biến khỏe khi và chỉ khi `recv_scan()` thành công đều đặn **và**
`get_diagnostics().has_fault() == false`.
### Xử lý lỗi & lifecycle
`open()` trả về `ErrorCode`; `last_error()` giữ kết quả của lần gọi gần nhất.
| ErrorCode | Ý nghĩa |
|---|---|
| `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
{ {
"lidars": [ "lidars": [
{"name":"front", "ip":"192.168.100.100", "port":2368, "brand":"OLEI", "model":"AUTO", "inverted":false}, {"name":"front", "ip":"192.168.100.100", "port":2368, "brand":"OLEI", "model":"AUTO", "inverted":false},
{"name":"rear", "ip":"192.168.100.100", "port":2369, "brand":"OLEI", "model":"AUTO", "inverted":true}, {"name":"rear", "ip":"192.168.100.100", "port":2369, "brand":"OLEI", "model":"AUTO", "inverted":true},
{"name":"sick1", "ip":"192.168.0.1", "port":2111, "brand":"SICK", "model":"SICK-TIM571"}, {"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/lidar.hpp` | Data model, interface `Lidar`, driver OLEI, các `MODEL_*` OLEI | | `include/lidarlib/lidarlib.hpp` | Include tổng hợp toàn bộ API |
| `include/lidarlib/sick_lidar.hpp` | `SickDriver`, `NanoScanDriver`, các `MODEL_SICK_*` | | `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/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()` sẽ thất
bại. Kiểm tra: `ss -lunp | grep 2368`.
- `inverted` đã verify bằng sniff sống: `false` góc tăng dần, `true` góc giảm
dần cùng bước.

218
docs/diagnostics.md Normal file
View File

@@ -0,0 +1,218 @@
# Nghiên cứu: Dữ liệu chẩn đoán (diagnosis) của lidar OLEI & SICK
Tài liệu này tổng hợp những gì các gói dữ liệu OLEI mang theo về **tình trạng
thiết bị** (self-diagnostics), ngoài dữ liệu điểm quét. Kết quả nghiên cứu này
là cơ sở cho API `lidarlib::Diagnostics` / `Lidar::get_diagnostics()`
(header `include/lidarlib/diagnostics.hpp`).
Điểm quan trọng: **lidar OLEI không có kênh/query chẩn đoán riêng** — driver
chỉ nhận UDP thụ động, thiết bị không nhận lệnh hỏi trạng thái. Toàn bộ thông
tin chẩn đoán được **nhúng trong header của chính gói dữ liệu quét**, nên "lấy
data diagnosis" = decode header của stream đang chạy, không tốn thêm băng thông
hay round-trip nào.
## 1. Family A (Frame ID `0xFAF0` — VB, VF, LR-1F)
Header 20 byte, thông tin chẩn đoán nằm ở các offset sau:
| Offset | Kích thước | Trường | Ý nghĩa |
|---|---|---|---|
| `[4]` | u8 | `distance_scale` | mm/count — gián tiếp cho biết chế độ đo |
| `[5]` | u8 | **`error_status`** | Byte lỗi thiết bị, xem bảng bit bên dưới |
| `[10-11]` | u16 LE | `rotation_raw` | Tốc độ quay motor (raw, **đơn vị chưa xác minh** — nghi là RPM hoặc Hz×100) |
| `[12-15]` | u32 LE | `timestamp` | Đồng hồ thiết bị (ms) — dùng phát hiện thiết bị treo/reset |
### Bit map của `error_status` (byte `[5]`)
| Bit | Mask | Tên | Ý nghĩa khi = 1 |
|---|---|---|---|
| 0 | `0x01` | Monitor | Khối giám sát/motor bất thường (motor không đạt tốc độ, mất đồng bộ encoder) |
| 1 | `0x02` | Voltage | Điện áp nguồn ngoài dải cho phép |
| 2 | `0x04` | Temperature | Nhiệt độ bên trong bất thường (quá nóng/quá lạnh) |
| 3-7 | `0xF8` | Reserved | Chưa định nghĩa trong tài liệu OLEI; driver vẫn coi ≠0 là fault và log dạng `reserved(0xXX)` |
Ghi chú thực nghiệm:
- Trên thiết bị chạy bình thường byte này luôn `0x00`; chưa tái tạo được
fault thật trên phần cứng (chưa thử hạ áp/che gương), nên ý nghĩa bit lấy
theo tài liệu giao thức OLEI, **chưa verify từng bit bằng lỗi thật**.
- `error_status` lặp lại trong *mỗi packet* (~22°/packet), nhưng driver chỉ
chốt giá trị theo **vòng quét** (lần `flush_scan()` gần nhất) — đủ nhanh
(10-20 Hz) và nhất quán với `ScanResult`.
## 2. Family B (Frame ID `0xFEF0` — LR-1BS5, LR-1FMI)
Header 40 byte mang **tên model ASCII** tại `[7-16]` và `distance_scale` tại
`[6]`, nhưng **không có byte lỗi, không có timestamp**. Đã dò toàn bộ 40 byte
header trên stream thật của LR-1FMI: các byte còn lại là hằng số/counter,
không thấy trường nào đổi giá trị khi thiết bị hoạt động — kết luận Family B
**không phát dữ liệu chẩn đoán trên wire**.
Chẩn đoán khả dụng duy nhất với Family B là **gián tiếp**:
- Mất gói / `ErrorCode::Timeout` từ `recv_scan()` → thiết bị ngắt kết nối
hoặc treo.
- Số điểm mỗi vòng tụt bất thường (LR-1FMI chuẩn ~2400 điểm/vòng) → nghi
bẩn kính / lỗi quang học.
`get_diagnostics()` với Family B trả về `valid = true` sau scan đầu tiên nhưng
`error_status = 0` và mọi trường optional là `nullopt` — nghĩa là "không có
thông tin", **không** đồng nghĩa "thiết bị khỏe".
## 3. Family C / Protocol V3 (Magic `0xFEAC` — GS1-5)
Header 48 byte, giàu thông tin trạng thái nhất (GS1-5 là dòng có field
an toàn kiểu safety-scanner). Các trường chẩn đoán (port từ driver C#
`OleiGS15Driver.cs`, **chưa verify trên phần cứng thật**):
| Offset | Kích thước | Trường | Ý nghĩa (theo driver C#) |
|---|---|---|---|
| `[24-25]` | u16 LE | `scan_frequency_raw` | Tần số quét raw (nghi Hz×100) |
| `[28-29]` | u16 LE | `input_status` | Trạng thái các chân input số |
| `[30-31]` | u16 LE | `output_status` | Trạng thái các chân output số (OSSD?) |
| `[32-35]` | u32 LE | `field_status` | Trạng thái các vùng field an toàn (bit nào = vùng nào chưa rõ) |
| `[44-47]` | u32 LE | `status_flags` | Cờ trạng thái tổng — bit map chưa có tài liệu |
Vì bit map chưa xác minh, driver **truyền nguyên giá trị raw** qua
`Diagnostics` (các trường `std::optional`) thay vì decode sai. Khi có tài
liệu V3 chính thức hoặc thiết bị GS1-5 để thử, bổ sung decode tại
`decode_diagnostics()` trong `src/olei_lidar.cpp`.
## 4. SICK TiM (TCP/CoLa-A — telegram `LMDscandata`)
Khác OLEI, TiM có **hai đường** lấy chẩn đoán:
1. **Nhúng trong stream** (driver dùng đường này): telegram `sSN LMDscandata`
mang cặp **Device Status** ngay sau SerialNumber. Theo SICK Telegram
Listing (8014631):
| Cặp giá trị | Ý nghĩa |
|---|---|
| `0 0` | OK |
| `0 1` | Error — thiết bị lỗi, dữ liệu không tin được |
| `0 2` | Pollution warning — kính bắt đầu bẩn, vẫn đo được |
| `0 4` | Pollution error — kính bẩn nặng, phải lau |
Driver ghép cặp này vào `info.sick_device_status` (`(word0<<8)|word1`) và
decode qua `sick_error()` / `pollution_warning()` / `pollution_error()`.
Ngoài ra telegram còn mang input/output số (`input_status`/`output_status`)
và tần số quét (`scan_frequency_raw`, đơn vị 1/100 Hz).
**Chưa verify trên TiM781S thật với kính bẩn** — cần che/bôi bẩn kính để
xác nhận giá trị 2/4.
2. **Query chủ động qua SOPAS** (chưa implement): `sRN SCdevicestate`
(0=busy, 1=ready, 2=error), `sRN LCMstate` (mức nhiễm bẩn),
`sRN DItype`/`sRN ODoprh` (giờ vận hành). Cần cơ chế request/response
xen giữa stream — xem mục Hướng mở rộng.
## 5. SICK nanoScan3 (UDP safety-data — block General System State)
Packet UDP của nanoScan3 gồm nhiều block, header trỏ tới từng block bằng cặp
offset/size. Block **General System State** (offset tại header `[32]`, size
`[34]`) là block trạng thái an toàn; **byte 0** của block (layout theo
`sick_safetyscanners`, **chưa verify phần cứng**):
| Bit | Mask | Ý nghĩa khi = 1 |
|---|---|---|
| 0 | `0x01` | Run mode active (đang chạy bình thường) |
| 1 | `0x02` | Standby mode |
| 2 | `0x04` | Contamination warning — kính bẩn nhẹ |
| 3 | `0x08` | Contamination error — kính bẩn nặng, vùng an toàn không tin được |
| 4 | `0x10` | Reference contour status |
| 5 | `0x20` | Manipulation — nghi bị che/can thiệp cố ý |
Driver đọc byte này vào `info.nano_general_state`, decode qua
`contamination_warning()` / `contamination_error()` / `manipulation()`.
Lưu ý: block này **chỉ có mặt nếu được tick chọn** trong cấu hình data output
của Safety Designer — thiếu block thì trường giữ `nullopt`.
## 6. Chẩn đoán tầng transport (mọi driver)
Ngoài dữ liệu trên wire, bản thân driver cung cấp lớp chẩn đoán kết nối:
| Tín hiệu | API | Ý nghĩa |
|---|---|---|
| Không mở được socket | `open()` → `ErrorCode` | Lỗi cấu hình host (port bận, IP sai...) |
| Không có gói trong `timeout_ms` | `recv_scan()` = false + `last_error() == Timeout` | Đứt cáp, thiết bị mất nguồn, sai port |
| Lỗi recv giữa chừng | `last_error() == DeviceDisconnected` | Socket lỗi cứng |
| `timestamp_ms` nhảy lùi | so sánh giữa 2 scan (Family A) | Thiết bị vừa reset/reboot |
Chiến lược giám sát khuyến nghị cho app: coi cảm biến **healthy** khi và chỉ
khi `recv_scan()` thành công đều đặn **và** `get_diagnostics().has_fault() == false`.
## 7. Kiểm tra sẵn sàng: `is_ready()` / `wait_ready()`
Chiến lược trên được gói sẵn trong hai hàm của `Lidar`:
```cpp
lidar->open();
if (!lidar->wait_ready(5000)) { // bơm recv_scan() tới khi ready
// timeout: xem last_error() (Timeout, DeviceDisconnected...)
}
// ... vòng lặp chính đang bơm recv_scan()/spin_once() ...
if (!lidar->is_ready()) { /* mất dữ liệu hoặc thiết bị báo fault */ }
```
- `is_ready(max_age_ms = 3000)` = `is_open()` **và** `get_diagnostics().healthy()`
**và** scan decode gần nhất chưa quá `max_age_ms` (truyền `0` để bỏ kiểm tra
tuổi). Kiểm tra tuổi giúp phát hiện cảm biến chết giữa chừng — socket UDP
vẫn "mở" và snapshot diagnostics vẫn "khỏe" dù thiết bị đã ngừng phát.
Diagnostics chỉ được cập nhật bởi `recv_scan()`/`spin_once()`, nên phải có
vòng lặp đang bơm dữ liệu thì `is_ready()` mới có nghĩa.
- `wait_ready(timeout_ms = 5000)` = bơm `recv_scan()` (bỏ qua dữ liệu, không
gọi callback) cho tới khi `is_ready()` hoặc hết giờ — dùng lúc khởi động,
trước khi giao quyền bơm cho vòng lặp chính.
- `is_ready()` **không** tính `has_warning()` (kính bẩn nhẹ vẫn đo được →
vẫn ready); app muốn chặt hơn thì tự kiểm tra thêm
`!get_diagnostics().has_warning()`.
## 8. API
```cpp
#include "lidarlib/lidarlib.hpp"
lidarlib::ScanResult r;
if (lidar->recv_scan(r, 1000)) {
lidarlib::Diagnostics d = lidar->get_diagnostics();
if (!d.valid) {
// chưa có scan nào được decode
} else if (d.has_fault()) {
// Family A: đọc từng bit
if (d.voltage_fault()) /* điện áp bất thường */;
if (d.temperature_fault()) /* nhiệt độ bất thường */;
if (d.monitor_fault()) /* motor/giám sát bất thường */;
printf("lidar fault: %s\n", lidarlib::to_string(d).c_str());
}
// SICK: cảnh báo kính bẩn — chưa phải fault nhưng nên lên lịch lau
if (d.has_warning()) {
d.pollution_warning(); // TiM
d.contamination_warning(); // nanoScan3
}
if (d.manipulation()) /* nanoScan3: nghi bị che/can thiệp */;
// Family C raw (nullopt nếu không phải GS1-5)
if (d.status_flags) printf("status_flags=0x%08X\n", *d.status_flags);
}
```
- `Lidar::get_diagnostics()` — snapshot từ vòng quét decode gần nhất; gọi từ
cùng thread đang bơm `recv_scan()`/`spin_once()` (driver không khóa nội bộ).
- `decode_diagnostics(const ExtraInfo&)` — hàm free, decode trực tiếp từ
`ScanResult::info` nếu app muốn gắn chẩn đoán với đúng scan cụ thể.
- `to_string(Diagnostics)` — chuỗi log 1 dòng: `no data` / `ok` /
`WARN: pollution` / `FAULT: voltage temperature`.
- `has_fault()` gộp mọi nguồn lỗi (OLEI byte lỗi, TiM device/pollution error,
nano contamination error/manipulation); `has_warning()` gộp các mức cảnh
báo kính bẩn.
## 9. Hướng mở rộng
- **SICK SOPAS query chủ động**: `sRN SCdevicestate` (0=busy, 1=ready,
2=error), `sRN LCMstate` (mức nhiễm bẩn chi tiết) — cần cơ chế
request/response xen giữa stream `LMDscandata`.
- **Family A bit 3-7**: cần bảng tra từ OLEI hoặc thử nghiệm gây lỗi có chủ
đích (hạ áp nguồn, chặn tản nhiệt) để xác minh.
- **GS1-5**: cần thiết bị thật để xác minh toàn bộ Family C.
- **TiM pollution 2/4**: cần thử che/bôi bẩn kính TiM781S thật để xác nhận.

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

24
examples/example.cpp
View File

@@ -5,8 +5,19 @@
int main() { int main() {
lidarlib::Driver drv(lidarlib::MODEL_VB); lidarlib::Driver drv(lidarlib::MODEL_VB);
if (!drv.open()) { lidarlib::ErrorCode err = drv.open();
fprintf(stderr, "Không mở được socket\n"); if (err != lidarlib::ErrorCode::Ok) {
fprintf(stderr, "Không mở được socket: %s\n", lidarlib::to_string(err));
return 1;
}
// Chờ cảm biến sẵn sàng: đã nhận được ít nhất một scan hoàn chỉnh
// và thiết bị không báo lỗi (motor/điện áp/nhiệt độ...)
if (!drv.wait_ready(5000)) {
fprintf(stderr, "Cảm biến chưa sẵn sàng: %s (diag: %s)\n",
lidarlib::to_string(drv.last_error()),
lidarlib::to_string(drv.get_diagnostics()).c_str());
drv.close();
return 1; return 1;
} }
@@ -22,6 +33,15 @@ int main() {
i, scan.ranges.size(), scan.timestamp_ms, info.error_status, i, scan.ranges.size(), scan.timestamp_ms, info.error_status,
info.detected_model.c_str()); info.detected_model.c_str());
// Chẩn đoán thiết bị từ scan mới nhất
lidarlib::Diagnostics diag = drv.get_diagnostics();
printf(" diag: %s\n", lidarlib::to_string(diag).c_str());
if (diag.has_fault()) {
if (diag.monitor_fault()) printf(" !! lỗi monitor/motor\n");
if (diag.voltage_fault()) printf(" !! điện áp bất thường\n");
if (diag.temperature_fault()) printf(" !! nhiệt độ bất thường\n");
}
for (size_t j = 0; j < 20 && j < scan.ranges.size(); ++j) { for (size_t j = 0; j < 20 && j < scan.ranges.size(); ++j) {
float angle_deg = (scan.angle_min + j * scan.angle_increment) * 180.f / 3.14159265f; float angle_deg = (scan.angle_min + j * scan.angle_increment) * 180.f / 3.14159265f;
printf(" [%zu] angle=%.2f° dist=%.3fm intensity=%.0f\n", printf(" [%zu] angle=%.2f° dist=%.3fm intensity=%.0f\n",

8
examples/lidar_app.cpp
View File

@@ -15,9 +15,11 @@ void on_signal(int) { g_running = false; }
void run_lidar(lidarlib::LidarConfig cfg) { void run_lidar(lidarlib::LidarConfig cfg) {
std::unique_ptr<lidarlib::Lidar> lidar = lidarlib::make_lidar(cfg); std::unique_ptr<lidarlib::Lidar> lidar = lidarlib::make_lidar(cfg);
if (!lidar->open()) { lidarlib::ErrorCode err = lidar->open();
fprintf(stderr, "[%s] khong mo duoc %s %s:%u\n", if (err != lidarlib::ErrorCode::Ok) {
cfg.name.c_str(), cfg.brand.c_str(), cfg.ip.c_str(), cfg.port); fprintf(stderr, "[%s] khong mo duoc %s %s:%u (%s)\n",
cfg.name.c_str(), cfg.brand.c_str(), cfg.ip.c_str(), cfg.port,
lidarlib::to_string(err));
return; return;
} }
printf("[%s] da mo %s %s:%u (model=%s, inverted=%d)\n", printf("[%s] da mo %s %s:%u (model=%s, inverted=%d)\n",

6
examples/nanoscan_example.cpp
View File

@@ -6,8 +6,10 @@
int main() { int main() {
lidarlib::NanoScanDriver drv(lidarlib::MODEL_SICK_NANOSCAN3, "0.0.0.0", 6060); lidarlib::NanoScanDriver drv(lidarlib::MODEL_SICK_NANOSCAN3, "0.0.0.0", 6060);
if (!drv.open()) { lidarlib::ErrorCode err = drv.open();
fprintf(stderr, "Không mở được UDP socket cho nanoScan3\n"); if (err != lidarlib::ErrorCode::Ok) {
fprintf(stderr, "Không mở được UDP socket cho nanoScan3: %s\n",
lidarlib::to_string(err));
return 1; return 1;
} }

6
examples/sick_example.cpp
View File

@@ -5,8 +5,10 @@
int main() { int main() {
lidarlib::SickDriver drv(lidarlib::MODEL_SICK_TIM571, "192.168.0.1", 2111); lidarlib::SickDriver drv(lidarlib::MODEL_SICK_TIM571, "192.168.0.1", 2111);
if (!drv.open()) { lidarlib::ErrorCode err = drv.open();
fprintf(stderr, "Không kết nối được TCP tới lidar SICK\n"); if (err != lidarlib::ErrorCode::Ok) {
fprintf(stderr, "Không kết nối được TCP tới lidar SICK: %s\n",
lidarlib::to_string(err));
return 1; return 1;
} }

7
examples/test_dual.cpp
View File

@@ -6,9 +6,10 @@
static void run_lidar(const char* tag, const lidarlib::ModelConfig& cfg, static void run_lidar(const char* tag, const lidarlib::ModelConfig& cfg,
const std::string& local_ip, uint16_t port, bool inverted, int n_scans) { const std::string& local_ip, uint16_t port, bool inverted, int n_scans) {
lidarlib::Driver drv(cfg, local_ip, port, inverted); lidarlib::Driver drv(cfg, local_ip, port, inverted);
if (!drv.open()) { lidarlib::ErrorCode err = drv.open();
fprintf(stderr, "[%s] Khong mo duoc socket tren %s:%u (interface khong ton tai?)\n", if (err != lidarlib::ErrorCode::Ok) {
tag, local_ip.c_str(), port); fprintf(stderr, "[%s] Khong mo duoc socket tren %s:%u (%s)\n",
tag, local_ip.c_str(), port, lidarlib::to_string(err));
return; return;
} }
printf("[%s] Da bind %s:%u, dang doi scan...\n", tag, local_ip.c_str(), port); printf("[%s] Da bind %s:%u, dang doi scan...\n", tag, local_ip.c_str(), port);

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);

119
include/lidarlib/diagnostics.hpp Normal file
View File

@@ -0,0 +1,119 @@
#pragma once
#include <cstdint>
#include <cstdio>
#include <optional>
#include <string>
namespace lidarlib {
struct ExtraInfo; // lidar.hpp
// Family A (0xFAF0) error_status bits, header byte [5]. Bits 3-7 are reserved
// on the wire; a nonzero reserved bit is still reported as a fault.
inline constexpr uint8_t kFaultMonitor = 1u << 0; // monitor / motor abnormal
inline constexpr uint8_t kFaultVoltage = 1u << 1; // supply voltage out of range
inline constexpr uint8_t kFaultTemperature = 1u << 2; // internal temperature abnormal
// SICK TiM LMDscandata device status (low word; Telegram Listing).
inline constexpr uint16_t kSickStatusError = 1u << 0;
inline constexpr uint16_t kSickStatusPollutionWarning = 1u << 1;
inline constexpr uint16_t kSickStatusPollutionError = 1u << 2;
// SICK nanoScan3 General System State byte 0 (layout from sick_safetyscanners;
// NOT verified on real hardware).
inline constexpr uint8_t kNanoStateRunMode = 1u << 0;
inline constexpr uint8_t kNanoStateStandby = 1u << 1;
inline constexpr uint8_t kNanoStateContaminationWarning = 1u << 2;
inline constexpr uint8_t kNanoStateContaminationError = 1u << 3;
inline constexpr uint8_t kNanoStateReferenceContour = 1u << 4;
inline constexpr uint8_t kNanoStateManipulation = 1u << 5;
// Device self-diagnostics decoded from the data stream. Fields the family
// doesn't carry stay std::nullopt (see docs/diagnostics.md for the per-family
// wire layout). valid stays false until the driver has decoded one full scan.
struct Diagnostics {
bool valid = false;
std::string model = "AUTO";
uint32_t device_timestamp_ms = 0; // device clock; 0 if not on the wire
// Family A error byte (0 = no fault; Family B/C don't carry it)
uint8_t error_status = 0;
bool monitor_fault() const { return (error_status & kFaultMonitor) != 0; }
bool voltage_fault() const { return (error_status & kFaultVoltage) != 0; }
bool temperature_fault() const { return (error_status & kFaultTemperature) != 0; }
// Family A only — raw motor speed field, unit unverified
std::optional<uint16_t> rotation_raw;
// Family C / V3 (GS1-5) only — raw passthroughs, bit meanings unverified
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;
// SICK TiM — LMDscandata status pair (word0<<8)|word1
std::optional<uint16_t> sick_device_status;
bool sick_error() const { return sick_device_status && (*sick_device_status & kSickStatusError); }
bool pollution_warning() const { return sick_device_status && (*sick_device_status & kSickStatusPollutionWarning); }
bool pollution_error() const { return sick_device_status && (*sick_device_status & kSickStatusPollutionError); }
// SICK nanoScan3 — General System State byte 0
std::optional<uint8_t> nano_general_state;
bool contamination_warning() const { return nano_general_state && (*nano_general_state & kNanoStateContaminationWarning); }
bool contamination_error() const { return nano_general_state && (*nano_general_state & kNanoStateContaminationError); }
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
// still measuring (dirty optics) — schedule cleaning/service.
bool has_fault() const {
return error_status != 0 || sick_error() || pollution_error()
|| contamination_error() || manipulation() || espe_fault();
}
bool has_warning() const { return pollution_warning() || contamination_warning(); }
bool healthy() const { return valid && !has_fault(); }
};
// Decode the diagnostic fields of one scan; sets valid = true.
Diagnostics decode_diagnostics(const ExtraInfo& info);
// One-line log summary: "no data" / "ok" / "WARN: pollution" /
// "FAULT: voltage temperature".
inline std::string to_string(const Diagnostics& d) {
if (!d.valid) return "no data";
if (!d.has_fault()) return d.has_warning()
? std::string("WARN:") + (d.pollution_warning() ? " pollution" : "")
+ (d.contamination_warning() ? " contamination" : "")
: "ok";
std::string s = "FAULT:";
if (d.monitor_fault()) s += " monitor";
if (d.voltage_fault()) s += " voltage";
if (d.temperature_fault()) s += " temperature";
if (d.sick_error()) s += " device";
if (d.pollution_error()) s += " pollution";
if (d.contamination_error()) s += " contamination";
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)) {
char buf[24];
std::snprintf(buf, sizeof(buf), " reserved(0x%02X)", rest);
s += buf;
}
return s;
}
} // namespace lidarlib

46
include/lidarlib/error.hpp Normal file
View File

@@ -0,0 +1,46 @@
#pragma once
namespace lidarlib {
// Result of open() and the sticky status behind last_error(). Ok == 0 so
// `if (err != ErrorCode::Ok)` reads naturally at call sites.
enum class ErrorCode {
Ok = 0,
// Lifecycle misuse — the call was refused, the instance state is unchanged.
AlreadyOpen, // open() called while already open
NotOpen, // recv_scan()/spin_once() called before open()
// open() failures
SocketError, // socket() creation failed
InvalidAddress, // ip string is not a valid IPv4 address
PortInUse, // bind: local port already taken (EADDRINUSE/EACCES)
BindFailed, // bind failed for another reason
ConnectionRefused, // TCP connect refused (device up, port closed)
ConnectionFailed, // TCP connect failed (unreachable, no route, ...)
HandshakeFailed, // connected, but the start-stream command failed
// Runtime failures
Timeout, // no (complete) scan within timeout_ms
DeviceDisconnected, // peer closed the connection / socket recv error
};
inline const char* to_string(ErrorCode e) {
switch (e) {
case ErrorCode::Ok: return "Ok";
case ErrorCode::AlreadyOpen: return "AlreadyOpen";
case ErrorCode::NotOpen: return "NotOpen";
case ErrorCode::SocketError: return "SocketError";
case ErrorCode::InvalidAddress: return "InvalidAddress";
case ErrorCode::PortInUse: return "PortInUse";
case ErrorCode::BindFailed: return "BindFailed";
case ErrorCode::ConnectionRefused: return "ConnectionRefused";
case ErrorCode::ConnectionFailed: return "ConnectionFailed";
case ErrorCode::HandshakeFailed: return "HandshakeFailed";
case ErrorCode::Timeout: return "Timeout";
case ErrorCode::DeviceDisconnected: return "DeviceDisconnected";
}
return "Unknown";
}
} // namespace lidarlib

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

87
include/lidarlib/lidar.hpp
View File

@@ -1,4 +1,7 @@
#pragma once #pragma once
#include "lidarlib/diagnostics.hpp"
#include "lidarlib/error.hpp"
#include <chrono>
#include <cstdint> #include <cstdint>
#include <vector> #include <vector>
#include <string> #include <string>
@@ -38,6 +41,17 @@ struct ExtraInfo {
std::optional<uint16_t> output_status; std::optional<uint16_t> output_status;
std::optional<uint32_t> field_status; std::optional<uint32_t> field_status;
std::optional<uint32_t> status_flags; std::optional<uint32_t> status_flags;
// SICK TiM only — LMDscandata status pair (word0<<8)|word1:
// 0 ok, 1 error, 2 pollution warning, 4 pollution error.
std::optional<uint16_t> sick_device_status;
// SICK nanoScan3 only — General System State byte 0 (see kNanoState* bits).
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 {
@@ -83,14 +97,72 @@ class Lidar {
public: public:
virtual ~Lidar() = default; virtual ~Lidar() = default;
virtual bool open() = 0; // ErrorCode::Ok on success. Calling open() on an already-open instance
// returns AlreadyOpen and leaves the connection untouched.
virtual ErrorCode open() = 0;
// Idempotent: safe to call before open() or more than once.
virtual void close() = 0; virtual void close() = 0;
// Block until one full scan; false on error/timeout. timeout_ms = 0 → block // Block until one full scan; false on error/timeout (see last_error()).
// indefinitely. No default on purpose: drivers differ (OLEI 1000, SICK 2000). // timeout_ms = 0 → block indefinitely. No default on purpose: drivers
// differ (OLEI 1000, SICK 2000).
virtual bool recv_scan(ScanResult& out, int timeout_ms) = 0; 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;
virtual bool is_open() const = 0;
// Status of the most recent open()/recv_scan()/spin_once() call.
ErrorCode last_error() const { return last_error_; }
// Device self-diagnostics from the newest fully decoded scan. valid stays
// false until one scan has been seen. Updated by recv_scan()/spin_once();
// call from the same thread that pumps them.
virtual Diagnostics get_diagnostics() const { return {}; }
// True when the sensor is usable right now: connection open, at least one
// fault-free scan decoded, and that scan no older than max_age_ms
// (0 = skip the age check). Diagnostics only refresh from
// recv_scan()/spin_once(), so unless something is pumping them this goes
// stale and reports not-ready; call from the pump thread.
bool is_ready(int max_age_ms = 3000) const {
if (!is_open() || !get_diagnostics().healthy()) return false;
if (max_age_ms <= 0) return true;
return last_scan_time_.time_since_epoch().count() != 0
&& std::chrono::steady_clock::now() - last_scan_time_
<= std::chrono::milliseconds(max_age_ms);
}
// Pump recv_scan() until is_ready() or timeout_ms elapses; false on
// timeout (see last_error() for the underlying failure). Scans consumed
// while waiting are discarded and the scan callback does not fire —
// intended for startup, before handing the pump to the main loop.
bool wait_ready(int timeout_ms = 5000) {
const auto deadline = std::chrono::steady_clock::now()
+ std::chrono::milliseconds(timeout_ms);
ScanResult tmp;
while (!is_ready()) {
if (!is_open()) return false;
const auto left = std::chrono::duration_cast<std::chrono::milliseconds>(
deadline - std::chrono::steady_clock::now()).count();
if (left <= 0) return false;
recv_scan(tmp, static_cast<int>(left));
}
return true;
}
protected:
ErrorCode set_error(ErrorCode e) { last_error_ = e; return e; }
// Drivers call this each time a full scan is decoded; feeds the freshness
// side of is_ready().
void mark_scan_decoded() { last_scan_time_ = std::chrono::steady_clock::now(); }
private:
ErrorCode last_error_ = ErrorCode::Ok;
std::chrono::steady_clock::time_point last_scan_time_{};
}; };
// OLEI UDP driver. // OLEI UDP driver.
@@ -109,16 +181,20 @@ public:
Driver(const Driver&) = delete; Driver(const Driver&) = delete;
Driver& operator=(const Driver&) = delete; Driver& operator=(const Driver&) = delete;
bool open() override; ErrorCode open() override;
void close() override; void close() override;
bool recv_scan(ScanResult& out, int timeout_ms = 1000) override; bool recv_scan(ScanResult& out, int timeout_ms = 1000) override;
void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); } void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
bool spin_once() override; bool spin_once() override;
bool is_open() const override { return sock_fd_ >= 0; }
// Model name read from the Family B/C header; "AUTO" until one is seen. // Model name read from the Family B/C header; "AUTO" until one is seen.
const char* detected_model() const override { return detected_model_name_.c_str(); } const char* detected_model() const override { return detected_model_name_.c_str(); }
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)
@@ -143,6 +219,9 @@ private:
ExtraInfo pending_info_; ExtraInfo pending_info_;
// Snapshot for get_diagnostics(); refreshed by flush_scan().
Diagnostics latest_diag_;
ScanResult ready_result_; ScanResult ready_result_;
bool scan_ready_ = false; bool scan_ready_ = false;

2
include/lidarlib/lidarlib.hpp
View File

@@ -1,5 +1,7 @@
#pragma once #pragma once
// One-include convenience header for the whole public API. // One-include convenience header for the whole public API.
#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"

30
include/lidarlib/sick_lidar.hpp
View File

@@ -20,24 +20,29 @@ 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;
SickDriver& operator=(const SickDriver&) = delete; SickDriver& operator=(const SickDriver&) = delete;
// Connect + send "sEN LMDscandata 1" to start continuous scan output. // Connect + send "sEN LMDscandata 1" to start continuous scan output.
bool open() override; ErrorCode open() override;
void close() override; void close() override;
bool recv_scan(ScanResult& out, int timeout_ms = 2000) override; bool recv_scan(ScanResult& out, int timeout_ms = 2000) override;
void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); } void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
bool spin_once() override; bool spin_once() override;
bool is_open() const override { return sock_fd_ >= 0; }
// No model string on the wire — returns the configured name. // No model string on the wire — returns the configured name.
const char* detected_model() const override { return detected_model_name_.c_str(); } const char* detected_model() const override { return detected_model_name_.c_str(); }
Diagnostics get_diagnostics() const override { return latest_diag_; }
private: private:
bool send_telegram(const std::string& body); bool send_telegram(const std::string& body);
bool read_telegram(std::string& out, int timeout_ms); bool read_telegram(std::string& out, int timeout_ms);
@@ -47,9 +52,13 @@ 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_;
// Snapshot for get_diagnostics(); refreshed by parse_lmdscandata().
Diagnostics latest_diag_;
// Leftover TCP bytes carried across telegram boundaries; per-instance. // Leftover TCP bytes carried across telegram boundaries; per-instance.
std::string recv_buf_; std::string recv_buf_;
}; };
@@ -64,24 +73,29 @@ 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;
NanoScanDriver& operator=(const NanoScanDriver&) = delete; NanoScanDriver& operator=(const NanoScanDriver&) = delete;
bool open() override; ErrorCode open() override;
void close() override; void close() override;
bool recv_scan(ScanResult& out, int timeout_ms = 1000) override; bool recv_scan(ScanResult& out, int timeout_ms = 1000) override;
void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); } void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
bool spin_once() override; bool spin_once() override;
bool is_open() const override { return sock_fd_ >= 0; }
// No model string on the wire — returns the configured name. // No model string on the wire — returns the configured name.
const char* detected_model() const override { return detected_model_name_.c_str(); } const char* detected_model() const override { return detected_model_name_.c_str(); }
Diagnostics get_diagnostics() const override { return latest_diag_; }
private: private:
int recv_datagram(int timeout_ms); int recv_datagram(int timeout_ms);
bool parse_packet(const uint8_t* buf, int len, ScanResult& out); bool parse_packet(const uint8_t* buf, int len, ScanResult& out);
@@ -90,9 +104,13 @@ 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_;
// Snapshot for get_diagnostics(); refreshed by parse_packet().
Diagnostics latest_diag_;
// Per-instance; sized for a full safety-data packet (max ~2751 beams). // Per-instance; sized for a full safety-data packet (max ~2751 beams).
std::vector<uint8_t> recv_buf_; std::vector<uint8_t> recv_buf_;
}; };

266
src/espe_lidar.cpp Normal file
View File

@@ -0,0 +1,266 @@
#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;
mark_scan_decoded();
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

78
src/olei_lidar.cpp
View File

@@ -1,6 +1,7 @@
#include "lidarlib/lidar.hpp" #include "lidarlib/lidar.hpp"
#include "lidar_bytes.hpp" #include "lidar_bytes.hpp"
#include <cerrno>
#include <cstring> #include <cstring>
#include <cmath> #include <cmath>
#include <stdexcept> #include <stdexcept>
@@ -38,6 +39,23 @@ static constexpr uint16_t FRAME_ID_A = 0xFAF0; // 2D Ethernet (VB, VF, LR-1F)
static constexpr uint16_t FRAME_ID_B = 0xFEF0; // LR-1BS5 / LR-1BS2 Ethernet variant static constexpr uint16_t FRAME_ID_B = 0xFEF0; // LR-1BS5 / LR-1BS2 Ethernet variant
static constexpr uint16_t FRAME_ID_C = 0xFEAC; // Protocol V3 (GS1-5) static constexpr uint16_t FRAME_ID_C = 0xFEAC; // Protocol V3 (GS1-5)
Diagnostics decode_diagnostics(const ExtraInfo& info) {
Diagnostics d;
d.valid = true;
d.model = info.detected_model;
d.error_status = info.error_status;
d.rotation_raw = info.rotation_raw;
d.scan_frequency_raw = info.scan_frequency_raw;
d.input_status = info.input_status;
d.output_status = info.output_status;
d.field_status = info.field_status;
d.status_flags = info.status_flags;
d.sick_device_status = info.sick_device_status;
d.nano_general_state = info.nano_general_state;
d.espe_error_status = info.espe_error_status;
return d;
}
Driver::Driver(const ModelConfig& cfg, const std::string& ip, uint16_t port, bool inverted) Driver::Driver(const ModelConfig& cfg, const std::string& ip, uint16_t port, bool inverted)
: cfg_(cfg), ip_(ip), port_(port), inverted_(inverted) : cfg_(cfg), ip_(ip), port_(port), inverted_(inverted)
{ {
@@ -46,9 +64,17 @@ Driver::Driver(const ModelConfig& cfg, const std::string& ip, uint16_t port, boo
Driver::~Driver() { close(); } Driver::~Driver() { close(); }
bool Driver::open() { ErrorCode Driver::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, SOCK_DGRAM, 0); sock_fd_ = ::socket(AF_INET, SOCK_DGRAM, 0);
if (sock_fd_ < 0) return false; if (sock_fd_ < 0) return set_error(ErrorCode::SocketError);
int reuse = 1; int reuse = 1;
::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)); ::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));
@@ -56,20 +82,27 @@ bool Driver::open() {
::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEPORT, &reuse, sizeof(reuse)); ::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEPORT, &reuse, sizeof(reuse));
#endif #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) { if (::bind(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0) {
int err = errno;
::close(sock_fd_); ::close(sock_fd_);
sock_fd_ = -1; sock_fd_ = -1;
return false; return set_error((err == EADDRINUSE || err == EACCES) ? ErrorCode::PortInUse
: ErrorCode::BindFailed);
} }
// Reset per-revolution state so a close()/open() cycle starts clean.
pending_angle_deg_.clear();
pending_dist_m_.clear();
pending_intensity_.clear();
pending_info_ = ExtraInfo{};
latest_diag_ = Diagnostics{};
last_angle_ = -1.f;
scan_ready_ = false;
pending_angle_deg_.reserve(2048); pending_angle_deg_.reserve(2048);
pending_dist_m_.reserve(2048); pending_dist_m_.reserve(2048);
pending_intensity_.reserve(2048); pending_intensity_.reserve(2048);
return true; return set_error(ErrorCode::Ok);
} }
void Driver::close() { void Driver::close() {
@@ -80,6 +113,7 @@ void Driver::close() {
} }
bool Driver::recv_scan(ScanResult& out, int timeout_ms) { bool Driver::recv_scan(ScanResult& out, int timeout_ms) {
if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }
scan_ready_ = false; scan_ready_ = false;
while (!scan_ready_) { while (!scan_ready_) {
@@ -87,22 +121,36 @@ bool Driver::recv_scan(ScanResult& out, int timeout_ms) {
fd_set fds; FD_ZERO(&fds); FD_SET(sock_fd_, &fds); fd_set fds; FD_ZERO(&fds); FD_SET(sock_fd_, &fds);
timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 }; timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 };
int r = ::select(sock_fd_ + 1, &fds, nullptr, nullptr, &tv); int r = ::select(sock_fd_ + 1, &fds, nullptr, nullptr, &tv);
if (r <= 0) return false; if (r <= 0) {
set_error(r == 0 ? ErrorCode::Timeout : ErrorCode::DeviceDisconnected);
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);
return true; return true;
} }
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; }
uint8_t* buf = recv_buf_; uint8_t* buf = recv_buf_;
sockaddr_in from{}; sockaddr_in from{};
socklen_t fromlen = sizeof(from); socklen_t fromlen = sizeof(from);
ssize_t n = ::recvfrom(sock_fd_, buf, sizeof(recv_buf_), 0, ssize_t n = ::recvfrom(sock_fd_, buf, sizeof(recv_buf_), 0,
reinterpret_cast<sockaddr*>(&from), &fromlen); reinterpret_cast<sockaddr*>(&from), &fromlen);
if (n < 0) return false; if (n < 0) { set_error(ErrorCode::DeviceDisconnected); return false; }
// A/C carry the frame id at [0-1]; B has a 0x010F preamble, real id at [2-3]. // A/C carry the frame id at [0-1]; B has a 0x010F preamble, real id at [2-3].
if (n < 4) return true; if (n < 4) return true;
@@ -155,13 +203,15 @@ void Driver::flush_scan() {
info.detected_model = detected_model_name_; info.detected_model = detected_model_name_;
info.error_status = pending_err_; info.error_status = pending_err_;
latest_diag_ = decode_diagnostics(info);
latest_diag_.device_timestamp_ms = scan.timestamp_ms;
mark_scan_decoded();
pending_angle_deg_.clear(); pending_angle_deg_.clear();
pending_dist_m_.clear(); pending_dist_m_.clear();
pending_intensity_.clear(); pending_intensity_.clear();
pending_info_ = ExtraInfo{}; 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).

150
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,57 +47,41 @@ 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(); }
bool SickDriver::open() { ErrorCode SickDriver::open() {
sock_fd_ = ::socket(AF_INET, SOCK_STREAM, 0); if (is_open()) return set_error(ErrorCode::AlreadyOpen);
if (sock_fd_ < 0) return false;
sockaddr_in addr{}; sockaddr_in addr{};
addr.sin_family = AF_INET; addr.sin_family = AF_INET;
addr.sin_port = htons(port_); addr.sin_port = htons(port_);
addr.sin_addr.s_addr = inet_addr(ip_.c_str()); if (::inet_pton(AF_INET, ip_.c_str(), &addr.sin_addr) != 1)
return set_error(ErrorCode::InvalidAddress);
// Non-blocking connect with a bounded timeout — a blocking connect() to an sock_fd_ = ::socket(AF_INET, SOCK_STREAM, 0);
// unreachable device would stall for the OS default (~2 min on Linux). if (sock_fd_ < 0) return set_error(ErrorCode::SocketError);
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)); ErrorCode conn_err = connect_tcp_with_timeout(sock_fd_, addr, kConnectTimeoutMs);
if (rc < 0 && errno == EINPROGRESS) { if (conn_err != ErrorCode::Ok) {
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;
}
}
::fcntl(sock_fd_, F_SETFL, flags);
if (rc < 0) {
::close(sock_fd_); ::close(sock_fd_);
sock_fd_ = -1; sock_fd_ = -1;
return false; 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{};
// Device is passive until told to stream. // Device is passive until told to stream.
if (!send_telegram("sEN LMDscandata 1")) { if (!send_telegram("sEN LMDscandata 1")) {
close(); close();
return false; return set_error(ErrorCode::HandshakeFailed);
} }
return true; return set_error(ErrorCode::Ok);
} }
void SickDriver::close() { void SickDriver::close() {
@@ -129,7 +112,7 @@ bool SickDriver::send_telegram(const std::string& body) {
// CoLa-A has no length prefix, so ETX is the only frame boundary; recv_buf_ // CoLa-A has no length prefix, so ETX is the only frame boundary; recv_buf_
// carries leftover bytes across calls. // carries leftover bytes across calls.
bool SickDriver::read_telegram(std::string& out, int timeout_ms) { bool SickDriver::read_telegram(std::string& out, int timeout_ms) {
if (sock_fd_ < 0) return false; if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }
for (;;) { for (;;) {
size_t etx_pos = recv_buf_.find(kEtx); size_t etx_pos = recv_buf_.find(kEtx);
@@ -148,12 +131,15 @@ bool SickDriver::read_telegram(std::string& out, int timeout_ms) {
fd_set fds; FD_ZERO(&fds); FD_SET(sock_fd_, &fds); fd_set fds; FD_ZERO(&fds); FD_SET(sock_fd_, &fds);
timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 }; timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 };
int r = ::select(sock_fd_ + 1, &fds, nullptr, nullptr, &tv); int r = ::select(sock_fd_ + 1, &fds, nullptr, nullptr, &tv);
if (r <= 0) return false; if (r <= 0) {
set_error(r == 0 ? ErrorCode::Timeout : ErrorCode::DeviceDisconnected);
return false;
}
} }
char buf[4096]; char buf[4096];
ssize_t n = ::recv(sock_fd_, buf, sizeof(buf), 0); ssize_t n = ::recv(sock_fd_, buf, sizeof(buf), 0);
if (n <= 0) return false; if (n <= 0) { set_error(ErrorCode::DeviceDisconnected); return false; }
recv_buf_.append(buf, static_cast<size_t>(n)); recv_buf_.append(buf, static_cast<size_t>(n));
} }
} }
@@ -162,7 +148,7 @@ bool SickDriver::recv_scan(ScanResult& out, int timeout_ms) {
for (;;) { for (;;) {
std::string telegram; std::string telegram;
if (!read_telegram(telegram, timeout_ms)) return false; if (!read_telegram(telegram, timeout_ms)) return false;
if (parse_lmdscandata(telegram, out)) return true; if (parse_lmdscandata(telegram, out)) { set_error(ErrorCode::Ok); return true; }
// Non-scan telegram (e.g. an ack) — keep waiting. // Non-scan telegram (e.g. an ack) — keep waiting.
} }
} }
@@ -246,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;
} }
} }
}; };
@@ -271,48 +259,62 @@ 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);
ExtraInfo& info = out.info; ExtraInfo& info = out.info;
info = ExtraInfo{}; info = ExtraInfo{};
info.detected_model = cfg_.name; info.detected_model = cfg_.name;
info.error_status = static_cast<uint8_t>(status0 & 0xFF); info.sick_device_status = static_cast<uint16_t>(((status0 & 0xFF) << 8) | (status1 & 0xFF));
info.status_flags = (status0 << 8) | status1; info.status_flags = (status0 << 8) | status1;
info.scan_frequency_raw = static_cast<uint16_t>(scanning_frequency); info.scan_frequency_raw = static_cast<uint16_t>(scanning_frequency);
info.input_status = static_cast<uint16_t>((in0 << 8) | in1); info.input_status = static_cast<uint16_t>((in0 << 8) | in1);
info.output_status = static_cast<uint16_t>((out0 << 8) | out1); info.output_status = static_cast<uint16_t>((out0 << 8) | out1);
latest_diag_ = decode_diagnostics(info);
latest_diag_.device_timestamp_ms = scan.timestamp_ms;
mark_scan_decoded();
return true; return true;
} }
// ── 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(); }
bool NanoScanDriver::open() { ErrorCode NanoScanDriver::open() {
sock_fd_ = ::socket(AF_INET, SOCK_DGRAM, 0); if (is_open()) return set_error(ErrorCode::AlreadyOpen);
if (sock_fd_ < 0) return false;
int reuse = 1;
::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));
sockaddr_in addr{}; sockaddr_in addr{};
addr.sin_family = AF_INET; addr.sin_family = AF_INET;
addr.sin_port = htons(port_); addr.sin_port = htons(port_);
addr.sin_addr.s_addr = (ip_ == "0.0.0.0" || ip_.empty()) ? INADDR_ANY if (ip_ == "0.0.0.0" || ip_.empty()) {
: inet_addr(ip_.c_str()); addr.sin_addr.s_addr = INADDR_ANY;
} else if (::inet_pton(AF_INET, ip_.c_str(), &addr.sin_addr) != 1) {
return set_error(ErrorCode::InvalidAddress);
}
sock_fd_ = ::socket(AF_INET, SOCK_DGRAM, 0);
if (sock_fd_ < 0) return set_error(ErrorCode::SocketError);
// No SO_REUSEADDR: UDP has no TIME_WAIT, and on Linux it would let two
// sockets bind the same port, hiding PortInUse from the second app.
if (::bind(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0) { if (::bind(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0) {
int err = errno;
::close(sock_fd_); ::close(sock_fd_);
sock_fd_ = -1; sock_fd_ = -1;
return false; return set_error((err == EADDRINUSE || err == EACCES) ? ErrorCode::PortInUse
: ErrorCode::BindFailed);
} }
return true; latest_diag_ = Diagnostics{};
return set_error(ErrorCode::Ok);
} }
void NanoScanDriver::close() { void NanoScanDriver::close() {
@@ -323,17 +325,21 @@ void NanoScanDriver::close() {
} }
int NanoScanDriver::recv_datagram(int timeout_ms) { int NanoScanDriver::recv_datagram(int timeout_ms) {
if (sock_fd_ < 0) return -1; if (!is_open()) { set_error(ErrorCode::NotOpen); return -1; }
if (timeout_ms > 0) { if (timeout_ms > 0) {
fd_set fds; FD_ZERO(&fds); FD_SET(sock_fd_, &fds); fd_set fds; FD_ZERO(&fds); FD_SET(sock_fd_, &fds);
timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 }; timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 };
int r = ::select(sock_fd_ + 1, &fds, nullptr, nullptr, &tv); int r = ::select(sock_fd_ + 1, &fds, nullptr, nullptr, &tv);
if (r <= 0) return -1; if (r <= 0) {
set_error(r == 0 ? ErrorCode::Timeout : ErrorCode::DeviceDisconnected);
return -1;
}
} }
ssize_t n = ::recv(sock_fd_, recv_buf_.data(), recv_buf_.size(), 0); ssize_t n = ::recv(sock_fd_, recv_buf_.data(), recv_buf_.size(), 0);
return (n <= 0) ? -1 : static_cast<int>(n); if (n <= 0) { set_error(ErrorCode::DeviceDisconnected); return -1; }
return static_cast<int>(n);
} }
// A scan is split across datagrams at the application layer. Each starts with // A scan is split across datagrams at the application layer. Each starts with
@@ -342,6 +348,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;
@@ -351,7 +358,7 @@ bool NanoScanDriver::recv_scan(ScanResult& out, int timeout_ms) {
const uint8_t* d = recv_buf_.data(); const uint8_t* d = recv_buf_.data();
if (n < 24 || std::memcmp(d, "MS3 ", 4) != 0) { if (n < 24 || std::memcmp(d, "MS3 ", 4) != 0) {
if (parse_packet(d, n, out)) return true; if (parse_packet(d, n, out)) { set_error(ErrorCode::Ok); return true; }
continue; continue;
} }
@@ -365,17 +372,22 @@ 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) {
assembling = false; assembling = false;
if (parse_packet(tele.data(), static_cast<int>(total), out)) return true; if (parse_packet(tele.data(), static_cast<int>(total), out)) {
set_error(ErrorCode::Ok);
return true;
}
} }
} }
} }
@@ -397,6 +409,8 @@ bool NanoScanDriver::spin_once() {
bool NanoScanDriver::parse_packet(const uint8_t* buf, int len, ScanResult& out) { bool NanoScanDriver::parse_packet(const uint8_t* buf, int len, ScanResult& out) {
if (len < 52) return false; if (len < 52) return false;
uint16_t gss_off = le16(buf + 32); // General System State block
uint16_t gss_size = le16(buf + 34);
uint16_t dv_off = le16(buf + 36); uint16_t dv_off = le16(buf + 36);
uint16_t dv_size = le16(buf + 38); uint16_t dv_size = le16(buf + 38);
uint16_t md_off = le16(buf + 40); uint16_t md_off = le16(buf + 40);
@@ -453,12 +467,22 @@ 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);
ExtraInfo& info = out.info; ExtraInfo& info = out.info;
info = ExtraInfo{}; info = ExtraInfo{};
info.detected_model = cfg_.name; info.detected_model = cfg_.name;
// Byte 0 holds the run/standby/contamination/manipulation flags
// (kNanoState*); the block is absent when not configured in the sensor.
if (gss_off != 0 && gss_size != 0 && static_cast<int>(gss_off) < len)
info.nano_general_state = buf[gss_off];
latest_diag_ = decode_diagnostics(info);
latest_diag_.device_timestamp_ms = scan.timestamp_ms;
mark_scan_decoded();
return true; return true;
} }