sensor_fusion.h

#pragma once
 
#include <google/protobuf/repeated_field.h>
 
#include "proto/message_translation/ssl_detection.h"
#include "proto/message_translation/ssl_geometry.h"
#include "proto/message_translation/ssl_referee.h"
#include "proto/parameters.pb.h"
#include "proto/sensor_msg.pb.h"
#include "software/sensor_fusion/filter/ball_filter.h"
#include "software/sensor_fusion/filter/robot_team_filter.h"
#include "software/sensor_fusion/filter/vision_detection.h"
#include "software/sensor_fusion/possession/possession_tracker.h"
#include "software/world/ball.h"
#include "software/world/team.h"
#include "software/world/world.h"
 
class SensorFusion
{
   public:
    explicit SensorFusion(TbotsProto::SensorFusionConfig sensor_fusion_config);
 
    virtual ~SensorFusion() = default;
 
    void processSensorProto(const SensorProto &sensor_msg);
 
    std::optional<World> getWorld() const;
 
    // Number of vision packets to indicate that the vision client most likely reset,
    // determined experimentally with the simulator
    static constexpr unsigned int VISION_PACKET_RESET_COUNT_THRESHOLD = 5;
    // Vision packets before this threshold time indicate that the vision client has just
    // started, determined experimentally with the simulator
    static constexpr double VISION_PACKET_RESET_TIME_THRESHOLD = 0.5;
 
    // This is used for detecting if the breakbeam is at fault. The idea is that distance
    // between the robot that has the ball is greater than the distance below, then we
    // know that the breakbeam has a problem see for more:
    // https://github.com/UBC-Thunderbots/Software/issues/3197
    static constexpr double DISTANCE_THRESHOLD_FOR_BREAKBEAM_FAULT_DETECTION = 0.5;
 
   private:
    void updateWorld(const SSLProto::SSL_WrapperPacket &packet);
    void updateWorld(const SSLProto::Referee &packet);
    void updateWorld(const google::protobuf::RepeatedPtrField<TbotsProto::RobotStatus>
                         &robot_status_msgs);
    void updateWorld(const SSLProto::SSL_GeometryData &geometry_packet);
    void updateWorld(const SSLProto::SSL_DetectionFrame &ssl_detection_frame);
 
    void updateBall(Ball new_ball);
 
    std::optional<Ball> createBall(const std::vector<BallDetection> &ball_detections);
 
    Team createFriendlyTeam(const std::vector<RobotDetection> &robot_detections);
    Team createEnemyTeam(const std::vector<RobotDetection> &robot_detections);
 
 
    std::optional<Point> getBallPlacementPoint(const SSLProto::Referee &packet);
 
    RobotDetection invert(RobotDetection robot_detection) const;
    BallDetection invert(BallDetection ball_detection) const;
 
    void updateDribbleDisplacement();
 
    bool checkForVisionReset(double t_capture);
 
    void resetWorldComponents();
 
    static bool teamHasBall(const Team &team, const Ball &ball);
 
    bool shouldTrustRobotStatus();
    TbotsProto::SensorFusionConfig sensor_fusion_config;
    std::optional<Field> field;
    std::optional<Ball> ball;
    Team friendly_team;
    Team enemy_team;
    GameState game_state;
    std::optional<RefereeStage> referee_stage;
 
    // Points on the field where a friendly bot initially touched the ball
    std::map<RobotId, Point> ball_contacts_by_friendly_robots;
    // Segment representing the displacement of the ball (in metres) due to
    // the friendly team continuously dribbling the ball across the field.
    //
    // - The start point of the segment is the point on the field where the friendly
    //   team started dribbling the ball.
    //
    // - The end point of the segment is the current position of the ball.
    //
    // - The length of the segment is the distance between where the friendly team
    //   started dribbling the ball and where the ball is now.
    //
    // If the friendly team does not have possession over the ball, this is std::nullopt.
    std::optional<Segment> dribble_displacement;
 
 
    BallFilter ball_filter;
    RobotTeamFilter friendly_team_filter;
    RobotTeamFilter enemy_team_filter;
 
    TeamPossession possession;
    std::shared_ptr<PossessionTracker> possession_tracker;
 
    std::optional<RobotId> friendly_robot_id_with_ball_in_dribbler;
 
    unsigned int friendly_goalie_id;
    unsigned int enemy_goalie_id;
    bool defending_positive_side;
    int ball_in_dribbler_timeout;
 
    // The number of "reset packets" we have received. These indicate that the
    // vision time should be reset. Please see `checkForVisionReset` to see how
    // this is defined.
    unsigned int reset_time_vision_packets_detected;
 
    // The timestamp, in seconds, of the most recently received vision packet
    double last_t_capture;
};