LidarSensor.h

#pragma once
 
#include <memory>
#include <string>
#include <vector>
 
#include <frc/TimedRobot.h>
#include <frc/filter/LinearFilter.h>
#include <frc/geometry/Pose2d.h>
#include <units/length.h>
#include <valkyrie/sensors/BaseSensor.h>
#include <valkyrie/sim/FieldFeature.h>
 
namespace valor {
namespace sensors {

class LidarSensor : public BaseSensor<units::millimeter_t> {
   public:
    LidarSensor();
 
    inline void SetMaxDistance(units::millimeter_t dist) { maxDistance = dist; }
 
    inline units::millimeter_t GetMaxDistance() const { return maxDistance; }
 
    inline void SetDefaultDistance(units::millimeter_t dist) { defaultDistance = dist; }
 
    inline units::millimeter_t GetDefaultDistance() const { return defaultDistance; }

    virtual bool IsHealthy() const { return true; }
 
    void SetupSim(std::function<frc::Pose2d()> robotPoseGetter, frc::Transform2d sensorPosition, std::vector<sim::FieldFeature> features);
 
    void SetGetter(std::function<units::millimeter_t()> getter);
 
    void LoggablePeriodic() override;
 
   protected:
    struct SimState {
        std::function<frc::Pose2d()> robotPoseGetter;
        frc::Transform2d sensorPosition;
        std::vector<sim::FieldFeature> fieldFeatures;
        units::millimeter_t calculatedDistance;  
    };
 
    std::unique_ptr<SimState> simState;
 
   private:
    void SimulationLoop();

    units::millimeter_t maxDistance{std::numeric_limits<double>::infinity()};

    units::millimeter_t defaultDistance = 0_mm;
};
 
}  // namespace sensors
}  // namespace valor