From b4a10967fe51fdee5a346d368cccf37ff7c927f6 Mon Sep 17 00:00:00 2001 From: SeanChangX Date: Thu, 20 Mar 2025 17:42:30 +0800 Subject: [PATCH] Add USB status monitoring and power consumption metrics to robot status node --- app/robot_status_bridge/docker-compose.yaml | 1 + .../src/robot_status_node.cpp | 25 +++++++++++++------ 2 files changed, 19 insertions(+), 7 deletions(-) diff --git a/app/robot_status_bridge/docker-compose.yaml b/app/robot_status_bridge/docker-compose.yaml index b939c41..9ababe6 100644 --- a/app/robot_status_bridge/docker-compose.yaml +++ b/app/robot_status_bridge/docker-compose.yaml @@ -27,6 +27,7 @@ services: # Mount app resources into container. - ./robot_status_br/:/home/ros/robot_status_br/ # ros2 workspace - /sys/class:/sys/class:ro # for system status monitoring + - /dev:/dev # for usb status monitoring # Mount local timezone into container. - /etc/timezone:/etc/timezone:ro diff --git a/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/src/robot_status_node.cpp b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/src/robot_status_node.cpp index 2f2f41d..4aba20f 100644 --- a/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/src/robot_status_node.cpp +++ b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/src/robot_status_node.cpp @@ -17,17 +17,22 @@ std::string hostname = get_valid_hostname(); // Set the refresh rate for each topic (seconds) topic_refresh_rates_ = { - {"/robot_status/battery", 5.0}, - {"/robot_status/charging", 10.0}, - {"/robot_status/cpu", 1.0}, - {"/robot_status/cpu_temp", 1.0}, - {"/robot_status/ram", 1.0}, - {"/robot_status/wifi_signal", 1.0}, + {"/robot_status/battery", 5.0}, + {"/robot_status/charging", 10.0}, + {"/robot_status/power", 1.0}, + {"/robot_status/cpu", 1.0}, + {"/robot_status/cpu_temp", 1.0}, + {"/robot_status/ram", 1.0}, + {"/robot_status/wifi_signal", 1.0}, {"/robot_status/wifi_connected", 5.0}, {"/robot_status/wifi_ssid", 5.0}, {"/robot_status/disk", 60.0}, {"/robot_status/robot_ip", 5.0}, - {"/robot_status/uptime", 30.0} + {"/robot_status/uptime", 30.0}, + {"/robot_status/usb/mission", 10.0}, + {"/robot_status/usb/chassis", 10.0}, + {"/robot_status/usb/lidar", 10.0}, + {"/robot_status/usb/esp", 10.0}, }; // Set the shell commands for each topic @@ -36,6 +41,8 @@ std::string hostname = get_valid_hostname(); {"/robot_status/battery", {"std_msgs::msg::Int32", "cat /sys/class/power_supply/BAT0/capacity 2>/dev/null || cat /sys/class/power_supply/BAT1/capacity 2>/dev/null" }}, // Check if BAT0 or BAT1 is charging {"/robot_status/charging", {"std_msgs::msg::Bool", "cat /sys/class/power_supply/BAT0/status 2>/dev/null | grep -q 'Charging' || cat /sys/class/power_supply/BAT1/status 2>/dev/null | grep -q 'Charging' && echo 1 || echo 0" }}, + // Get real-time power consumption in watts + {"/robot_status/power", {"std_msgs::msg::Float32", "awk '{getline v < \"/sys/class/power_supply/BAT0/voltage_now\"; printf \"%.1f\", v * $1 / 1000000000000}' /sys/class/power_supply/BAT0/current_now" }}, // Get CPU usage percentage {"/robot_status/cpu", {"std_msgs::msg::Float32", "top -bn1 | grep 'Cpu(s)' | awk '{print 100 - $8}'" }}, // Get CPU temperature in Celsius @@ -54,6 +61,10 @@ std::string hostname = get_valid_hostname(); {"/robot_status/robot_ip", {"std_msgs::msg::String", "hostname -I | awk '{print $1}'" }}, // Get system uptime in hours {"/robot_status/uptime", {"std_msgs::msg::Float32", "awk '{print $1/3600}' /proc/uptime" }}, + {"/robot_status/usb/mission", {"std_msgs::msg::Bool", "[ -e /dev/mission ] && echo 1 || echo 0" }}, + {"/robot_status/usb/chassis", {"std_msgs::msg::Bool", "[ -e /dev/chassis ] && echo 1 || echo 0" }}, + {"/robot_status/usb/lidar", {"std_msgs::msg::Bool", "[ -e /dev/lidar ] && echo 1 || echo 0" }}, + {"/robot_status/usb/esp", {"std_msgs::msg::Bool", "[ -e /dev/esp ] && echo 1 || echo 0" }}, }; // Create publishers and timers based on the data type