From 917b4fe4c56f1ece1dd1e052cd27c039ff7a6cdb Mon Sep 17 00:00:00 2001 From: QUYVN Date: Tue, 7 Jul 2026 10:38:42 +0700 Subject: [PATCH] update brand ESPE --- CMakeLists.txt | 4 + README.md | 405 +++++++++++++++---------------- examples/espe_example.cpp | 39 +++ include/lidarlib/config.hpp | 9 +- include/lidarlib/diagnostics.hpp | 13 +- include/lidarlib/espe_lidar.hpp | 83 +++++++ include/lidarlib/lidar.hpp | 9 + include/lidarlib/lidarlib.hpp | 1 + include/lidarlib/sick_lidar.hpp | 14 +- src/espe_lidar.cpp | 265 ++++++++++++++++++++ src/lidar_bytes.hpp | 11 + src/lidar_config.cpp | 27 ++- src/lidar_net.hpp | 51 ++++ src/olei_lidar.cpp | 14 +- src/sick_lidar.cpp | 59 ++--- 15 files changed, 741 insertions(+), 263 deletions(-) create mode 100644 examples/espe_example.cpp create mode 100644 include/lidarlib/espe_lidar.hpp create mode 100644 src/espe_lidar.cpp create mode 100644 src/lidar_net.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 81d1f27..e34fa97 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,6 +14,7 @@ option(BUILD_SHARED_LIBS "Build shared (.so) libraries instead of static" ON) set(LIDARLIB_SOURCES src/olei_lidar.cpp src/sick_lidar.cpp + src/espe_lidar.cpp src/lidar_config.cpp ) @@ -43,6 +44,9 @@ if(LIDARLIB_BUILD_EXAMPLES) add_executable(nanoscan_example examples/nanoscan_example.cpp) 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) target_link_libraries(lidar_app PRIVATE lidarlib) endif() diff --git a/README.md b/README.md index f47da80..1597813 100644 --- a/README.md +++ b/README.md @@ -1,143 +1,145 @@ # Lidarlib -Thư viện C++17 cho lidar **OLEI** (UDP) và **SICK** (TCP/UDP). Build bằng CMake -ra shared lib, hỗ trợ `find_package(lidarlib)`. Mọi driver dùng chung một -interface `lidarlib::Lidar` và một hàm khởi tạo duy nhất `lidarlib::make_lidar()`. +Thư viện C++17 thu nhận dữ liệu lidar 2D cho **OLEI** (UDP), **SICK** +(TCP/UDP) và **ESPE** (TCP/UDP) -- Tự nhận diện họ giao thức OLEI (Family A/B/C) theo từng gói tin -- Tự dò model (`MODEL_AUTO`) với Family B/C -- Chạy nhiều lidar song song (mỗi instance độc lập, an toàn đa luồng) -- Output chuẩn ROS `sensor_msgs/LaserScan` (radian, mét) -- Không có UI — tự viết giao diện trên API này +Mọi driver cùng implement một interface `lidarlib::Lidar`, khởi tạo qua một +factory duy nhất `lidarlib::make_lidar()`, output thống nhất theo định dạng +ROS `sensor_msgs/LaserScan`. -## 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 cmake -S . -B build -DCMAKE_BUILD_TYPE=Release 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 ``` +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 find_package(lidarlib REQUIRED) target_link_libraries(my_app PRIVATE lidarlib::lidarlib) ``` -## Quick start +## Sử dụng + +### Đọc scan ```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"}; std::unique_ptr lidar = lidarlib::make_lidar(c); -lidarlib::ErrorCode err = lidar->open(); -if (err != lidarlib::ErrorCode::Ok) { - fprintf(stderr, "open that bai: %s\n", lidarlib::to_string(err)); +if (lidar->open() != lidarlib::ErrorCode::Ok) { + fprintf(stderr, "open: %s\n", lidarlib::to_string(lidar->last_error())); return 1; } lidarlib::ScanResult r; -if (!lidar->recv_scan(r, 1000)) { - // Timeout / DeviceDisconnected / NotOpen — xem lidar->last_error() +if (lidar->recv_scan(r, 1000)) { + // r.scan : LaserScan — điểm đo, format ROS + // r.info : ExtraInfo — metadata tuỳ model +} else { + // Timeout / DeviceDisconnected — xem lidar->last_error() } -// r.scan : LaserScan — format sensor_msgs/LaserScan của ROS, chung mọi lidar -// r.info : ExtraInfo — thông tin thêm tuỳ family/model -printf("%zu diem, model=%s\n", r.scan.ranges.size(), r.info.detected_model.c_str()); ``` -Có thể khởi tạo driver trực tiếp thay vì qua `make_lidar()`: +Có thể khởi tạo driver trực tiếp không qua factory: ```cpp -lidarlib::Driver olei(lidarlib::MODEL_AUTO, "192.168.100.100", 2368); -lidarlib::SickDriver tim (lidarlib::MODEL_SICK_TIM571, "192.168.0.1", 2111); -lidarlib::NanoScanDriver nano(lidarlib::MODEL_SICK_NANOSCAN3, "0.0.0.0", 6060); +lidarlib::Driver olei(lidarlib::MODEL_AUTO, "192.168.100.100", 2368); +lidarlib::SickDriver tim (lidarlib::MODEL_SICK_TIM571, "192.168.0.1", 2111); +lidarlib::NanoScanDriver nano(lidarlib::MODEL_SICK_NANOSCAN3, "0.0.0.0", 6060); +lidarlib::EspeDriver espe(lidarlib::MODEL_ESPE_LGA60, "192.168.1.88", 8080); ``` -Ngoài `recv_scan()` blocking còn có callback: `set_scan_callback()` + -`spin_once()` trong vòng lặp riêng. +### Chế độ callback -### Error handling & lifecycle +Thay cho `recv_scan()` blocking: -`open()` trả về `lidarlib::ErrorCode` (header `lidarlib/error.hpp`, -`to_string()` để log). Các code chính: +```cpp +lidar->set_scan_callback([](const lidarlib::ScanResult& r) { /* mỗi vòng quét */ }); +while (running) lidar->spin_once(); +``` -| ErrorCode | Khi nào | -|---|---| -| `Ok` | Thành công | -| `AlreadyOpen` | Gọi `open()` khi đang mở — kết nối cũ giữ nguyên | -| `NotOpen` | Gọi `recv_scan()`/`spin_once()` khi chưa `open()` | -| `InvalidAddress` | Chuỗi IP không hợp lệ | -| `PortInUse` | Port local đã bị chiếm (bind `EADDRINUSE`/`EACCES`) | -| `BindFailed` / `SocketError` | Lỗi bind khác / không tạo được socket | -| `ConnectionRefused` / `ConnectionFailed` / `Timeout` | TCP connect (SICK TiM) bị từ chối / không tới được / quá 2s | -| `HandshakeFailed` | TCP nối được nhưng gửi `sEN LMDscandata 1` thất bại | -| `DeviceDisconnected` | Thiết bị đóng kết nối / lỗi recv giữa chừng | +### Chẩn đoán thiết bị -Trạng thái instance: - -- `is_open()` — socket đang mở hay không. -- `last_error()` — kết quả của lần `open()`/`recv_scan()`/`spin_once()` gần - nhất (`recv_scan()` trả `false` thì gọi hàm này để biết `Timeout` hay - `DeviceDisconnected`). -- Lifecycle chịu lỗi mọi thứ tự gọi: `close()` trước `open()` hoặc `close()` - hai lần là no-op; `open()` hai lần trả `AlreadyOpen` và không đụng kết nối - đang chạy; sau `close()` có thể `open()` lại (state scan dở được reset). - -### Chẩn đoán thiết bị (diagnostics) - -Lidar OLEI nhúng thông tin tự chẩn đoán trong header gói dữ liệu (không có -kênh query riêng). API đọc ra qua `lidarlib/diagnostics.hpp` — nghiên cứu -chi tiết layout từng family xem [docs/diagnostics.md](docs/diagnostics.md). +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 scan nào + // 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 + 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("lidar fault: %s\n", lidarlib::to_string(d).c_str()); + printf("fault: %s\n", lidarlib::to_string(d).c_str()); } else if (d.has_warning()) { // pollution_warning() / contamination_warning() — kính bẩn nhẹ, nên lau } ``` -- `get_diagnostics()` — snapshot từ vòng quét gần nhất; gọi cùng thread với - `recv_scan()`/`spin_once()`. `d.healthy()` = có data và không có fault. -- `decode_diagnostics(result.info)` — decode gắn với một scan cụ thể. -- Nguồn dữ liệu: OLEI Family A byte lỗi + `rotation_raw` + timestamp; - Family B không có gì trên wire; Family C `input/output/field_status`, - `status_flags` (raw, chưa verify); SICK TiM cặp device status trong - `LMDscandata`; nanoScan3 block General System State (cần bật trong - Safety Designer). +`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`. -## Cấu hình (config.json) +### Xử lý lỗi & lifecycle -`lidar_app` là app mẫu headless: đọc `config.json`, mở từng lidar bằng -`make_lidar()`, một thread mỗi con. +`open()` trả về `ErrorCode`; `last_error()` giữ kết quả của lần gọi gần nhất. -```bash -./build/lidar_app [my_config.json] -``` +| 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 { @@ -145,157 +147,146 @@ if (!d.valid) { {"name":"front", "ip":"192.168.100.100", "port":2368, "brand":"OLEI", "model":"AUTO", "inverted":false}, {"name":"rear", "ip":"192.168.100.100", "port":2369, "brand":"OLEI", "model":"AUTO", "inverted":true}, {"name":"sick1", "ip":"192.168.0.1", "port":2111, "brand":"SICK", "model":"SICK-TIM571"}, - {"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 | |---|---| -| `brand` | `"OLEI"` (mặc định) hoặc `"SICK"` | -| `model` | Tên trong bảng model bên dưới; tên lạ → mặc định của hãng (`AUTO` / `SICK-TIM571`). Với SICK, `"SICK-nanoScan3"` → driver UDP, còn lại → driver TCP | -| `inverted` | Chỉ OLEI: `true` nếu lidar lắp úp ngược, driver tự đảo góc về hệ quy chiếu xe | -| `angle_min_deg` / `angle_max_deg` | Tuỳ chọn: remap tuyến tính góc output sang cửa sổ này (độ). Không cắt điểm nào, chỉ ghi lại `angle_min/max/increment`. Bỏ trống (±360) = tắt | +| `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 | +| `inverted` | `true` nếu lidar lắp úp ngược — driver tự đảo góc (mọi hãng) | +| `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)`. -## 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_VF` | -180…180 | 0.05…30 | 2D 360°, Family A | -| `MODEL_LR1F` | -180…180 | 0.05…50 | Family A; 0° thô của máy chỉ về đuôi (offset +180°) | -| `MODEL_LR1FMI` | -180…180 | 0.05…30 | Family B, ~2400 điểm/vòng; 0° thô chỉ về đuôi (offset +180°) | -| `MODEL_LR1BS5` | -180…180 | 0.05…30 | Family B | -| `MODEL_LR16F` | -135…135 | 0.05…30 | 3D 16-line | -| `MODEL_GS15` | -180…180 | 0.05…30 | 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_VB` | −135…135 | 0.05…30 | Family A | +| `MODEL_VF` | −180…180 | 0.05…30 | Family A | +| `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° hướng đuôi) | +| `MODEL_LR1BS5` | −180…180 | 0.05…30 | Family B | +| `MODEL_LR16F` | −135…135 | 0.05…30 | 3D 16-line | +| `MODEL_GS15` | −180…180 | 0.05…30 | Family C/V3 — chưa verify phần cứ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: -**Family A** `0xFAF0` (header 20B, 3B/điểm, CRC32) · -**Family B** `0xFEF0` (header 40B kèm tên model ASCII, 8B/điểm) · -**Family C/V3** `0xFEAC` (header 48B, 2/4B/điểm — port từ driver C#, chưa verify). +Driver nhận diện họ giao thức theo frame ID từng gói: **Family A** `0xFAF0` +(header 20 B, 3 B/điểm, CRC32) · **Family B** `0xFEF0` (header 40 B kèm tên +model ASCII, 8 B/điểm) · **Family C/V3** `0xFEAC` (header 48 B, 2–4 B/điểm). +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 -`result.info.detected_model`. - -### SICK (`brand = "SICK"`) +### SICK | Constant | FOV (°) | Range (m) | Transport | |---|---|---|---| -| `MODEL_SICK_TIM5XX` | -135…135 | 0.05…10 | TCP/SOPAS (CoLa-A), port 2111 | -| `MODEL_SICK_TIM571` | -135…135 | 0.05…25 | TCP/SOPAS, port 2111 | -| `MODEL_SICK_TIM7XX` | -135…135 | 0.05…25 | TCP/SOPAS, port 2111 | -| `MODEL_SICK_NANOSCAN3` | -137.5…137.5 | 0.05…40 | UDP safety-data, port 6060 | +| `MODEL_SICK_TIM5XX` | −135…135 | 0.05…10 | TCP/SOPAS (CoLa-A), port 2111 | +| `MODEL_SICK_TIM571` | −135…135 | 0.05…25 | TCP/SOPAS, port 2111 | +| `MODEL_SICK_TIM7XX` | −135…135 | 0.05…25 | TCP/SOPAS, port 2111 — verify trên TiM781S thật | +| `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. -Hệ góc trên dây đặt 90° = trước mặt nên preset có `angle_offset_deg = -90`, -output ra -135…135° với 0° = phía trước. **Đã verify trên TiM781S thật** -(811 điểm/scan, increment 0.333°, DIST1/RSSI1 đúng layout). Chưa verify: -encoder, kênh 8-bit, thông số TiM5xx/571. +- **TiM (`SickDriver`)** — `open()` tự gửi lệnh start-stream; góc output đã + quy về 0° = phía trước. +- **nanoScan3 (`NanoScanDriver`)** — receiver UDP thụ động; đích UDP phải + cấu hình sẵn trong SICK Safety Designer. Chưa verify phần cứng thật. -**nanoScan3 (`NanoScanDriver`)** — UDP receiver thụ động: chỉ bind cổng và -parse datagram; **đích UDP phải cấu hình sẵn trong SICK Safety Designer** -(driver không bắt tay CoLa2). Layout port từ `sick_safetyscanners` (Apache-2.0). -**Chưa verify phần cứng thật** — mới test bằng gói tổng hợp qua loopback. +### ESPE -## 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, - đã unwrap liên tục, không giới hạn ±π), `ranges[]` (m), `intensities[]` - (0-255), `timestamp_ms` (đồng hồ thiết bị, 0 nếu family không có). - `range_min/max` lấy từ `ModelConfig` (đặt sẵn, không đo mỗi scan); - `time_increment/scan_time` luôn 0. -- **`ExtraInfo`** — field tuỳ family: `detected_model`, `error_status` (Family A), - `distance_scale_mm` (A/B), và các trường raw của Family C (chưa verify). - Field thiết bị không có giữ `std::nullopt`. +## Kiểu dữ liệu + +### `ScanResult` + +Kết quả một vòng quét: `{ LaserScan scan; ExtraInfo info; }`. + +### `LaserScan` + +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` | Khoảng cách (m), theo thứ tự quét | +| `intensities` | `vector` | 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 | File | Vai trò | |---|---| +| `include/lidarlib/lidarlib.hpp` | Include tổng hợp toàn bộ API | +| `include/lidarlib/lidar.hpp` | Kiểu dữ liệu, interface `Lidar`, driver OLEI, preset `MODEL_*` | +| `include/lidarlib/sick_lidar.hpp` | `SickDriver`, `NanoScanDriver`, preset `MODEL_SICK_*` | +| `include/lidarlib/espe_lidar.hpp` | `EspeDriver`, preset `MODEL_ESPE_LGA60` | +| `include/lidarlib/diagnostics.hpp` | `Diagnostics`, bit lỗi, `decode_diagnostics()` | | `include/lidarlib/error.hpp` | `enum class ErrorCode` + `to_string()` | -| `include/lidarlib/diagnostics.hpp` | `Diagnostics`, bit lỗi Family A, `decode_diagnostics()` | -| `include/lidarlib/lidar.hpp` | Data model, interface `Lidar`, driver OLEI, các `MODEL_*` OLEI | -| `include/lidarlib/sick_lidar.hpp` | `SickDriver`, `NanoScanDriver`, các `MODEL_SICK_*` | | `include/lidarlib/config.hpp` | `LidarConfig`, load/save JSON, `make_lidar()` | -| `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/espe_lidar.cpp` | Parse frame `HISN`/`WSimu` (LGA60), gom vòng quét | | `src/lidar_config.cpp` | Bảng model/brand, config JSON, factory | -| `examples/` | Demo: 1 lidar, 2 lidar song song, SICK TiM, nanoScan3, app khung | - -## Ghi chú - -- Nếu port UDP đã bị app khác giữ (không bật `SO_REUSEPORT`), `open()` trả - `ErrorCode::PortInUse`. Kiểm tra: `ss -lunp | grep 2368`. -- `inverted` đã verify bằng sniff sống: `false` góc tăng dần, `true` góc giảm - dần cùng bước. - -## Changelog - -### 2026-07-07 — Diagnostics API cho SICK - -- **`SickDriver` (TiM)**: decode cặp Device Status của `LMDscandata` - (0 ok / 1 error / 2 pollution warning / 4 pollution error) vào - `Diagnostics` — `sick_error()`, `pollution_warning()`, `pollution_error()`. - `info.sick_device_status` là trường mới; `info.error_status` **không còn** - nhận `status0` (trường đó thuộc Family A OLEI). -- **`NanoScanDriver`**: parse block General System State (offset header - `[32]/[34]`, byte 0) → `contamination_warning()`, `contamination_error()`, - `manipulation()`. Layout theo `sick_safetyscanners`, chưa verify phần cứng; - block phải được bật trong Safety Designer. -- **`Diagnostics` thêm `has_warning()`** (kính bẩn mức cảnh báo) và - `to_string()` in thêm `WARN: pollution` / `FAULT: ... contamination`. -- Test loopback: TiM server TCP giả (status `0 4` → pollution error) và gói - nanoScan3 tổng hợp (state `0x09` → contamination error, gói sạch → - `healthy()`); tài liệu tại [docs/diagnostics.md](docs/diagnostics.md) §4-5. - -### 2026-07-07 — Diagnostics API - -- **Header mới `lidarlib/diagnostics.hpp`**: struct `Diagnostics` (bit lỗi - Family A decode sẵn: `monitor/voltage/temperature_fault()`, `has_fault()`, - `healthy()`; trường raw Family C), hằng `kFaultMonitor/Voltage/Temperature`, - `to_string()` để log. -- **`Lidar::get_diagnostics()`**: snapshot chẩn đoán từ vòng quét decode gần - nhất; `valid = false` khi chưa có scan. Driver OLEI implement đầy đủ; - SICK tạm trả mặc định. -- **`decode_diagnostics(const ExtraInfo&)`**: decode gắn với một - `ScanResult` cụ thể. -- **Tài liệu nghiên cứu [docs/diagnostics.md](docs/diagnostics.md)**: layout - byte chẩn đoán từng family (A: byte lỗi `[5]` + rotation + timestamp; - B: không có; C: input/output/field/status raw), chẩn đoán tầng transport, - hướng mở rộng SICK. -- Đã test bằng gói Family A tổng hợp qua loopback (fault set → decode đúng - từng bit, vòng sạch → `healthy()`); bit map lấy theo tài liệu OLEI, chưa - tái tạo fault trên phần cứng thật. - -### 2026-07-06 — Error handling & lifecycle API - -- **Header mới `lidarlib/error.hpp`**: `enum class ErrorCode` — `Ok`, - `AlreadyOpen`, `NotOpen`, `SocketError`, `InvalidAddress`, `PortInUse`, - `BindFailed`, `ConnectionRefused`, `ConnectionFailed`, `HandshakeFailed`, - `Timeout`, `DeviceDisconnected` — kèm `to_string()` để log. -- **`open()` đổi chữ ký `bool` → `ErrorCode`** trên cả 3 driver (`Driver`, - `SickDriver`, `NanoScanDriver`). Lỗi phân loại từ `errno` thật: bind - `EADDRINUSE` → `PortInUse`, connect TCP bị từ chối → `ConnectionRefused`, - quá 2s → `Timeout`, gửi lệnh start-stream fail → `HandshakeFailed`, - IP sai format → `InvalidAddress`. -- **API trạng thái mới trên interface `Lidar`**: - - `is_open()` — socket đang mở hay không (cả 3 driver implement); - - `last_error()` — kết quả lần `open()`/`recv_scan()`/`spin_once()` gần - nhất; `recv_scan()` trả `false` thì gọi hàm này để biết `Timeout` hay - `DeviceDisconnected`. -- **Lifecycle chịu lỗi mọi thứ tự gọi**: `close()` trước `open()` hoặc gọi - hai lần là no-op; `open()` khi đang mở trả `AlreadyOpen` và không đụng kết - nối đang chạy; `recv_scan()`/`spin_once()` khi chưa mở trả `false` + - `NotOpen`; sau `close()` có thể `open()` lại (state scan dở được reset). -- **`NanoScanDriver` bỏ `SO_REUSEADDR`**: với UDP không có tác dụng (không có - TIME_WAIT) mà còn che mất lỗi trùng port — giờ `PortInUse` báo được thật. -- ⚠️ **Breaking change**: code cũ viết `if (!lidar->open())` bị đảo ngược - logic vì `ErrorCode::Ok == 0` — phải đổi thành - `if (lidar->open() != lidarlib::ErrorCode::Ok)`. +| `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 | diff --git a/examples/espe_example.cpp b/examples/espe_example.cpp new file mode 100644 index 0000000..217d706 --- /dev/null +++ b/examples/espe_example.cpp @@ -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 + +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 diff --git a/include/lidarlib/config.hpp b/include/lidarlib/config.hpp index c27f900..ab555a0 100644 --- a/include/lidarlib/config.hpp +++ b/include/lidarlib/config.hpp @@ -12,8 +12,9 @@ struct LidarConfig { std::string ip = "0.0.0.0"; uint16_t port = 2368; std::string model = "AUTO"; - bool inverted = false; // OLEI only - std::string brand = "OLEI"; // "OLEI" or "SICK" + bool inverted = false; // unit mounted upside-down → mirror the scan + 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 // [angle_min_deg, angle_max_deg] without dropping points. @@ -24,6 +25,7 @@ struct LidarConfig { friend bool operator==(const LidarConfig& a, const LidarConfig& b) { return a.name == b.name && a.ip == b.ip && a.port == b.port && a.model == b.model && a.inverted == b.inverted && a.brand == b.brand && + a.use_udp == b.use_udp && 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); } @@ -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 // 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. std::unique_ptr make_lidar(const LidarConfig& cfg); diff --git a/include/lidarlib/diagnostics.hpp b/include/lidarlib/diagnostics.hpp index 84b294a..12e539d 100644 --- a/include/lidarlib/diagnostics.hpp +++ b/include/lidarlib/diagnostics.hpp @@ -67,11 +67,17 @@ struct Diagnostics { 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 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(); + || contamination_error() || manipulation() || espe_fault(); } bool has_warning() const { return pollution_warning() || contamination_warning(); } bool healthy() const { return valid && !has_fault(); } @@ -97,6 +103,11 @@ inline std::string to_string(const Diagnostics& d) { if (d.pollution_error()) s += " pollution"; if (d.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); diff --git a/include/lidarlib/espe_lidar.hpp b/include/lidarlib/espe_lidar.hpp new file mode 100644 index 0000000..c84086e --- /dev/null +++ b/include/lidarlib/espe_lidar.hpp @@ -0,0 +1,83 @@ +#pragma once +#include "lidarlib/lidar.hpp" +#include +#include + +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 pending_ranges_; + std::vector 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 espe_error_status_; + + // Snapshot for get_diagnostics(); refreshed by finish_scan(). + Diagnostics latest_diag_; + + ScanResult ready_result_; + bool scan_ready_ = false; +}; + +} // namespace lidarlib diff --git a/include/lidarlib/lidar.hpp b/include/lidarlib/lidar.hpp index ea56aab..c59fcf7 100644 --- a/include/lidarlib/lidar.hpp +++ b/include/lidarlib/lidar.hpp @@ -47,6 +47,10 @@ struct ExtraInfo { // SICK nanoScan3 only — General System State byte 0 (see kNanoState* bits). std::optional 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 espe_error_status; }; struct ScanResult { @@ -101,7 +105,11 @@ public: // 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; + // 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; + // 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 const char* detected_model() const = 0; @@ -151,6 +159,7 @@ public: Diagnostics get_diagnostics() const override { return latest_diag_; } 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_b(const uint8_t* buf, int len); // ID=0xFEF0 bool parse_family_c(const uint8_t* buf, int len); // Magic=0xFEAC (GS1-5) diff --git a/include/lidarlib/lidarlib.hpp b/include/lidarlib/lidarlib.hpp index 3b40bb1..b8ef873 100644 --- a/include/lidarlib/lidarlib.hpp +++ b/include/lidarlib/lidarlib.hpp @@ -3,4 +3,5 @@ #include "lidarlib/error.hpp" #include "lidarlib/lidar.hpp" #include "lidarlib/sick_lidar.hpp" +#include "lidarlib/espe_lidar.hpp" #include "lidarlib/config.hpp" diff --git a/include/lidarlib/sick_lidar.hpp b/include/lidarlib/sick_lidar.hpp index dc9ca21..ab9e719 100644 --- a/include/lidarlib/sick_lidar.hpp +++ b/include/lidarlib/sick_lidar.hpp @@ -20,9 +20,11 @@ class SickDriver : public Lidar { public: using ScanCallback = lidarlib::ScanCallback; + // inverted: unit mounted upside-down → mirror the scan. explicit SickDriver(const ModelConfig& cfg, const std::string& ip, - uint16_t port = 2111); + uint16_t port = 2111, + bool inverted = false); ~SickDriver(); SickDriver(const SickDriver&) = delete; @@ -50,6 +52,7 @@ private: std::string detected_model_name_; // owned copy of cfg_.name (stable lifetime) std::string ip_; uint16_t port_; + bool inverted_ = false; int sock_fd_ = -1; ScanCallback cb_; @@ -70,10 +73,12 @@ class NanoScanDriver : public Lidar { public: 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, - const std::string& ip = "0.0.0.0", - uint16_t port = 6060); + const std::string& ip = "0.0.0.0", + uint16_t port = 6060, + bool inverted = false); ~NanoScanDriver(); NanoScanDriver(const NanoScanDriver&) = delete; @@ -99,6 +104,7 @@ private: std::string detected_model_name_; // owned copy of cfg_.name (stable lifetime) std::string ip_; uint16_t port_; + bool inverted_ = false; int sock_fd_ = -1; ScanCallback cb_; diff --git a/src/espe_lidar.cpp b/src/espe_lidar.cpp new file mode 100644 index 0000000..86be65b --- /dev/null +++ b/src/espe_lidar.cpp @@ -0,0 +1,265 @@ +#include "lidarlib/espe_lidar.hpp" +#include "lidar_bytes.hpp" +#include "lidar_net.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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((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(&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(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(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(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(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(start_angle); + angle_inc_deg_ = static_cast(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(std::lround( + (static_cast(start_angle) - rev_start_deg_) / angle_inc_deg_)) + + static_cast(data_position) - static_cast(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(points_total_)) continue; + uint16_t dist = le16(p + 0); + uint16_t inten = le16(p + 2); + pending_ranges_[idx] = (dist > kMaxDistanceMm) + ? std::numeric_limits::infinity() + : static_cast(dist) * 1e-3f; // mm -> m + // Wire intensity is 0..30000 — rescale to the 0-255 LaserScan contract. + pending_intensities_[idx] = + static_cast(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(scan.ranges.size() - 1); + scan.range_min = cfg_.range_min_m; + scan.range_max = cfg_.range_max_m; + + if (inverted_) + invert_scan(scan); + if (cfg_.remap_angles) + remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max); + + ExtraInfo& info = ready_result_.info; + info = ExtraInfo{}; + info.detected_model = cfg_.name; + info.espe_error_status = espe_error_status_; + + latest_diag_ = decode_diagnostics(info); + latest_diag_.device_timestamp_ms = scan.timestamp_ms; + + pending_ranges_.clear(); + pending_intensities_.clear(); + points_total_ = 0; + scan_ready_ = true; +} + +bool EspeDriver::recv_scan(ScanResult& out, int timeout_ms) { + for (;;) { + if (parse_buffer()) { + scan_ready_ = false; + out = std::move(ready_result_); + set_error(ErrorCode::Ok); + return true; + } + if (!fill_buffer(timeout_ms)) return false; + } +} + +bool EspeDriver::spin_once() { + if (!parse_buffer()) { + if (!fill_buffer(0)) return false; + parse_buffer(); + } + if (scan_ready_) { + scan_ready_ = false; + if (cb_) cb_(ready_result_); + } + return true; +} + +} // namespace lidarlib diff --git a/src/lidar_bytes.hpp b/src/lidar_bytes.hpp index a3a865b..c98b14d 100644 --- a/src/lidar_bytes.hpp +++ b/src/lidar_bytes.hpp @@ -1,6 +1,7 @@ // Internal helpers shared by the driver TUs — not part of the public API. #pragma once #include "lidarlib/lidar.hpp" +#include #include #include @@ -20,6 +21,16 @@ inline void remap_scan_window(LaserScan& scan, float min_deg, float max_deg) { 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). inline uint8_t le_u8 (const uint8_t* p) { return p[0]; } inline uint16_t le16(const uint8_t* p) { diff --git a/src/lidar_config.cpp b/src/lidar_config.cpp index 94145dd..f9146f6 100644 --- a/src/lidar_config.cpp +++ b/src/lidar_config.cpp @@ -1,5 +1,6 @@ #include "lidarlib/config.hpp" #include "lidarlib/sick_lidar.hpp" +#include "lidarlib/espe_lidar.hpp" #include "json_mini.hpp" #include #include @@ -26,6 +27,7 @@ constexpr ModelEntry kModels[] = { { "SICK-TIM571", &MODEL_SICK_TIM571, "SICK" }, { "SICK-TIM7xx", &MODEL_SICK_TIM7XX, "SICK" }, { "SICK-nanoScan3", &MODEL_SICK_NANOSCAN3, "SICK" }, + { "ESPE-LGA60", &MODEL_ESPE_LGA60, "ESPE" }, }; 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("model", json::Value::make_string(c.model)); 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_max_deg", json::Value::make_number(c.angle_max_deg)); 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.model = v.get_string("model", def.model); 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(v.get_number("angle_min_deg", def.angle_min_deg)); c.angle_max_deg = static_cast(v.get_number("angle_max_deg", def.angle_max_deg)); return c; @@ -80,7 +84,7 @@ const std::vector& model_names() { } const std::vector& brand_names() { - static const std::vector names = {"OLEI", "SICK"}; + static const std::vector names = {"OLEI", "SICK", "ESPE"}; return names; } @@ -137,11 +141,20 @@ void save_config(const std::string& path, const Config& cfg) { } std::unique_ptr 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_espe = (cfg.brand == "ESPE"); - const ModelConfig* model = model_by_name_for_brand(cfg.model, is_sick ? "SICK" : "OLEI"); - if (!model) model = is_sick ? &MODEL_SICK_TIM571 : &MODEL_AUTO; + const char* brand; + 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; if (cfg.angle_min_deg > -360.f || cfg.angle_max_deg < 360.f) { @@ -152,9 +165,11 @@ std::unique_ptr make_lidar(const LidarConfig& cfg) { if (is_sick) { if (model == &MODEL_SICK_NANOSCAN3) - return std::make_unique(mc, cfg.ip, cfg.port); - return std::make_unique(mc, cfg.ip, cfg.port); + return std::make_unique(mc, cfg.ip, cfg.port, cfg.inverted); + return std::make_unique(mc, cfg.ip, cfg.port, cfg.inverted); } + if (is_espe) + return std::make_unique(mc, cfg.ip, cfg.port, cfg.use_udp, cfg.inverted); return std::make_unique(mc, cfg.ip, cfg.port, cfg.inverted); } diff --git a/src/lidar_net.hpp b/src/lidar_net.hpp new file mode 100644 index 0000000..137abc8 --- /dev/null +++ b/src/lidar_net.hpp @@ -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 +#include +#include +#include +#include +#include + +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(&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 diff --git a/src/olei_lidar.cpp b/src/olei_lidar.cpp index ceaa512..d2e8bc0 100644 --- a/src/olei_lidar.cpp +++ b/src/olei_lidar.cpp @@ -52,6 +52,7 @@ Diagnostics decode_diagnostics(const ExtraInfo& info) { 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; } @@ -125,7 +126,7 @@ bool Driver::recv_scan(ScanResult& out, int timeout_ms) { return false; } } - if (!spin_once()) return false; + if (!poll_packet()) return false; } out = std::move(ready_result_); set_error(ErrorCode::Ok); @@ -133,6 +134,15 @@ bool Driver::recv_scan(ScanResult& out, int timeout_ms) { } 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_; sockaddr_in from{}; @@ -201,8 +211,6 @@ void Driver::flush_scan() { pending_intensity_.clear(); pending_info_ = ExtraInfo{}; scan_ready_ = true; - - if (cb_) cb_(ready_result_); } // Family A (0xFAF0): 20B header + 3B blocks (u16 dist, u8 intensity). diff --git a/src/sick_lidar.cpp b/src/sick_lidar.cpp index 6f0a3a6..bed21ae 100644 --- a/src/sick_lidar.cpp +++ b/src/sick_lidar.cpp @@ -1,5 +1,6 @@ #include "lidarlib/sick_lidar.hpp" #include "lidar_bytes.hpp" +#include "lidar_net.hpp" #include #include @@ -8,13 +9,11 @@ #include #include #include -#include #include #include #include #include #include -#include namespace lidarlib { @@ -48,8 +47,10 @@ constexpr size_t kNanoRecvBufSize = 65536; constexpr double kNanoAngleResolution = 4194304.0; } // namespace -SickDriver::SickDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port) - : cfg_(cfg), detected_model_name_(cfg.name ? cfg.name : ""), ip_(ip), port_(port) {} +SickDriver::SickDriver(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), + inverted_(inverted) {} SickDriver::~SickDriver() { close(); } @@ -65,43 +66,13 @@ ErrorCode SickDriver::open() { sock_fd_ = ::socket(AF_INET, SOCK_STREAM, 0); if (sock_fd_ < 0) return set_error(ErrorCode::SocketError); - // Non-blocking connect with a bounded timeout — a blocking connect() to an - // unreachable device would stall for the OS default (~2 min on Linux). - int flags = ::fcntl(sock_fd_, F_GETFL, 0); - ::fcntl(sock_fd_, F_SETFL, flags | O_NONBLOCK); - - ErrorCode conn_err = ErrorCode::Ok; - int rc = ::connect(sock_fd_, reinterpret_cast(&addr), sizeof(addr)); - if (rc < 0 && errno != EINPROGRESS) { - conn_err = (errno == ECONNREFUSED) ? ErrorCode::ConnectionRefused - : ErrorCode::ConnectionFailed; - } else if (rc < 0) { - fd_set wfds; FD_ZERO(&wfds); FD_SET(sock_fd_, &wfds); - timeval tv{ kConnectTimeoutMs / 1000, (kConnectTimeoutMs % 1000) * 1000 }; - rc = ::select(sock_fd_ + 1, nullptr, &wfds, nullptr, &tv); - if (rc == 0) { - conn_err = ErrorCode::Timeout; - } else if (rc < 0) { - conn_err = ErrorCode::ConnectionFailed; - } else { - int err = 0; socklen_t errlen = sizeof(err); - ::getsockopt(sock_fd_, SOL_SOCKET, SO_ERROR, &err, &errlen); - if (err != 0) - conn_err = (err == ECONNREFUSED) ? ErrorCode::ConnectionRefused - : ErrorCode::ConnectionFailed; - } - } - ::fcntl(sock_fd_, F_SETFL, flags); - + ErrorCode 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); } - int nodelay = 1; - ::setsockopt(sock_fd_, IPPROTO_TCP, TCP_NODELAY, &nodelay, sizeof(nodelay)); - recv_buf_.clear(); latest_diag_ = Diagnostics{}; @@ -261,7 +232,9 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out) scan.ranges[d] = static_cast(raw) * scale * 0.001f; // mm -> m got_dist = true; } else if (is_rssi && d < scan.intensities.size()) { - scan.intensities[d] = static_cast(raw) * scale; + // Clamp to the 0-255 LaserScan contract (16-bit RSSI can exceed it). + float v = static_cast(raw) * scale; + scan.intensities[d] = v > 255.f ? 255.f : v; } } }; @@ -286,6 +259,8 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out) if (scan.intensities.size() != scan.ranges.size()) scan.intensities.assign(scan.ranges.size(), 0.f); + if (inverted_) + invert_scan(scan); if (cfg_.remap_angles) remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max); @@ -306,9 +281,10 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out) // ── NanoScanDriver — SICK nanoScan3/microScan3 safety-scanner UDP output ──── -NanoScanDriver::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), - recv_buf_(kNanoRecvBufSize) {} + inverted_(inverted), recv_buf_(kNanoRecvBufSize) {} NanoScanDriver::~NanoScanDriver() { close(); } @@ -371,6 +347,7 @@ int NanoScanDriver::recv_datagram(int timeout_ms) { // drops that scan and we resync on the next scanNumber. bool NanoScanDriver::recv_scan(ScanResult& out, int timeout_ms) { std::vector tele; + std::vector have; // per-byte coverage so duplicate fragments don't count twice uint32_t cur_scan = 0, total = 0, got = 0; bool assembling = false; @@ -394,12 +371,14 @@ bool NanoScanDriver::recv_scan(ScanResult& out, int timeout_ms) { if (!assembling || scan != cur_scan || tl != total) { cur_scan = scan; total = tl; got = 0; tele.assign(total, 0); + have.assign(total, 0); assembling = true; } if (static_cast(foff) + pl_len <= total) { 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) { @@ -487,6 +466,8 @@ bool NanoScanDriver::parse_packet(const uint8_t* buf, int len, ScanResult& out) // Raw device time from the DataHeader — an opaque tag, not ms since power-on. scan.timestamp_ms = le32(buf + 28); + if (inverted_) + invert_scan(scan); if (cfg_.remap_angles) remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max);