feat: snmp monitoring of wireless link

Dockerfile

@@ -120,7 +120,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \
     gnupg2 libssl-dev libpng16-16 \
     zlib1g-dev libffi-dev \
     libglib2.0-dev libmount-dev \
-    python3-mrcal \
+    python3-mrcal libsnmp-dev snmp \
     && rm -rf /var/lib/apt/lists/*
 
 RUN source /opt/ros/humble/setup.bash \

src/Teleop-Control/system-telemetry-cpp/CMakeLists.txt

@@ -12,6 +12,10 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(interfaces REQUIRED)
+find_package(std_msgs REQUIRED)
+
+find_path(NETSNMP_INCLUDE_DIR net-snmp/net-snmp-config.h)
+find_library(NETSNMP_LIBRARY netsnmp)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
@@ -20,7 +24,11 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
-include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/system-telemetry-cpp)
+include_directories(
+  ${CMAKE_CURRENT_SOURCE_DIR}/include/system-telemetry-cpp
+  ${NETSNMP_INCLUDE_DIR}
+)
+
 add_executable(system_telemetry_publisher
   src/system_telemetry_publisher.cpp
   src/cpu_collector.cpp
@@ -33,10 +41,28 @@ ament_target_dependencies(system_telemetry_publisher
   interfaces
 )
 
+add_executable(snmp_network_stats
+  src/snmp_network_stats.cpp
+)
+
+ament_export_include_directories(
+  ${CMAKE_CURRENT_SOURCE_DIR}/include/system-telemetry-cpp
+  ${NETSNMP_INCLUDE_DIR}
+)
+
+ament_target_dependencies(snmp_network_stats
+  rclcpp
+  std_msgs
+)
+
+target_link_libraries(snmp_network_stats
+  ${NETSNMP_LIBRARY}
+)
 
 install(TARGETS
   system_telemetry_publisher
+  snmp_network_stats
   DESTINATION lib/${PROJECT_NAME}
 )
 
-ament_package()
+ament_package()

src/Teleop-Control/system-telemetry-cpp/include/system-telemetry-cpp/snmp_network_stats.hpp

@@ -0,0 +1,59 @@
+#pragma once
+
+#include <chrono>
+#include <cstdint>
+#include <memory>
+#include <optional>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/float32.hpp"
+
+class SnmpNetworkStatsNode : public rclcpp::Node {
+public:
+  SnmpNetworkStatsNode();
+
+private:
+  void poll_and_publish();
+
+  std::optional<int> snmp_get_int(const std::string &oid);
+  std::optional<int> snmp_walk_first_int(const std::string &base_oid);
+
+  void publish_float(
+      const rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr &publisher,
+      float value);
+
+  std::optional<float> compute_throughput_bps(uint32_t current_counter,
+                                              uint32_t previous_counter,
+                                              double dt_sec) const;
+
+  std::string ip_address_;
+  double frequency_;
+  std::string community_;
+  int snmp_version_;
+  long timeout_us_;
+  int retries_;
+
+  const std::string oid_signal_strength_base_ = "1.3.6.1.4.1.41112.1.4.7.1.3";
+  const std::string oid_noise_floor_base_ = "1.3.6.1.4.1.41112.1.4.7.1.4";
+  const std::string oid_ccq_tx_base_ = "1.3.6.1.4.1.41112.1.4.7.1.6";
+  const std::string oid_bandwidth_tx_base_ = "1.3.6.1.4.1.41112.1.4.7.1.11";
+  const std::string oid_bandwidth_rx_base_ = "1.3.6.1.4.1.41112.1.4.7.1.12";
+
+  const std::string oid_rx_counter_ = "1.3.6.1.2.1.2.2.1.10.5";
+  const std::string oid_tx_counter_ = "1.3.6.1.2.1.2.2.1.16.5";
+
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_bandwidth_tx_;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_bandwidth_rx_;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_throughput_tx_;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_throughput_rx_;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_signal_strength_;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_noise_floor_;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_ccq_tx_;
+
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  std::optional<uint32_t> prev_rx_counter_;
+  std::optional<uint32_t> prev_tx_counter_;
+  std::optional<std::chrono::steady_clock::time_point> prev_time_;
+};