logic ready

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ế

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

include/lidarlib/lidar.hpp

@@ -1,6 +1,7 @@
 #pragma once
 #include "lidarlib/diagnostics.hpp"
 #include "lidarlib/error.hpp"
+#include <chrono>
 #include <cstdint>
 #include <vector>
 #include <string>
@@ -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
+    // false until one scan has been seen. Updated by recv_scan()/spin_once();
-    // wire format carries no diagnostic fields (SICK, for now). Updated by
+    // call from the same thread that pumps them.
-    // recv_scan()/spin_once(); call from the same thread that pumps them.
     virtual Diagnostics get_diagnostics() const { return {}; }
 
+    // True when the sensor is usable right now: connection open, at least one
+    // fault-free scan decoded, and that scan no older than max_age_ms
+    // (0 = skip the age check). Diagnostics only refresh from
+    // recv_scan()/spin_once(), so unless something is pumping them this goes
+    // stale and reports not-ready; call from the pump thread.
+    bool is_ready(int max_age_ms = 3000) const {
+        if (!is_open() || !get_diagnostics().healthy()) return false;
+        if (max_age_ms <= 0) return true;
+        return last_scan_time_.time_since_epoch().count() != 0
+            && std::chrono::steady_clock::now() - last_scan_time_
+                   <= std::chrono::milliseconds(max_age_ms);
+    }
+
+    // Pump recv_scan() until is_ready() or timeout_ms elapses; false on
+    // timeout (see last_error() for the underlying failure). Scans consumed
+    // while waiting are discarded and the scan callback does not fire —
+    // intended for startup, before handing the pump to the main loop.
+    bool wait_ready(int timeout_ms = 5000) {
+        const auto deadline = std::chrono::steady_clock::now()
+                            + std::chrono::milliseconds(timeout_ms);
+        ScanResult tmp;
+        while (!is_ready()) {
+            if (!is_open()) return false;
+            const auto left = std::chrono::duration_cast<std::chrono::milliseconds>(
+                deadline - std::chrono::steady_clock::now()).count();
+            if (left <= 0) return false;
+            recv_scan(tmp, static_cast<int>(left));
+        }
+        return true;
+    }
+
 protected:
     ErrorCode set_error(ErrorCode e) { last_error_ = e; return e; }
+    // Drivers call this each time a full scan is decoded; feeds the freshness
+    // side of is_ready().
+    void mark_scan_decoded() { last_scan_time_ = std::chrono::steady_clock::now(); }
 
 private:
     ErrorCode last_error_ = ErrorCode::Ok;
+    std::chrono::steady_clock::time_point last_scan_time_{};
 };
 
 // OLEI UDP driver.

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

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

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