Sys info node (#72)

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/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()

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/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
                       ]
         ))
 

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}]
         )
     ]
 

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 = {}

rae_hw/scripts/mock_battery.py → 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_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()

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

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