feat(config,diagnostics): explicit transport in DeviceConfig, vendor-neutral diagnostics
DeviceConfig now carries an optional transport (serial/udp/tcp) instead of
the ESPE-only use_udp bool. Plugins validate it in create_driver_instance:
a fixed-transport driver configured with the wrong transport fails open()
with InvalidConfig (via InvalidConfigDriver — the plugin ABI forbids
returning nullptr) rather than silently ignoring the setting. Selectable
drivers (ESPE) switch TCP/UDP through the same field. config.json
load/save round-trips "transport" for every transport, including serial,
and migrates legacy use_udp:true entries.
Diagnostics drops the per-vendor accessors (espe_fault, rplidar_fault,
monitor_fault, sick_error, pollution_*, contamination_*, manipulation) for
one common shape: a list of DiagnosticIssue{severity, code, detail} with
cross-vendor codes, plus a raw map of vendor passthrough values and
to_json() for hosts that prefer a string. Vendor bit decoding now lives in
one place (decode_diagnostics); has_fault/has_warning/healthy keep their
meaning, so is_ready()/wait_ready() are unchanged.
Also: README regains the model/protocol and ExtraInfo tables lost in the
lidarlib->xlidar refactor (verified against current code), and the empty
xlocd/ tree left by a stray sync run is gone.
Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
This commit is contained in:
133
README.md
133
README.md
@@ -122,30 +122,37 @@ Callback chỉ phát từ `spin_once()` — chọn một kiểu bơm dữ liệu
|
|||||||
|
|
||||||
### Sẵn sàng & chẩn đoán thiết bị
|
### Sẵn sàng & chẩn đoán thiết bị
|
||||||
|
|
||||||
|
API chẩn đoán **chung cho mọi hãng**: `Diagnostics` trả về danh sách
|
||||||
|
`issues` (mỗi issue = `severity` fault/warning + `code` trung lập + `detail`
|
||||||
|
người-đọc-được), không có hàm riêng từng hãng. Giá trị thô của hãng nằm
|
||||||
|
trong map `raw` (VD `"sick.device_status"`, `"rplidar.error_code"`); cần
|
||||||
|
dạng chuỗi thì dùng `to_json()`.
|
||||||
|
|
||||||
```cpp
|
```cpp
|
||||||
if (!lidar->wait_ready(5000)) { /* chưa có scan sạch nào trong 5s */ }
|
if (!lidar->wait_ready(5000)) { /* chưa có scan sạch nào trong 5s */ }
|
||||||
|
|
||||||
xlidar::Diagnostics d = lidar->get_diagnostics();
|
xlidar::Diagnostics d = lidar->get_diagnostics();
|
||||||
if (d.has_fault()) {
|
for (const xlidar::DiagnosticIssue& issue : d.issues) {
|
||||||
d.monitor_fault(); // OLEI: motor/giám sát
|
// issue.severity : DiagSeverity::Fault | DiagSeverity::Warning
|
||||||
d.voltage_fault(); // OLEI: điện áp
|
// issue.code : "motor" | "voltage" | "temperature" | "optics_dirty"
|
||||||
d.temperature_fault(); // OLEI: nhiệt độ
|
// | "manipulation" | "device_error" | "device_warning"
|
||||||
d.sick_error(); // SICK TiM: device error
|
// issue.detail : mô tả kèm tên hãng + giá trị thô, VD "ESPE fault word 0x0004"
|
||||||
d.pollution_error(); // SICK TiM: kính bẩn nặng
|
printf("[%s] %s — %s\n", xlidar::to_string(issue.severity),
|
||||||
d.contamination_error(); // nanoScan3: kính bẩn nặng
|
issue.code.c_str(), issue.detail.c_str());
|
||||||
d.manipulation(); // nanoScan3: nghi bị che/can thiệp
|
|
||||||
d.espe_fault(); // ESPE: từ lỗi thiết bị
|
|
||||||
d.rplidar_fault(); // RPLIDAR: health = Error (kèm rplidar_error_code)
|
|
||||||
printf("fault: %s\n", xlidar::to_string(d).c_str());
|
|
||||||
} else if (d.has_warning()) {
|
|
||||||
// pollution/contamination warning, rplidar health warning
|
|
||||||
}
|
}
|
||||||
|
if (d.has_fault()) { /* thiết bị báo hỏng — dừng tin dữ liệu */ }
|
||||||
|
if (d.has_warning()) { /* suy giảm (kính bẩn, ...) — lên lịch bảo trì */ }
|
||||||
|
|
||||||
|
printf("%s\n", xlidar::to_string(d).c_str()); // "ok" / "FAULT: voltage | WARN: optics_dirty"
|
||||||
|
printf("%s\n", xlidar::to_json(d).c_str()); // JSON đầy đủ cho REST/telemetry
|
||||||
```
|
```
|
||||||
|
|
||||||
`d.healthy()` = đã có dữ liệu và không fault. `is_ready(max_age_ms)` = đang
|
`d.healthy()` = đã có dữ liệu và không fault. `is_ready(max_age_ms)` = đang
|
||||||
mở + healthy + scan mới nhất chưa quá hạn — diagnostics chỉ refresh qua
|
mở + healthy + scan mới nhất chưa quá hạn — diagnostics chỉ refresh qua
|
||||||
`recv_scan()`/`spin_once()`, nên gọi từ chính thread bơm dữ liệu. Layout
|
`recv_scan()`/`spin_once()`, nên gọi từ chính thread bơm dữ liệu. Ý nghĩa
|
||||||
chẩn đoán từng giao thức: [docs/diagnostics.md](docs/diagnostics.md).
|
từng `code`, key `raw` và layout chẩn đoán từng giao thức:
|
||||||
|
[docs/diagnostics.md](docs/diagnostics.md) + comment trong
|
||||||
|
`include/lidar_diagnostics.hpp`.
|
||||||
|
|
||||||
### Xử lý lỗi & lifecycle
|
### Xử lý lỗi & lifecycle
|
||||||
|
|
||||||
@@ -181,8 +188,8 @@ mỗi lidar một thread, không cần khóa.
|
|||||||
{"name":"front", "driver_id":"olei_lidar_driver", "model":"AUTO", "ip":"192.168.100.100", "port":2368},
|
{"name":"front", "driver_id":"olei_lidar_driver", "model":"AUTO", "ip":"192.168.100.100", "port":2368},
|
||||||
{"name":"sick1", "driver_id":"sick_tim_driver", "model":"SICK-TIM7xx", "ip":"192.168.0.1", "port":2111},
|
{"name":"sick1", "driver_id":"sick_tim_driver", "model":"SICK-TIM7xx", "ip":"192.168.0.1", "port":2111},
|
||||||
{"name":"nano1", "driver_id":"sick_nanoscan3_driver","model":"SICK-nanoScan3", "ip":"0.0.0.0", "port":6060},
|
{"name":"nano1", "driver_id":"sick_nanoscan3_driver","model":"SICK-nanoScan3", "ip":"0.0.0.0", "port":6060},
|
||||||
{"name":"espe1", "driver_id":"espe_lga60_driver", "model":"ESPE-LGA60", "ip":"192.168.1.88", "port":8080},
|
{"name":"espe1", "driver_id":"espe_lga60_driver", "model":"ESPE-LGA60", "transport":"udp", "ip":"192.168.1.88", "port":8080},
|
||||||
{"name":"rp1", "driver_id":"rplidar_c1_driver", "model":"AUTO", "serial_port":"/dev/ttyUSB0", "baudrate":460800}
|
{"name":"rp1", "driver_id":"rplidar_c1_driver", "model":"AUTO", "transport":"serial", "serial_port":"/dev/ttyUSB0", "baudrate":460800}
|
||||||
]
|
]
|
||||||
}
|
}
|
||||||
```
|
```
|
||||||
@@ -193,17 +200,18 @@ Các trường `DeviceConfig` (mọi trường có default, chỉ khai báo cái
|
|||||||
|---|---|
|
|---|---|
|
||||||
| `driver_id` | Plugin phụ trách thiết bị (bắt buộc) |
|
| `driver_id` | Plugin phụ trách thiết bị (bắt buộc) |
|
||||||
| `model` | Một trong `supported_models` của driver; tên lạ → mặc định của driver |
|
| `model` | Một trong `supported_models` của driver; tên lạ → mặc định của driver |
|
||||||
| `ip` / `port` | Driver mạng: UDP = địa chỉ bind cục bộ, TCP = địa chỉ thiết bị; `port` 0 = mặc định của driver |
|
| `transport` | `"serial"` / `"udp"` / `"tcp"`. Bỏ trống = transport mặc định của driver. Driver cố định transport mà bị cấu hình sai → `open()` trả `InvalidConfig`; driver `transport_selectable` (ESPE) chuyển TCP↔UDP qua trường này |
|
||||||
| `serial_port` / `baudrate` | Driver serial (rplidar); mặc định `/dev/ttyUSB0` @ 460800 |
|
| `ip` / `port` | Transport mạng: UDP = địa chỉ bind cục bộ, TCP = địa chỉ thiết bị; `port` 0 = mặc định của driver |
|
||||||
|
| `serial_port` / `baudrate` | Transport serial (rplidar); mặc định `/dev/ttyUSB0` @ 460800 |
|
||||||
| `inverted` | `true` nếu lidar lắp úp ngược — driver tự đảo góc |
|
| `inverted` | `true` nếu lidar lắp úp ngược — driver tự đảo góc |
|
||||||
| `use_udp` | Chỉ driver `transport_selectable` (ESPE): chuyển sang UDP |
|
|
||||||
| `angle_min_deg` / `angle_max_deg` | Cửa sổ FOV hợp lệ (hệ góc có dấu, 0 = phía trước): điểm ngoài cửa sổ thành NaN. Bỏ trống = tắt |
|
| `angle_min_deg` / `angle_max_deg` | Cửa sổ FOV hợp lệ (hệ góc có dấu, 0 = phía trước): điểm ngoài cửa sổ thành NaN. Bỏ trống = tắt |
|
||||||
| `range_min_m` / `range_max_m` | Ghi đè dải đo; 0 = theo model |
|
| `range_min_m` / `range_max_m` | Ghi đè dải đo; 0 = theo model |
|
||||||
| `remap_angle_min_deg` / `remap_angle_max_deg` | (legacy) remap tuyến tính góc output, không cắt điểm |
|
| `remap_angle_min_deg` / `remap_angle_max_deg` | (legacy) remap tuyến tính góc output, không cắt điểm |
|
||||||
| `extra` | map chuỗi→chuỗi cho tuỳ chọn riêng của driver |
|
| `extra` | map chuỗi→chuỗi cho tuỳ chọn riêng của driver |
|
||||||
|
|
||||||
File format cũ của lidarlib (`{"brand": "OLEI", ...}`) được migrate tự động
|
File format cũ được migrate tự động khi load: `brand`+`model` (lidarlib) →
|
||||||
khi load: `brand`+`model` → `driver_id`, cặp `angle_*_deg` cũ → remap.
|
`driver_id`, cặp `angle_*_deg` cũ → remap, `use_udp: true` → `transport:
|
||||||
|
"udp"`.
|
||||||
|
|
||||||
## Driver đi kèm
|
## Driver đi kèm
|
||||||
|
|
||||||
@@ -215,6 +223,57 @@ khi load: `brand`+`model` → `driver_id`, cặp `angle_*_deg` cũ → remap.
|
|||||||
| `sick_nanoscan3_driver` | `driver_sick_safety.so` | SICK nanoScan3/microScan3 | udp | `SICK-nanoScan3` | Receiver thụ động UDP safety-data; đích UDP cấu hình sẵn bằng Safety Designer. Port 6060. Chưa verify phần cứng |
|
| `sick_nanoscan3_driver` | `driver_sick_safety.so` | SICK nanoScan3/microScan3 | udp | `SICK-nanoScan3` | Receiver thụ động UDP safety-data; đích UDP cấu hình sẵn bằng Safety Designer. Port 6060. Chưa verify phần cứng |
|
||||||
| `espe_lga60_driver` | `driver_espe.so` | ESPE LGA60 | tcp (+udp) | `ESPE-LGA60` | FOV 320°; `open()` gửi `RAuto`; tham số thiết bị theo tool Windows của hãng. Port 8080. Chưa verify phần cứng |
|
| `espe_lga60_driver` | `driver_espe.so` | ESPE LGA60 | tcp (+udp) | `ESPE-LGA60` | FOV 320°; `open()` gửi `RAuto`; tham số thiết bị theo tool Windows của hãng. Port 8080. Chưa verify phần cứng |
|
||||||
|
|
||||||
|
### Thông số model
|
||||||
|
|
||||||
|
FOV/range dưới đây là mặc định theo `ModelConfig` của từng driver — ghi đè
|
||||||
|
bằng `range_min_m`/`range_max_m` và cửa sổ `angle_min_deg`/`angle_max_deg`.
|
||||||
|
Driver mạng dùng hệ góc có dấu, 0° = phía trước; rplidar giữ hệ góc thiết bị
|
||||||
|
[0, 2π).
|
||||||
|
|
||||||
|
**OLEI** (UDP, port 2368) — 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 tuỳ kiểu dữ liệu). Tên model đọc từ packet
|
||||||
|
(Family B/C): `detected_model()` hoặc `result.info.detected_model`.
|
||||||
|
|
||||||
|
| Model | FOV (°) | Range (m) | Giao thức |
|
||||||
|
|---|---|---|---|
|
||||||
|
| `AUTO` | −180…180 | 0.05…30 | Tự dò model từ dữ liệu (Family B/C) |
|
||||||
|
| `VB` | −135…135 | 0.05…30 | Family A |
|
||||||
|
| `VF` | −180…180 | 0.05…30 | Family A |
|
||||||
|
| `LR-1F` | −180…180 | 0.05…50 | Family A — 0° thiết bị hướng đuôi (offset +180°) |
|
||||||
|
| `LR-1FMI` | −180…180 | 0.05…30 | Family B — 0° thiết bị hướng đuôi |
|
||||||
|
| `LR-1BS5` | −180…180 | 0.05…30 | Family B |
|
||||||
|
| `LR-16F` | −135…135 | 0.05…30 | 3D 16-line |
|
||||||
|
| `GS1-5` | −180…180 | 0.05…30 | Family C/V3 — chưa verify phần cứng |
|
||||||
|
|
||||||
|
**SICK** — TiM qua TCP/SOPAS (CoLa-A) port 2111: `open()` gửi
|
||||||
|
`sEN LMDscandata 1` để start stream; frame wire đặt 90° ở phía trước nên
|
||||||
|
driver offset −90° để 0° = phía trước. nanoScan3 là receiver UDP thụ động
|
||||||
|
port 6060 — đích UDP phải cấu hình sẵn bằng SICK Safety Designer, driver
|
||||||
|
không handshake CoLa2/TCP.
|
||||||
|
|
||||||
|
| Model | FOV (°) | Range (m) | Ghi chú |
|
||||||
|
|---|---|---|---|
|
||||||
|
| `SICK-TIM5xx` | −135…135 | 0.05…10 | TiM551/561 — FOV/range theo datasheet, chưa verify |
|
||||||
|
| `SICK-TIM571` | −135…135 | 0.05…25 | Chưa verify phần cứng |
|
||||||
|
| `SICK-TIM7xx` | −135…135 | 0.05…25 | Verify trên TiM781S thật (FW V5.11) |
|
||||||
|
| `SICK-nanoScan3` | −137.5…137.5 | 0.05…40 | Cả microScan3; layout port từ sick_safetyscanners, chưa verify |
|
||||||
|
|
||||||
|
**ESPE LGA60** (TCP mặc định / UDP, port 8080; IP mặc định của hãng
|
||||||
|
192.168.1.88) — FOV 320°: thiết bị quét 20°→340° với 0° hướng đuôi (offset
|
||||||
|
−180°), preset −160…160°, range 0.05…50 m. Frame đo `HISN` (header 16 B
|
||||||
|
big-endian; điểm = distance mm + intensity, distance 50 000 mm = không có
|
||||||
|
phản hồi → ∞); frame vùng `WSimu` (nếu thiết bị gửi) được đọc lấy từ lỗi.
|
||||||
|
Độ phân giải (0.025–0.5°), tốc độ quay, mức lọc nhiễu theo cấu hình đã nạp
|
||||||
|
bằng tool Windows của hãng — driver không tự đổi. Chưa verify phần cứng.
|
||||||
|
|
||||||
|
**RPLIDAR** (serial, mặc định `/dev/ttyUSB0` @ 460800) — preset C1: range
|
||||||
|
0.05…16 m (datasheet 12 m trên nền trắng), ~10 Hz, ~400–500 điểm/vòng
|
||||||
|
(DenseBoost); A/S series dùng được với baud tương ứng. `open()` chạy health
|
||||||
|
check (Error → `DeviceError`) và tự nhận model/firmware. Riêng driver này
|
||||||
|
đo được `time_increment`/`scan_time` thực.
|
||||||
|
|
||||||
Chi tiết giao thức từng hãng (frame layout, offset góc, đơn vị) nằm trong
|
Chi tiết giao thức từng hãng (frame layout, offset góc, đơn vị) nằm trong
|
||||||
comment đầu mỗi file plugin và [docs/diagnostics.md](docs/diagnostics.md).
|
comment đầu mỗi file plugin và [docs/diagnostics.md](docs/diagnostics.md).
|
||||||
|
|
||||||
@@ -233,12 +292,30 @@ comment đầu mỗi file plugin và [docs/diagnostics.md](docs/diagnostics.md).
|
|||||||
| `timestamp_ms` | Đồng hồ thiết bị (ms); 0 nếu giao thức không có |
|
| `timestamp_ms` | Đồng hồ thiết bị (ms); 0 nếu giao thức không có |
|
||||||
| `time_increment` / `scan_time` | Chỉ rplidar đo được (chu kỳ grab thực); driver khác = 0 |
|
| `time_increment` / `scan_time` | Chỉ rplidar đo được (chu kỳ grab thực); driver khác = 0 |
|
||||||
|
|
||||||
`ExtraInfo`: metadata thô tuỳ giao thức (`detected_model`, byte lỗi OLEI,
|
`ExtraInfo`: metadata thô tuỳ giao thức — field thiết bị không có giữ
|
||||||
status SICK, health RPLIDAR, ...) — field thiết bị không có giữ
|
`std::nullopt`:
|
||||||
`std::nullopt`. `Diagnostics` (từ `get_diagnostics()` hoặc
|
|
||||||
`decode_diagnostics(info)`) là bản decode tiện dùng: `has_fault()` /
|
| Field | Nguồn | Ý nghĩa |
|
||||||
`has_warning()` / `healthy()` / `to_string()` , kèm `model`, `firmware`
|
|---|---|---|
|
||||||
(rplidar), `device_timestamp_ms`.
|
| `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ị: BIT0 monitor, BIT1 điện áp, BIT2 nhiệt độ |
|
||||||
|
| `distance_scale_mm` | OLEI A/B | Hệ số mm/count của khoảng cách; 0 = không báo |
|
||||||
|
| `rotation_raw` | OLEI Family A | Tốc độ motor (raw) |
|
||||||
|
| `distance_ratio_raw`, `scan_frequency_raw`, `input_status`, `output_status`, `field_status`, `status_flags` | OLEI Family C/V3 | Passthrough raw — ý nghĩa bit chưa verify |
|
||||||
|
| `sick_device_status` | SICK TiM | Cặp Device Status `(word0<<8)\|word1`: 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 từ frame vùng `WSimu` — chỉ có khi thiết bị gửi area data |
|
||||||
|
| `rplidar_health_status`, `rplidar_error_code` | RPLIDAR | Health từ SDK (0 ok · 1 warning · 2 error) + mã lỗi thiết bị đi kèm |
|
||||||
|
|
||||||
|
`Diagnostics` (từ `get_diagnostics()` hoặc `decode_diagnostics(info)`) là
|
||||||
|
bản decode **trung lập hãng**: `issues` (danh sách `{severity, code,
|
||||||
|
detail}` với code chung: `motor` / `voltage` / `temperature` /
|
||||||
|
`optics_dirty` / `manipulation` / `device_error` / `device_warning`), map
|
||||||
|
`raw` giữ giá trị thô theo key ổn định (`"olei.error_status"`,
|
||||||
|
`"sick.device_status"`, `"nano.general_state"`, `"espe.error_status"`,
|
||||||
|
`"rplidar.health_status"`, `"rplidar.error_code"`, ...), cùng `model`,
|
||||||
|
`firmware` (rplidar), `device_timestamp_ms`, và helper `has_fault()` /
|
||||||
|
`has_warning()` / `healthy()` / `to_string()` / `to_json()`.
|
||||||
|
|
||||||
## Viết một plugin mới
|
## Viết một plugin mới
|
||||||
|
|
||||||
|
|||||||
@@ -1 +1 @@
|
|||||||
{"lidars":[{"name":"front","driver_id":"olei_lidar_driver","model":"AUTO","ip":"192.168.100.100","port":2368,"inverted":false},{"name":"rear","driver_id":"olei_lidar_driver","model":"AUTO","ip":"192.168.100.100","port":2371,"inverted":true},{"name":"sick1","driver_id":"sick_tim_driver","model":"SICK-TIM7xx","ip":"192.168.100.22","port":2111,"inverted":false},{"name":"rp1","driver_id":"rplidar_c1_driver","model":"AUTO","serial_port":"/dev/ttyUSB0","baudrate":460800,"inverted":false}]}
|
{"lidars":[{"name":"front","driver_id":"olei_lidar_driver","model":"AUTO","transport":"udp","ip":"192.168.100.100","port":2368,"inverted":false},{"name":"rear","driver_id":"olei_lidar_driver","model":"AUTO","ip":"192.168.100.100","port":2371,"inverted":true},{"name":"sick1","driver_id":"sick_tim_driver","model":"SICK-TIM7xx","transport":"tcp","ip":"192.168.100.22","port":2111,"inverted":false},{"name":"rp1","driver_id":"rplidar_c1_driver","model":"AUTO","transport":"serial","serial_port":"/dev/ttyUSB0","baudrate":460800,"inverted":false}]}
|
||||||
|
|||||||
@@ -74,8 +74,10 @@ an toàn kiểu safety-scanner). Các trường chẩn đoán (port từ driver
|
|||||||
| `[44-47]` | u32 LE | `status_flags` | Cờ trạng thái tổng — bit map chưa có tài liệu |
|
| `[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
|
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
|
`Diagnostics::raw` (key `"olei.scan_frequency"`, `"olei.input_status"`,
|
||||||
liệu V3 chính thức hoặc thiết bị GS1-5 để thử, bổ sung decode tại
|
`"olei.output_status"`, `"olei.field_status"`, `"olei.status_flags"`,
|
||||||
|
`"olei.distance_ratio"`) thay vì decode sai thành issue. 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 `include/lidar_interface.hpp` (và bảng field trong `plugins/driver_olei/olei_driver.cpp`).
|
`decode_diagnostics()` trong `include/lidar_interface.hpp` (và bảng field trong `plugins/driver_olei/olei_driver.cpp`).
|
||||||
|
|
||||||
## 4. SICK TiM (TCP/CoLa-A — telegram `LMDscandata`)
|
## 4. SICK TiM (TCP/CoLa-A — telegram `LMDscandata`)
|
||||||
@@ -93,8 +95,11 @@ Khác OLEI, TiM có **hai đường** lấy chẩn đoán:
|
|||||||
| `0 2` | Pollution warning — kính bắt đầu bẩn, vẫn đo đượ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 |
|
| `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à
|
Driver ghép cặp này vào `info.sick_device_status` (`(word0<<8)|word1`);
|
||||||
decode qua `sick_error()` / `pollution_warning()` / `pollution_error()`.
|
`decode_diagnostics()` biến nó thành issue chung: bit error →
|
||||||
|
`{fault, device_error}`, pollution warning → `{warning, optics_dirty}`,
|
||||||
|
pollution error → `{fault, optics_dirty}` (raw giữ ở
|
||||||
|
`raw["sick.device_status"]`).
|
||||||
Ngoài ra telegram còn mang input/output số (`input_status`/`output_status`)
|
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).
|
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 để
|
**Chưa verify trên TiM781S thật với kính bẩn** — cần che/bôi bẩn kính để
|
||||||
@@ -121,8 +126,10 @@ offset/size. Block **General System State** (offset tại header `[32]`, size
|
|||||||
| 4 | `0x10` | Reference contour status |
|
| 4 | `0x10` | Reference contour status |
|
||||||
| 5 | `0x20` | Manipulation — nghi bị che/can thiệp cố ý |
|
| 5 | `0x20` | Manipulation — nghi bị che/can thiệp cố ý |
|
||||||
|
|
||||||
Driver đọc byte này vào `info.nano_general_state`, decode qua
|
Driver đọc byte này vào `info.nano_general_state`; `decode_diagnostics()`
|
||||||
`contamination_warning()` / `contamination_error()` / `manipulation()`.
|
biến nó thành issue chung: contamination warning → `{warning, optics_dirty}`,
|
||||||
|
contamination error → `{fault, optics_dirty}`, manipulation →
|
||||||
|
`{fault, manipulation}` (raw giữ ở `raw["nano.general_state"]`).
|
||||||
Lưu ý: block này **chỉ có mặt nếu được tick chọn** trong cấu hình data output
|
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`.
|
của Safety Designer — thiếu block thì trường giữ `nullopt`.
|
||||||
|
|
||||||
@@ -134,8 +141,9 @@ RPLIDAR không nhúng chẩn đoán trong stream điểm quét; thay vào đó S
|
|||||||
**một lần lúc `open()`** — status Error thì `open()` trả `DeviceError` và
|
**một lần lúc `open()`** — status Error thì `open()` trả `DeviceError` và
|
||||||
không dùng thiết bị; Warning vẫn chạy nhưng để lại dấu.
|
không dùng thiết bị; Warning vẫn chạy nhưng để lại dấu.
|
||||||
|
|
||||||
- Snapshot health nằm ở `Diagnostics::rplidar_health_status` /
|
- Snapshot health nằm ở `raw["rplidar.health_status"]` /
|
||||||
`rplidar_error_code`, decode qua `rplidar_fault()` / `rplidar_warning()`.
|
`raw["rplidar.error_code"]`; `decode_diagnostics()` sinh issue chung:
|
||||||
|
health Error → `{fault, device_error}`, Warning → `{warning, device_warning}`.
|
||||||
- `Diagnostics::model` (`slamtec-0xNN` từ device info) và
|
- `Diagnostics::model` (`slamtec-0xNN` từ device info) và
|
||||||
`Diagnostics::firmware` (`fw M.mm hw H`) được tự nhận lúc `open()`.
|
`Diagnostics::firmware` (`fw M.mm hw H`) được tự nhận lúc `open()`.
|
||||||
- Chẩn đoán runtime chủ yếu là gián tiếp: `recv_scan()` timeout / mất kết
|
- Chẩn đoán runtime chủ yếu là gián tiếp: `recv_scan()` timeout / mất kết
|
||||||
@@ -183,6 +191,19 @@ if (!lidar->is_ready()) { /* mất dữ liệu hoặc thiết bị báo fault */
|
|||||||
|
|
||||||
## 9. API
|
## 9. API
|
||||||
|
|
||||||
|
API chung cho mọi hãng — không có hàm decode riêng từng vendor. Mỗi vấn đề
|
||||||
|
thiết bị là một `DiagnosticIssue{severity, code, detail}`; bảng map từ wire
|
||||||
|
sang code chung:
|
||||||
|
|
||||||
|
| Nguồn wire | Issue (severity, code) |
|
||||||
|
|---|---|
|
||||||
|
| OLEI Family A bit monitor / voltage / temp | fault `motor` / `voltage` / `temperature` |
|
||||||
|
| OLEI Family A bit 3-7 (reserved) ≠ 0 | fault `device_error` |
|
||||||
|
| SICK TiM device error / pollution warning / pollution error | fault `device_error` / warning `optics_dirty` / fault `optics_dirty` |
|
||||||
|
| nanoScan3 contamination warning / error / manipulation | warning `optics_dirty` / fault `optics_dirty` / fault `manipulation` |
|
||||||
|
| ESPE fault word ≠ 0 | fault `device_error` (detail kèm giá trị hex) |
|
||||||
|
| RPLIDAR health Warning / Error | warning `device_warning` / fault `device_error` |
|
||||||
|
|
||||||
```cpp
|
```cpp
|
||||||
#include "lidar_manager.hpp"
|
#include "lidar_manager.hpp"
|
||||||
|
|
||||||
@@ -192,23 +213,19 @@ if (lidar->recv_scan(r, 1000)) {
|
|||||||
|
|
||||||
if (!d.valid) {
|
if (!d.valid) {
|
||||||
// chưa có scan nào được decode
|
// 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", xlidar::to_string(d).c_str());
|
|
||||||
}
|
}
|
||||||
|
for (const xlidar::DiagnosticIssue& issue : d.issues) {
|
||||||
// SICK: cảnh báo kính bẩn — chưa phải fault nhưng nên lên lịch lau
|
printf("[%s] %s — %s\n", xlidar::to_string(issue.severity),
|
||||||
if (d.has_warning()) {
|
issue.code.c_str(), issue.detail.c_str());
|
||||||
d.pollution_warning(); // TiM
|
|
||||||
d.contamination_warning(); // nanoScan3
|
|
||||||
}
|
}
|
||||||
if (d.manipulation()) /* nanoScan3: nghi bị che/can thiệp */;
|
if (d.has_fault()) { /* dừng tin dữ liệu */ }
|
||||||
|
if (d.has_warning()) { /* lên lịch bảo trì */ }
|
||||||
|
|
||||||
// Family C raw (nullopt nếu không phải GS1-5)
|
// Giá trị thô của hãng (chỉ có khi wire mang nó), VD Family C raw:
|
||||||
if (d.status_flags) printf("status_flags=0x%08X\n", *d.status_flags);
|
if (auto it = d.raw.find("olei.status_flags"); it != d.raw.end())
|
||||||
|
printf("status_flags=0x%08X\n", it->second);
|
||||||
|
|
||||||
|
printf("%s\n", xlidar::to_json(d).c_str()); // JSON cho REST/telemetry
|
||||||
}
|
}
|
||||||
```
|
```
|
||||||
|
|
||||||
@@ -217,10 +234,11 @@ if (lidar->recv_scan(r, 1000)) {
|
|||||||
- `decode_diagnostics(const ExtraInfo&)` — hàm free, decode trực tiếp từ
|
- `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ể.
|
`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` /
|
- `to_string(Diagnostics)` — chuỗi log 1 dòng: `no data` / `ok` /
|
||||||
`WARN: pollution` / `FAULT: voltage temperature`.
|
`WARN: optics_dirty` / `FAULT: voltage temperature | WARN: optics_dirty`.
|
||||||
- `has_fault()` gộp mọi nguồn lỗi (OLEI byte lỗi, TiM device/pollution error,
|
- `to_json(Diagnostics)` — chuỗi JSON đầy đủ (`valid`, `model`, `firmware`,
|
||||||
nano contamination error/manipulation); `has_warning()` gộp các mức cảnh
|
`healthy`, `issues[]`, `raw{}`) cho host nào muốn nhận string thay struct.
|
||||||
báo kính bẩn.
|
- `has_fault()` / `has_warning()` quét `issues` theo severity;
|
||||||
|
`healthy()` = `valid && !has_fault()`.
|
||||||
|
|
||||||
## 10. Hướng mở rộng
|
## 10. Hướng mở rộng
|
||||||
|
|
||||||
|
|||||||
@@ -1,14 +1,23 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
// xlidar-driver — device self-diagnostics decoded from the data stream.
|
// xlidar-driver — device self-diagnostics decoded from the data stream.
|
||||||
|
//
|
||||||
|
// The public surface is vendor-neutral: every driver reports through the same
|
||||||
|
// Diagnostics struct — a list of DiagnosticIssue with stable cross-vendor
|
||||||
|
// codes, plus a raw field map for vendor-specific passthrough. Hosts never
|
||||||
|
// need per-vendor accessors; serialize with to_json() when a string API is
|
||||||
|
// more convenient.
|
||||||
#include <cstdint>
|
#include <cstdint>
|
||||||
#include <cstdio>
|
#include <cstdio>
|
||||||
#include <optional>
|
#include <map>
|
||||||
#include <string>
|
#include <string>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
namespace xlidar {
|
namespace xlidar {
|
||||||
|
|
||||||
struct ExtraInfo; // lidar_interface.hpp
|
struct ExtraInfo; // lidar_interface.hpp
|
||||||
|
|
||||||
|
// ── Raw wire constants (document the values in Diagnostics::raw) ────────────
|
||||||
|
|
||||||
// OLEI Family A (0xFAF0) error_status bits, header byte [5]. Bits 3-7 are
|
// OLEI 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.
|
// 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 kFaultMonitor = 1u << 0; // monitor / motor abnormal
|
||||||
@@ -34,72 +43,60 @@ inline constexpr uint8_t kRplidarHealthOk = 0;
|
|||||||
inline constexpr uint8_t kRplidarHealthWarning = 1;
|
inline constexpr uint8_t kRplidarHealthWarning = 1;
|
||||||
inline constexpr uint8_t kRplidarHealthError = 2;
|
inline constexpr uint8_t kRplidarHealthError = 2;
|
||||||
|
|
||||||
// Device self-diagnostics decoded from the data stream. Fields the device
|
// ── Common diagnostics structure ────────────────────────────────────────────
|
||||||
// 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
|
// Fault = device says something is wrong now, stop trusting the data;
|
||||||
// full scan.
|
// Warning = degraded but still measuring (dirty optics, weak motor) —
|
||||||
|
// schedule cleaning/service.
|
||||||
|
enum class DiagSeverity { Warning, Fault };
|
||||||
|
|
||||||
|
inline const char* to_string(DiagSeverity s) {
|
||||||
|
return s == DiagSeverity::Fault ? "fault" : "warning";
|
||||||
|
}
|
||||||
|
|
||||||
|
// One decoded device issue. `code` is a stable, machine-readable identifier
|
||||||
|
// shared across vendors:
|
||||||
|
// "motor" — motor/monitor subsystem abnormal
|
||||||
|
// "voltage" — supply voltage out of range
|
||||||
|
// "temperature" — internal temperature abnormal
|
||||||
|
// "optics_dirty" — pollution/contamination of the optics window
|
||||||
|
// (warning: clean soon; fault: data no longer reliable)
|
||||||
|
// "manipulation" — safety scanner suspects tampering/covering
|
||||||
|
// "device_error" — device-level fault the vendor doesn't break down
|
||||||
|
// "device_warning" — device-level warning the vendor doesn't break down
|
||||||
|
// `detail` is human-readable, names the vendor, and may carry the raw value.
|
||||||
|
struct DiagnosticIssue {
|
||||||
|
DiagSeverity severity = DiagSeverity::Fault;
|
||||||
|
std::string code;
|
||||||
|
std::string detail;
|
||||||
|
};
|
||||||
|
|
||||||
|
// Device self-diagnostics decoded from the data stream. valid stays false
|
||||||
|
// until the driver has decoded one full scan; issues is empty while the
|
||||||
|
// device reports healthy. Vendor-specific raw fields appear in `raw` keyed
|
||||||
|
// by stable names ("olei.error_status", "sick.device_status",
|
||||||
|
// "nano.general_state", "espe.error_status", "rplidar.health_status",
|
||||||
|
// "rplidar.error_code", ...) — only fields present on the wire are set.
|
||||||
struct Diagnostics {
|
struct Diagnostics {
|
||||||
bool valid = false;
|
bool valid = false;
|
||||||
std::string model = "AUTO";
|
std::string model = "AUTO";
|
||||||
std::string firmware; // e.g. "fw 1.32 hw 18"; empty if unknown
|
std::string firmware; // e.g. "fw 1.32 hw 18"; empty if unknown
|
||||||
uint32_t device_timestamp_ms = 0; // device clock; 0 if not on the wire
|
uint32_t device_timestamp_ms = 0; // device clock; 0 if not on the wire
|
||||||
|
|
||||||
// OLEI Family A error byte (0 = no fault; Family B/C don't carry it)
|
std::vector<DiagnosticIssue> issues;
|
||||||
uint8_t error_status = 0;
|
std::map<std::string, uint32_t> raw;
|
||||||
|
|
||||||
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; }
|
|
||||||
|
|
||||||
// OLEI Family A only — raw motor speed field, unit unverified
|
|
||||||
std::optional<uint16_t> rotation_raw;
|
|
||||||
|
|
||||||
// OLEI 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; }
|
|
||||||
|
|
||||||
// RPLIDAR — SDK getHealth() status (refreshed at open(); the streaming
|
|
||||||
// protocol carries no health) plus the device error code that goes with it.
|
|
||||||
std::optional<uint8_t> rplidar_health_status;
|
|
||||||
std::optional<uint16_t> rplidar_error_code;
|
|
||||||
|
|
||||||
bool rplidar_fault() const { return rplidar_health_status && *rplidar_health_status == kRplidarHealthError; }
|
|
||||||
bool rplidar_warning() const { return rplidar_health_status && *rplidar_health_status == kRplidarHealthWarning; }
|
|
||||||
|
|
||||||
// Fault = device says something is wrong now; warning = degraded but
|
|
||||||
// still measuring (dirty optics, weak motor) — schedule cleaning/service.
|
|
||||||
bool has_fault() const {
|
bool has_fault() const {
|
||||||
return error_status != 0 || sick_error() || pollution_error()
|
for (const auto& i : issues)
|
||||||
|| contamination_error() || manipulation() || espe_fault()
|
if (i.severity == DiagSeverity::Fault) return true;
|
||||||
|| rplidar_fault();
|
return false;
|
||||||
}
|
}
|
||||||
bool has_warning() const {
|
bool has_warning() const {
|
||||||
return pollution_warning() || contamination_warning() || rplidar_warning();
|
for (const auto& i : issues)
|
||||||
|
if (i.severity == DiagSeverity::Warning) return true;
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
bool healthy() const { return valid && !has_fault(); }
|
bool healthy() const { return valid && !has_fault(); }
|
||||||
};
|
};
|
||||||
|
|
||||||
// Decode the diagnostic fields of one scan; sets valid = true.
|
// Decode the diagnostic fields of one scan; sets valid = true.
|
||||||
@@ -107,40 +104,77 @@ struct Diagnostics {
|
|||||||
// every plugin .so must carry its own copy).
|
// every plugin .so must carry its own copy).
|
||||||
Diagnostics decode_diagnostics(const ExtraInfo& info);
|
Diagnostics decode_diagnostics(const ExtraInfo& info);
|
||||||
|
|
||||||
// One-line log summary: "no data" / "ok" / "WARN: pollution" /
|
// One-line log summary: "no data" / "ok" / "WARN: optics_dirty" /
|
||||||
// "FAULT: voltage temperature".
|
// "FAULT: voltage temperature | WARN: optics_dirty".
|
||||||
inline std::string to_string(const Diagnostics& d) {
|
inline std::string to_string(const Diagnostics& d) {
|
||||||
if (!d.valid) return "no data";
|
if (!d.valid) return "no data";
|
||||||
if (!d.has_fault()) return d.has_warning()
|
if (d.issues.empty()) return "ok";
|
||||||
? std::string("WARN:") + (d.pollution_warning() ? " pollution" : "")
|
std::string faults, warnings;
|
||||||
+ (d.contamination_warning() ? " contamination" : "")
|
for (const auto& i : d.issues)
|
||||||
+ (d.rplidar_warning() ? " rplidar" : "")
|
(i.severity == DiagSeverity::Fault ? faults : warnings) += " " + i.code;
|
||||||
: "ok";
|
std::string s;
|
||||||
|
if (!faults.empty()) s += "FAULT:" + faults;
|
||||||
|
if (!warnings.empty()) s += (s.empty() ? "WARN:" : " | WARN:") + warnings;
|
||||||
|
return s;
|
||||||
|
}
|
||||||
|
|
||||||
std::string s = "FAULT:";
|
namespace detail {
|
||||||
if (d.monitor_fault()) s += " monitor";
|
// Minimal JSON string escaping (quotes, backslash, control characters) —
|
||||||
if (d.voltage_fault()) s += " voltage";
|
// model/firmware come off the wire and may hold arbitrary bytes.
|
||||||
if (d.temperature_fault()) s += " temperature";
|
inline std::string json_escape(const std::string& in) {
|
||||||
if (d.sick_error()) s += " device";
|
std::string out;
|
||||||
if (d.pollution_error()) s += " pollution";
|
out.reserve(in.size());
|
||||||
if (d.contamination_error()) s += " contamination";
|
for (unsigned char c : in) {
|
||||||
if (d.manipulation()) s += " manipulation";
|
switch (c) {
|
||||||
if (d.espe_fault()) {
|
case '"': out += "\\\""; break;
|
||||||
char buf[24];
|
case '\\': out += "\\\\"; break;
|
||||||
std::snprintf(buf, sizeof(buf), " espe(0x%04X)", *d.espe_error_status);
|
case '\n': out += "\\n"; break;
|
||||||
s += buf;
|
case '\r': out += "\\r"; break;
|
||||||
|
case '\t': out += "\\t"; break;
|
||||||
|
default:
|
||||||
|
if (c < 0x20) {
|
||||||
|
char buf[8];
|
||||||
|
std::snprintf(buf, sizeof(buf), "\\u%04X", c);
|
||||||
|
out += buf;
|
||||||
|
} else {
|
||||||
|
out += static_cast<char>(c);
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
if (d.rplidar_fault()) {
|
return out;
|
||||||
char buf[32];
|
}
|
||||||
std::snprintf(buf, sizeof(buf), " rplidar(0x%04X)",
|
} // namespace detail
|
||||||
d.rplidar_error_code ? *d.rplidar_error_code : 0);
|
|
||||||
s += buf;
|
// Full JSON snapshot, e.g. for a REST/telemetry payload:
|
||||||
|
// {"valid":true,"model":"C1","firmware":"fw 1.32 hw 18",
|
||||||
|
// "device_timestamp_ms":0,"healthy":false,
|
||||||
|
// "issues":[{"severity":"fault","code":"voltage","detail":"..."}],
|
||||||
|
// "raw":{"olei.error_status":2}}
|
||||||
|
inline std::string to_json(const Diagnostics& d) {
|
||||||
|
std::string s = "{\"valid\":";
|
||||||
|
s += d.valid ? "true" : "false";
|
||||||
|
s += ",\"model\":\"" + detail::json_escape(d.model) + "\"";
|
||||||
|
s += ",\"firmware\":\"" + detail::json_escape(d.firmware) + "\"";
|
||||||
|
s += ",\"device_timestamp_ms\":" + std::to_string(d.device_timestamp_ms);
|
||||||
|
s += ",\"healthy\":";
|
||||||
|
s += d.healthy() ? "true" : "false";
|
||||||
|
s += ",\"issues\":[";
|
||||||
|
for (size_t i = 0; i < d.issues.size(); ++i) {
|
||||||
|
const DiagnosticIssue& issue = d.issues[i];
|
||||||
|
if (i) s += ',';
|
||||||
|
s += "{\"severity\":\"";
|
||||||
|
s += to_string(issue.severity);
|
||||||
|
s += "\",\"code\":\"" + detail::json_escape(issue.code) + "\"";
|
||||||
|
s += ",\"detail\":\"" + detail::json_escape(issue.detail) + "\"}";
|
||||||
}
|
}
|
||||||
if (uint8_t rest = d.error_status & ~(kFaultMonitor | kFaultVoltage | kFaultTemperature)) {
|
s += "],\"raw\":{";
|
||||||
char buf[24];
|
bool first = true;
|
||||||
std::snprintf(buf, sizeof(buf), " reserved(0x%02X)", rest);
|
for (const auto& [key, value] : d.raw) {
|
||||||
s += buf;
|
if (!first) s += ',';
|
||||||
|
first = false;
|
||||||
|
s += "\"" + detail::json_escape(key) + "\":" + std::to_string(value);
|
||||||
}
|
}
|
||||||
|
s += "}}";
|
||||||
return s;
|
return s;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -132,23 +132,89 @@ struct ScanResult {
|
|||||||
ExtraInfo info;
|
ExtraInfo info;
|
||||||
};
|
};
|
||||||
|
|
||||||
// Decode the diagnostic fields of one scan; sets valid = true.
|
// Decode the diagnostic fields of one scan into the vendor-neutral
|
||||||
|
// Diagnostics structure; sets valid = true. Vendor bit layouts are decoded
|
||||||
|
// here (constants in lidar_diagnostics.hpp) so hosts only ever see common
|
||||||
|
// issue codes; the raw values ride along in Diagnostics::raw.
|
||||||
inline Diagnostics decode_diagnostics(const ExtraInfo& info) {
|
inline Diagnostics decode_diagnostics(const ExtraInfo& info) {
|
||||||
Diagnostics d;
|
Diagnostics d;
|
||||||
d.valid = true;
|
d.valid = true;
|
||||||
d.model = info.detected_model;
|
d.model = info.detected_model;
|
||||||
d.error_status = info.error_status;
|
|
||||||
d.rotation_raw = info.rotation_raw;
|
const auto add = [&d](DiagSeverity severity, const char* code, std::string detail) {
|
||||||
d.scan_frequency_raw = info.scan_frequency_raw;
|
d.issues.push_back({severity, code, std::move(detail)});
|
||||||
d.input_status = info.input_status;
|
};
|
||||||
d.output_status = info.output_status;
|
char buf[48];
|
||||||
d.field_status = info.field_status;
|
|
||||||
d.status_flags = info.status_flags;
|
// OLEI Family A error byte (Family B/C don't carry it — stays 0).
|
||||||
d.sick_device_status = info.sick_device_status;
|
if (info.error_status != 0) {
|
||||||
d.nano_general_state = info.nano_general_state;
|
d.raw["olei.error_status"] = info.error_status;
|
||||||
d.espe_error_status = info.espe_error_status;
|
if (info.error_status & kFaultMonitor)
|
||||||
d.rplidar_health_status = info.rplidar_health_status;
|
add(DiagSeverity::Fault, "motor", "OLEI monitor/motor abnormal");
|
||||||
d.rplidar_error_code = info.rplidar_error_code;
|
if (info.error_status & kFaultVoltage)
|
||||||
|
add(DiagSeverity::Fault, "voltage", "OLEI supply voltage out of range");
|
||||||
|
if (info.error_status & kFaultTemperature)
|
||||||
|
add(DiagSeverity::Fault, "temperature", "OLEI internal temperature abnormal");
|
||||||
|
if (const uint8_t rest = info.error_status
|
||||||
|
& static_cast<uint8_t>(~(kFaultMonitor | kFaultVoltage | kFaultTemperature))) {
|
||||||
|
std::snprintf(buf, sizeof(buf), "OLEI reserved error bits 0x%02X", rest);
|
||||||
|
add(DiagSeverity::Fault, "device_error", buf);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// OLEI raw passthroughs (meanings unverified — no issue decoding).
|
||||||
|
if (info.rotation_raw) d.raw["olei.rotation"] = *info.rotation_raw;
|
||||||
|
if (info.distance_ratio_raw) d.raw["olei.distance_ratio"] = *info.distance_ratio_raw;
|
||||||
|
if (info.scan_frequency_raw) d.raw["olei.scan_frequency"] = *info.scan_frequency_raw;
|
||||||
|
if (info.input_status) d.raw["olei.input_status"] = *info.input_status;
|
||||||
|
if (info.output_status) d.raw["olei.output_status"] = *info.output_status;
|
||||||
|
if (info.field_status) d.raw["olei.field_status"] = *info.field_status;
|
||||||
|
if (info.status_flags) d.raw["olei.status_flags"] = *info.status_flags;
|
||||||
|
|
||||||
|
// SICK TiM device status pair.
|
||||||
|
if (info.sick_device_status) {
|
||||||
|
d.raw["sick.device_status"] = *info.sick_device_status;
|
||||||
|
if (*info.sick_device_status & kSickStatusError)
|
||||||
|
add(DiagSeverity::Fault, "device_error", "SICK TiM device error");
|
||||||
|
if (*info.sick_device_status & kSickStatusPollutionWarning)
|
||||||
|
add(DiagSeverity::Warning, "optics_dirty", "SICK TiM pollution warning");
|
||||||
|
if (*info.sick_device_status & kSickStatusPollutionError)
|
||||||
|
add(DiagSeverity::Fault, "optics_dirty", "SICK TiM pollution error");
|
||||||
|
}
|
||||||
|
|
||||||
|
// SICK nanoScan3 general system state.
|
||||||
|
if (info.nano_general_state) {
|
||||||
|
d.raw["nano.general_state"] = *info.nano_general_state;
|
||||||
|
if (*info.nano_general_state & kNanoStateContaminationWarning)
|
||||||
|
add(DiagSeverity::Warning, "optics_dirty", "nanoScan3 contamination warning");
|
||||||
|
if (*info.nano_general_state & kNanoStateContaminationError)
|
||||||
|
add(DiagSeverity::Fault, "optics_dirty", "nanoScan3 contamination error");
|
||||||
|
if (*info.nano_general_state & kNanoStateManipulation)
|
||||||
|
add(DiagSeverity::Fault, "manipulation", "nanoScan3 manipulation suspected");
|
||||||
|
}
|
||||||
|
|
||||||
|
// ESPE fault word (bit meanings unverified).
|
||||||
|
if (info.espe_error_status) {
|
||||||
|
d.raw["espe.error_status"] = *info.espe_error_status;
|
||||||
|
if (*info.espe_error_status != 0) {
|
||||||
|
std::snprintf(buf, sizeof(buf), "ESPE fault word 0x%04X", *info.espe_error_status);
|
||||||
|
add(DiagSeverity::Fault, "device_error", buf);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// RPLIDAR SDK health.
|
||||||
|
if (info.rplidar_health_status) {
|
||||||
|
d.raw["rplidar.health_status"] = *info.rplidar_health_status;
|
||||||
|
if (info.rplidar_error_code) d.raw["rplidar.error_code"] = *info.rplidar_error_code;
|
||||||
|
if (*info.rplidar_health_status == kRplidarHealthError) {
|
||||||
|
std::snprintf(buf, sizeof(buf), "RPLIDAR health error, code 0x%04X",
|
||||||
|
info.rplidar_error_code ? *info.rplidar_error_code : 0);
|
||||||
|
add(DiagSeverity::Fault, "device_error", buf);
|
||||||
|
} else if (*info.rplidar_health_status == kRplidarHealthWarning) {
|
||||||
|
add(DiagSeverity::Warning, "device_warning", "RPLIDAR health warning");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
return d;
|
return d;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -183,7 +249,7 @@ using ScanCallback = std::function<void(const ScanResult&)>;
|
|||||||
|
|
||||||
// Transport a driver uses to reach the device. A driver declares exactly one
|
// Transport a driver uses to reach the device. A driver declares exactly one
|
||||||
// primary transport; drivers that can switch (e.g. ESPE TCP/UDP) declare the
|
// primary transport; drivers that can switch (e.g. ESPE TCP/UDP) declare the
|
||||||
// default and honor DeviceConfig::use_udp.
|
// default and honor DeviceConfig::transport.
|
||||||
enum class Transport { Serial, Udp, Tcp };
|
enum class Transport { Serial, Udp, Tcp };
|
||||||
|
|
||||||
inline const char* to_string(Transport t) {
|
inline const char* to_string(Transport t) {
|
||||||
@@ -195,6 +261,14 @@ inline const char* to_string(Transport t) {
|
|||||||
return "unknown";
|
return "unknown";
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Parse the strings written by to_string(Transport); nullopt for anything else.
|
||||||
|
inline std::optional<Transport> transport_from_string(const std::string& s) {
|
||||||
|
if (s == "serial") return Transport::Serial;
|
||||||
|
if (s == "udp") return Transport::Udp;
|
||||||
|
if (s == "tcp") return Transport::Tcp;
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
|
||||||
// Static identity a plugin registers about itself (get_driver_info entry
|
// Static identity a plugin registers about itself (get_driver_info entry
|
||||||
// point and LidarDriverInterface::get_driver_info()).
|
// point and LidarDriverInterface::get_driver_info()).
|
||||||
struct DriverInfo {
|
struct DriverInfo {
|
||||||
@@ -207,18 +281,25 @@ struct DriverInfo {
|
|||||||
|
|
||||||
// Extra metadata for hosts/UIs (not part of the required identity):
|
// Extra metadata for hosts/UIs (not part of the required identity):
|
||||||
Transport transport = Transport::Udp; // primary transport
|
Transport transport = Transport::Udp; // primary transport
|
||||||
bool transport_selectable = false; // true → use_udp switches TCP/UDP
|
bool transport_selectable = false; // true → DeviceConfig::transport
|
||||||
|
// may pick either TCP or UDP
|
||||||
std::vector<std::string> supported_models; // valid DeviceConfig::model values
|
std::vector<std::string> supported_models; // valid DeviceConfig::model values
|
||||||
};
|
};
|
||||||
|
|
||||||
// Settings for one lidar instance. `name` is the unique key across saves.
|
// Settings for one lidar instance. `name` is the unique key across saves.
|
||||||
// Which fields matter depends on the driver's transport:
|
// Which fields matter depends on the transport in effect:
|
||||||
// serial → serial_port + baudrate; udp/tcp → ip + port.
|
// serial → serial_port + baudrate; udp/tcp → ip + port.
|
||||||
struct DeviceConfig {
|
struct DeviceConfig {
|
||||||
std::string name = "lidar";
|
std::string name = "lidar";
|
||||||
std::string driver_id; // plugin that owns this device
|
std::string driver_id; // plugin that owns this device
|
||||||
std::string model = "AUTO"; // one of DriverInfo::supported_models
|
std::string model = "AUTO"; // one of DriverInfo::supported_models
|
||||||
|
|
||||||
|
// Transport to reach the device. nullopt = the driver's declared default
|
||||||
|
// (DriverInfo::transport). A fixed-transport driver rejects a mismatch
|
||||||
|
// from open() with InvalidConfig; transport-selectable drivers (ESPE)
|
||||||
|
// switch between TCP and UDP through this field.
|
||||||
|
std::optional<Transport> transport;
|
||||||
|
|
||||||
// Network transports (udp: local bind address / tcp: device address)
|
// Network transports (udp: local bind address / tcp: device address)
|
||||||
std::string ip = "0.0.0.0";
|
std::string ip = "0.0.0.0";
|
||||||
uint16_t port = 0; // 0 = driver default
|
uint16_t port = 0; // 0 = driver default
|
||||||
@@ -228,7 +309,6 @@ struct DeviceConfig {
|
|||||||
uint32_t baudrate = 460800;
|
uint32_t baudrate = 460800;
|
||||||
|
|
||||||
bool inverted = false; // unit mounted upside-down → mirror the scan
|
bool inverted = false; // unit mounted upside-down → mirror the scan
|
||||||
bool use_udp = false; // only for transport-selectable drivers (ESPE)
|
|
||||||
|
|
||||||
// Valid field-of-view window (deg, signed system: 0 = ahead, + = left).
|
// Valid field-of-view window (deg, signed system: 0 = ahead, + = left).
|
||||||
// Points outside are reported as NaN (invalid), the scan geometry is
|
// Points outside are reported as NaN (invalid), the scan geometry is
|
||||||
@@ -252,9 +332,10 @@ struct DeviceConfig {
|
|||||||
|
|
||||||
friend bool operator==(const DeviceConfig& a, const DeviceConfig& b) {
|
friend bool operator==(const DeviceConfig& a, const DeviceConfig& b) {
|
||||||
return a.name == b.name && a.driver_id == b.driver_id && a.model == b.model &&
|
return a.name == b.name && a.driver_id == b.driver_id && a.model == b.model &&
|
||||||
|
a.transport == b.transport &&
|
||||||
a.ip == b.ip && a.port == b.port &&
|
a.ip == b.ip && a.port == b.port &&
|
||||||
a.serial_port == b.serial_port && a.baudrate == b.baudrate &&
|
a.serial_port == b.serial_port && a.baudrate == b.baudrate &&
|
||||||
a.inverted == b.inverted && a.use_udp == b.use_udp &&
|
a.inverted == b.inverted &&
|
||||||
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 &&
|
||||||
a.range_min_m == b.range_min_m && a.range_max_m == b.range_max_m &&
|
a.range_min_m == b.range_min_m && a.range_max_m == b.range_max_m &&
|
||||||
a.remap_angle_min_deg == b.remap_angle_min_deg &&
|
a.remap_angle_min_deg == b.remap_angle_min_deg &&
|
||||||
|
|||||||
@@ -8,18 +8,65 @@
|
|||||||
#include <cerrno>
|
#include <cerrno>
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
#include <cstdint>
|
#include <cstdint>
|
||||||
|
#include <cstdio>
|
||||||
#include <cstring>
|
#include <cstring>
|
||||||
#include <fcntl.h>
|
#include <fcntl.h>
|
||||||
#include <limits>
|
#include <limits>
|
||||||
#include <netinet/in.h>
|
#include <netinet/in.h>
|
||||||
#include <netinet/tcp.h>
|
#include <netinet/tcp.h>
|
||||||
|
#include <string>
|
||||||
#include <sys/select.h>
|
#include <sys/select.h>
|
||||||
#include <sys/socket.h>
|
#include <sys/socket.h>
|
||||||
|
#include <utility>
|
||||||
|
|
||||||
namespace xlidar {
|
namespace xlidar {
|
||||||
|
|
||||||
inline constexpr float kDeg2Rad = 3.14159265358979323846f / 180.f;
|
inline constexpr float kDeg2Rad = 3.14159265358979323846f / 180.f;
|
||||||
|
|
||||||
|
// True when the requested DeviceConfig::transport is one this driver can
|
||||||
|
// serve: unset always matches (driver default); otherwise the declared
|
||||||
|
// transport, or — for transport-selectable drivers — either side of the
|
||||||
|
// TCP/UDP pair.
|
||||||
|
inline bool transport_supported(const DriverInfo& info, const DeviceConfig& cfg) {
|
||||||
|
if (!cfg.transport || *cfg.transport == info.transport) return true;
|
||||||
|
if (info.transport_selectable)
|
||||||
|
return (*cfg.transport == Transport::Udp && info.transport == Transport::Tcp) ||
|
||||||
|
(*cfg.transport == Transport::Tcp && info.transport == Transport::Udp);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Stand-in returned by create_driver_instance() when a structurally valid
|
||||||
|
// config still can't be served (e.g. transport mismatch): the plugin ABI
|
||||||
|
// forbids returning nullptr there, so the error surfaces from open() as
|
||||||
|
// InvalidConfig instead of the setting being silently ignored.
|
||||||
|
class InvalidConfigDriver : public LidarDriverInterface {
|
||||||
|
public:
|
||||||
|
InvalidConfigDriver(DriverInfo info, std::string reason)
|
||||||
|
: info_(std::move(info)), reason_(std::move(reason)) {}
|
||||||
|
|
||||||
|
DriverInfo get_driver_info() const override { return info_; }
|
||||||
|
ErrorCode open() override {
|
||||||
|
std::fprintf(stderr, "[xlidar] %s: %s\n", info_.driver_id.c_str(), reason_.c_str());
|
||||||
|
return set_error(ErrorCode::InvalidConfig);
|
||||||
|
}
|
||||||
|
void close() override {}
|
||||||
|
bool recv_scan(ScanResult&, int) override {
|
||||||
|
set_error(ErrorCode::NotOpen);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
void set_scan_callback(ScanCallback) override {}
|
||||||
|
bool spin_once() override {
|
||||||
|
set_error(ErrorCode::NotOpen);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
const char* detected_model() const override { return info_.model.c_str(); }
|
||||||
|
bool is_open() const override { return false; }
|
||||||
|
|
||||||
|
private:
|
||||||
|
DriverInfo info_;
|
||||||
|
std::string reason_;
|
||||||
|
};
|
||||||
|
|
||||||
// Remap a finished scan's angular window onto [min_deg, max_deg]. Only
|
// Remap a finished scan's angular window onto [min_deg, max_deg]. Only
|
||||||
// angle_min/angle_max/angle_increment are rewritten; points are untouched.
|
// angle_min/angle_max/angle_increment are rewritten; points are untouched.
|
||||||
inline void remap_scan_window(LaserScan& scan, float min_deg, float max_deg) {
|
inline void remap_scan_window(LaserScan& scan, float min_deg, float max_deg) {
|
||||||
|
|||||||
@@ -270,12 +270,13 @@ const DriverInfo kDriverInfo = [] {
|
|||||||
info.model = "LGA60";
|
info.model = "LGA60";
|
||||||
info.driver_id = "espe_lga60_driver";
|
info.driver_id = "espe_lga60_driver";
|
||||||
info.description = "ESPE LGA60 320° laser scanner — TCP by default, UDP via "
|
info.description = "ESPE LGA60 320° laser scanner — TCP by default, UDP via "
|
||||||
"use_udp; open() sends the RAuto start command; device "
|
"DeviceConfig::transport; open() sends the RAuto start "
|
||||||
"parameters come from the vendor Windows tool. Default "
|
"command; device parameters come from the vendor Windows "
|
||||||
"port 8080 (vendor default IP 192.168.1.88). Ported from "
|
"tool. Default port 8080 (vendor default IP 192.168.1.88). "
|
||||||
"the vendor ROS driver; not verified on real hardware.";
|
"Ported from the vendor ROS driver; not verified on real "
|
||||||
|
"hardware.";
|
||||||
info.transport = Transport::Tcp;
|
info.transport = Transport::Tcp;
|
||||||
info.transport_selectable = true; // use_udp switches to UDP
|
info.transport_selectable = true; // transport = udp switches to UDP
|
||||||
info.supported_models = {"ESPE-LGA60"};
|
info.supported_models = {"ESPE-LGA60"};
|
||||||
return info;
|
return info;
|
||||||
}();
|
}();
|
||||||
@@ -293,7 +294,11 @@ XLIDAR_PLUGIN_EXPORT void get_driver_info(xlidar::DriverInfo* out) {
|
|||||||
XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface*
|
XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface*
|
||||||
create_driver_instance(const xlidar::DeviceConfig* cfg) {
|
create_driver_instance(const xlidar::DeviceConfig* cfg) {
|
||||||
using namespace xlidar;
|
using namespace xlidar;
|
||||||
const uint16_t port = cfg->port ? cfg->port : 8080;
|
if (!transport_supported(kDriverInfo, *cfg))
|
||||||
|
return new InvalidConfigDriver(kDriverInfo,
|
||||||
|
std::string("unsupported transport '") + to_string(*cfg->transport) + "'");
|
||||||
|
const uint16_t port = cfg->port ? cfg->port : 8080;
|
||||||
|
const bool use_udp = cfg->transport == Transport::Udp;
|
||||||
return new EspeDriver(apply_device_config(MODEL_ESPE_LGA60, *cfg),
|
return new EspeDriver(apply_device_config(MODEL_ESPE_LGA60, *cfg),
|
||||||
cfg->ip, port, cfg->use_udp, cfg->inverted);
|
cfg->ip, port, use_udp, cfg->inverted);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -450,6 +450,9 @@ XLIDAR_PLUGIN_EXPORT void get_driver_info(xlidar::DriverInfo* out) {
|
|||||||
XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface*
|
XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface*
|
||||||
create_driver_instance(const xlidar::DeviceConfig* cfg) {
|
create_driver_instance(const xlidar::DeviceConfig* cfg) {
|
||||||
using namespace xlidar;
|
using namespace xlidar;
|
||||||
|
if (!transport_supported(kDriverInfo, *cfg))
|
||||||
|
return new InvalidConfigDriver(kDriverInfo,
|
||||||
|
std::string("unsupported transport '") + to_string(*cfg->transport) + "'");
|
||||||
const ModelConfig* preset = model_by_name(cfg->model);
|
const ModelConfig* preset = model_by_name(cfg->model);
|
||||||
if (!preset) preset = &MODEL_AUTO; // unknown model → auto-detect
|
if (!preset) preset = &MODEL_AUTO; // unknown model → auto-detect
|
||||||
const uint16_t port = cfg->port ? cfg->port : 2368;
|
const uint16_t port = cfg->port ? cfg->port : 2368;
|
||||||
|
|||||||
@@ -318,6 +318,9 @@ XLIDAR_PLUGIN_EXPORT void get_driver_info(xlidar::DriverInfo* out) {
|
|||||||
XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface*
|
XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface*
|
||||||
create_driver_instance(const xlidar::DeviceConfig* cfg) {
|
create_driver_instance(const xlidar::DeviceConfig* cfg) {
|
||||||
using namespace xlidar;
|
using namespace xlidar;
|
||||||
|
if (!transport_supported(kDriverInfo, *cfg))
|
||||||
|
return new InvalidConfigDriver(kDriverInfo,
|
||||||
|
std::string("unsupported transport '") + to_string(*cfg->transport) + "'");
|
||||||
// "AUTO" and "C1" share the same preset; the real model is read from the
|
// "AUTO" and "C1" share the same preset; the real model is read from the
|
||||||
// device at open().
|
// device at open().
|
||||||
const uint32_t baud = cfg->baudrate ? cfg->baudrate : 460800;
|
const uint32_t baud = cfg->baudrate ? cfg->baudrate : 460800;
|
||||||
|
|||||||
@@ -255,6 +255,9 @@ XLIDAR_PLUGIN_EXPORT void get_driver_info(xlidar::DriverInfo* out) {
|
|||||||
XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface*
|
XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface*
|
||||||
create_driver_instance(const xlidar::DeviceConfig* cfg) {
|
create_driver_instance(const xlidar::DeviceConfig* cfg) {
|
||||||
using namespace xlidar;
|
using namespace xlidar;
|
||||||
|
if (!transport_supported(kDriverInfo, *cfg))
|
||||||
|
return new InvalidConfigDriver(kDriverInfo,
|
||||||
|
std::string("unsupported transport '") + to_string(*cfg->transport) + "'");
|
||||||
const uint16_t port = cfg->port ? cfg->port : 6060;
|
const uint16_t port = cfg->port ? cfg->port : 6060;
|
||||||
return new SickSafetyDriver(apply_device_config(MODEL_SICK_NANOSCAN3, *cfg),
|
return new SickSafetyDriver(apply_device_config(MODEL_SICK_NANOSCAN3, *cfg),
|
||||||
cfg->ip, port, cfg->inverted);
|
cfg->ip, port, cfg->inverted);
|
||||||
|
|||||||
@@ -312,6 +312,9 @@ XLIDAR_PLUGIN_EXPORT void get_driver_info(xlidar::DriverInfo* out) {
|
|||||||
XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface*
|
XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface*
|
||||||
create_driver_instance(const xlidar::DeviceConfig* cfg) {
|
create_driver_instance(const xlidar::DeviceConfig* cfg) {
|
||||||
using namespace xlidar;
|
using namespace xlidar;
|
||||||
|
if (!transport_supported(kDriverInfo, *cfg))
|
||||||
|
return new InvalidConfigDriver(kDriverInfo,
|
||||||
|
std::string("unsupported transport '") + to_string(*cfg->transport) + "'");
|
||||||
const ModelConfig* preset = model_by_name(cfg->model);
|
const ModelConfig* preset = model_by_name(cfg->model);
|
||||||
if (!preset) preset = &MODEL_SICK_TIM571; // brand default
|
if (!preset) preset = &MODEL_SICK_TIM571; // brand default
|
||||||
const uint16_t port = cfg->port ? cfg->port : 2111;
|
const uint16_t port = cfg->port ? cfg->port : 2111;
|
||||||
|
|||||||
@@ -27,12 +27,14 @@ json::Value to_json(const DeviceConfig& c) {
|
|||||||
v.set("name", json::Value::make_string(c.name));
|
v.set("name", json::Value::make_string(c.name));
|
||||||
v.set("driver_id", json::Value::make_string(c.driver_id));
|
v.set("driver_id", json::Value::make_string(c.driver_id));
|
||||||
v.set("model", json::Value::make_string(c.model));
|
v.set("model", json::Value::make_string(c.model));
|
||||||
|
// Omitted when unset — the driver's declared default transport applies.
|
||||||
|
if (c.transport)
|
||||||
|
v.set("transport", json::Value::make_string(to_string(*c.transport)));
|
||||||
v.set("ip", json::Value::make_string(c.ip));
|
v.set("ip", json::Value::make_string(c.ip));
|
||||||
v.set("port", json::Value::make_number(c.port));
|
v.set("port", json::Value::make_number(c.port));
|
||||||
v.set("serial_port", json::Value::make_string(c.serial_port));
|
v.set("serial_port", json::Value::make_string(c.serial_port));
|
||||||
v.set("baudrate", json::Value::make_number(c.baudrate));
|
v.set("baudrate", json::Value::make_number(c.baudrate));
|
||||||
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));
|
||||||
v.set("range_min_m", json::Value::make_number(c.range_min_m));
|
v.set("range_min_m", json::Value::make_number(c.range_min_m));
|
||||||
@@ -68,7 +70,17 @@ DeviceConfig lidar_from_json(const json::Value& v) {
|
|||||||
c.serial_port = v.get_string("serial_port", c.serial_port);
|
c.serial_port = v.get_string("serial_port", c.serial_port);
|
||||||
c.baudrate = static_cast<uint32_t>(v.get_number("baudrate", c.baudrate));
|
c.baudrate = static_cast<uint32_t>(v.get_number("baudrate", c.baudrate));
|
||||||
c.inverted = v.get_bool("inverted", c.inverted);
|
c.inverted = v.get_bool("inverted", c.inverted);
|
||||||
c.use_udp = v.get_bool("use_udp", c.use_udp);
|
|
||||||
|
// "transport": "serial" | "udp" | "tcp"; unknown strings fall back to the
|
||||||
|
// driver default. Legacy files carried a use_udp bool instead.
|
||||||
|
if (const std::string t = v.get_string("transport"); !t.empty()) {
|
||||||
|
c.transport = transport_from_string(t);
|
||||||
|
if (!c.transport)
|
||||||
|
std::fprintf(stderr, "[xlidar] lidar '%s': unknown transport '%s' — using driver default\n",
|
||||||
|
c.name.c_str(), t.c_str());
|
||||||
|
} else if (v.get_bool("use_udp", false)) {
|
||||||
|
c.transport = Transport::Udp;
|
||||||
|
}
|
||||||
c.range_min_m = static_cast<float>(v.get_number("range_min_m", c.range_min_m));
|
c.range_min_m = static_cast<float>(v.get_number("range_min_m", c.range_min_m));
|
||||||
c.range_max_m = static_cast<float>(v.get_number("range_max_m", c.range_max_m));
|
c.range_max_m = static_cast<float>(v.get_number("range_max_m", c.range_max_m));
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user