robot_filter.h

#pragma once
 
#include <optional>
#include <vector>
 
#include "software/geom/angle.h"
#include "software/geom/point.h"
#include "software/sensor_fusion/filter/vision_detection.h"
#include "software/time/timestamp.h"
#include "software/world/robot.h"
 
typedef struct FilteredRobotData_t
{
    unsigned int id;
    Point position;
    Vector velocity;
    Angle orientation;
    AngularVelocity angular_velocity;
    Timestamp timestamp;
} FilteredRobotData;
 
class RobotFilter
{
   public:
    explicit RobotFilter(Robot current_robot_state, Duration expiry_buffer_duration);
    explicit RobotFilter(RobotDetection current_robot_state,
                         Duration expiry_buffer_duration);
 
    std::optional<Robot> getFilteredData(
        const std::vector<RobotDetection>& new_robot_data);
 
    unsigned int getRobotId() const;
 
   private:
    Robot current_robot_state;
    Duration expiry_buffer_duration;
};