From d7e25d750dd48c7057598c5488eae016b70585b4 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Tue, 27 Feb 2024 13:51:40 +0000 Subject: [PATCH] add state info display --- Dockerfile | 2 +- rae_hw/config/controller.yaml | 14 ++-- rae_hw/scripts/mock/mock_battery.py | 5 +- rae_sdk/rae_sdk/robot/display.py | 49 ++++++++++- rae_sdk/rae_sdk/robot/robot.py | 10 ++- rae_sdk/rae_sdk/robot/robot_options.py | 12 ++- rae_sdk/rae_sdk/robot/state.py | 110 ++++++++++++++----------- 7 files changed, 134 insertions(+), 68 deletions(-) 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/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/scripts/mock/mock_battery.py b/rae_hw/scripts/mock/mock_battery.py index 7df13eb..a774005 100755 --- a/rae_hw/scripts/mock/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_sdk/rae_sdk/robot/display.py b/rae_sdk/rae_sdk/robot/display.py index 96a8bce..d285fc3 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,60 @@ 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, 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) + + cv2.putText(img, text, location, 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 111b917..2b015ab 100644 --- a/rae_sdk/rae_sdk/robot/state.py +++ b/rae_sdk/rae_sdk/robot/state.py @@ -1,9 +1,35 @@ 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: """ @@ -12,24 +38,16 @@ 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._battery_state = BatteryState() - self._cpu_usage = 0.0 - self._mem_usage = 0.0 - self._temp = 0.0 - self._disk = 0.0 - self._net_up = 0.0 - self._net_down = 0.0 + self._state_info = StateInfo() self._ros_interface.create_subscriber( "battery_status", BatteryState, self.battery_state_cb) self._ros_interface.create_subscriber( @@ -44,54 +62,52 @@ def __init__(self, ros_interface): "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._cpu_usage = data.data + self._state_info.cpu_usage = data.data def mem_usage_cb(self, data): - self._mem_usage = data.data + self._state_info.mem_usage = data.data def temp_cb(self, data): - self._temp = data.temperature + self._state_info.temp = data.temperature def disk_cb(self, data): - self._disk = data.data + self._state_info.disk = data.data def net_up_cb(self, data): - self._net_up = data.data + self._state_info.net_up = data.data def net_down_cb(self, data): - self._net_down = data.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 - - @property - def cpu_usage(self): - return self._cpu_usage - - @property - def mem_usage(self): - return self._mem_usage - - @property - def temp(self): - return self._temp - - @property - def disk(self): - return self._disk - - @property - def net_up(self): - return self._net_up - + def state_info(self): + return self._state_info + @property - def net_down(self): - return self._net_down + 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')