MechanismTelemetry.hpp

Go to the documentation of this file.

// Copyright (c) 2026 Yet Another Software Suite
// SPDX-License-Identifier: LGPL-3.0-or-later
 
#pragma once
 
#include <functional>
#include <memory>
#include <optional>
#include <span>
#include <string>
#include <wpi/datalog/DataLog.hpp>
#include <wpi/nt/DoubleTopic.hpp>
#include <wpi/nt/NetworkTable.hpp>
#include <wpi/nt/StructArrayTopic.hpp>
#include <wpi/nt/StructTopic.hpp>
#include <wpi/system/DataLogManager.hpp>
#include <wpi/util/json.hpp>
 
namespace yams::motorcontrollers {
class SmartMotorController;
}
 
namespace yams::telemetry {
 
class MechanismTelemetry {
 public:
  void SetupTelemetry(const std::string& mechanismName,
                      motorcontrollers::SmartMotorController& motorController);
 
  void SetupTelemetry(const std::string& mechanismName);
 
  void SetupTelemetry(const std::string& mechanismName, const std::string& dataLogName);
 
  void AddMotorController(const std::string& subTableName,
                          motorcontrollers::SmartMotorController& motorController);
 
  std::function<void(double)> PublishDouble(const std::string& key, const std::string& unit = "");
 
  template <typename T>
  std::function<void(const T&)> PublishStruct(const std::string& key) {
    auto publisher = std::make_shared<wpi::nt::StructPublisher<T>>(
        m_networkTable->template GetStructTopic<T>(key).Publish());
    std::shared_ptr<wpi::log::StructLogEntry<T>> logEntry;
    if (m_dataLogName) {
      logEntry = std::make_shared<wpi::log::StructLogEntry<T>>(wpi::DataLogManager::GetLog(),
                                                               *m_dataLogName + "/" + key);
    }
    return [publisher, logEntry](const T& value) {
      publisher->Set(value);
      if (logEntry) logEntry->Append(value);
    };
  }
 
  template <typename T>
  std::function<void(std::span<const T>)> PublishStructArray(const std::string& key) {
    auto publisher = std::make_shared<wpi::nt::StructArrayPublisher<T>>(
        m_networkTable->template GetStructArrayTopic<T>(key).Publish());
    std::shared_ptr<wpi::log::StructArrayLogEntry<T>> logEntry;
    if (m_dataLogName) {
      logEntry = std::make_shared<wpi::log::StructArrayLogEntry<T>>(wpi::DataLogManager::GetLog(),
                                                                    *m_dataLogName + "/" + key);
    }
    return [publisher, logEntry](std::span<const T> value) {
      publisher->Set(value);
      if (logEntry) logEntry->Append(value);
    };
  }
 
  void UpdateLoopTime();
 
  std::shared_ptr<wpi::nt::NetworkTable> GetDataTable() const;
 
  std::shared_ptr<wpi::nt::NetworkTable> GetTuningTable() const;
 
 private:
  void SetupLoopTime();
 
  std::shared_ptr<wpi::nt::NetworkTable> m_networkTable;
  std::shared_ptr<wpi::nt::NetworkTable> m_tuningNetworkTable;
  std::optional<wpi::nt::DoublePublisher> m_loopTimePublisher;
  double m_prevTimestamp{0.0};
  std::optional<std::string> m_dataLogName;
};
 
}  // namespace yams::telemetry