[sysid] Load DataLog files directly for analysis (#6103)

Co-authored-by: Oblarg <emichaelbrnett@gmail.com>

glass/src/lib/native/cpp/support/DataLogReaderThread.cpp

@@ -0,0 +1,124 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/support/DataLogReaderThread.h"
+
+#include <utility>
+
+#include <fmt/format.h>
+#include <wpi/StringExtras.h>
+
+using namespace glass;
+
+DataLogReaderThread::~DataLogReaderThread() {
+  if (m_thread.joinable()) {
+    m_active = false;
+    m_thread.join();
+  }
+}
+
+void DataLogReaderThread::ReadMain() {
+  wpi::SmallDenseMap<
+      int, std::pair<DataLogReaderEntry*, std::span<const uint8_t>>, 8>
+      schemaEntries;
+
+  for (auto recordIt = m_reader.begin(), recordEnd = m_reader.end();
+       recordIt != recordEnd; ++recordIt) {
+    auto& record = *recordIt;
+    if (!m_active) {
+      break;
+    }
+    ++m_numRecords;
+    if (record.IsStart()) {
+      DataLogReaderEntry data;
+      if (record.GetStartData(&data)) {
+        std::scoped_lock lock{m_mutex};
+        auto& entryPtr = m_entriesById[data.entry];
+        if (entryPtr) {
+          fmt::print("...DUPLICATE entry ID, overriding\n");
+        }
+        auto [it, isNew] = m_entriesByName.emplace(data.name, data);
+        if (isNew) {
+          it->second.ranges.emplace_back(recordIt, recordEnd);
+        }
+        entryPtr = &it->second;
+        if (data.type == "structschema" ||
+            data.type == "proto:FileDescriptorProto") {
+          schemaEntries.try_emplace(data.entry, entryPtr,
+                                    std::span<const uint8_t>{});
+        }
+        sigEntryAdded(data);
+      } else {
+        fmt::print("Start(INVALID)\n");
+      }
+    } else if (record.IsFinish()) {
+      int entry;
+      if (record.GetFinishEntry(&entry)) {
+        std::scoped_lock lock{m_mutex};
+        auto it = m_entriesById.find(entry);
+        if (it == m_entriesById.end()) {
+          fmt::print("...ID not found\n");
+        } else {
+          it->second->ranges.back().m_end = recordIt;
+          m_entriesById.erase(it);
+        }
+      } else {
+        fmt::print("Finish(INVALID)\n");
+      }
+    } else if (record.IsSetMetadata()) {
+      wpi::log::MetadataRecordData data;
+      if (record.GetSetMetadataData(&data)) {
+        std::scoped_lock lock{m_mutex};
+        auto it = m_entriesById.find(data.entry);
+        if (it == m_entriesById.end()) {
+          fmt::print("...ID not found\n");
+        } else {
+          it->second->metadata = data.metadata;
+        }
+      } else {
+        fmt::print("SetMetadata(INVALID)\n");
+      }
+    } else if (record.IsControl()) {
+      fmt::print("Unrecognized control record\n");
+    } else {
+      auto it = schemaEntries.find(record.GetEntry());
+      if (it != schemaEntries.end()) {
+        it->second.second = record.GetRaw();
+      }
+    }
+  }
+
+  // build schema databases
+  for (auto&& schemaPair : schemaEntries) {
+    auto name = schemaPair.second.first->name;
+    auto data = schemaPair.second.second;
+    if (data.empty()) {
+      continue;
+    }
+    if (wpi::starts_with(name, "NT:")) {
+      name = wpi::drop_front(name, 3);
+    }
+    if (wpi::starts_with(name, "/.schema/struct:")) {
+      auto typeStr = wpi::drop_front(name, 16);
+      std::string_view schema{reinterpret_cast<const char*>(data.data()),
+                              data.size()};
+      std::string err;
+      auto desc = m_structDb.Add(typeStr, schema, &err);
+      if (!desc) {
+        fmt::print("could not decode struct '{}' schema '{}': {}\n", name,
+                   schema, err);
+      }
+    } else if (wpi::starts_with(name, "/.schema/proto:")) {
+      // protobuf descriptor handling
+      auto filename = wpi::drop_front(name, 15);
+      if (!m_protoDb.Add(filename, data)) {
+        fmt::print("could not decode protobuf '{}' filename '{}'\n", name,
+                   filename);
+      }
+    }
+  }
+
+  sigDone();
+  m_done = true;
+}

glass/src/lib/native/include/glass/support/DataLogReaderThread.h

@@ -0,0 +1,101 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <atomic>
+#include <functional>
+#include <map>
+#include <string>
+#include <string_view>
+#include <thread>
+#include <utility>
+#include <vector>
+
+#include <wpi/DataLogReader.h>
+#include <wpi/DenseMap.h>
+#include <wpi/Signal.h>
+#include <wpi/mutex.h>
+#include <wpi/protobuf/ProtobufMessageDatabase.h>
+#include <wpi/struct/DynamicStruct.h>
+
+namespace glass {
+
+class DataLogReaderRange {
+ public:
+  DataLogReaderRange(wpi::log::DataLogReader::iterator begin,
+                     wpi::log::DataLogReader::iterator end)
+      : m_begin{begin}, m_end{end} {}
+
+  wpi::log::DataLogReader::iterator begin() const { return m_begin; }
+  wpi::log::DataLogReader::iterator end() const { return m_end; }
+
+  wpi::log::DataLogReader::iterator m_begin;
+  wpi::log::DataLogReader::iterator m_end;
+};
+
+class DataLogReaderEntry : public wpi::log::StartRecordData {
+ public:
+  std::vector<DataLogReaderRange> ranges;  // ranges where this entry is valid
+};
+
+class DataLogReaderThread {
+ public:
+  explicit DataLogReaderThread(wpi::log::DataLogReader reader)
+      : m_reader{std::move(reader)}, m_thread{[this] { ReadMain(); }} {}
+  ~DataLogReaderThread();
+
+  bool IsDone() const { return m_done; }
+  std::string_view GetBufferIdentifier() const {
+    return m_reader.GetBufferIdentifier();
+  }
+  unsigned int GetNumRecords() const { return m_numRecords; }
+  unsigned int GetNumEntries() const {
+    std::scoped_lock lock{m_mutex};
+    return m_entriesByName.size();
+  }
+
+  // Passes Entry& to func
+  template <typename T>
+  void ForEachEntryName(T&& func) {
+    std::scoped_lock lock{m_mutex};
+    for (auto&& kv : m_entriesByName) {
+      func(kv.second);
+    }
+  }
+
+  const DataLogReaderEntry* GetEntry(std::string_view name) const {
+    std::scoped_lock lock{m_mutex};
+    auto it = m_entriesByName.find(name);
+    if (it == m_entriesByName.end()) {
+      return nullptr;
+    }
+    return &it->second;
+  }
+
+  wpi::StructDescriptorDatabase& GetStructDatabase() { return m_structDb; }
+  wpi::ProtobufMessageDatabase& GetProtobufDatabase() { return m_protoDb; }
+
+  const wpi::log::DataLogReader& GetReader() const { return m_reader; }
+
+  // note: these are called on separate thread
+  wpi::sig::Signal_mt<const DataLogReaderEntry&> sigEntryAdded;
+  wpi::sig::Signal_mt<> sigDone;
+
+ private:
+  void ReadMain();
+
+  wpi::log::DataLogReader m_reader;
+  mutable wpi::mutex m_mutex;
+  std::atomic_bool m_active{true};
+  std::atomic_bool m_done{false};
+  std::atomic<unsigned int> m_numRecords{0};
+  std::map<std::string, DataLogReaderEntry, std::less<>> m_entriesByName;
+  wpi::DenseMap<int, DataLogReaderEntry*> m_entriesById;
+  wpi::StructDescriptorDatabase m_structDb;
+  wpi::ProtobufMessageDatabase m_protoDb;
+  std::thread m_thread;
+};
+
+}  // namespace glass