add state info display

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

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

rae_hw/scripts/mock/mock_battery.py

@@ -6,19 +6,20 @@
 from rclpy.lifecycle import TransitionCallbackReturn, Node
 
 from sensor_msgs.msg import BatteryState
+import random
 
 class MockBattery(Node):
     def __init__(self):
         super().__init__('battery_node')
 
     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:

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):

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

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

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')