AprilTagsSensor.h

 
#pragma once
 
#include <memory>
#include <string>
#include <unordered_set>
 
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/estimator/PoseEstimator.h>
#include <frc/estimator/SwerveDrivePoseEstimator.h>
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Rotation3d.h>
#include <networktables/NetworkTableEntry.h>
#include <networktables/NetworkTableInstance.h>
#include <units/angle.h>
#include <units/angular_velocity.h>
#include <units/length.h>
#include <units/time.h>
#include <units/velocity.h>
 
#include "Eigen/Core"
#include "frc/geometry/Twist3d.h"
#include "valkyrie/sensors/VisionSensor.h"
 
namespace valor {
 
class AprilTagsSensor : public VisionSensor {
   public:
    enum class IMUMode {
        EXTERNAL_ONLY = 0,
        EXTERNAL_SEED = 1,
        INTERNAL_ONLY = 2,
        INTERNAL_MT1_ASSIST = 3,
        INTERNAL_EXTERNAL_ASSIST = 4,
    };
 
    enum class Solver {
        MT1,
        MT2,
    };
 
    struct Config {
        // Field border margin for field border rejection
        units::meter_t fieldBorderMargin = 0.5_m;
 
        // Power-law std dev coefficients
        double translationalStdDevCoefficient = 0.01;
        double rotationalStdDevCoefficient = 0.03;
        double perTagFactor = 1;
 
        // Stale measurement rejection and MT1 ambiguity
        units::second_t maxMeasurementAge = 2.0_s;
        double ambiguityThreshold = 0.4;
    };
 
    AprilTagsSensor(frc::TimedRobot& robot, std::string name, frc::Pose3d _cameraPose, frc::AprilTagField field,
                    std::optional<Config> config = std::nullopt);
 
    int getTagID();
 
    void LoggablePeriodic() override;
 
    void applyVisionMeasurement(frc::SwerveDrivePoseEstimator<4>* estimator);
 
    frc::Pose3d getPoseFromAprilTag();
 
    struct Orientation {
        Eigen::Vector3<units::degree_t> theta;
 
        Eigen::Vector3<units::degrees_per_second_t> omega;
    };
 
    frc::Pose3d getMegaTagPose2(Orientation orient);
 
    frc::Pose3d getTargetToBotPose();
 
    frc::Pose3d get_botpose_targetspace();
 
    void setupSim(std::function<frc::Pose2d()> robotPoseGetter, std::unordered_set<int> tagIDs, units::degree_t horizontalFOV,
                  units::degree_t verticalFOV, units::meter_t maxDistance = units::meter_t{std::numeric_limits<double>::infinity()},
                  units::degree_t viewableAngle = 70_deg);
 
    void setIMUMode(IMUMode);
    void setSolver(Solver);
    constexpr Solver getSolver() const { return solver; }
    void setIMUDoubt(double doubt);
 
    void setOrientation(Orientation orientation);
 
    constexpr Config getConfig() const { return config; }
    constexpr void setConfig(Config c) { config = c; }
 
   protected:
    struct SimState {
        std::function<frc::Pose2d()> robotPoseGetter;
        std::unordered_set<int> tagIDs;
        units::degree_t horizontalFOV;
        units::degree_t verticalFOV;
        units::meter_t maxDistance;
        units::degree_t viewableAngle;
    };
 
    void setGetter(std::function<frc::Pose3d()> getter);
 
    std::unique_ptr<SimState> simState;
 
   private:
    void simulationLoop();
 
    frc::Pose3d getGlobalPose() override;
 
    Config config;
 
    frc::Pose3d megaTag2Pose = frc::Pose3d();  
    units::meter_t distance{0_m};              
    Solver solver = Solver::MT1;
    frc::AprilTagFieldLayout fieldLayout;
};
 
}  // namespace valor