network.h

#pragma once
 
#include <mutex>
#include <queue>
 
#include "proto/ip_notification.pb.h"
#include "proto/primitive.pb.h"
#include "proto/robot_log_msg.pb.h"
#include "proto/robot_status_msg.pb.h"
#include "shared/constants.h"
#include "shared/robot_constants.h"
#include "software/embedded/services/network/proto_tracker.h"
#include "software/networking/radio/threaded_proto_radio_listener.hpp"
#include "software/networking/udp/threaded_proto_udp_listener.hpp"
#include "software/networking/udp/threaded_proto_udp_sender.hpp"
#include "software/time/duration.h"
#include "software/time/timestamp.h"
#include "software/world/robot_state.h"
 
class NetworkService
{
   public:
    NetworkService(const RobotId& robot_id, const std::string& ip_address,
                   unsigned short primitive_listener_port,
                   unsigned short robot_status_sender_port,
                   unsigned short full_system_to_robot_ip_notification_port,
                   unsigned short robot_to_full_system_ip_notification_port,
                   unsigned short robot_logs_port, const std::string& interface);
 
    TbotsProto::Primitive poll(TbotsProto::RobotStatus& robot_status);
 
   private:
    bool shouldSendNewRobotStatus(const TbotsProto::RobotStatus& robot_status) const;
 
    void logNewPrimitive(const TbotsProto::Primitive& new_primitive);
 
    void updatePrimitiveLog(TbotsProto::RobotStatus& robot_status);
 
    double getCurrentEpochTimeInSeconds();
 
    void primitiveCallback(const TbotsProto::Primitive& input);
 
    void onFullSystemIpNotification(const TbotsProto::IpNotification& ip_notification);
 
    void sendRobotStatus(const TbotsProto::RobotStatus& robot_status);
 
    // Constants
    static constexpr unsigned int ROBOT_STATUS_BROADCAST_RATE_HZ = 30;
    static constexpr double ROBOT_STATUS_TO_THUNDERLOOP_HZ_RATIO =
        ROBOT_STATUS_BROADCAST_RATE_HZ / (THUNDERLOOP_HZ + 1.0);
    static constexpr int IP_DISCOVERY_NOTIFICATION_RATE_HZ = 1 * THUNDERLOOP_HZ;
 
    // increases size of deque when robot status messages are sent less frequently
    static constexpr unsigned int PRIMITIVE_DEQUE_MAX_SIZE =
        static_cast<unsigned int>(1500 / ROBOT_STATUS_BROADCAST_RATE_HZ);
 
    // Variables
 
    // Mutex protects the primitive message
    std::mutex primitive_mutex;
    TbotsProto::Primitive primitive_msg;
 
    // Mutex protects the fullsystem IP address
    std::mutex fullsystem_ip_mutex;
    std::optional<std::string> fullsystem_ip;
 
    // Mutex protects the robot status sender
    std::mutex robot_status_sender_mutex;
    std::unique_ptr<ThreadedProtoUdpSender<TbotsProto::RobotStatus>> robot_status_sender;
 
    std::unique_ptr<ThreadedProtoUdpListener<TbotsProto::IpNotification>>
        fullsystem_to_robot_ip_listener;
    std::unique_ptr<ThreadedProtoUdpSender<TbotsProto::IpNotification>>
        robot_to_fullsystem_ip_sender;
    std::unique_ptr<ThreadedProtoUdpListener<TbotsProto::Primitive>>
        udp_listener_primitive;
    std::unique_ptr<ThreadedProtoRadioListener<TbotsProto::Primitive>>
        radio_listener_primitive_set;
    std::shared_ptr<ThreadedProtoUdpSender<TbotsProto::RobotLog>> robot_log_sender;
 
    // The network interface to listen and send messages on
    std::string interface;
 
    // Port to send robot status messages
    unsigned short robot_status_sender_port;
 
    // Counters for tracking rate-limited events
    unsigned int ip_notification_ticks = 0;
    unsigned int network_ticks         = 0;
    unsigned int thunderloop_ticks     = 0;
 
    // ProtoTrackers for tracking recent primitive_set packet loss
    ProtoTracker primitive_tracker;
 
    // track last breakbeam state for sending RobotStatus outside of specified rate
    bool last_breakbeam_state_sent = false;
 
    struct RoundTripTime
    {
        // Primitive Sequence Number
        uint64_t primitive_sequence_num = 0;
        // Epoch time of primitive set sent time from Thunderscope in seconds
        double thunderscope_sent_time_seconds = 0;
        // System time for when primitive set was received by Thunderloop in seconds
        double thunderloop_recieved_time_seconds = 0;
    };
 
    // Stores the most recent primitives for calculating round-trip time
    std::deque<RoundTripTime> primitive_rtt;
 
    // IP discovery message to send on the network
    TbotsProto::IpNotification robot_ip_notification_msg;
};