add r1 robot project

r1/include/vision_io.hh

@@ -0,0 +1,115 @@
+#pragma once
+
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <fstream>
+#include <sstream>
+#include <string>
+#include <thread>
+#include <vector>
+#include <unistd.h>
+
+namespace r1_vision {
+
+struct Detection {
+    bool valid = false;
+    std::string class_name;
+    double timestamp = 0.0;
+    double confidence = 0.0;
+    double u = 0.0;
+    double v = 0.0;
+};
+
+inline bool ExtractNumber(const std::string& json, const std::string& key, double& value) {
+    const std::string search = "\"" + key + "\"";
+    const size_t p = json.find(search);
+    if (p == std::string::npos) return false;
+    const size_t colon = json.find(':', p);
+    if (colon == std::string::npos) return false;
+    try {
+        value = std::stod(json.substr(colon + 1));
+        return true;
+    } catch (...) {
+        return false;
+    }
+}
+
+inline bool ExtractString(const std::string& json, const std::string& key, std::string& value) {
+    const std::string search = "\"" + key + "\"";
+    const size_t p = json.find(search);
+    if (p == std::string::npos) return false;
+    const size_t colon = json.find(':', p);
+    if (colon == std::string::npos) return false;
+    const size_t q1 = json.find('"', colon + 1);
+    if (q1 == std::string::npos) return false;
+    const size_t q2 = json.find('"', q1 + 1);
+    if (q2 == std::string::npos) return false;
+    value = json.substr(q1 + 1, q2 - q1 - 1);
+    return true;
+}
+
+inline bool ReadDetection(const std::string& path, Detection& d) {
+    std::ifstream f(path);
+    if (!f) return false;
+    std::stringstream buffer;
+    buffer << f.rdbuf();
+    const std::string json = buffer.str();
+    if (json.empty()) return false;
+
+    if (!ExtractNumber(json, "timestamp", d.timestamp)) return false;
+    if (!ExtractNumber(json, "confidence", d.confidence)) return false;
+    if (!ExtractNumber(json, "u", d.u)) return false;
+    if (!ExtractNumber(json, "v", d.v)) return false;
+    ExtractString(json, "class", d.class_name);
+    d.valid = true;
+    return true;
+}
+
+inline bool IsFresh(const Detection& d, double max_age_sec = 0.5) {
+    const double now = std::chrono::duration<double>(
+        std::chrono::system_clock::now().time_since_epoch()).count();
+    return std::isfinite(d.timestamp) && std::abs(now - d.timestamp) <= max_age_sec;
+}
+
+inline bool GetStableBottleCenter(
+    const std::string& path,
+    double& u,
+    double& v,
+    int samples = 8,
+    double min_confidence = 0.7,
+    double timeout_sec = 5.0)
+{
+    std::vector<double> us;
+    std::vector<double> vs;
+    const auto start = std::chrono::steady_clock::now();
+
+    while (static_cast<int>(us.size()) < samples) {
+        Detection d;
+        if (ReadDetection(path, d) && d.valid &&
+            (d.class_name.empty() || d.class_name == "bottle") &&
+            d.confidence >= min_confidence && IsFresh(d)) {
+            us.push_back(d.u);
+            vs.push_back(d.v);
+        }
+
+        const double elapsed = std::chrono::duration<double>(
+            std::chrono::steady_clock::now() - start).count();
+        if (elapsed > timeout_sec) return false;
+        std::this_thread::sleep_for(std::chrono::milliseconds(25));
+    }
+
+    std::sort(us.begin(), us.end());
+    std::sort(vs.begin(), vs.end());
+    const auto median = [](const std::vector<double>& x) {
+        const size_t n = x.size();
+        if (n & 1u) return x[n / 2];
+        return 0.5 * (x[n / 2 - 1] + x[n / 2]);
+    };
+
+    u = median(us);
+    v = median(vs);
+    return true;
+}
+
+} // namespace r1_vision