diff --git a/Dockerfile b/Dockerfile index d05367bc..5c8e7a20 100644 --- a/Dockerfile +++ b/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 \ diff --git a/src/Teleop-Control/system-telemetry-cpp/CMakeLists.txt b/src/Teleop-Control/system-telemetry-cpp/CMakeLists.txt index a28a9dc0..eec9cc89 100644 --- a/src/Teleop-Control/system-telemetry-cpp/CMakeLists.txt +++ b/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() \ No newline at end of file diff --git a/src/Teleop-Control/system-telemetry-cpp/include/system-telemetry-cpp/snmp_network_stats.hpp b/src/Teleop-Control/system-telemetry-cpp/include/system-telemetry-cpp/snmp_network_stats.hpp new file mode 100644 index 00000000..2b6d1f1f --- /dev/null +++ b/src/Teleop-Control/system-telemetry-cpp/include/system-telemetry-cpp/snmp_network_stats.hpp @@ -0,0 +1,59 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/float32.hpp" + +class SnmpNetworkStatsNode : public rclcpp::Node { +public: + SnmpNetworkStatsNode(); + +private: + void poll_and_publish(); + + std::optional snmp_get_int(const std::string &oid); + std::optional snmp_walk_first_int(const std::string &base_oid); + + void publish_float( + const rclcpp::Publisher::SharedPtr &publisher, + float value); + + std::optional 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::SharedPtr pub_bandwidth_tx_; + rclcpp::Publisher::SharedPtr pub_bandwidth_rx_; + rclcpp::Publisher::SharedPtr pub_throughput_tx_; + rclcpp::Publisher::SharedPtr pub_throughput_rx_; + rclcpp::Publisher::SharedPtr pub_signal_strength_; + rclcpp::Publisher::SharedPtr pub_noise_floor_; + rclcpp::Publisher::SharedPtr pub_ccq_tx_; + + rclcpp::TimerBase::SharedPtr timer_; + + std::optional prev_rx_counter_; + std::optional prev_tx_counter_; + std::optional prev_time_; +}; \ No newline at end of file diff --git a/src/Teleop-Control/system-telemetry-cpp/src/snmp_network_stats.cpp b/src/Teleop-Control/system-telemetry-cpp/src/snmp_network_stats.cpp new file mode 100644 index 00000000..133f321d --- /dev/null +++ b/src/Teleop-Control/system-telemetry-cpp/src/snmp_network_stats.cpp @@ -0,0 +1,310 @@ +#include "snmp_network_stats.hpp" + +#include +#include +#include + +extern "C" { +#include +#include +} + +SnmpNetworkStatsNode::SnmpNetworkStatsNode() + : Node("snmp_network_stats"), + ip_address_( + this->declare_parameter("ip_address", "192.168.0.3")), + frequency_(this->declare_parameter("frequency", 1.0)), + community_("public"), snmp_version_(SNMP_VERSION_1), + timeout_us_(1000000L), retries_(0) { + pub_bandwidth_tx_ = + this->create_publisher("~/bandwidth_tx", 10); + pub_bandwidth_rx_ = + this->create_publisher("~/bandwidth_rx", 10); + pub_throughput_tx_ = + this->create_publisher("~/throughput_tx", 10); + pub_throughput_rx_ = + this->create_publisher("~/throughput_rx", 10); + pub_signal_strength_ = + this->create_publisher("~/signal_strength", 10); + pub_noise_floor_ = + this->create_publisher("~/noise_floor", 10); + pub_ccq_tx_ = this->create_publisher("~/ccq_tx", 10); + + if (frequency_ <= 0.0) { + frequency_ = 1.0; + } + + init_snmp("snmp_network_stats"); + + const auto period = std::chrono::duration(1.0 / frequency_); + timer_ = this->create_wall_timer( + std::chrono::duration_cast(period), + std::bind(&SnmpNetworkStatsNode::poll_and_publish, this)); + RCLCPP_INFO(this->get_logger(), "Initialized with IP: %s, Frequency: %.2f Hz", + ip_address_.c_str(), frequency_); +} + +std::optional +SnmpNetworkStatsNode::snmp_get_int(const std::string &oid_str) { + netsnmp_session session{}; + snmp_sess_init(&session); + + session.peername = strdup(ip_address_.c_str()); + session.version = snmp_version_; + session.community = + reinterpret_cast(const_cast(community_.c_str())); + session.community_len = community_.size(); + session.timeout = timeout_us_; + session.retries = retries_; + + netsnmp_session *ss = snmp_open(&session); + if (!ss) { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, + "Failed to open SNMP session to %s", + ip_address_.c_str()); + free(session.peername); + return std::nullopt; + } + + netsnmp_pdu *pdu = snmp_pdu_create(SNMP_MSG_GET); + + oid an_oid[MAX_OID_LEN]; + size_t an_oid_len = MAX_OID_LEN; + if (!snmp_parse_oid(oid_str.c_str(), an_oid, &an_oid_len)) { + snmp_free_pdu(pdu); + snmp_close(ss); + free(session.peername); + return std::nullopt; + } + + snmp_add_null_var(pdu, an_oid, an_oid_len); + + netsnmp_pdu *response = nullptr; + const int status = snmp_synch_response(ss, pdu, &response); + + std::optional result = std::nullopt; + + if (status == STAT_SUCCESS && response && + response->errstat == SNMP_ERR_NOERROR) { + for (netsnmp_variable_list *vars = response->variables; vars; + vars = vars->next_variable) { + switch (vars->type) { + case ASN_INTEGER: + case ASN_COUNTER: + case ASN_GAUGE: + case ASN_TIMETICKS: + case ASN_UINTEGER: + result = static_cast(*vars->val.integer); + break; + case ASN_COUNTER64: + result = static_cast(vars->val.counter64->low); + break; + default: + break; + } + if (result.has_value()) { + break; + } + } + } + + if (response) { + snmp_free_pdu(response); + } + snmp_close(ss); + free(session.peername); + + return result; +} + +std::optional +SnmpNetworkStatsNode::snmp_walk_first_int(const std::string &base_oid_str) { + netsnmp_session session{}; + snmp_sess_init(&session); + + session.peername = strdup(ip_address_.c_str()); + session.version = snmp_version_; + session.community = + reinterpret_cast(const_cast(community_.c_str())); + session.community_len = community_.size(); + session.timeout = timeout_us_; + session.retries = retries_; + + netsnmp_session *ss = snmp_open(&session); + if (!ss) { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, + "Failed to open SNMP session to %s", + ip_address_.c_str()); + free(session.peername); + return std::nullopt; + } + + oid root_oid[MAX_OID_LEN]; + size_t root_oid_len = MAX_OID_LEN; + if (!snmp_parse_oid(base_oid_str.c_str(), root_oid, &root_oid_len)) { + RCLCPP_WARN(this->get_logger(), "Failed to parse OID: %s", + base_oid_str.c_str()); + snmp_close(ss); + free(session.peername); + return std::nullopt; + } + + oid current_oid[MAX_OID_LEN]; + size_t current_oid_len = root_oid_len; + std::memcpy(current_oid, root_oid, root_oid_len * sizeof(oid)); + + std::optional result = std::nullopt; + + while (true) { + netsnmp_pdu *pdu = snmp_pdu_create(SNMP_MSG_GETNEXT); + snmp_add_null_var(pdu, current_oid, current_oid_len); + + netsnmp_pdu *response = nullptr; + const int status = snmp_synch_response(ss, pdu, &response); + + if (status != STAT_SUCCESS || !response || + response->errstat != SNMP_ERR_NOERROR) { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, + "SNMP walk failed for OID: %s", + base_oid_str.c_str()); + if (response) { + snmp_free_pdu(response); + } + break; + } + + netsnmp_variable_list *vars = response->variables; + if (!vars) { + snmp_free_pdu(response); + break; + } + + if (snmp_oid_compare(root_oid, root_oid_len, vars->name, + vars->name_length) != 0 && + (vars->name_length < root_oid_len || + std::memcmp(root_oid, vars->name, root_oid_len * sizeof(oid)) != 0)) { + snmp_free_pdu(response); + break; + } + + switch (vars->type) { + case ASN_INTEGER: + case ASN_COUNTER: + case ASN_GAUGE: + case ASN_TIMETICKS: + case ASN_UINTEGER: + result = static_cast(*vars->val.integer); + break; + case ASN_COUNTER64: + result = static_cast(vars->val.counter64->low); + break; + default: + break; + } + + snmp_free_pdu(response); + break; + } + + snmp_close(ss); + free(session.peername); + + return result; +} + +void SnmpNetworkStatsNode::publish_float( + const rclcpp::Publisher::SharedPtr &publisher, + float value) { + std_msgs::msg::Float32 msg; + msg.data = value; + publisher->publish(msg); +} + +std::optional SnmpNetworkStatsNode::compute_throughput_bps( + uint32_t current_counter, uint32_t previous_counter, double dt_sec) const { + if (dt_sec <= 0.0) { + RCLCPP_WARN(this->get_logger(), + "Invalid time delta for throughput calculation: %.6f seconds", + dt_sec); + return std::nullopt; + } + + uint64_t delta_bytes = 0; + if (current_counter >= previous_counter) { + delta_bytes = static_cast(current_counter) - + static_cast(previous_counter); + } else { + delta_bytes = (static_cast(std::numeric_limits::max()) - + previous_counter + 1ULL) + + current_counter; + } + + return static_cast((static_cast(delta_bytes) * 8.0) / dt_sec); +} + +void SnmpNetworkStatsNode::poll_and_publish() { + const auto now = std::chrono::steady_clock::now(); + RCLCPP_DEBUG(this->get_logger(), "Polling SNMP data at time: %.2f seconds", + std::chrono::duration(now.time_since_epoch()).count()); + + const auto signal_strength = snmp_walk_first_int(oid_signal_strength_base_); + const auto noise_floor = snmp_walk_first_int(oid_noise_floor_base_); + const auto ccq_tx = snmp_walk_first_int(oid_ccq_tx_base_); + const auto bandwidth_tx = snmp_walk_first_int(oid_bandwidth_tx_base_); + const auto bandwidth_rx = snmp_walk_first_int(oid_bandwidth_rx_base_); + + const auto rx_counter_raw = snmp_get_int(oid_rx_counter_); + const auto tx_counter_raw = snmp_get_int(oid_tx_counter_); + + if (signal_strength.has_value()) { + publish_float(pub_signal_strength_, + static_cast(signal_strength.value())); + } + if (noise_floor.has_value()) { + publish_float(pub_noise_floor_, static_cast(noise_floor.value())); + } + if (ccq_tx.has_value()) { + publish_float(pub_ccq_tx_, static_cast(ccq_tx.value())); + } + if (bandwidth_tx.has_value()) { + publish_float(pub_bandwidth_tx_, static_cast(bandwidth_tx.value())); + } + if (bandwidth_rx.has_value()) { + publish_float(pub_bandwidth_rx_, static_cast(bandwidth_rx.value())); + } + + if (rx_counter_raw.has_value() && tx_counter_raw.has_value()) { + const uint32_t rx_counter = static_cast(rx_counter_raw.value()); + const uint32_t tx_counter = static_cast(tx_counter_raw.value()); + + if (prev_time_.has_value() && prev_rx_counter_.has_value() && + prev_tx_counter_.has_value()) { + const double dt = + std::chrono::duration(now - prev_time_.value()).count(); + + const auto throughput_rx = + compute_throughput_bps(rx_counter, prev_rx_counter_.value(), dt); + const auto throughput_tx = + compute_throughput_bps(tx_counter, prev_tx_counter_.value(), dt); + + if (throughput_rx.has_value()) { + publish_float(pub_throughput_rx_, throughput_rx.value()); + } + if (throughput_tx.has_value()) { + publish_float(pub_throughput_tx_, throughput_tx.value()); + } + } + + prev_rx_counter_ = rx_counter; + prev_tx_counter_ = tx_counter; + prev_time_ = now; + } +} + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file