GamePieceSensor.h

#pragma once
 
#include <frc/estimator/SwerveDrivePoseEstimator.h>
#include <frc/geometry/Pose3d.h>
#include <units/length.h>
#include <valkyrie/cameras/Limelight.h>
#include <valkyrie/sensors/BaseSensor.h>
#include <wpi/sendable/SendableBuilder.h>
 
namespace valor {
namespace sensors {
 
class GamePieceSensor : public BaseSensor<std::optional<frc::Pose3d>> {
   public:
    explicit GamePieceSensor(std::shared_ptr<cameras::Limelight> camera);
 
    void UpdateRelativeToCenter();
 
   private:
    struct RelativePose2d {
        units::meter_t x{0_m};
        units::meter_t y{0_m};
    };
 
    std::shared_ptr<cameras::Limelight> cam;
 
    std::optional<frc::Pose3d> GetGlobalPose();
 
    RelativePose2d relativePoseFromCenter;
    RelativePose2d relativePoseFromCamera;
};
 
}  // namespace sensors
}  // namespace valor