logic ready

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
-    // 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<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.