PoseEstimateSensor.h

#pragma once
#include <frc/geometry/Pose2d.h>
 
#include "valkyrie/sensors/BaseSensor.h"
 
namespace valor {
namespace sensors {
 
struct PoseEstimate {
    struct Doubts {
        units::meter_t translationalStandardDeviation;  
        units::radian_t rotationalStandardDeviation;    

        operator wpi::array<double, 3>() const {
            return {
                translationalStandardDeviation.value(),
                translationalStandardDeviation.value(),
                rotationalStandardDeviation.value(),
            };
        }
    };
 
    frc::Pose2d pose;           
    units::second_t timestamp;  
    Doubts measurementDoubts;   
};
 
typedef BaseSensor<std::optional<PoseEstimate>> PoseEstimateSensor;
 
}  // namespace sensors
}  // namespace valor