From 27c49ac127b639df6532885da419c0a5c34e863d Mon Sep 17 00:00:00 2001 From: SeanChangX Date: Mon, 17 Mar 2025 23:35:47 +0800 Subject: [PATCH 1/5] Add Robot Status Bridge with battery monitoring functionality --- app/robot_status_bridge/.bashrc | 50 ++++++++++++ app/robot_status_bridge/.env | 6 ++ app/robot_status_bridge/Dockerfile | 76 +++++++++++++++++++ app/robot_status_bridge/docker-compose.yaml | 37 +++++++++ app/robot_status_bridge/entrypoint.sh | 18 +++++ .../src/battery_monitor/CMakeLists.txt | 35 +++++++++ .../src/battery_monitor/package.xml | 21 +++++ .../battery_monitor/src/battery_publisher.cpp | 59 ++++++++++++++ .../src/battery_subscriber.cpp | 25 ++++++ 9 files changed, 327 insertions(+) create mode 100644 app/robot_status_bridge/.bashrc create mode 100644 app/robot_status_bridge/.env create mode 100644 app/robot_status_bridge/Dockerfile create mode 100644 app/robot_status_bridge/docker-compose.yaml create mode 100755 app/robot_status_bridge/entrypoint.sh create mode 100644 app/robot_status_bridge/robot_status_br/src/battery_monitor/CMakeLists.txt create mode 100644 app/robot_status_bridge/robot_status_br/src/battery_monitor/package.xml create mode 100644 app/robot_status_bridge/robot_status_br/src/battery_monitor/src/battery_publisher.cpp create mode 100644 app/robot_status_bridge/robot_status_br/src/battery_monitor/src/battery_subscriber.cpp diff --git a/app/robot_status_bridge/.bashrc b/app/robot_status_bridge/.bashrc new file mode 100644 index 0000000..edc47c8 --- /dev/null +++ b/app/robot_status_bridge/.bashrc @@ -0,0 +1,50 @@ +# ==================== ROS ==================== # +# ==== Description: ROS environment setup ===== # +# =================== BEGIN =================== # + +source_ros_environment() { + if [ "$ROS_DISTRO" = "humble" ]; then + # Custom Alias + alias rosdep-check='rosdep install -i --from-path src --rosdistro humble -y' + alias build='colcon build --symlink-install' + + # Source ROS environment + source /opt/ros/humble/setup.bash + + # Source workspace environment + # latest_setup_bash: Find the latest setup.bash over user's root directory based on the last modified time + latest_setup_bash=$(find $(pwd) -type f -name "setup.bash" -wholename "*/install/setup.bash" -printf "%T@ %p\n" | sort -nr | awk '{print $2}' | head -n 1) + if [ -n "$latest_setup_bash" ]; then + source $latest_setup_bash + # Source colcon environment + source /usr/share/colcon_argcomplete/hook/colcon-argcomplete.bash + source /usr/share/colcon_cd/function/colcon_cd.sh + export _colcon_cd_root="${latest_setup_bash%/install/setup.bash}" + else + echo "No setup.bash file found for humble" + fi + elif [ "$ROS_DISTRO" = "noetic" ]; then + source /opt/ros/noetic/setup.bash + # Source workspace environment + if [ -n "$ROS_WS" ]; then + source $ROS_WS/devel/setup.bash + else + echo "ROS_WS variable is not set for noetic" + fi + else + echo "Unsupported ROS_DISTRO: $ROS_DISTRO" + fi +} + +# ==================== END ==================== # + + + +# ================= Functions ================= # +# Note: If you want to use the custom function, # +# you need to uncomment the line below. # +# =================== BEGIN =================== # + +source_ros_environment + +# ==================== END ==================== # diff --git a/app/robot_status_bridge/.env b/app/robot_status_bridge/.env new file mode 100644 index 0000000..17fff1a --- /dev/null +++ b/app/robot_status_bridge/.env @@ -0,0 +1,6 @@ + +# COMPOSE_BUILDER=bake +COMPOSE_BAKE=true + +ROS_DISTRO=humble +# ROS_DOMAIN_ID=25 \ No newline at end of file diff --git a/app/robot_status_bridge/Dockerfile b/app/robot_status_bridge/Dockerfile new file mode 100644 index 0000000..6e31432 --- /dev/null +++ b/app/robot_status_bridge/Dockerfile @@ -0,0 +1,76 @@ +###################################################################### +# - Base stage +# - This stage serves as the base image for the following stages. +###################################################################### + +ARG ROS_DISTRO=humble +FROM osrf/ros:${ROS_DISTRO}-desktop AS base + +LABEL org.opencontainers.image.title="Robot Status Bridge" +LABEL org.opencontainers.image.authors="scx@gapp.nthu.edu.tw" +LABEL org.opencontainers.image.licenses="MIT" + +ARG USERNAME=ros +# Use the same UID and GID as the host user +ARG USER_UID=1000 +ARG USER_GID=1000 + +###################################################################### +# - User setup stage +# - Create a non-root user with default bash shell. +###################################################################### + +FROM base AS user-setup + +RUN groupadd --gid $USER_GID $USERNAME \ + && useradd --uid $USER_UID --gid $USER_GID -m $USERNAME \ + && apt-get update \ + && apt-get install -y sudo \ + && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \ + && chmod 0440 /etc/sudoers.d/$USERNAME \ + && rm -rf /var/lib/apt/lists/* + +###################################################################### +# - Tools Installation stage +# - Install common tools for development. +###################################################################### + +FROM user-setup AS tools + +RUN apt-get update && apt-get install -y \ + tree \ + vim \ + wget \ + unzip \ + python3-pip \ + bash-completion \ + lm-sensors + +RUN apt-get update && apt-get dist-upgrade -y \ + && apt-get autoremove -y \ + && apt-get autoclean -y \ + && apt-get clean -y \ + && rm -rf /var/lib/apt/lists/* + +###################################################################### +# - Final stage +# - Install the main packages and set the entrypoint. +###################################################################### + +FROM tools AS final + +# Set up the user environment +ENV TZ=Asia/Taipei +ENV SHELL=/bin/bash +USER $USERNAME +WORKDIR /robot_status_br + +# Set up bashrc +COPY .bashrc /home/$USERNAME/.bashrc.conf +RUN cat /home/$USERNAME/.bashrc.conf >> /home/$USERNAME/.bashrc + +# Set up python environment +RUN pip install --upgrade pip +# RUN pip install --no-cache-dir + +CMD ["/bin/bash"] diff --git a/app/robot_status_bridge/docker-compose.yaml b/app/robot_status_bridge/docker-compose.yaml new file mode 100644 index 0000000..1fdf7b0 --- /dev/null +++ b/app/robot_status_bridge/docker-compose.yaml @@ -0,0 +1,37 @@ +services: + voice_nav: + build: + context: . + dockerfile: Dockerfile + target: final + args: + ROS_DISTRO: ${ROS_DISTRO} + USERNAME: ros + image: scx/robot_status_bridge:${ROS_DISTRO} + container_name: robot_status_bridge + stdin_open: true + tty: true + privileged: true + network_mode: "host" + # restart: unless-stopped + + working_dir: /home/ros + + environment: + - DISPLAY=${DISPLAY} + - ROS_DOMAIN_ID=${ROS_DOMAIN_ID} + + volumes: + - ./entrypoint.sh:/entrypoint.sh + # Mount app resources into container. + - ./robot_status_br/:/home/ros/robot_status_br/ # ros2 workspace + - /sys/class/power_supply:/sys/class/power_supply:ro # battery status + + # Mount local timezone into container. + - /etc/timezone:/etc/timezone:ro + - /etc/localtime:/etc/localtime:ro + # Mount X11 server + - /tmp/.X11-unix:/tmp/.X11-unix + + entrypoint: ["/bin/bash", "-c", "/entrypoint.sh"] + command: ["/bin/bash"] diff --git a/app/robot_status_bridge/entrypoint.sh b/app/robot_status_bridge/entrypoint.sh new file mode 100755 index 0000000..99bd654 --- /dev/null +++ b/app/robot_status_bridge/entrypoint.sh @@ -0,0 +1,18 @@ +#!/bin/bash + +# sudo chmod a+rw /dev/snd/* + +cd ~/robot_status_br +# . /opt/ros/humble/setup.sh && colcon build --symlink-install + +# source ~/robot_status_br/install/setup.bash +# ros2 run voice_nav speech_to_nav + # bash -c "source /opt/ros/humble/setup.bash && + # source /root/ros2_ws/install/setup.bash && + # ros2 run battery_monitor battery_publisher" + +while true; do + sleep 60 +done + +exec "$@" \ No newline at end of file diff --git a/app/robot_status_bridge/robot_status_br/src/battery_monitor/CMakeLists.txt b/app/robot_status_bridge/robot_status_br/src/battery_monitor/CMakeLists.txt new file mode 100644 index 0000000..d41c1fb --- /dev/null +++ b/app/robot_status_bridge/robot_status_br/src/battery_monitor/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.8) +project(battery_monitor) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) + +# add executable +add_executable(battery_publisher src/battery_publisher.cpp) +ament_target_dependencies(battery_publisher rclcpp std_msgs) +add_executable(battery_subscriber src/battery_subscriber.cpp) +ament_target_dependencies(battery_subscriber rclcpp std_msgs) + +# install executable +install(TARGETS battery_publisher battery_subscriber +DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/app/robot_status_bridge/robot_status_br/src/battery_monitor/package.xml b/app/robot_status_bridge/robot_status_br/src/battery_monitor/package.xml new file mode 100644 index 0000000..daacc05 --- /dev/null +++ b/app/robot_status_bridge/robot_status_br/src/battery_monitor/package.xml @@ -0,0 +1,21 @@ + + + + battery_monitor + 0.0.0 + TODO: Package description + ros + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/app/robot_status_bridge/robot_status_br/src/battery_monitor/src/battery_publisher.cpp b/app/robot_status_bridge/robot_status_br/src/battery_monitor/src/battery_publisher.cpp new file mode 100644 index 0000000..5c8fb81 --- /dev/null +++ b/app/robot_status_bridge/robot_status_br/src/battery_monitor/src/battery_publisher.cpp @@ -0,0 +1,59 @@ +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/float32.hpp" +#include +#include + +class BatteryMonitor : public rclcpp::Node { +public: + BatteryMonitor() : Node("battery_monitor") { + publisher_ = this->create_publisher("battery_percentage", 10); + timer_ = this->create_wall_timer(std::chrono::seconds(10), std::bind(&BatteryMonitor::publish_battery_status, this)); + } + +private: + void publish_battery_status() { + float battery_percentage = get_battery_percentage(); + if (battery_percentage < 0.0) { + RCLCPP_WARN(this->get_logger(), "Failed to read battery percentage!"); + return; + } + + auto msg = std_msgs::msg::Float32(); + msg.data = battery_percentage; + publisher_->publish(msg); + + RCLCPP_INFO(this->get_logger(), "Published Battery Percentage: %.2f%%", battery_percentage); + } + + float get_battery_percentage() { + + std::ifstream file("/sys/class/power_supply/BAT0/capacity"); + if (!file.is_open()) { + file.open("/sys/class/power_supply/BAT1/capacity"); + } + if (!file.is_open()) { + return -1.0; + } + + std::string line; + std::getline(file, line); + file.close(); + + try { + return std::stof(line); + } catch (...) { + return -1.0; + } + } + + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/app/robot_status_bridge/robot_status_br/src/battery_monitor/src/battery_subscriber.cpp b/app/robot_status_bridge/robot_status_br/src/battery_monitor/src/battery_subscriber.cpp new file mode 100644 index 0000000..4fe93ea --- /dev/null +++ b/app/robot_status_bridge/robot_status_br/src/battery_monitor/src/battery_subscriber.cpp @@ -0,0 +1,25 @@ +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/float32.hpp" + +class BatterySubscriber : public rclcpp::Node { +public: + BatterySubscriber() : Node("battery_subscriber") { + subscription_ = this->create_subscription( + "battery_percentage", 10, std::bind(&BatterySubscriber::topic_callback, this, std::placeholders::_1)); + } + +private: + void topic_callback(const std_msgs::msg::Float32::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Received Battery Percentage: %.2f%%", msg->data); + } + + rclcpp::Subscription::SharedPtr subscription_; +}; + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} From c6f912331f9fc07c58bb319ed7369e1bf9b863db Mon Sep 17 00:00:00 2001 From: SeanChangX Date: Tue, 18 Mar 2025 02:44:09 +0800 Subject: [PATCH 2/5] Add robot status monitoring with publisher and subscriber for battery, CPU, and RAM usage --- app/robot_status_bridge/Dockerfile | 3 +- app/robot_status_bridge/docker-compose.yaml | 9 +- app/robot_status_bridge/entrypoint.sh | 9 +- .../src/robot_status_monitor/CMakeLists.txt | 43 +++++++++ .../robot_status_monitor/msg/RobotStatus.msg | 4 + .../src/robot_status_monitor/package.xml | 22 +++++ .../src/robot_status_publisher.cpp | 90 +++++++++++++++++++ .../src/robot_status_subscriber.cpp | 27 ++++++ 8 files changed, 194 insertions(+), 13 deletions(-) create mode 100644 app/robot_status_bridge/robot_status_br/src/robot_status_monitor/CMakeLists.txt create mode 100644 app/robot_status_bridge/robot_status_br/src/robot_status_monitor/msg/RobotStatus.msg create mode 100644 app/robot_status_bridge/robot_status_br/src/robot_status_monitor/package.xml create mode 100644 app/robot_status_bridge/robot_status_br/src/robot_status_monitor/src/robot_status_publisher.cpp create mode 100644 app/robot_status_bridge/robot_status_br/src/robot_status_monitor/src/robot_status_subscriber.cpp diff --git a/app/robot_status_bridge/Dockerfile b/app/robot_status_bridge/Dockerfile index 6e31432..33b0137 100644 --- a/app/robot_status_bridge/Dockerfile +++ b/app/robot_status_bridge/Dockerfile @@ -43,8 +43,7 @@ RUN apt-get update && apt-get install -y \ wget \ unzip \ python3-pip \ - bash-completion \ - lm-sensors + bash-completion RUN apt-get update && apt-get dist-upgrade -y \ && apt-get autoremove -y \ diff --git a/app/robot_status_bridge/docker-compose.yaml b/app/robot_status_bridge/docker-compose.yaml index 1fdf7b0..bb87545 100644 --- a/app/robot_status_bridge/docker-compose.yaml +++ b/app/robot_status_bridge/docker-compose.yaml @@ -7,8 +7,8 @@ services: args: ROS_DISTRO: ${ROS_DISTRO} USERNAME: ros - image: scx/robot_status_bridge:${ROS_DISTRO} - container_name: robot_status_bridge + image: scx/robot-status-bridge:${ROS_DISTRO} + container_name: robot-status-bridge stdin_open: true tty: true privileged: true @@ -20,12 +20,13 @@ services: environment: - DISPLAY=${DISPLAY} - ROS_DOMAIN_ID=${ROS_DOMAIN_ID} + - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp volumes: - ./entrypoint.sh:/entrypoint.sh # Mount app resources into container. - - ./robot_status_br/:/home/ros/robot_status_br/ # ros2 workspace - - /sys/class/power_supply:/sys/class/power_supply:ro # battery status + - ./robot_status_br/:/home/ros/robot_status_br/ # ros2 workspace + - /sys/class:/sys/class:ro # for system status monitoring # Mount local timezone into container. - /etc/timezone:/etc/timezone:ro diff --git a/app/robot_status_bridge/entrypoint.sh b/app/robot_status_bridge/entrypoint.sh index 99bd654..12d5bb1 100755 --- a/app/robot_status_bridge/entrypoint.sh +++ b/app/robot_status_bridge/entrypoint.sh @@ -1,15 +1,10 @@ #!/bin/bash -# sudo chmod a+rw /dev/snd/* - cd ~/robot_status_br # . /opt/ros/humble/setup.sh && colcon build --symlink-install -# source ~/robot_status_br/install/setup.bash -# ros2 run voice_nav speech_to_nav - # bash -c "source /opt/ros/humble/setup.bash && - # source /root/ros2_ws/install/setup.bash && - # ros2 run battery_monitor battery_publisher" +source ~/robot_status_br/install/setup.bash +# ros2 run robot_status_monitor robot_status_publisher while true; do sleep 60 diff --git a/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/CMakeLists.txt b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/CMakeLists.txt new file mode 100644 index 0000000..d167978 --- /dev/null +++ b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/CMakeLists.txt @@ -0,0 +1,43 @@ +cmake_minimum_required(VERSION 3.5) +project(robot_status_monitor) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +# 1. 設定 msg 檔案 +set(msg_files + "msg/RobotStatus.msg" +) + +# 2. 生成 ROS 2 介面 (msg) +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + DEPENDENCIES std_msgs +) + +# 3. 設定包含目錄 +include_directories(${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_cpp) + +# 4. 建立 Publisher +add_executable(robot_status_publisher src/robot_status_publisher.cpp) +ament_target_dependencies(robot_status_publisher rclcpp std_msgs) + +# 5. 建立 Subscriber +add_executable(robot_status_subscriber src/robot_status_subscriber.cpp) +ament_target_dependencies(robot_status_subscriber rclcpp std_msgs) + +# 6. 讓執行檔依賴已生成的介面 +rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} rosidl_typesupport_cpp) +target_link_libraries(robot_status_publisher ${cpp_typesupport_target}) +target_link_libraries(robot_status_subscriber ${cpp_typesupport_target}) + +# 7. 安裝執行檔 +install(TARGETS + robot_status_publisher + robot_status_subscriber + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/msg/RobotStatus.msg b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/msg/RobotStatus.msg new file mode 100644 index 0000000..7514dfb --- /dev/null +++ b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/msg/RobotStatus.msg @@ -0,0 +1,4 @@ +float32 battery_percentage +float32 cpu_usage +float32 cpu_temperature +float32 ram_usage diff --git a/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/package.xml b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/package.xml new file mode 100644 index 0000000..a556909 --- /dev/null +++ b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/package.xml @@ -0,0 +1,22 @@ + + + robot_status_monitor + 0.0.1 + Monitor robot status including battery, CPU usage, and RAM usage. + Your Name + Apache-2.0 + + ament_cmake + rosidl_default_generators + + rclcpp + std_msgs + rosidl_default_runtime + + + rosidl_interface_packages + + + ament_cmake + + diff --git a/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/src/robot_status_publisher.cpp b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/src/robot_status_publisher.cpp new file mode 100644 index 0000000..62eabf7 --- /dev/null +++ b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/src/robot_status_publisher.cpp @@ -0,0 +1,90 @@ +#include "rclcpp/rclcpp.hpp" +#include "robot_status_monitor/msg/robot_status.hpp" +#include +#include +#include +#include +#include +#include + +class RobotStatusPublisher : public rclcpp::Node { +public: + RobotStatusPublisher() : Node("robot_status_publisher") { + std::string hostname = get_valid_hostname(); + topic_name_ = "/" + hostname + "/robot_status"; + + publisher_ = this->create_publisher(topic_name_, 10); + timer_ = this->create_wall_timer(std::chrono::seconds(5), std::bind(&RobotStatusPublisher::publish_status, this)); + + RCLCPP_INFO(this->get_logger(), "Publishing robot status to topic: %s", topic_name_.c_str()); + } + +private: + std::string get_valid_hostname() { + char hostname[128]; + if (gethostname(hostname, sizeof(hostname)) != 0) { + return "unknown_robot"; + } + std::string valid_hostname(hostname); + std::replace(valid_hostname.begin(), valid_hostname.end(), '-', '_'); + valid_hostname.erase(std::remove_if(valid_hostname.begin(), valid_hostname.end(), + [](char c) { return !std::isalnum(c) && c != '_'; }), valid_hostname.end()); + return valid_hostname.empty() ? "unknown_robot" : valid_hostname; + } + + float get_battery_percentage() { + return execute_command("cat /sys/class/power_supply/BAT1/capacity"); + } + + float get_cpu_usage() { + return execute_command("top -bn1 | grep 'Cpu(s)' | awk '{print 100 - $8}'"); + } + + float get_cpu_temperature() { + return execute_command("cat /sys/class/thermal/thermal_zone0/temp") / 1000.0; + } + + float get_ram_usage() { + return execute_command("free | grep Mem | awk '{print ($3/$2) * 100.0}'"); + } + + float execute_command(const std::string& command) { + char buffer[128]; + std::string result; + FILE* pipe = popen(command.c_str(), "r"); + if (!pipe) return -1.0; + while (fgets(buffer, sizeof(buffer), pipe) != nullptr) { + result += buffer; + } + pclose(pipe); + try { + return std::stof(result); + } catch (...) { + return -1.0; + } + } + + void publish_status() { + auto msg = robot_status_monitor::msg::RobotStatus(); + msg.battery_percentage = get_battery_percentage(); + msg.cpu_usage = get_cpu_usage(); + msg.cpu_temperature = get_cpu_temperature(); + msg.ram_usage = get_ram_usage(); + + publisher_->publish(msg); + RCLCPP_INFO(this->get_logger(), "Battery: %.2f%%, CPU: %.2f%%, Temp: %.2f°C, RAM: %.2f%%", + msg.battery_percentage, msg.cpu_usage, msg.cpu_temperature, msg.ram_usage); + } + + std::string topic_name_; + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/src/robot_status_subscriber.cpp b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/src/robot_status_subscriber.cpp new file mode 100644 index 0000000..e0bd5e6 --- /dev/null +++ b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/src/robot_status_subscriber.cpp @@ -0,0 +1,27 @@ +#include "rclcpp/rclcpp.hpp" +#include "robot_status_monitor/msg/robot_status.hpp" + +class RobotStatusSubscriber : public rclcpp::Node { +public: + RobotStatusSubscriber() : Node("robot_status_subscriber") { + subscription_ = this->create_subscription( + "/SCX_ROG_Zephyrus_G16/robot_status", 10, + std::bind(&RobotStatusSubscriber::callback, this, std::placeholders::_1)); + } + +private: + void callback(const robot_status_monitor::msg::RobotStatus::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Battery: %.2f%%, CPU: %.2f%%, Temp: %.2f°C, RAM: %.2f%%", + msg->battery_percentage, msg->cpu_usage, msg->cpu_temperature, msg->ram_usage); + } + + rclcpp::Subscription::SharedPtr subscription_; +}; + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} From b5d8f6f959525b5c18faebc5a0edb0039b7c47fd Mon Sep 17 00:00:00 2001 From: SeanChangX Date: Tue, 18 Mar 2025 06:09:18 +0800 Subject: [PATCH 3/5] Update robot status monitoring to stable version --- app/robot_status_bridge/Dockerfile | 8 +- app/robot_status_bridge/docker-compose.yaml | 6 +- app/robot_status_bridge/entrypoint.sh | 2 +- .../src/battery_monitor/package.xml | 1 - .../src/robot_status_monitor/CMakeLists.txt | 13 +-- .../robot_status_monitor/msg/RobotStatus.msg | 9 +- .../src/robot_status_monitor/package.xml | 9 +- .../src/robot_status_publisher.cpp | 86 +++++++++++++++++-- .../src/robot_status_subscriber.cpp | 33 ++++++- 9 files changed, 133 insertions(+), 34 deletions(-) diff --git a/app/robot_status_bridge/Dockerfile b/app/robot_status_bridge/Dockerfile index 33b0137..3ebc426 100644 --- a/app/robot_status_bridge/Dockerfile +++ b/app/robot_status_bridge/Dockerfile @@ -43,7 +43,9 @@ RUN apt-get update && apt-get install -y \ wget \ unzip \ python3-pip \ - bash-completion + bash-completion \ + wireless-tools \ + ros-humble-rmw-cyclonedds-cpp RUN apt-get update && apt-get dist-upgrade -y \ && apt-get autoremove -y \ @@ -69,7 +71,7 @@ COPY .bashrc /home/$USERNAME/.bashrc.conf RUN cat /home/$USERNAME/.bashrc.conf >> /home/$USERNAME/.bashrc # Set up python environment -RUN pip install --upgrade pip +# RUN pip install --upgrade pip # RUN pip install --no-cache-dir -CMD ["/bin/bash"] +CMD ["/bin/bash"] \ No newline at end of file diff --git a/app/robot_status_bridge/docker-compose.yaml b/app/robot_status_bridge/docker-compose.yaml index bb87545..3422751 100644 --- a/app/robot_status_bridge/docker-compose.yaml +++ b/app/robot_status_bridge/docker-compose.yaml @@ -13,14 +13,14 @@ services: tty: true privileged: true network_mode: "host" - # restart: unless-stopped + restart: unless-stopped working_dir: /home/ros environment: - DISPLAY=${DISPLAY} - ROS_DOMAIN_ID=${ROS_DOMAIN_ID} - - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp + # - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp volumes: - ./entrypoint.sh:/entrypoint.sh @@ -35,4 +35,4 @@ services: - /tmp/.X11-unix:/tmp/.X11-unix entrypoint: ["/bin/bash", "-c", "/entrypoint.sh"] - command: ["/bin/bash"] + command: ["/bin/bash"] \ No newline at end of file diff --git a/app/robot_status_bridge/entrypoint.sh b/app/robot_status_bridge/entrypoint.sh index 12d5bb1..00a3a01 100755 --- a/app/robot_status_bridge/entrypoint.sh +++ b/app/robot_status_bridge/entrypoint.sh @@ -4,7 +4,7 @@ cd ~/robot_status_br # . /opt/ros/humble/setup.sh && colcon build --symlink-install source ~/robot_status_br/install/setup.bash -# ros2 run robot_status_monitor robot_status_publisher +ros2 run robot_status_monitor robot_status_publisher while true; do sleep 60 diff --git a/app/robot_status_bridge/robot_status_br/src/battery_monitor/package.xml b/app/robot_status_bridge/robot_status_br/src/battery_monitor/package.xml index daacc05..8d685f4 100644 --- a/app/robot_status_bridge/robot_status_br/src/battery_monitor/package.xml +++ b/app/robot_status_bridge/robot_status_br/src/battery_monitor/package.xml @@ -1,5 +1,4 @@ - battery_monitor 0.0.0 diff --git a/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/CMakeLists.txt b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/CMakeLists.txt index d167978..f8e1fe8 100644 --- a/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/CMakeLists.txt +++ b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/CMakeLists.txt @@ -6,34 +6,29 @@ find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) -# 1. 設定 msg 檔案 +# Set msg files set(msg_files "msg/RobotStatus.msg" ) -# 2. 生成 ROS 2 介面 (msg) +# Generate ROS 2 interfaces (msg) rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} DEPENDENCIES std_msgs ) -# 3. 設定包含目錄 +# Set include directories include_directories(${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_cpp) -# 4. 建立 Publisher add_executable(robot_status_publisher src/robot_status_publisher.cpp) ament_target_dependencies(robot_status_publisher rclcpp std_msgs) - -# 5. 建立 Subscriber add_executable(robot_status_subscriber src/robot_status_subscriber.cpp) ament_target_dependencies(robot_status_subscriber rclcpp std_msgs) - -# 6. 讓執行檔依賴已生成的介面 + rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} rosidl_typesupport_cpp) target_link_libraries(robot_status_publisher ${cpp_typesupport_target}) target_link_libraries(robot_status_subscriber ${cpp_typesupport_target}) -# 7. 安裝執行檔 install(TARGETS robot_status_publisher robot_status_subscriber diff --git a/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/msg/RobotStatus.msg b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/msg/RobotStatus.msg index 7514dfb..b740305 100644 --- a/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/msg/RobotStatus.msg +++ b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/msg/RobotStatus.msg @@ -1,4 +1,11 @@ -float32 battery_percentage +int32 battery_percentage +bool charging float32 cpu_usage float32 cpu_temperature float32 ram_usage +int32 wifi_signal_strength +bool wifi_connected +string wifi_ssid +int32 disk_usage +string robot_ip +float32 uptime diff --git a/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/package.xml b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/package.xml index a556909..6a7a211 100644 --- a/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/package.xml +++ b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/package.xml @@ -1,9 +1,9 @@ robot_status_monitor - 0.0.1 - Monitor robot status including battery, CPU usage, and RAM usage. - Your Name + 0.1.0 + Monitor robot status + SeanChangX Apache-2.0 ament_cmake @@ -13,10 +13,9 @@ std_msgs rosidl_default_runtime - rosidl_interface_packages ament_cmake - + \ No newline at end of file diff --git a/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/src/robot_status_publisher.cpp b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/src/robot_status_publisher.cpp index 62eabf7..f2f95f3 100644 --- a/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/src/robot_status_publisher.cpp +++ b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/src/robot_status_publisher.cpp @@ -2,8 +2,8 @@ #include "robot_status_monitor/msg/robot_status.hpp" #include #include -#include #include +#include #include #include @@ -32,22 +32,64 @@ class RobotStatusPublisher : public rclcpp::Node { return valid_hostname.empty() ? "unknown_robot" : valid_hostname; } - float get_battery_percentage() { - return execute_command("cat /sys/class/power_supply/BAT1/capacity"); + int get_battery_percentage() { // Host battery percentage + float battery_percentage = execute_command("cat /sys/class/power_supply/BAT0/capacity"); + if (battery_percentage < 0) { + battery_percentage = execute_command("cat /sys/class/power_supply/BAT1/capacity"); + } + return battery_percentage; + } + + bool get_charging() { // Host charging status + std::string status = execute_string_command("cat /sys/class/power_supply/BAT0/status"); + if (status.empty()) { + status = execute_string_command("cat /sys/class/power_supply/BAT1/status"); + } + return (status.find("Charging") != std::string::npos); } - float get_cpu_usage() { + float get_cpu_usage() { // Host CPU usage percentage return execute_command("top -bn1 | grep 'Cpu(s)' | awk '{print 100 - $8}'"); } - float get_cpu_temperature() { + float get_cpu_temperature() { // Host CPU temperature in Celsius return execute_command("cat /sys/class/thermal/thermal_zone0/temp") / 1000.0; } - float get_ram_usage() { + float get_ram_usage() { // Host RAM usage percentage return execute_command("free | grep Mem | awk '{print ($3/$2) * 100.0}'"); } + int get_wifi_signal_strength() { // Host Wi-Fi signal strength in percentage + return execute_command("awk 'NR==3 {print int($3 * 100 / 70)}' /proc/net/wireless"); + } + + bool get_wifi_connected() { // Host Wi-Fi connection status + std::string result = execute_string_command("cat /sys/class/net/wlp2s0/operstate"); + if (result.find("up") != std::string::npos) { + return true; + } + return false; + } + + std::string get_wifi_ssid() { // Host Wi-Fi SSID + return execute_string_command("iwgetid -r"); + } + + int get_disk_usage() { // Host disk usage percentage + return execute_command("df --output=pcent / | tail -1 | tr -d '%'"); + } + + std::string get_robot_ip() { // Host IP address + return execute_string_command("hostname -I | awk '{print $1}'"); + } + + float get_uptime() { // Host uptime in hours + return execute_command("awk '{print $1/3600}' /proc/uptime"); + } + + + float execute_command(const std::string& command) { char buffer[128]; std::string result; @@ -64,16 +106,44 @@ class RobotStatusPublisher : public rclcpp::Node { } } + std::string execute_string_command(const std::string& command) { + char buffer[128]; + std::string result; + FILE* pipe = popen(command.c_str(), "r"); + if (!pipe) return ""; + while (fgets(buffer, sizeof(buffer), pipe) != nullptr) { + result += buffer; + } + pclose(pipe); + return result; + } + void publish_status() { auto msg = robot_status_monitor::msg::RobotStatus(); msg.battery_percentage = get_battery_percentage(); + msg.charging = get_charging(); msg.cpu_usage = get_cpu_usage(); msg.cpu_temperature = get_cpu_temperature(); msg.ram_usage = get_ram_usage(); + msg.wifi_signal_strength = get_wifi_signal_strength(); + msg.wifi_connected = get_wifi_connected(); + msg.wifi_ssid = get_wifi_ssid(); + msg.disk_usage = get_disk_usage(); + msg.robot_ip = get_robot_ip(); + msg.uptime = get_uptime(); publisher_->publish(msg); - RCLCPP_INFO(this->get_logger(), "Battery: %.2f%%, CPU: %.2f%%, Temp: %.2f°C, RAM: %.2f%%", - msg.battery_percentage, msg.cpu_usage, msg.cpu_temperature, msg.ram_usage); + RCLCPP_INFO(this->get_logger(), "Battery: %d%%, Charging: %s, CPU: %.2f%%, Temp: %.2f°C, RAM: %.2f%%, WiFi Signal: %d%%, WiFi Connected: %s, WiFi SSID: %s, Disk Usage: %d%%, IP: %s, Uptime: %.2f hours", + msg.battery_percentage, + msg.charging ? "Yes" : "No", + msg.cpu_usage, msg.cpu_temperature, + msg.ram_usage, + msg.wifi_signal_strength, + msg.wifi_connected ? "Yes" : "No", + msg.wifi_ssid.c_str(), + msg.disk_usage, + msg.robot_ip.c_str(), + msg.uptime); } std::string topic_name_; diff --git a/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/src/robot_status_subscriber.cpp b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/src/robot_status_subscriber.cpp index e0bd5e6..2d2c677 100644 --- a/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/src/robot_status_subscriber.cpp +++ b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/src/robot_status_subscriber.cpp @@ -1,18 +1,45 @@ #include "rclcpp/rclcpp.hpp" #include "robot_status_monitor/msg/robot_status.hpp" +#include +#include class RobotStatusSubscriber : public rclcpp::Node { public: RobotStatusSubscriber() : Node("robot_status_subscriber") { + std::string hostname = get_valid_hostname(); + std::string topic_name = "/" + hostname + "/robot_status"; + subscription_ = this->create_subscription( - "/SCX_ROG_Zephyrus_G16/robot_status", 10, + topic_name, 10, std::bind(&RobotStatusSubscriber::callback, this, std::placeholders::_1)); } private: + std::string get_valid_hostname() { + char hostname[128]; + if (gethostname(hostname, sizeof(hostname)) != 0) { + return "unknown_robot"; + } + std::string valid_hostname(hostname); + std::replace(valid_hostname.begin(), valid_hostname.end(), '-', '_'); + valid_hostname.erase(std::remove_if(valid_hostname.begin(), valid_hostname.end(), + [](char c) { return !std::isalnum(c) && c != '_'; }), valid_hostname.end()); + return valid_hostname.empty() ? "unknown_robot" : valid_hostname; + } + void callback(const robot_status_monitor::msg::RobotStatus::SharedPtr msg) { - RCLCPP_INFO(this->get_logger(), "Battery: %.2f%%, CPU: %.2f%%, Temp: %.2f°C, RAM: %.2f%%", - msg->battery_percentage, msg->cpu_usage, msg->cpu_temperature, msg->ram_usage); + RCLCPP_INFO(this->get_logger(), + "Battery: %d%%, Charging: %s, CPU: %.2f%%, Temp: %.2f°C, RAM: %.2f%%, WiFi Signal: %d%%, WiFi Connected: %s, WiFi SSID: %s, Disk Usage: %d%%, IP: %s, Uptime: %.2f hours", + msg->battery_percentage, + msg->charging ? "Yes" : "No", + msg->cpu_usage, msg->cpu_temperature, + msg->ram_usage, + msg->wifi_signal_strength, + msg->wifi_connected ? "Yes" : "No", + msg->wifi_ssid.c_str(), + msg->disk_usage, + msg->robot_ip.c_str(), + msg->uptime); } rclcpp::Subscription::SharedPtr subscription_; From 1307a09d098b434e03b4413afc5dda5fa44bc1e3 Mon Sep 17 00:00:00 2001 From: SeanChangX Date: Tue, 18 Mar 2025 23:29:31 +0800 Subject: [PATCH 4/5] Change topic format from msg to topic layer --- app/robot_status_bridge/Dockerfile | 3 +- app/robot_status_bridge/docker-compose.yaml | 2 +- app/robot_status_bridge/entrypoint.sh | 4 +- .../robot_status_monitor.bak/CMakeLists.txt | 38 +++++ .../robot_status_monitor.bak/COLCON_IGNORE | 0 .../msg/RobotStatus.msg | 0 .../src/robot_status_monitor.bak/package.xml | 21 +++ .../src/robot_status_publisher.cpp | 0 .../src/robot_status_subscriber.cpp | 0 .../src/robot_status_monitor/CMakeLists.txt | 48 +++--- .../src/robot_status_monitor/package.xml | 7 +- .../src/robot_status_node.cpp | 148 ++++++++++++++++++ 12 files changed, 235 insertions(+), 36 deletions(-) create mode 100644 app/robot_status_bridge/robot_status_br/src/robot_status_monitor.bak/CMakeLists.txt create mode 100644 app/robot_status_bridge/robot_status_br/src/robot_status_monitor.bak/COLCON_IGNORE rename app/robot_status_bridge/robot_status_br/src/{robot_status_monitor => robot_status_monitor.bak}/msg/RobotStatus.msg (100%) create mode 100644 app/robot_status_bridge/robot_status_br/src/robot_status_monitor.bak/package.xml rename app/robot_status_bridge/robot_status_br/src/{robot_status_monitor => robot_status_monitor.bak}/src/robot_status_publisher.cpp (100%) rename app/robot_status_bridge/robot_status_br/src/{robot_status_monitor => robot_status_monitor.bak}/src/robot_status_subscriber.cpp (100%) create mode 100644 app/robot_status_bridge/robot_status_br/src/robot_status_monitor/src/robot_status_node.cpp diff --git a/app/robot_status_bridge/Dockerfile b/app/robot_status_bridge/Dockerfile index 3ebc426..bbcb172 100644 --- a/app/robot_status_bridge/Dockerfile +++ b/app/robot_status_bridge/Dockerfile @@ -45,7 +45,8 @@ RUN apt-get update && apt-get install -y \ python3-pip \ bash-completion \ wireless-tools \ - ros-humble-rmw-cyclonedds-cpp + ros-humble-rmw-cyclonedds-cpp \ + ros-humble-foxglove-bridge RUN apt-get update && apt-get dist-upgrade -y \ && apt-get autoremove -y \ diff --git a/app/robot_status_bridge/docker-compose.yaml b/app/robot_status_bridge/docker-compose.yaml index 3422751..9e8ef07 100644 --- a/app/robot_status_bridge/docker-compose.yaml +++ b/app/robot_status_bridge/docker-compose.yaml @@ -20,7 +20,7 @@ services: environment: - DISPLAY=${DISPLAY} - ROS_DOMAIN_ID=${ROS_DOMAIN_ID} - # - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp + - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp volumes: - ./entrypoint.sh:/entrypoint.sh diff --git a/app/robot_status_bridge/entrypoint.sh b/app/robot_status_bridge/entrypoint.sh index 00a3a01..394b5d1 100755 --- a/app/robot_status_bridge/entrypoint.sh +++ b/app/robot_status_bridge/entrypoint.sh @@ -3,8 +3,8 @@ cd ~/robot_status_br # . /opt/ros/humble/setup.sh && colcon build --symlink-install -source ~/robot_status_br/install/setup.bash -ros2 run robot_status_monitor robot_status_publisher +# source ~/robot_status_br/install/setup.bash +# ros2 run robot_status_monitor robot_status_publisher while true; do sleep 60 diff --git a/app/robot_status_bridge/robot_status_br/src/robot_status_monitor.bak/CMakeLists.txt b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor.bak/CMakeLists.txt new file mode 100644 index 0000000..f8e1fe8 --- /dev/null +++ b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor.bak/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 3.5) +project(robot_status_monitor) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +# Set msg files +set(msg_files + "msg/RobotStatus.msg" +) + +# Generate ROS 2 interfaces (msg) +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + DEPENDENCIES std_msgs +) + +# Set include directories +include_directories(${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_cpp) + +add_executable(robot_status_publisher src/robot_status_publisher.cpp) +ament_target_dependencies(robot_status_publisher rclcpp std_msgs) +add_executable(robot_status_subscriber src/robot_status_subscriber.cpp) +ament_target_dependencies(robot_status_subscriber rclcpp std_msgs) + +rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} rosidl_typesupport_cpp) +target_link_libraries(robot_status_publisher ${cpp_typesupport_target}) +target_link_libraries(robot_status_subscriber ${cpp_typesupport_target}) + +install(TARGETS + robot_status_publisher + robot_status_subscriber + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/app/robot_status_bridge/robot_status_br/src/robot_status_monitor.bak/COLCON_IGNORE b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor.bak/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/msg/RobotStatus.msg b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor.bak/msg/RobotStatus.msg similarity index 100% rename from app/robot_status_bridge/robot_status_br/src/robot_status_monitor/msg/RobotStatus.msg rename to app/robot_status_bridge/robot_status_br/src/robot_status_monitor.bak/msg/RobotStatus.msg diff --git a/app/robot_status_bridge/robot_status_br/src/robot_status_monitor.bak/package.xml b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor.bak/package.xml new file mode 100644 index 0000000..6a7a211 --- /dev/null +++ b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor.bak/package.xml @@ -0,0 +1,21 @@ + + + robot_status_monitor + 0.1.0 + Monitor robot status + SeanChangX + Apache-2.0 + + ament_cmake + rosidl_default_generators + + rclcpp + std_msgs + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + \ No newline at end of file diff --git a/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/src/robot_status_publisher.cpp b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor.bak/src/robot_status_publisher.cpp similarity index 100% rename from app/robot_status_bridge/robot_status_br/src/robot_status_monitor/src/robot_status_publisher.cpp rename to app/robot_status_bridge/robot_status_br/src/robot_status_monitor.bak/src/robot_status_publisher.cpp diff --git a/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/src/robot_status_subscriber.cpp b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor.bak/src/robot_status_subscriber.cpp similarity index 100% rename from app/robot_status_bridge/robot_status_br/src/robot_status_monitor/src/robot_status_subscriber.cpp rename to app/robot_status_bridge/robot_status_br/src/robot_status_monitor.bak/src/robot_status_subscriber.cpp diff --git a/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/CMakeLists.txt b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/CMakeLists.txt index f8e1fe8..b8ee59c 100644 --- a/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/CMakeLists.txt +++ b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/CMakeLists.txt @@ -1,38 +1,30 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(robot_status_monitor) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) -find_package(rosidl_default_generators REQUIRED) - -# Set msg files -set(msg_files - "msg/RobotStatus.msg" -) - -# Generate ROS 2 interfaces (msg) -rosidl_generate_interfaces(${PROJECT_NAME} - ${msg_files} - DEPENDENCIES std_msgs -) -# Set include directories -include_directories(${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_cpp) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() -add_executable(robot_status_publisher src/robot_status_publisher.cpp) -ament_target_dependencies(robot_status_publisher rclcpp std_msgs) -add_executable(robot_status_subscriber src/robot_status_subscriber.cpp) -ament_target_dependencies(robot_status_subscriber rclcpp std_msgs) - -rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} rosidl_typesupport_cpp) -target_link_libraries(robot_status_publisher ${cpp_typesupport_target}) -target_link_libraries(robot_status_subscriber ${cpp_typesupport_target}) +add_executable(robot_status_node src/robot_status_node.cpp) +ament_target_dependencies(robot_status_node rclcpp std_msgs) -install(TARGETS - robot_status_publisher - robot_status_subscriber - DESTINATION lib/${PROJECT_NAME} -) +install(TARGETS robot_status_node DESTINATION lib/${PROJECT_NAME}) ament_package() diff --git a/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/package.xml b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/package.xml index 6a7a211..78b2638 100644 --- a/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/package.xml +++ b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/package.xml @@ -2,18 +2,17 @@ robot_status_monitor 0.1.0 - Monitor robot status + Monitor robot status including battery, CPU usage, RAM usage, and network status. SeanChangX Apache-2.0 ament_cmake - rosidl_default_generators rclcpp std_msgs - rosidl_default_runtime - rosidl_interface_packages + ament_lint_auto + ament_lint_common ament_cmake 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 new file mode 100644 index 0000000..9b31cdd --- /dev/null +++ b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/src/robot_status_node.cpp @@ -0,0 +1,148 @@ +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/float32.hpp" +#include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/int32.hpp" +#include "std_msgs/msg/string.hpp" +#include +#include +#include +#include + +class RobotStatusPublisher : public rclcpp::Node { +public: + RobotStatusPublisher() : Node("robot_status_node") { + std::string hostname = get_valid_hostname(); + + // Set the refresh rate for each topic (seconds) + topic_refresh_rates_ = { + {"/" + hostname + "/robot_status/battery", 5.0}, + {"/" + hostname + "/robot_status/charging", 10.0}, + {"/" + hostname + "/robot_status/cpu", 1.0}, + {"/" + hostname + "/robot_status/cpu_temp", 1.0}, + {"/" + hostname + "/robot_status/ram", 1.0}, + {"/" + hostname + "/robot_status/wifi_signal", 1.0}, + {"/" + hostname + "/robot_status/wifi_connected", 5.0}, + {"/" + hostname + "/robot_status/wifi_ssid", 5.0}, + {"/" + hostname + "/robot_status/disk", 60.0}, + {"/" + hostname + "/robot_status/robot_ip", 5.0}, + {"/" + hostname + "/robot_status/uptime", 30.0} + }; + + // Set the shell commands for each topic + status_commands_ = { + // Get battery percentage from BAT0 or BAT1 + {"/" + 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 + {"/" + hostname + "/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 CPU usage percentage + {"/" + hostname + "/robot_status/cpu", {"std_msgs::msg::Float32", "top -bn1 | grep 'Cpu(s)' | awk '{print 100 - $8}'" }}, + // Get CPU temperature in Celsius + {"/" + hostname + "/robot_status/cpu_temp", {"std_msgs::msg::Float32", "cat /sys/class/thermal/thermal_zone0/temp 2>/dev/null | awk '{print $1/1000}'" }}, + // Get RAM usage percentage + {"/" + hostname + "/robot_status/ram", {"std_msgs::msg::Float32", "free | grep Mem | awk '{print ($3/$2) * 100.0}'" }}, + // Get Wi-Fi signal strength + {"/" + hostname + "/robot_status/wifi_signal", {"std_msgs::msg::Int32", "awk 'NR==3 {print int($3 * 100 / 70)}' /proc/net/wireless" }}, + // Check if Wi-Fi is connected + {"/" + hostname + "/robot_status/wifi_connected", {"std_msgs::msg::Bool", "cat /sys/class/net/wlp2s0/operstate 2>/dev/null | grep -q 'up' && echo 1 || echo 0" }}, + // Get Wi-Fi SSID + {"/" + hostname + "/robot_status/wifi_ssid", {"std_msgs::msg::String", "iwgetid -r 2>/dev/null" }}, + // Get disk usage percentage + {"/" + hostname + "/robot_status/disk", {"std_msgs::msg::Int32", "df --output=pcent / | tail -1 | tr -d '%'" }}, + // Get robot IP address + {"/" + hostname + "/robot_status/robot_ip", {"std_msgs::msg::String", "hostname -I | awk '{print $1}'" }}, + // Get system uptime in hours + {"/" + hostname + "/robot_status/uptime", {"std_msgs::msg::Float32", "awk '{print $1/3600}' /proc/uptime" }}, + }; + + // Create publishers and timers based on the data type + for (const auto& [topic, cmd_info] : status_commands_) { + double refresh_rate = topic_refresh_rates_[topic]; + + if (cmd_info.type == "std_msgs::msg::Float32") { + float_publishers_[topic] = this->create_publisher(topic, 10); + } else if (cmd_info.type == "std_msgs::msg::Bool") { + bool_publishers_[topic] = this->create_publisher(topic, 10); + } else if (cmd_info.type == "std_msgs::msg::Int32") { + int_publishers_[topic] = this->create_publisher(topic, 10); + } else if (cmd_info.type == "std_msgs::msg::String") { + string_publishers_[topic] = this->create_publisher(topic, 10); + } + + timers_[topic] = this->create_wall_timer( + std::chrono::duration(refresh_rate), + [this, topic, cmd_info]() { publish_status(topic, cmd_info); } + ); + } + } + +private: + struct CommandInfo { + std::string type; + std::string command; + }; + + std::map status_commands_; + std::map topic_refresh_rates_; + std::map::SharedPtr> float_publishers_; + std::map::SharedPtr> bool_publishers_; + std::map::SharedPtr> int_publishers_; + std::map::SharedPtr> string_publishers_; + std::map timers_; + + void publish_status(const std::string& topic, const CommandInfo& cmd_info) { + std::string result = execute_command(cmd_info.command); + + if (cmd_info.type == "std_msgs::msg::Float32") { + auto msg = std_msgs::msg::Float32(); + msg.data = std::stof(result); + float_publishers_[topic]->publish(msg); + } else if (cmd_info.type == "std_msgs::msg::Bool") { + auto msg = std_msgs::msg::Bool(); + msg.data = (result == "1"); + bool_publishers_[topic]->publish(msg); + } else if (cmd_info.type == "std_msgs::msg::Int32") { + auto msg = std_msgs::msg::Int32(); + msg.data = std::stoi(result); + int_publishers_[topic]->publish(msg); + } else if (cmd_info.type == "std_msgs::msg::String") { + auto msg = std_msgs::msg::String(); + msg.data = result; + string_publishers_[topic]->publish(msg); + } + + RCLCPP_INFO(this->get_logger(), "Published %s: %s", topic.c_str(), result.c_str()); + } + + std::string execute_command(const std::string& command) { + char buffer[128]; + std::string result; + FILE* pipe = popen(command.c_str(), "r"); + if (!pipe) return "0"; + while (fgets(buffer, sizeof(buffer), pipe) != nullptr) { + result += buffer; + } + pclose(pipe); + result.erase(std::remove(result.begin(), result.end(), '\n'), result.end()); + return result.empty() ? "0" : result; + } + + std::string get_valid_hostname() { + char hostname[128]; + if (gethostname(hostname, sizeof(hostname)) != 0) { + return "unknown_robot"; + } + std::string valid_hostname(hostname); + std::replace(valid_hostname.begin(), valid_hostname.end(), '-', '_'); + valid_hostname.erase(std::remove_if(valid_hostname.begin(), valid_hostname.end(), + [](char c) { return !std::isalnum(c) && c != '_'; }), valid_hostname.end()); + return valid_hostname.empty() ? "unknown_robot" : valid_hostname; + } +}; + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} From c3af408ba137554a7edb526a9ab9f50ca42f8ff2 Mon Sep 17 00:00:00 2001 From: SeanChangX Date: Tue, 18 Mar 2025 23:49:04 +0800 Subject: [PATCH 5/5] Refactor robot status node to remove hostname dependency and streamline topic commands --- app/robot_status_bridge/entrypoint.sh | 4 +- .../src/robot_status_node.cpp | 48 ++++++++++--------- 2 files changed, 27 insertions(+), 25 deletions(-) diff --git a/app/robot_status_bridge/entrypoint.sh b/app/robot_status_bridge/entrypoint.sh index 394b5d1..078a704 100755 --- a/app/robot_status_bridge/entrypoint.sh +++ b/app/robot_status_bridge/entrypoint.sh @@ -3,8 +3,8 @@ cd ~/robot_status_br # . /opt/ros/humble/setup.sh && colcon build --symlink-install -# source ~/robot_status_br/install/setup.bash -# ros2 run robot_status_monitor robot_status_publisher +source ~/robot_status_br/install/setup.bash +ros2 run robot_status_monitor robot_status_node while true; do sleep 60 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 9b31cdd..2f2f41d 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 @@ -11,47 +11,49 @@ class RobotStatusPublisher : public rclcpp::Node { public: RobotStatusPublisher() : Node("robot_status_node") { - std::string hostname = get_valid_hostname(); +std::string hostname = get_valid_hostname(); + + // std::string hostname = get_valid_hostname(); // Set the refresh rate for each topic (seconds) topic_refresh_rates_ = { - {"/" + hostname + "/robot_status/battery", 5.0}, - {"/" + hostname + "/robot_status/charging", 10.0}, - {"/" + hostname + "/robot_status/cpu", 1.0}, - {"/" + hostname + "/robot_status/cpu_temp", 1.0}, - {"/" + hostname + "/robot_status/ram", 1.0}, - {"/" + hostname + "/robot_status/wifi_signal", 1.0}, - {"/" + hostname + "/robot_status/wifi_connected", 5.0}, - {"/" + hostname + "/robot_status/wifi_ssid", 5.0}, - {"/" + hostname + "/robot_status/disk", 60.0}, - {"/" + hostname + "/robot_status/robot_ip", 5.0}, - {"/" + hostname + "/robot_status/uptime", 30.0} + {"/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/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} }; // Set the shell commands for each topic status_commands_ = { // Get battery percentage from BAT0 or BAT1 - {"/" + 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" }}, + {"/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 - {"/" + hostname + "/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" }}, + {"/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 CPU usage percentage - {"/" + hostname + "/robot_status/cpu", {"std_msgs::msg::Float32", "top -bn1 | grep 'Cpu(s)' | awk '{print 100 - $8}'" }}, + {"/robot_status/cpu", {"std_msgs::msg::Float32", "top -bn1 | grep 'Cpu(s)' | awk '{print 100 - $8}'" }}, // Get CPU temperature in Celsius - {"/" + hostname + "/robot_status/cpu_temp", {"std_msgs::msg::Float32", "cat /sys/class/thermal/thermal_zone0/temp 2>/dev/null | awk '{print $1/1000}'" }}, + {"/robot_status/cpu_temp", {"std_msgs::msg::Float32", "cat /sys/class/thermal/thermal_zone0/temp 2>/dev/null | awk '{print $1/1000}'" }}, // Get RAM usage percentage - {"/" + hostname + "/robot_status/ram", {"std_msgs::msg::Float32", "free | grep Mem | awk '{print ($3/$2) * 100.0}'" }}, + {"/robot_status/ram", {"std_msgs::msg::Float32", "free | grep Mem | awk '{print ($3/$2) * 100.0}'" }}, // Get Wi-Fi signal strength - {"/" + hostname + "/robot_status/wifi_signal", {"std_msgs::msg::Int32", "awk 'NR==3 {print int($3 * 100 / 70)}' /proc/net/wireless" }}, + {"/robot_status/wifi_signal", {"std_msgs::msg::Int32", "awk 'NR==3 {print int($3 * 100 / 70)}' /proc/net/wireless" }}, // Check if Wi-Fi is connected - {"/" + hostname + "/robot_status/wifi_connected", {"std_msgs::msg::Bool", "cat /sys/class/net/wlp2s0/operstate 2>/dev/null | grep -q 'up' && echo 1 || echo 0" }}, + {"/robot_status/wifi_connected", {"std_msgs::msg::Bool", "cat /sys/class/net/wlp2s0/operstate 2>/dev/null | grep -q 'up' && echo 1 || echo 0" }}, // Get Wi-Fi SSID - {"/" + hostname + "/robot_status/wifi_ssid", {"std_msgs::msg::String", "iwgetid -r 2>/dev/null" }}, + {"/robot_status/wifi_ssid", {"std_msgs::msg::String", "iwgetid -r 2>/dev/null" }}, // Get disk usage percentage - {"/" + hostname + "/robot_status/disk", {"std_msgs::msg::Int32", "df --output=pcent / | tail -1 | tr -d '%'" }}, + {"/robot_status/disk", {"std_msgs::msg::Int32", "df --output=pcent / | tail -1 | tr -d '%'" }}, // Get robot IP address - {"/" + hostname + "/robot_status/robot_ip", {"std_msgs::msg::String", "hostname -I | awk '{print $1}'" }}, + {"/robot_status/robot_ip", {"std_msgs::msg::String", "hostname -I | awk '{print $1}'" }}, // Get system uptime in hours - {"/" + hostname + "/robot_status/uptime", {"std_msgs::msg::Float32", "awk '{print $1/3600}' /proc/uptime" }}, + {"/robot_status/uptime", {"std_msgs::msg::Float32", "awk '{print $1/3600}' /proc/uptime" }}, }; // Create publishers and timers based on the data type