Improved buffer management

source/LibMultiSense/details/legacy/channel.cc

@@ -727,7 +727,8 @@ std::optional<MultiSenseStatus> LegacyChannel::get_system_status()
     const auto message_stats = m_message_assembler.get_message_statistics();
     MultiSenseStatus::ClientNetworkStatus client_stats{message_stats.received_messages,
                                                        message_stats.dropped_messages,
-                                                       message_stats.invalid_packets};
+                                                       message_stats.invalid_packets,
+                                                       (m_udp_receiver ? m_udp_receiver->dropped_packets() : 0)};
 
     return MultiSenseStatus{system_ok(status->message),
                             (ptp_status ? std::make_optional(convert(ptp_status.value())) : std::nullopt),

source/LibMultiSense/details/legacy/storage.cc

@@ -44,90 +44,116 @@ namespace multisense{
 namespace legacy{
 
 namespace {
-    constexpr size_t NUM_ALLOCATION_RETRIES = 5;
-}
 
-BufferPool::BufferPool(const BufferPoolConfig &config):
-    m_config(config)
+constexpr size_t NUM_ALLOCATION_RETRIES = 5;
+
+std::pair<
+    std::vector<std::vector<uint8_t>>,
+    std::vector<size_t>>
+allocate_buffers( size_t count, size_t buffer_size)
 {
+    std::vector<std::vector<uint8_t>> storage{};
+    std::vector<size_t> free_list{};
 
-    for (size_t i = 0 ; i < config.num_small_buffers ; ++i)
+    storage.reserve(count);
+    for (size_t i = 0 ; i < count ; ++i)
     {
-        for (size_t t = 0; t < NUM_ALLOCATION_RETRIES ; ++t)
+        bool allocated = false;
+        for (size_t attempt = 0; attempt < NUM_ALLOCATION_RETRIES; ++attempt)
         {
             try
             {
-                auto small_buffer = std::make_shared<std::vector<uint8_t>>();
-                small_buffer->reserve(config.small_buffer_size);
-                m_small_buffers.emplace_back(std::move(small_buffer));
+                storage.emplace_back(buffer_size, 0);
+                allocated = true;
                 break;
             }
             catch(const std::exception &e)
             {
-                (void) e;
-                CRL_DEBUG("Failed to allocate small buffer. Retrying\n");
+                CRL_DEBUG("Failed to allocate buffers: %s. Retrying\n", e.what());
             }
         }
-    }
 
-    for (size_t i = 0 ; i < config.num_large_buffers ; ++i)
-    {
-        for (size_t t = 0; t < NUM_ALLOCATION_RETRIES ; ++t)
+        if (!allocated)
         {
-            try
-            {
-                auto large_buffer = std::make_shared<std::vector<uint8_t>>();
-                large_buffer->reserve(config.large_buffer_size);
-                m_large_buffers.emplace_back(std::move(large_buffer));
-                break;
-            }
-            catch(const std::exception &e)
-            {
-                (void) e;
-                CRL_DEBUG("Failed to allocate large buffer\n");
-            }
+            CRL_EXCEPTION("Failed to allocate buffers");
         }
     }
 
-    if (m_small_buffers.size() != config.num_small_buffers || m_large_buffers.size() != config.num_large_buffers)
+    free_list.reserve(count);
+    for (size_t i = 0 ; i < count ; ++i)
     {
-        CRL_EXCEPTION("Failed to allocate buffers");
+        free_list.emplace_back(i);
     }
+
+    return std::make_pair(std::move(storage), std::move(free_list));
+}
+
+}
+
+BufferPool::BufferPool(const BufferPoolConfig &config):
+    m_config(config)
+{
+    std::tie(m_small_buffers, m_small_free_list) = allocate_buffers(config.num_small_buffers, config.small_buffer_size);
+    std::tie(m_large_buffers, m_large_free_list) = allocate_buffers(config.num_large_buffers, config.large_buffer_size);
 }
 
 std::shared_ptr<std::vector<uint8_t>> BufferPool::get_buffer(size_t target_size)
 {
     if (target_size <= m_config.small_buffer_size)
     {
-        const auto &small_buffer = std::find_if(std::begin(m_small_buffers), std::end(m_small_buffers),
-                                                [](const auto &small_buffer)
-                                                {
-                                                    return small_buffer.use_count() == 1;
-                                                });
-
-        if (small_buffer != std::end(m_small_buffers))
-        {
-            (*small_buffer)->resize(target_size, 0);
-            return *small_buffer;
-        }
+        return acquire_buffer(BufferType::Small, target_size);
     }
     else if (target_size <= m_config.large_buffer_size)
     {
-        const auto &large_buffer = std::find_if(std::begin(m_large_buffers), std::end(m_large_buffers),
-                                                [](const auto &large_buffer)
-                                                {
-                                                    return large_buffer.use_count() == 1;
-                                                });
-
-        if (large_buffer != std::end(m_large_buffers))
-        {
-            (*large_buffer)->resize(target_size, 0);
-            return *large_buffer;
-        }
+        return acquire_buffer(BufferType::Large, target_size);
     }
 
     return nullptr;
 }
 
+std::shared_ptr<std::vector<uint8_t>> BufferPool::acquire_buffer(BufferType type, size_t target_size)
+{
+    std::unique_lock<std::mutex> lock(m_mutex);
+
+    auto& free_list = (type == BufferType::Small) ? m_small_free_list : m_large_free_list;
+    auto& storage   = (type == BufferType::Small) ? m_small_buffers     : m_large_buffers;
+
+    if (free_list.empty())
+    {
+        return nullptr;
+    }
+
+    const size_t index = free_list.back();
+    free_list.pop_back();
+    auto* buffer = &storage[index];
+    lock.unlock();
+
+    buffer->resize(target_size, 0);
+
+    const auto self = shared_from_this();
+
+    return std::shared_ptr<std::vector<uint8_t>>(buffer,
+        [self, this, type, index](std::vector<uint8_t>* released_buffer)
+        {
+            this->release_buffer(type, index, released_buffer);
+        });
+}
+
+void BufferPool::release_buffer(BufferType type, size_t index, std::vector<uint8_t>* buffer)
+{
+    const size_t reserve_size = (type == BufferType::Small) ?
+                                m_config.small_buffer_size :
+                                m_config.large_buffer_size;
+
+    buffer->clear();
+    buffer->resize(0);
+    buffer->reserve(reserve_size);
+
+    std::lock_guard<std::mutex> lock(m_mutex);
+
+    auto& free_list = (type == BufferType::Small) ? m_small_free_list : m_large_free_list;
+    free_list.emplace_back(index);
+}
+
 }
 }

source/LibMultiSense/details/legacy/udp.cc

@@ -34,6 +34,7 @@
  *   2024-12-24, malvarado@carnegierobotics.com, IRAD, Created file.
  **/
 
+#include <cstring>
 #include <functional>
 #include <iostream>
 
@@ -46,25 +47,36 @@ namespace legacy{
 
 UdpReceiver::UdpReceiver(const NetworkSocket &socket,
                          size_t max_mtu,
-                         std::function<void(const std::vector<uint8_t>&)> receive_callback):
+                         std::function<void(const std::vector<uint8_t>&)> receive_callback,
+                         size_t max_packet_queue_depth):
     m_socket(socket.sensor_socket),
     m_stop(false),
     m_max_mtu(max_mtu),
-    m_incoming_buffer(max_mtu, 0),
+    m_packet_buffers(max_packet_queue_depth, std::vector<uint8_t>(max_mtu, 0)),
     m_receive_callback(receive_callback)
-
 {
+    for (size_t i = 0; i < m_packet_buffers.size(); ++i)
+    {
+        m_free_buffers.emplace_back(i);
+    }
+
     m_rx_thread = std::thread(&UdpReceiver::rx_thread, this);
+    m_dispatch_thread = std::thread(&UdpReceiver::dispatch_thread, this);
 }
 
 UdpReceiver::~UdpReceiver()
 {
-    if(!m_stop.exchange(true))
+    m_stop.store(true);
+    m_queue_valid.notify_all();
+
+    if (m_rx_thread.joinable())
     {
-        if (m_rx_thread.joinable())
-        {
-            m_rx_thread.join();
-        }
+        m_rx_thread.join();
+    }
+
+    if (m_dispatch_thread.joinable())
+    {
+        m_dispatch_thread.join();
     }
 }
 
@@ -93,16 +105,30 @@ void UdpReceiver::rx_thread()
         //
         while(true)
         {
+            size_t buffer_index = 0;
+            {
+                std::lock_guard<std::mutex> lock(m_queue_mutex);
+                if (m_free_buffers.empty())
+                {
+                    ++m_dropped_packets;
+                    continue;
+                }
+
+                buffer_index = m_free_buffers.front();
+                m_free_buffers.pop_front();
+            }
 
             try
             {
 #if defined(WIN32) && !defined(__MINGW64__)
 #pragma warning (push)
 #pragma warning (disable : 4267)
 #endif
+            auto& buffer = m_packet_buffers[buffer_index];
+            buffer.resize(m_max_mtu);
             const int bytes_read = recvfrom(m_socket,
-                                            reinterpret_cast<char*>(m_incoming_buffer.data()),
-                                            m_incoming_buffer.size(),
+                                            reinterpret_cast<char*>(buffer.data()),
+                                            buffer.size(),
                                             0, NULL, NULL);
 #if defined(WIN32) && !defined(__MINGW64__)
 #pragma warning (pop)
@@ -112,28 +138,75 @@ void UdpReceiver::rx_thread()
                 //
                 if (bytes_read < 0)
                 {
+                    std::lock_guard<std::mutex> lock(m_queue_mutex);
+                    m_free_buffers.emplace_back(buffer_index);
                     break;
                 }
 
-                m_incoming_buffer.resize(bytes_read);
-                m_receive_callback(m_incoming_buffer);
-                m_incoming_buffer.resize(m_max_mtu);
+                buffer.resize(static_cast<size_t>(bytes_read));
+
+                {
+                    std::lock_guard<std::mutex> lock(m_queue_mutex);
+                    m_ready_buffers.emplace_back(buffer_index);
+                }
+
+                m_queue_valid.notify_one();
             }
             catch (const std::exception& e)
             {
-
                 CRL_DEBUG("exception while decoding packet: %s\n", e.what());
-
             }
             catch ( ... )
             {
-
                 CRL_DEBUG_RAW("unknown exception while decoding packet\n");
             }
         }
     }
 }
 
+void UdpReceiver::dispatch_thread()
+{
+    while(true)
+    {
+        size_t buffer_index = 0;
+        {
+            std::unique_lock<std::mutex> lock(m_queue_mutex);
+            m_queue_valid.wait(lock, [this]()
+            {
+                return m_stop || !m_ready_buffers.empty();
+            });
+
+            if (m_stop && m_ready_buffers.empty())
+            {
+                break;
+            }
+
+            buffer_index = m_ready_buffers.front();
+            m_ready_buffers.pop_front();
+        }
+
+        try
+        {
+            auto& packet = m_packet_buffers[buffer_index];
+            m_receive_callback(packet);
+        }
+        catch (const std::exception& e)
+        {
+            CRL_DEBUG("exception while decoding packet: %s\n", e.what());
+        }
+        catch ( ... )
+        {
+            CRL_DEBUG_RAW("unknown exception while decoding packet\n");
+        }
+
+        {
+            std::lock_guard<std::mutex> lock(m_queue_mutex);
+            m_packet_buffers[buffer_index].resize(m_max_mtu);
+            m_free_buffers.emplace_back(buffer_index);
+        }
+    }
+}
+
 int64_t publish_data(const NetworkSocket &socket, const std::vector<uint8_t> &data)
 {
 // disable MSVC warning for narrowing conversion.

source/LibMultiSense/include/MultiSense/MultiSenseTypes.hh

@@ -1217,6 +1217,12 @@ struct MultiSenseStatus
         /// @brief The total number of invalid packets received on the client side
         ///
         size_t invalid_packets = 0;
+
+        ///
+        /// @brief The total number of packets we received over the wire, but were unable to process due to
+        ///        buffer limits
+        ///
+        size_t unprocessed_packets = 0;
     };
 
     struct TimeStatus