wired up voice and vision, testing motion

r1/src/vision_context.cc

@@ -0,0 +1,334 @@
+#include "r1_voice/vision_context.hh"
+
+#include <ctime>
+#include <fstream>
+#include <iomanip>
+#include <map>
+#include <regex>
+#include <sstream>
+#include <string>
+#include <vector>
+
+namespace r1_voice {
+
+namespace {
+
+struct Detection {
+    std::string name;
+    double confidence = 0.0;
+};
+
+std::string readFile(
+    const std::string& path)
+{
+    std::ifstream file(path);
+
+    if (!file)
+        return {};
+
+    std::ostringstream out;
+    out << file.rdbuf();
+
+    return out.str();
+}
+
+bool parseTimestamp(
+    const std::string& json,
+    double& timestamp)
+{
+    static const std::regex expression(
+        R"("timestamp"\s*:\s*([0-9]+(?:\.[0-9]+)?))"
+    );
+
+    std::smatch match;
+
+    if (!std::regex_search(
+            json,
+            match,
+            expression)) {
+
+        return false;
+    }
+
+    try {
+        timestamp =
+            std::stod(match[1].str());
+
+        return true;
+    }
+    catch (...) {
+        return false;
+    }
+}
+
+std::vector<Detection> parseDetections(
+    const std::string& json,
+    double min_confidence)
+{
+    std::vector<Detection> detections;
+
+    /*
+     * Expected JSON:
+     *
+     * "class": "person",
+     * "confidence": 0.95
+     */
+    static const std::regex expression(
+        R"REGEX("class"\s*:\s*"([^"]+)"[\s\S]*?"confidence"\s*:\s*([0-9]+(?:\.[0-9]+)?))REGEX"
+    );
+
+    auto begin =
+        std::sregex_iterator(
+            json.begin(),
+            json.end(),
+            expression
+        );
+
+    const auto end =
+        std::sregex_iterator();
+
+    for (auto it = begin;
+         it != end;
+         ++it) {
+
+        Detection detection;
+
+        detection.name =
+            (*it)[1].str();
+
+        try {
+            detection.confidence =
+                std::stod(
+                    (*it)[2].str()
+                );
+        }
+        catch (...) {
+            continue;
+        }
+
+        if (detection.confidence <
+            min_confidence) {
+
+            continue;
+        }
+
+        detections.push_back(
+            std::move(detection)
+        );
+    }
+
+    return detections;
+}
+
+} // namespace
+
+VisionContext::VisionContext(
+    VisionConfig config)
+    : config_(std::move(config))
+{
+}
+
+std::string VisionContext::lowerAscii(
+    std::string text)
+{
+    for (char& c : text) {
+        if (c >= 'A' && c <= 'Z') {
+            c =
+                static_cast<char>(
+                    c - 'A' + 'a'
+                );
+        }
+    }
+
+    return text;
+}
+
+bool VisionContext::isVisionQuestion(
+    const std::string& text) const
+{
+    const std::string lower =
+        lowerAscii(text);
+
+    static const std::vector<std::string>
+        triggers = {
+
+        // Czech
+        "co vidíš",
+        "co vidis",
+
+        "vidíš",
+        "vidis",
+
+        "vidět",
+        "videt",
+
+        "podívej",
+        "podivej",
+
+        "koukni",
+        "koukáš",
+        "koukas",
+
+        "před tebou",
+        "pred tebou",
+
+        "před sebou",
+        "pred sebou",
+
+        "co je před",
+        "co je pred",
+
+        "kolik lidí",
+        "kolik lidi",
+
+        "jakou barvu",
+        "kamera",
+
+        // English
+        "what do you see",
+        "can you see",
+        "do you see",
+        "look at",
+        "in front of you",
+        "how many people",
+        "camera"
+    };
+
+    for (const auto& trigger : triggers) {
+        if (lower.find(trigger) !=
+            std::string::npos) {
+
+            return true;
+        }
+    }
+
+    return false;
+}
+
+std::string VisionContext::buildContext() const
+{
+    const std::string json =
+        readFile(
+            config_.detections_path
+        );
+
+    if (json.empty()) {
+        return
+            "Camera vision information is currently "
+            "unavailable because there is no fresh "
+            "YOLO detection file. "
+            "Do not invent anything about what you see.";
+    }
+
+    double timestamp = 0.0;
+
+    if (!parseTimestamp(
+            json,
+            timestamp)) {
+
+        return
+            "Camera vision information is unavailable "
+            "because the YOLO output is invalid. "
+            "Do not invent visual information.";
+    }
+
+    const double now =
+        static_cast<double>(
+            std::time(nullptr)
+        );
+
+    const double age =
+        now - timestamp;
+
+    if (age < -1.0 ||
+        age > config_.max_age_seconds) {
+
+        std::ostringstream out;
+
+        out
+            << "Camera vision information is stale "
+            << "(approximately "
+            << std::fixed
+            << std::setprecision(1)
+            << age
+            << " seconds old). "
+            << "Do not claim to currently see objects.";
+
+        return out.str();
+    }
+
+    const auto detections =
+        parseDetections(
+            json,
+            config_.min_confidence
+        );
+
+    if (detections.empty()) {
+        return
+            "The current camera frame is fresh. "
+            "YOLO did not detect any known objects. "
+            "This does not prove that the scene is empty. "
+            "Do not invent objects.";
+    }
+
+    std::map<std::string, int> counts;
+
+    for (const auto& detection :
+         detections) {
+
+        ++counts[detection.name];
+    }
+
+    std::ostringstream out;
+
+    out
+        << "You are R1 and this is CURRENT information "
+        << "from your camera's YOLO detector. "
+        << "Use only these detections when answering "
+        << "visual questions. "
+        << "YOLO detects object classes, not actions, "
+        << "text, exact colors, identity, or intent. "
+        << "If the user asks for something YOLO cannot "
+        << "determine, say that you cannot determine it "
+        << "from the object detector yet. "
+        << "Current detected objects: ";
+
+    bool first = true;
+
+    for (const auto& [name, count] :
+         counts) {
+
+        if (!first)
+            out << ", ";
+
+        first = false;
+
+        out
+            << count
+            << " x "
+            << name;
+    }
+
+    out << ". Detection details: ";
+
+    for (size_t i = 0;
+         i < detections.size();
+         ++i) {
+
+        if (i != 0)
+            out << ", ";
+
+        out
+            << detections[i].name
+            << " confidence "
+            << std::fixed
+            << std::setprecision(2)
+            << detections[i].confidence;
+    }
+
+    out << '.';
+
+    return out.str();
+}
+
+} // namespace r1_voice