From 49d4e04530d82c07ba0450dfef113fff1244f9a7 Mon Sep 17 00:00:00 2001 From: QUYVN Date: Tue, 7 Jul 2026 13:46:20 +0700 Subject: [PATCH] logic ready --- docs/diagnostics.md | 32 ++++++++++++++++++++++++++--- examples/example.cpp | 10 ++++++++++ include/lidarlib/lidar.hpp | 41 +++++++++++++++++++++++++++++++++++--- src/espe_lidar.cpp | 1 + src/olei_lidar.cpp | 1 + src/sick_lidar.cpp | 2 ++ 6 files changed, 81 insertions(+), 6 deletions(-) diff --git a/docs/diagnostics.md b/docs/diagnostics.md index 46235aa..2d76a7d 100644 --- a/docs/diagnostics.md +++ b/docs/diagnostics.md @@ -140,7 +140,33 @@ Ngoài dữ liệu trên wire, bản thân driver cung cấp lớp chẩn đoán Chiến lược giám sát khuyến nghị cho app: coi cảm biến **healthy** khi và chỉ khi `recv_scan()` thành công đều đặn **và** `get_diagnostics().has_fault() == false`. -## 7. API +## 7. Kiểm tra sẵn sàng: `is_ready()` / `wait_ready()` + +Chiến lược trên được gói sẵn trong hai hàm của `Lidar`: + +```cpp +lidar->open(); +if (!lidar->wait_ready(5000)) { // bơm recv_scan() tới khi ready + // timeout: xem last_error() (Timeout, DeviceDisconnected...) +} +// ... vòng lặp chính đang bơm recv_scan()/spin_once() ... +if (!lidar->is_ready()) { /* mất dữ liệu hoặc thiết bị báo fault */ } +``` + +- `is_ready(max_age_ms = 3000)` = `is_open()` **và** `get_diagnostics().healthy()` + **và** scan decode gần nhất chưa quá `max_age_ms` (truyền `0` để bỏ kiểm tra + tuổi). Kiểm tra tuổi giúp phát hiện cảm biến chết giữa chừng — socket UDP + vẫn "mở" và snapshot diagnostics vẫn "khỏe" dù thiết bị đã ngừng phát. + Diagnostics chỉ được cập nhật bởi `recv_scan()`/`spin_once()`, nên phải có + vòng lặp đang bơm dữ liệu thì `is_ready()` mới có nghĩa. +- `wait_ready(timeout_ms = 5000)` = bơm `recv_scan()` (bỏ qua dữ liệu, không + gọi callback) cho tới khi `is_ready()` hoặc hết giờ — dùng lúc khởi động, + trước khi giao quyền bơm cho vòng lặp chính. +- `is_ready()` **không** tính `has_warning()` (kính bẩn nhẹ vẫn đo được → + vẫn ready); app muốn chặt hơn thì tự kiểm tra thêm + `!get_diagnostics().has_warning()`. + +## 8. API ```cpp #include "lidarlib/lidarlib.hpp" @@ -150,7 +176,7 @@ if (lidar->recv_scan(r, 1000)) { lidarlib::Diagnostics d = lidar->get_diagnostics(); if (!d.valid) { - // chưa có scan nào được decode (hoặc driver không hỗ trợ — SICK) + // 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 */; @@ -181,7 +207,7 @@ if (lidar->recv_scan(r, 1000)) { nano contamination error/manipulation); `has_warning()` gộp các mức cảnh báo kính bẩn. -## 8. Hướng mở rộng +## 9. Hướng mở rộng - **SICK SOPAS query chủ động**: `sRN SCdevicestate` (0=busy, 1=ready, 2=error), `sRN LCMstate` (mức nhiễm bẩn chi tiết) — cần cơ chế diff --git a/examples/example.cpp b/examples/example.cpp index ca7af2c..87ab76e 100644 --- a/examples/example.cpp +++ b/examples/example.cpp @@ -11,6 +11,16 @@ int main() { return 1; } + // Chờ cảm biến sẵn sàng: đã nhận được ít nhất một scan hoàn chỉnh + // và thiết bị không báo lỗi (motor/điện áp/nhiệt độ...) + if (!drv.wait_ready(5000)) { + fprintf(stderr, "Cảm biến chưa sẵn sàng: %s (diag: %s)\n", + lidarlib::to_string(drv.last_error()), + lidarlib::to_string(drv.get_diagnostics()).c_str()); + drv.close(); + return 1; + } + for (int i = 0; i < 10; ++i) { lidarlib::ScanResult result; if (!drv.recv_scan(result, 2000)) { diff --git a/include/lidarlib/lidar.hpp b/include/lidarlib/lidar.hpp index c59fcf7..e59b705 100644 --- a/include/lidarlib/lidar.hpp +++ b/include/lidarlib/lidar.hpp @@ -1,6 +1,7 @@ #pragma once #include "lidarlib/diagnostics.hpp" #include "lidarlib/error.hpp" +#include #include #include #include @@ -118,16 +119,50 @@ public: ErrorCode last_error() const { return last_error_; } // Device self-diagnostics from the newest fully decoded scan. valid stays - // false until one scan has been seen — and permanently on drivers whose - // wire format carries no diagnostic fields (SICK, for now). Updated by - // recv_scan()/spin_once(); call from the same thread that pumps them. + // false until one scan has been seen. Updated by recv_scan()/spin_once(); + // call from the same thread that pumps them. virtual Diagnostics get_diagnostics() const { return {}; } + // True when the sensor is usable right now: connection open, at least one + // fault-free scan decoded, and that scan no older than max_age_ms + // (0 = skip the age check). Diagnostics only refresh from + // recv_scan()/spin_once(), so unless something is pumping them this goes + // stale and reports not-ready; call from the pump thread. + bool is_ready(int max_age_ms = 3000) const { + if (!is_open() || !get_diagnostics().healthy()) return false; + if (max_age_ms <= 0) return true; + return last_scan_time_.time_since_epoch().count() != 0 + && std::chrono::steady_clock::now() - last_scan_time_ + <= std::chrono::milliseconds(max_age_ms); + } + + // Pump recv_scan() until is_ready() or timeout_ms elapses; false on + // timeout (see last_error() for the underlying failure). Scans consumed + // while waiting are discarded and the scan callback does not fire — + // intended for startup, before handing the pump to the main loop. + bool wait_ready(int timeout_ms = 5000) { + const auto deadline = std::chrono::steady_clock::now() + + std::chrono::milliseconds(timeout_ms); + ScanResult tmp; + while (!is_ready()) { + if (!is_open()) return false; + const auto left = std::chrono::duration_cast( + deadline - std::chrono::steady_clock::now()).count(); + if (left <= 0) return false; + recv_scan(tmp, static_cast(left)); + } + return true; + } + protected: ErrorCode set_error(ErrorCode e) { last_error_ = e; return e; } + // Drivers call this each time a full scan is decoded; feeds the freshness + // side of is_ready(). + void mark_scan_decoded() { last_scan_time_ = std::chrono::steady_clock::now(); } private: ErrorCode last_error_ = ErrorCode::Ok; + std::chrono::steady_clock::time_point last_scan_time_{}; }; // OLEI UDP driver. diff --git a/src/espe_lidar.cpp b/src/espe_lidar.cpp index 86be65b..923cf93 100644 --- a/src/espe_lidar.cpp +++ b/src/espe_lidar.cpp @@ -231,6 +231,7 @@ void EspeDriver::finish_scan() { latest_diag_ = decode_diagnostics(info); latest_diag_.device_timestamp_ms = scan.timestamp_ms; + mark_scan_decoded(); pending_ranges_.clear(); pending_intensities_.clear(); diff --git a/src/olei_lidar.cpp b/src/olei_lidar.cpp index d2e8bc0..15dbadd 100644 --- a/src/olei_lidar.cpp +++ b/src/olei_lidar.cpp @@ -205,6 +205,7 @@ void Driver::flush_scan() { latest_diag_ = decode_diagnostics(info); latest_diag_.device_timestamp_ms = scan.timestamp_ms; + mark_scan_decoded(); pending_angle_deg_.clear(); pending_dist_m_.clear(); diff --git a/src/sick_lidar.cpp b/src/sick_lidar.cpp index bed21ae..40c891d 100644 --- a/src/sick_lidar.cpp +++ b/src/sick_lidar.cpp @@ -275,6 +275,7 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out) latest_diag_ = decode_diagnostics(info); latest_diag_.device_timestamp_ms = scan.timestamp_ms; + mark_scan_decoded(); return true; } @@ -481,6 +482,7 @@ bool NanoScanDriver::parse_packet(const uint8_t* buf, int len, ScanResult& out) latest_diag_ = decode_diagnostics(info); latest_diag_.device_timestamp_ms = scan.timestamp_ms; + mark_scan_decoded(); return true; }