Frame.hpp

Go to the documentation of this file.

 
#pragma once
 
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <cstdint>
#include <cstring>
#include <stdexcept>
#include <vector>
 
#include <vulcan/core/VulcanError.hpp>
#include <vulcan/io/TelemetrySchema.hpp>
 
namespace vulcan::io {

class Frame {
  public:
    explicit Frame(const TelemetrySchema &schema)
        : schema_(&schema), time_(0.0),
          buffer_(schema.frame_size_bytes(), std::byte{0}) {}
 
    // =========================================================================
    // Time
    // =========================================================================

    void set_time(double t) { time_ = t; }

    double time() const { return time_; }
 
    // =========================================================================
    // Scalar setters
    // =========================================================================

    void set(const std::string &signal, double value) {
        const auto &desc = schema_->signal(signal);
        if (desc.type != SignalType::Double) {
            throw vulcan::SignalError("Type mismatch for signal: " + signal);
        }
        write_at<double>(desc.offset, value);
    }

    void set(const std::string &signal, int32_t value) {
        const auto &desc = schema_->signal(signal);
        if (desc.type != SignalType::Int32) {
            throw vulcan::SignalError("Type mismatch for signal: " + signal);
        }
        // Store as int32 in 8-byte slot (4 bytes value + 4 bytes padding)
        write_at<int64_t>(desc.offset, static_cast<int64_t>(value));
    }

    void set(const std::string &signal, int64_t value) {
        const auto &desc = schema_->signal(signal);
        if (desc.type != SignalType::Int64) {
            throw vulcan::SignalError("Type mismatch for signal: " + signal);
        }
        write_at<int64_t>(desc.offset, value);
    }
 
    // =========================================================================
    // Convenience setters: vectors
    // =========================================================================

    void set(const std::string &signal, const Eigen::Vector3d &v) {
        set(signal + ".x", v.x());
        set(signal + ".y", v.y());
        set(signal + ".z", v.z());
    }

    void set(const std::string &signal, const Eigen::Vector4d &v) {
        set(signal + ".w", v.w());
        set(signal + ".x", v.x());
        set(signal + ".y", v.y());
        set(signal + ".z", v.z());
    }

    void set(const std::string &signal, const Eigen::Quaterniond &q) {
        set(signal + ".w", q.w());
        set(signal + ".x", q.x());
        set(signal + ".y", q.y());
        set(signal + ".z", q.z());
    }
 
    // =========================================================================
    // Scalar getters
    // =========================================================================

    double get_double(const std::string &signal) const {
        const auto &desc = schema_->signal(signal);
        if (desc.type != SignalType::Double) {
            throw vulcan::SignalError("Type mismatch for signal: " + signal);
        }
        return read_at<double>(desc.offset);
    }

    int32_t get_int32(const std::string &signal) const {
        const auto &desc = schema_->signal(signal);
        if (desc.type != SignalType::Int32) {
            throw vulcan::SignalError("Type mismatch for signal: " + signal);
        }
        return static_cast<int32_t>(read_at<int64_t>(desc.offset));
    }

    int64_t get_int64(const std::string &signal) const {
        const auto &desc = schema_->signal(signal);
        if (desc.type != SignalType::Int64) {
            throw vulcan::SignalError("Type mismatch for signal: " + signal);
        }
        return read_at<int64_t>(desc.offset);
    }
 
    // =========================================================================
    // Convenience getters: vectors
    // =========================================================================

    Eigen::Vector3d get_vec3(const std::string &signal) const {
        return Eigen::Vector3d(get_double(signal + ".x"),
                               get_double(signal + ".y"),
                               get_double(signal + ".z"));
    }

    Eigen::Quaterniond get_quat(const std::string &signal) const {
        return Eigen::Quaterniond(
            get_double(signal + ".w"), get_double(signal + ".x"),
            get_double(signal + ".y"), get_double(signal + ".z"));
    }

    Eigen::Vector4d get_vec4(const std::string &signal) const {
        return Eigen::Vector4d(
            get_double(signal + ".w"), get_double(signal + ".x"),
            get_double(signal + ".y"), get_double(signal + ".z"));
    }
 
    // =========================================================================
    // Raw access (for serialization)
    // =========================================================================

    const std::byte *data() const { return buffer_.data(); }

    std::byte *data() { return buffer_.data(); }

    size_t size_bytes() const { return buffer_.size(); }

    const TelemetrySchema &schema() const { return *schema_; }

    void clear() {
        time_ = 0.0;
        std::fill(buffer_.begin(), buffer_.end(), std::byte{0});
    }
 
  private:
    const TelemetrySchema *schema_;
    double time_;
    std::vector<std::byte> buffer_;
 
    template <typename T> void write_at(size_t offset, T value) {
        std::memcpy(buffer_.data() + offset, &value, sizeof(T));
    }
 
    template <typename T> T read_at(size_t offset) const {
        T value;
        std::memcpy(&value, buffer_.data() + offset, sizeof(T));
        return value;
    }
};
 
} // namespace vulcan::io