Reduce DataLink latency

components/rpc/include/CommunicationFactory.h

@@ -13,15 +13,15 @@
 #include "IConnectionManager.h"
 #include "IDiscoveryService.h"
 #include "IRPCServer.h"
-#include "PtrQueue.h"
+#include "BlockingQueue.h"
 #include "enums.h"
 
 class CommunicationFactory {
 public:
     static std::unique_ptr<IConnectionManager> create_connection_manager(CommunicationMethod type);
     static std::unique_ptr<IDiscoveryService> create_discovery_service(CommunicationMethod type);
-    static std::unique_ptr<IRPCServer> create_lossy_server(CommunicationMethod type, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>> &rx_queue);
-    static std::unique_ptr<IRPCServer> create_lossless_server(CommunicationMethod type, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue);
+    static std::unique_ptr<IRPCServer> create_lossy_server(CommunicationMethod type, const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> &rx_queue);
+    static std::unique_ptr<IRPCServer> create_lossless_server(CommunicationMethod type, const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>>& rx_queue);
 };
 
 #endif //COMMUNICATIONFACTORY_H

components/rpc/include/CommunicationRouter.h

@@ -21,13 +21,15 @@
 #include "wireless/TCPServer.h"
 #include "wireless/WifiManager.h"
 
+#define MAX_NETWORK_QUEUE_SIZE 10
+
 class CommunicationRouter {
 
 public:
   explicit CommunicationRouter(
-      const std::function<void(char *, int)> &rx_callback)
-      : m_tcp_rx_queue(std::make_shared<PtrQueue<std::vector<uint8_t>>>(10)),
-        m_rx_callback(rx_callback),
+      const std::function<void(std::unique_ptr<std::vector<uint8_t>>&&)> &rx_callback)
+      : m_tcp_rx_queue(std::make_shared<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>>(MAX_NETWORK_QUEUE_SIZE)),
+        m_rx_callback(std::move(rx_callback)),
         m_config_manager(ConfigManager::get_instance()),
         m_pc_connection(CommunicationFactory::create_connection_manager(
             m_config_manager.get_communication_method())),
@@ -56,15 +58,15 @@ public:
   [[noreturn]] static void link_layer_thread(void *args);
   int send_msg(char *buffer, size_t length) const;
   void update_leader();
-  void route(uint8_t *buffer, size_t length) const;
+  void route(std::unique_ptr<std::vector<uint8_t>>&& buffer) const;
+  void route(uint8_t* buffer, size_t size) const;
   [[nodiscard]] std::pair<std::vector<uint8_t>, std::vector<Orientation>>
   get_physically_connected_modules() const;
   [[nodiscard]] uint8_t get_leader() const;
 
   // todo: does this really need to be here (so i can access from thread)?
-  std::shared_ptr<PtrQueue<std::vector<uint8_t>>>
-      m_tcp_rx_queue; // todo: this should probably be thread safe
-  std::function<void(char *, int)> m_rx_callback;
+  std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> m_tcp_rx_queue;
+  std::function<void(std::unique_ptr<std::vector<std::uint8_t>>)> m_rx_callback;
 
 private:
   TaskHandle_t m_router_thread = nullptr;

components/rpc/include/IRPCServer.h

@@ -5,12 +5,15 @@
 #ifndef IRPCSERVER_H
 #define IRPCSERVER_H
 
+#include <memory>
+#include <vector>
+
 class IRPCServer {
-public:
+  public:
     virtual ~IRPCServer() = default;
     virtual void startup() = 0;
     virtual void shutdown() = 0;
-    virtual int send_msg(char* buffer, uint32_t length) const = 0;
+    virtual int send_msg(uint8_t *buffer, size_t size) const = 0;
 };
 
 #endif //IRPCSERVER_H

components/rpc/include/MessagingInterface.h

@@ -9,6 +9,7 @@
 #include <unordered_map>
 #include <flatbuffers_generated/TopologyMessage_generated.h>
 
+#include "BlockingQueue.h"
 #include "constants/app_comms.h"
 #include "CommunicationRouter.h"
 
@@ -16,8 +17,8 @@ class MessagingInterface {
 public:
     explicit MessagingInterface()
         : m_config_manager(ConfigManager::get_instance()),
-            m_mpi_rx_queue(xQueueCreate(MAX_RX_BUFFER_SIZE, RX_QUEUE_SIZE)),
-            m_router(std::make_unique<CommunicationRouter>([this](const char* buffer, const int size) { handleRecv(buffer, size); })),
+            m_mpi_rx_queue(std::make_unique<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>>(RX_QUEUE_SIZE)),
+            m_router(std::make_unique<CommunicationRouter>([this](std::unique_ptr<std::vector<uint8_t>>&& buffer) { handleRecv(std::move(buffer)); })),
             m_map_semaphore(xSemaphoreCreateMutex()) {};
 
     ~MessagingInterface();
@@ -31,13 +32,13 @@ public:
     uint8_t get_leader() const;
 
 private:
-    void handleRecv(const char* recv_buffer, int recv_size);
+    void handleRecv(std::unique_ptr<std::vector<uint8_t>>&& buffer);
 
     void checkOrInsertTag(uint8_t tag);
 
     ConfigManager& m_config_manager;
     uint16_t m_sequence_number = 0;
-    QueueHandle_t m_mpi_rx_queue; // todo: maybe move this down classes more
+    std::unique_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> m_mpi_rx_queue;
     std::unique_ptr<CommunicationRouter> m_router;
     SemaphoreHandle_t m_map_semaphore;
     std::unordered_map<uint8_t, QueueHandle_t> m_tag_to_queue;

components/rpc/include/wireless/TCPServer.h

@@ -6,22 +6,22 @@
 #define TCPSERVER_H
 
 #include <memory>
-#include <vector>
 #include <unordered_set>
+#include <vector>
 
-#include "freertos/FreeRTOS.h"
 #include "IRPCServer.h"
-#include "PtrQueue.h"
+#include "BlockingQueue.h"
+#include "freertos/FreeRTOS.h"
 
 class TCPServer final : public IRPCServer {
-public:
-    TCPServer(int port, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue);
+  public:
+    TCPServer(int port, const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> &rx_queue);
     ~TCPServer() override;
     void startup() override;
     void shutdown() override;
-    int send_msg(char* buffer, uint32_t length) const override;
+    int send_msg(uint8_t* buffer, size_t size) const override;
 
-private:
+  private:
     bool authenticate_client(int client_sock);
 
     static bool is_network_connected();
@@ -34,7 +34,7 @@ private:
     TaskHandle_t m_task;
     TaskHandle_t m_rx_task;
 
-    std::shared_ptr<PtrQueue<std::vector<uint8_t>>> m_rx_queue;
+    std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> m_rx_queue;
 
     SemaphoreHandle_t m_mutex;
     std::unordered_set<int> m_clients;

components/rpc/include/wireless/UDPServer.h

@@ -6,36 +6,37 @@
 #define UDPSERVER_H
 
 #include <memory>
-#include <vector>
 #include <unordered_set>
+#include <vector>
 
-#include "freertos/FreeRTOS.h"
 #include "IRPCServer.h"
-#include "PtrQueue.h"
+#include "BlockingQueue.h"
+#include "freertos/FreeRTOS.h"
 
 class UDPServer final : public IRPCServer {
-public:
-    UDPServer(int rx_port, int tx_port, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue);
+  public:
+    UDPServer(int rx_port, int tx_port,
+              const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> &rx_queue);
     ~UDPServer() override;
     void startup() override;
     void shutdown() override;
-    int send_msg(char* buffer, uint32_t length) const override;
+    int send_msg(uint8_t *buffer, size_t size) const override;
 
-private:
+  private:
     bool authenticate_client(int client_sock);
 
     static bool is_network_connected();
     [[noreturn]] static void socket_monitor_thread(void *args);
 
-	int m_tx_port;
+    int m_tx_port;
     int m_rx_port;
 
     int m_tx_server_sock;
-	int m_rx_server_sock;
+    int m_rx_server_sock;
 
     TaskHandle_t m_rx_task;
 
-    std::shared_ptr<PtrQueue<std::vector<uint8_t>>> m_rx_queue;
+    std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> m_rx_queue;
 };
 
 #endif //UDPSERVER_H