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/sim/FieldFeature.h"
#include "valkyrie/sensors/BaseSensor.h"
 
namespace valor {
 
class LidarSensor : public BaseSensor<units::millimeter_t> {
   public:
    explicit LidarSensor(frc::TimedRobot& robot);
 
    units::millimeter_t getMaxDistance();
 
    void setMaxDistance(units::millimeter_t maxDistance);
 
    void setupSim(std::function<frc::Pose2d()> robotPoseGetter, frc::Transform2d sensorPosition, std::vector<sim::FieldFeature> features);
 
    void setGetter(std::function<units::millimeter_t()> getter);
 
   protected:
    struct SimState {
        std::function<frc::Pose2d()> robotPoseGetter;
        frc::Transform2d sensorPosition;
        std::vector<sim::FieldFeature> fieldFeatures;
        units::millimeter_t calculatedDistance;  
    };
 
    void calculate() override;
 
    LidarSensor() = default;
 
    std::unique_ptr<SimState> simState;
 
   private:
    void simulationLoop();
 
    units::millimeter_t maxDistance{std::numeric_limits<double>::infinity()};
};
}  // namespace valor