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..bbcb172 --- /dev/null +++ b/app/robot_status_bridge/Dockerfile @@ -0,0 +1,78 @@ +###################################################################### +# - 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 \ + wireless-tools \ + ros-humble-rmw-cyclonedds-cpp \ + ros-humble-foxglove-bridge + +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"] \ No newline at end of file diff --git a/app/robot_status_bridge/docker-compose.yaml b/app/robot_status_bridge/docker-compose.yaml new file mode 100644 index 0000000..9e8ef07 --- /dev/null +++ b/app/robot_status_bridge/docker-compose.yaml @@ -0,0 +1,38 @@ +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} + - 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:/sys/class:ro # for system status monitoring + + # 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"] \ No newline at end of file diff --git a/app/robot_status_bridge/entrypoint.sh b/app/robot_status_bridge/entrypoint.sh new file mode 100755 index 0000000..078a704 --- /dev/null +++ b/app/robot_status_bridge/entrypoint.sh @@ -0,0 +1,13 @@ +#!/bin/bash + +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_node + +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..8d685f4 --- /dev/null +++ b/app/robot_status_bridge/robot_status_br/src/battery_monitor/package.xml @@ -0,0 +1,20 @@ + + + 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; +} 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.bak/msg/RobotStatus.msg b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor.bak/msg/RobotStatus.msg new file mode 100644 index 0000000..b740305 --- /dev/null +++ b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor.bak/msg/RobotStatus.msg @@ -0,0 +1,11 @@ +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.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.bak/src/robot_status_publisher.cpp b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor.bak/src/robot_status_publisher.cpp new file mode 100644 index 0000000..f2f95f3 --- /dev/null +++ b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor.bak/src/robot_status_publisher.cpp @@ -0,0 +1,160 @@ +#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; + } + + 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() { // Host CPU usage percentage + return execute_command("top -bn1 | grep 'Cpu(s)' | awk '{print 100 - $8}'"); + } + + 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() { // 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; + 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; + } + } + + 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: %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_; + 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.bak/src/robot_status_subscriber.cpp b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor.bak/src/robot_status_subscriber.cpp new file mode 100644 index 0000000..2d2c677 --- /dev/null +++ b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor.bak/src/robot_status_subscriber.cpp @@ -0,0 +1,54 @@ +#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( + 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: %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_; +}; + +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/CMakeLists.txt b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/CMakeLists.txt new file mode 100644 index 0000000..b8ee59c --- /dev/null +++ b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/CMakeLists.txt @@ -0,0 +1,30 @@ +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) + +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_node src/robot_status_node.cpp) +ament_target_dependencies(robot_status_node rclcpp std_msgs) + +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 new file mode 100644 index 0000000..78b2638 --- /dev/null +++ b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/package.xml @@ -0,0 +1,20 @@ + + + robot_status_monitor + 0.1.0 + Monitor robot status including battery, CPU usage, RAM usage, and network status. + SeanChangX + Apache-2.0 + + ament_cmake + + rclcpp + std_msgs + + ament_lint_auto + ament_lint_common + + + 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_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..2f2f41d --- /dev/null +++ b/app/robot_status_bridge/robot_status_br/src/robot_status_monitor/src/robot_status_node.cpp @@ -0,0 +1,150 @@ +#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(); + + // 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/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 + {"/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 CPU usage percentage + {"/robot_status/cpu", {"std_msgs::msg::Float32", "top -bn1 | grep 'Cpu(s)' | awk '{print 100 - $8}'" }}, + // Get CPU temperature in Celsius + {"/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 + {"/robot_status/ram", {"std_msgs::msg::Float32", "free | grep Mem | awk '{print ($3/$2) * 100.0}'" }}, + // Get Wi-Fi signal strength + {"/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 + {"/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 + {"/robot_status/wifi_ssid", {"std_msgs::msg::String", "iwgetid -r 2>/dev/null" }}, + // Get disk usage percentage + {"/robot_status/disk", {"std_msgs::msg::Int32", "df --output=pcent / | tail -1 | tr -d '%'" }}, + // Get robot IP address + {"/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" }}, + }; + + // 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; +}