From 62a7406210c14555698838c0887a0a40aec47ed9 Mon Sep 17 00:00:00 2001
From: Max Schmeller <6088931+mojomex@users.noreply.github.com>
Date: Sat, 7 Sep 2024 00:32:14 +0900
Subject: [PATCH] feat(hesai_hw_interface): configure and check `SO_RCVBUF`
 automatically (#186)

* feat(hesai_hw_interface): automatically set and confirm UDP socket's internal buffer size

* fix: open socket before setting its options
---
 build_depends.repos                                |  2 +-
 .../hesai_hw_interface.hpp                         |  8 ++++++++
 .../hesai_hw_interface.cpp                         | 14 ++++++++++++++
 3 files changed, 23 insertions(+), 1 deletion(-)

diff --git a/build_depends.repos b/build_depends.repos
index 6b7e58ff7..337cf8720 100644
--- a/build_depends.repos
+++ b/build_depends.repos
@@ -3,7 +3,7 @@ repositories:
   transport_drivers:
     type: git
     url: https://github.com/mojomex/transport_drivers
-    version: mutable-buffer-in-udp-callback
+    version: feat-set-rmem
   # Continental compatible version of ROS 2 socket CAN
   ros2_socketcan:
     type: git
diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp
index 668469b2e..e41ae449a 100644
--- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp
+++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp
@@ -18,6 +18,8 @@
 // boost/property_tree/ in some versions of boost.
 // See: https://github.com/boostorg/property_tree/issues/51
 #include <boost/version.hpp>
+
+#include <cstddef>
 #if (BOOST_VERSION / 100 >= 1073 && BOOST_VERSION / 100 <= 1076)  // Boost 1.73 - 1.76
 #define BOOST_BIND_GLOBAL_PLACEHOLDERS
 #endif
@@ -103,6 +105,12 @@ const uint16_t PANDAR128_E4X_PACKET_SIZE = 861;
 const uint16_t PANDAR128_E4X_EXTENDED_PACKET_SIZE = 1117;
 const uint16_t MTU_SIZE = 1500;
 
+/// @brief The kernel buffer size in bytes to use for receiving UDP packets. If the buffer is too
+/// small to bridge scheduling and processing delays, packets will be dropped. This corresponds to
+/// the net.core.rmem_default setting in Linux. The current value is hardcoded to accommodate one
+/// pointcloud worth of OT128 packets (currently the highest data rate sensor supported).
+const size_t UDP_SOCKET_BUFFER_SIZE = MTU_SIZE * 3600;
+
 // Time interval between Announce messages, in units of log seconds (default: 1)
 const int PTP_LOG_ANNOUNCE_INTERVAL = 1;
 // Time interval between Sync messages, in units of log seconds (default: 1)
diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp
index 1657b72f7..2244cd9af 100644
--- a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp
+++ b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp
@@ -2,7 +2,12 @@
 
 #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp"
 
+#include "nebula_common/nebula_status.hpp"
+
+#include <boost/asio/socket_base.hpp>
+
 #include <sstream>
+#include <string>
 
 // #define WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE
 
@@ -153,6 +158,15 @@ Status HesaiHwInterface::SensorInterfaceStart()
 #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE
     PrintError("open ok");
 #endif
+
+    bool success = cloud_udp_driver_->receiver()->setKernelBufferSize(UDP_SOCKET_BUFFER_SIZE);
+    if (!success) {
+      PrintError(
+        "Could not set receive buffer size. Try increasing net.core.rmem_max to " +
+        std::to_string(UDP_SOCKET_BUFFER_SIZE) + " B.");
+      return Status::ERROR_1;
+    }
+
     cloud_udp_driver_->receiver()->bind();
 #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE
     PrintError("bind ok");