From 4079e94eb162b982adc5fbfb02b814a72e0195e2 Mon Sep 17 00:00:00 2001 From: Adam Serafin Date: Wed, 28 Feb 2024 15:26:01 +0100 Subject: [PATCH] Sys info node (#72) --- Dockerfile | 2 +- rae_hw/CMakeLists.txt | 13 +-- rae_hw/config/controller.yaml | 14 +-- rae_hw/launch/control.launch.py | 8 +- rae_hw/launch/control_mock.launch.py | 7 ++ rae_hw/scripts/lifecycle_manager.py | 2 +- rae_hw/scripts/{ => mock}/mock_battery.py | 5 +- rae_hw/scripts/{ => mock}/mock_lcd.py | 0 rae_hw/scripts/{ => mock}/mock_leds.py | 0 rae_hw/scripts/{ => mock}/mock_mic.py | 0 rae_hw/scripts/{ => mock}/mock_speakers.py | 0 rae_hw/scripts/{ => mock}/mock_wheels.py | 0 rae_hw/scripts/sys_info_node.py | 74 +++++++++++++++ rae_sdk/rae_sdk/robot/display.py | 60 +++++++++++- rae_sdk/rae_sdk/robot/robot.py | 10 +- rae_sdk/rae_sdk/robot/robot_options.py | 12 ++- rae_sdk/rae_sdk/robot/state.py | 103 ++++++++++++++++++--- 17 files changed, 270 insertions(+), 40 deletions(-) rename rae_hw/scripts/{ => mock}/mock_battery.py (93%) rename rae_hw/scripts/{ => mock}/mock_lcd.py (100%) rename rae_hw/scripts/{ => mock}/mock_leds.py (100%) rename rae_hw/scripts/{ => mock}/mock_mic.py (100%) rename rae_hw/scripts/{ => mock}/mock_speakers.py (100%) rename rae_hw/scripts/{ => mock}/mock_wheels.py (100%) create mode 100755 rae_hw/scripts/sys_info_node.py diff --git a/Dockerfile b/Dockerfile index 2dd890d..f0368ef 100644 --- a/Dockerfile +++ b/Dockerfile @@ -35,7 +35,7 @@ COPY ./ .$WS/src/rae-ros RUN rm -rf .$WS/src/rae-ros/assets RUN rm -rf .$WS/src/rae-ros/rae_gazebo -RUN cd .$WS/ && rosdep install --from-paths src --ignore-src -y --skip-keys depthai --skip-keys depthai_bridge --skip-keys depthai_ros_driver --skip-keys audio_msgs --skip-keys laserscan_kinect --skip-keys ira_laser_tools +RUN cd .$WS/ && apt update && rosdep install --from-paths src --ignore-src -y --skip-keys depthai --skip-keys depthai_bridge --skip-keys depthai_ros_driver --skip-keys audio_msgs --skip-keys laserscan_kinect --skip-keys ira_laser_tools RUN cd .$WS/ && . /opt/ros/${ROS_DISTRO}/setup.sh && . /sai_ros/spectacularai_ros2/install/setup.sh && . /${UNDERLAY_WS}/install/setup.sh && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=${BUILD_TYPE} RUN echo "if [ -f ${WS}/install/setup.bash ]; then source ${WS}/install/setup.bash; fi" >> $HOME/.bashrc RUN echo "if [ -f ${WS}/install/setup.zsh ]; then source ${WS}/install/setup.zsh; fi" >> $HOME/.zshrc diff --git a/rae_hw/CMakeLists.txt b/rae_hw/CMakeLists.txt index ba4fd96..9f34ae0 100644 --- a/rae_hw/CMakeLists.txt +++ b/rae_hw/CMakeLists.txt @@ -148,13 +148,14 @@ ament_python_install_package(${PROJECT_NAME}) # Install Python executables install( PROGRAMS - scripts/mock_battery.py - scripts/mock_lcd.py - scripts/mock_leds.py - scripts/mock_mic.py - scripts/mock_speakers.py - scripts/mock_wheels.py + scripts/mock/mock_battery.py + scripts/mock/mock_lcd.py + scripts/mock/mock_leds.py + scripts/mock/mock_mic.py + scripts/mock/mock_speakers.py + scripts/mock/mock_wheels.py scripts/lifecycle_manager.py + scripts/sys_info_node.py DESTINATION lib/${PROJECT_NAME} ) ament_package() diff --git a/rae_hw/config/controller.yaml b/rae_hw/config/controller.yaml index bc67899..fbd1daa 100644 --- a/rae_hw/config/controller.yaml +++ b/rae_hw/config/controller.yaml @@ -42,18 +42,18 @@ diff_controller: linear.x.has_velocity_limits: true linear.x.has_acceleration_limits: true linear.x.has_jerk_limits: false - linear.x.max_velocity: 0.36 - linear.x.min_velocity: -0.36 - linear.x.max_acceleration: 1.0 + linear.x.max_velocity: 0.18 + linear.x.min_velocity: -0.18 + linear.x.max_acceleration: 0.3 linear.x.max_jerk: 0.0 linear.x.min_jerk: 0.0 angular.z.has_velocity_limits: true angular.z.has_acceleration_limits: false angular.z.has_jerk_limits: false - angular.z.max_velocity: 2.5 - angular.z.min_velocity: -2.5 - angular.z.max_acceleration: 1.0 - angular.z.min_acceleration: -1.0 + angular.z.max_velocity: 1.8 + angular.z.min_velocity: -1.8 + angular.z.max_acceleration: 1.5 + angular.z.min_acceleration: -1.5 angular.z.max_jerk: 0.0 angular.z.min_jerk: 0.0 diff --git a/rae_hw/launch/control.launch.py b/rae_hw/launch/control.launch.py index 6866cc9..2fe9694 100644 --- a/rae_hw/launch/control.launch.py +++ b/rae_hw/launch/control.launch.py @@ -107,6 +107,12 @@ def launch_setup(context, *args, **kwargs): package='rae_hw', executable='battery_node', ) + sys_info = LifecycleNode( + package='rae_hw', + executable='sys_info_node.py', + name='sys_info', + namespace=LaunchConfiguration('namespace'), + ) return [ lifecycle_manager, @@ -120,10 +126,10 @@ def launch_setup(context, *args, **kwargs): speakers, robot_state_pub, ekf_node, - imu_comp_filt, controller_manager, diff_controller, joint_state_broadcaster, + sys_info ] )) diff --git a/rae_hw/launch/control_mock.launch.py b/rae_hw/launch/control_mock.launch.py index 722f145..49b22ea 100644 --- a/rae_hw/launch/control_mock.launch.py +++ b/rae_hw/launch/control_mock.launch.py @@ -64,6 +64,13 @@ def launch_setup(context, *args, **kwargs): executable='mock_mic.py', name='mic_node', namespace=LaunchConfiguration('namespace') + ), + LifecycleNode( + package='rae_hw', + executable='sys_info_node.py', + name='sys_info', + namespace=LaunchConfiguration('namespace'), + parameters=[{'mock': True}] ) ] diff --git a/rae_hw/scripts/lifecycle_manager.py b/rae_hw/scripts/lifecycle_manager.py index 8927899..595bbbf 100755 --- a/rae_hw/scripts/lifecycle_manager.py +++ b/rae_hw/scripts/lifecycle_manager.py @@ -26,7 +26,7 @@ def __init__(self): 'silent_startup', False).value self._startup_sound_path = self.declare_parameter('startup_sound_path', os.path.join( get_package_share_directory('rae_hw'), 'assets', 'startup.mp3')).value - self._node_names = ['mic_node', 'battery_node', 'speakers_node'] + self._node_names = ['mic_node', 'battery_node', 'speakers_node', 'sys_info'] # for each node, create a service client to change state self._change_state_clients = {} diff --git a/rae_hw/scripts/mock_battery.py b/rae_hw/scripts/mock/mock_battery.py similarity index 93% rename from rae_hw/scripts/mock_battery.py rename to rae_hw/scripts/mock/mock_battery.py index 7df13eb..a774005 100755 --- a/rae_hw/scripts/mock_battery.py +++ b/rae_hw/scripts/mock/mock_battery.py @@ -6,6 +6,7 @@ from rclpy.lifecycle import TransitionCallbackReturn, Node from sensor_msgs.msg import BatteryState +import random class MockBattery(Node): def __init__(self): @@ -13,12 +14,12 @@ def __init__(self): def timer_callback(self): msg = BatteryState() + msg.capacity= float(random.randint(0, 100)) self._battery_pub.publish(msg) def on_configure(self, state: LifecycleState) -> TransitionCallbackReturn: self.get_logger().info('Configuring') - self._battery_pub = self.create_publisher(BatteryState, 'battery', 10) + self._battery_pub = self.create_publisher(BatteryState, 'battery_status', 10) self.timer = self.create_timer(1, self.timer_callback) - sleep(0.5) return TransitionCallbackReturn.SUCCESS def on_activate(self, state: LifecycleState) -> TransitionCallbackReturn: diff --git a/rae_hw/scripts/mock_lcd.py b/rae_hw/scripts/mock/mock_lcd.py similarity index 100% rename from rae_hw/scripts/mock_lcd.py rename to rae_hw/scripts/mock/mock_lcd.py diff --git a/rae_hw/scripts/mock_leds.py b/rae_hw/scripts/mock/mock_leds.py similarity index 100% rename from rae_hw/scripts/mock_leds.py rename to rae_hw/scripts/mock/mock_leds.py diff --git a/rae_hw/scripts/mock_mic.py b/rae_hw/scripts/mock/mock_mic.py similarity index 100% rename from rae_hw/scripts/mock_mic.py rename to rae_hw/scripts/mock/mock_mic.py diff --git a/rae_hw/scripts/mock_speakers.py b/rae_hw/scripts/mock/mock_speakers.py similarity index 100% rename from rae_hw/scripts/mock_speakers.py rename to rae_hw/scripts/mock/mock_speakers.py diff --git a/rae_hw/scripts/mock_wheels.py b/rae_hw/scripts/mock/mock_wheels.py similarity index 100% rename from rae_hw/scripts/mock_wheels.py rename to rae_hw/scripts/mock/mock_wheels.py diff --git a/rae_hw/scripts/sys_info_node.py b/rae_hw/scripts/sys_info_node.py new file mode 100755 index 0000000..592b272 --- /dev/null +++ b/rae_hw/scripts/sys_info_node.py @@ -0,0 +1,74 @@ +#!/usr/bin/env python3 + +import psutil +import rclpy +from rclpy.lifecycle.node import LifecycleState, TransitionCallbackReturn +from rclpy.lifecycle import TransitionCallbackReturn, Node + +from std_msgs.msg import Float32 +from sensor_msgs.msg import Temperature + +class SysInfoNode(Node): + def __init__(self): + super().__init__('sys_info_node') + self._prev_bytes_sent = 0 + self._prev_bytes_recv = 0 + + def on_configure(self, state: LifecycleState) -> TransitionCallbackReturn: + self.get_logger().info('Configuring') + self._cpu_pub = self.create_publisher(Float32, 'cpu', 10) + self._mem_pub = self.create_publisher(Float32, 'mem', 10) + self._temp_pub = self.create_publisher(Temperature, 'temp', 10) + self._net_up_pub = self.create_publisher(Float32, 'net_up', 10) + self._net_down_pub = self.create_publisher(Float32, 'net_down', 10) + self._disk_pub = self.create_publisher(Float32, 'disk', 10) + self._mock = self.declare_parameter('mock', False).value + + self._timer = self.create_timer(1, self.timer_callback) + return TransitionCallbackReturn.SUCCESS + + def on_activate(self, state: LifecycleState) -> TransitionCallbackReturn: + self.get_logger().info('Activating') + return TransitionCallbackReturn.SUCCESS + + def on_deactivate(self, state: LifecycleState) -> TransitionCallbackReturn: + self.get_logger().info('Deactivating') + return TransitionCallbackReturn.SUCCESS + + def on_shutdown(self, state: LifecycleState) -> TransitionCallbackReturn: + self.get_logger().info('Shutting down') + return TransitionCallbackReturn.SUCCESS + + def timer_callback(self): + cpu = psutil.cpu_percent() + if cpu > 90: + self.get_logger().warn(f'CPU usage is {cpu}%') + mem = psutil.virtual_memory().percent + if mem > 90: + self.get_logger().warn(f'Memory usage is {mem}%') + temp = psutil.sensors_temperatures() + net = psutil.net_io_counters() + disk = psutil.disk_usage('/').percent + self._cpu_pub.publish(Float32(data=cpu)) + self._mem_pub.publish(Float32(data=mem)) + self._disk_pub.publish(Float32(data=disk)) + if not self._mock: + curr_temp = temp['bq27441_0'][0].current + if curr_temp > 50: + self.get_logger().warn(f'Temperature is {curr_temp}°C') + self._temp_pub.publish(Temperature(temperature=curr_temp)) + + mbs_up = (net.bytes_sent - self._prev_bytes_sent) / 1024 / 1024 + mbs_down = (net.bytes_recv - self._prev_bytes_recv) / 1024 / 1024 + + self._net_up_pub.publish(Float32(data=mbs_up)) + self._net_down_pub.publish(Float32(data=mbs_down)) + + self._prev_bytes_sent = net.bytes_sent + self._prev_bytes_recv = net.bytes_recv + +if __name__ == '__main__': + rclpy.init() + sys_info_node = SysInfoNode() + rclpy.spin(sys_info_node) + rclpy.shutdown() diff --git a/rae_sdk/rae_sdk/robot/display.py b/rae_sdk/rae_sdk/robot/display.py index 96a8bce..e2e220f 100644 --- a/rae_sdk/rae_sdk/robot/display.py +++ b/rae_sdk/rae_sdk/robot/display.py @@ -1,10 +1,12 @@ import os import logging as log +from copy import deepcopy import cv2 import numpy as np from sensor_msgs.msg import Image from cv_bridge import CvBridge from ament_index_python import get_package_share_directory +from .state import StateInfo def quaternion_to_rotation_matrix(q): @@ -68,19 +70,71 @@ def __init__(self, ros_interface): self._screen_height = 80 self._assets_path = os.path.join( get_package_share_directory('rae_sdk'), 'assets') + self._default_img = cv2.imread(os.path.join( + self._assets_path, 'img', 'rae-logo-white.jpg')) + self._last_image = None + self._state_info = None + self.display_default() log.info("Display Controller ready") def stop(self): self.display_default() def display_image(self, image_data): + self._last_image = image_data + if self._state_info: + overlay = self.battery_overlay() + image_data = cv2.addWeighted(image_data, 1, overlay, 1, 0) ros_image = self._bridge.cv2_to_imgmsg(image_data, encoding='bgra8') self._ros_interface.publish('/lcd', ros_image) + def add_state_overlay(self, info: StateInfo): + self._state_info = info + self.display_image(deepcopy(self._last_image)) + + def display_text(self, text, on_default=True, centerX=True, centerY=False, location=(30, 16), color=(255, 255, 255), font_scale=0.5, thickness=1, font=cv2.FONT_HERSHEY_SIMPLEX, line_type=cv2.LINE_AA): + img = np.zeros( + (self._screen_height, self._screen_width, 3), dtype=np.uint8) + if on_default: + self.display_default() + img = deepcopy(self._last_image) + textY = location[1] + textX = location[0] + textsize = cv2.getTextSize(text, font, font_scale, thickness)[0] + if centerX: + textX = int((img.shape[1] - textsize[0]) / 2) + if centerY: + textY = int((img.shape[0] + textsize[1]) / 2) + print(textX, textY, textsize, img.shape) + cv2.putText(img, text, (textX, textY), font, font_scale, + color, thickness, line_type) + bgra_image = cv2.cvtColor(img, cv2.COLOR_BGR2BGRA) + self.display_image(bgra_image) + + def battery_overlay(self): + # display battery state in a rectangle on the top right corner of the screen + battery_state = self._state_info.battery_state + img = self._last_image + # create battery symbol + cv2.rectangle(img, (140, 5), (156, 15), (255, 255, 255), 1) + cv2.rectangle(img, (156, 7), (158, 13), (255, 255, 255), -1) + # create 3 bars based on battery percentage, if above 66% color is green, 66-33% is yellow, below 33% is red + if battery_state.capacity > 66: + color = (0, 255, 0) + elif battery_state.capacity > 33: + color = (0, 255, 255) + else: + color = (0, 0, 255) + cv2.rectangle( + img, (142, 7), (143 + int(battery_state.capacity / 10), 13), color, -1) + # fill the rest with black + cv2.rectangle(img, (143 + int(battery_state.capacity / 10), + 7), (156, 13), (0, 0, 0), -1) + bgra_image = cv2.cvtColor(img, cv2.COLOR_BGR2BGRA) + return bgra_image + def display_default(self): - path = os.path.join(self._assets_path, 'img', 'rae-logo-white.jpg') - image = cv2.imread(path) - bgra_image = cv2.cvtColor(image, cv2.COLOR_BGR2BGRA) + bgra_image = cv2.cvtColor(self._default_img, cv2.COLOR_BGR2BGRA) self.display_image(bgra_image) def display_face(self, payload): diff --git a/rae_sdk/rae_sdk/robot/robot.py b/rae_sdk/rae_sdk/robot/robot.py index d332d94..86fc610 100644 --- a/rae_sdk/rae_sdk/robot/robot.py +++ b/rae_sdk/rae_sdk/robot/robot.py @@ -46,10 +46,11 @@ def __init__(self, robot_options: RobotOptions = RobotOptions()): if robot_options.launch_controllers: self._led_controller = LEDController(self._ros_interface) self._display_controller = DisplayController(self._ros_interface) - self._navigation_controller = NavigationController(self._ros_interface) + self._navigation_controller = NavigationController( + self._ros_interface) self._audio_controller = AudioController(self._ros_interface) - self._state_controller = StateController(self._ros_interface) - + self._state_controller = StateController( + self._ros_interface, robot_options.publish_state_info, self._display_controller) self._perception_system = None log.info('Robot ready') @@ -76,7 +77,8 @@ def state(self) -> StateController: def perception(self) -> PerceptionSystem: """Create perception system if it doesn't exist and return it.""" if self._perception_system is None: - self._perception_system = PerceptionSystem(self._robot_options.namespace) + self._perception_system = PerceptionSystem( + self._robot_options.namespace) return self._perception_system @property diff --git a/rae_sdk/rae_sdk/robot/robot_options.py b/rae_sdk/rae_sdk/robot/robot_options.py index e795bfd..1c0f5fa 100644 --- a/rae_sdk/rae_sdk/robot/robot_options.py +++ b/rae_sdk/rae_sdk/robot/robot_options.py @@ -4,20 +4,22 @@ class RobotOptions: Attributes ---------- - start_hardware (bool): Whether to start the robot's hardware. - launch_mock (bool): Whether to launch the robot's mock interfaces if start_hardware=True. name (str): The robot's name. namespace (str): The robot's namespace. launch_controllers (bool): Whether to launch the robot's controllers. + start_hardware (bool): Whether to start the robot's hardware. + launch_mock (bool): Whether to launch the robot's mock interfaces if start_hardware=True. + publish_state_info (bool): Whether to publish state information. """ - def __init__(self, name='rae_api', namespace='', launch_controllers=True, start_hardware=True, launch_mock=False): + def __init__(self, name='rae_api', namespace='', launch_controllers=True, start_hardware=True, launch_mock=False, publish_state_info=True): self._start_hardware = start_hardware self._launch_mock = launch_mock self._name = name self._namespace = namespace self._launch_controllers = launch_controllers + self._publish_state_info = publish_state_info @property def start_hardware(self): @@ -38,4 +40,6 @@ def namespace(self): @property def launch_controllers(self): return self._launch_controllers - + @property + def publish_state_info(self): + return self._publish_state_info diff --git a/rae_sdk/rae_sdk/robot/state.py b/rae_sdk/rae_sdk/robot/state.py index 6e4e951..2b015ab 100644 --- a/rae_sdk/rae_sdk/robot/state.py +++ b/rae_sdk/rae_sdk/robot/state.py @@ -1,5 +1,34 @@ import logging as log +from dataclasses import dataclass from sensor_msgs.msg import BatteryState +from std_msgs.msg import Float32 +from sensor_msgs.msg import Temperature + + +@dataclass +class StateInfo: + """ + A class for representing the robot's state. + + Attributes + ---------- + battery_state (BatteryState): The current state of the robot's battery. + cpu_usage (float): The current CPU usage of the robot. + mem_usage (float): The current memory usage of the robot. + temp (float): The current temperature of the robot. + disk (float): The current disk usage of the robot. + net_up (float): The current upload speed of the robot. + net_down (float): The current download speed of the robot. + + """ + + battery_state: BatteryState = BatteryState() + cpu_usage: float = 0.0 + mem_usage: float = 0.0 + temp: float = 0.0 + disk: float = 0.0 + net_up: float = 0.0 + net_down: float = 0.0 class StateController: @@ -9,24 +38,76 @@ class StateController: Attributes ---------- ros_interface (ROSInterface): An object for managing ROS2 communications and functionalities. - battery_state (BatteryState): Stores the current state of the robot's battery. - - Methods - ------- - battery_state_cb(data): Callback method for updating battery state. + state_info (StateInfo): Stores the current state of the robot's battery. """ - def __init__(self, ros_interface): + def __init__(self, ros_interface, publish_state_info=True, display=None): self._ros_interface = ros_interface + self._display = display + self._publish_state_info = publish_state_info + + self._state_info = StateInfo() + self._ros_interface.create_subscriber( + "battery_status", BatteryState, self.battery_state_cb) + self._ros_interface.create_subscriber( + "cpu", Float32, self.cpu_usage_cb) + self._ros_interface.create_subscriber( + "mem", Float32, self.mem_usage_cb) self._ros_interface.create_subscriber( - "/battery_status", BatteryState, self.battery_state_cb) - self._battery_state = BatteryState() + "temp", Temperature, self.temp_cb) + self._ros_interface.create_subscriber( + "disk", Float32, self.disk_cb) + self._ros_interface.create_subscriber( + "net_up", Float32, self.net_up_cb) + self._ros_interface.create_subscriber( + "net_down", Float32, self.net_down_cb) + if self._publish_state_info: + self._ros_interface.create_timer( + 'state_info', 1, self.state_info_cb) + else: + self._ros_interface.destroy_timer('state_info') + log.info("State Controller ready") def battery_state_cb(self, data): - self._battery_state = data + self._state_info.battery_state = data + + def cpu_usage_cb(self, data): + self._state_info.cpu_usage = data.data + + def mem_usage_cb(self, data): + self._state_info.mem_usage = data.data + + def temp_cb(self, data): + self._state_info.temp = data.temperature + + def disk_cb(self, data): + self._state_info.disk = data.data + + def net_up_cb(self, data): + self._state_info.net_up = data.data + + def net_down_cb(self, data): + self._state_info.net_down = data.data + + def state_info_cb(self): + if self._display is not None: + self._display.add_state_overlay(self._state_info) @property - def battery_state(self): - return self._battery_state + def state_info(self): + return self._state_info + + @property + def publish_state_info(self): + return self._publish_state_info + + @publish_state_info.setter + def publish_state_info(self, value): + self._publish_state_info = value + if self._publish_state_info: + self._ros_interface.create_timer( + 'state_info', 1, self.state_info_cb) + else: + self._ros_interface.destroy_timer('state_info')