From ee71c80edd086b8309dedb9cae7c119fa1454449 Mon Sep 17 00:00:00 2001 From: Matt Spencer Date: Fri, 1 May 2026 11:34:56 +0000 Subject: [PATCH] Add a simple oled display controller --- docker-compose.yml | 15 ++ oled/Dockerfile | 41 ++++ oled/src/raspbot_oled/package.xml | 16 ++ .../src/raspbot_oled/raspbot_oled/__init__.py | 0 .../raspbot_oled/raspbot_oled/oled_node.py | 208 ++++++++++++++++++ oled/src/raspbot_oled/resource/raspbot_oled | 0 oled/src/raspbot_oled/setup.cfg | 4 + oled/src/raspbot_oled/setup.py | 20 ++ 8 files changed, 304 insertions(+) create mode 100644 oled/Dockerfile create mode 100644 oled/src/raspbot_oled/package.xml create mode 100644 oled/src/raspbot_oled/raspbot_oled/__init__.py create mode 100644 oled/src/raspbot_oled/raspbot_oled/oled_node.py create mode 100644 oled/src/raspbot_oled/resource/raspbot_oled create mode 100644 oled/src/raspbot_oled/setup.cfg create mode 100644 oled/src/raspbot_oled/setup.py diff --git a/docker-compose.yml b/docker-compose.yml index 2b2aa30..7018cd5 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -46,6 +46,21 @@ services: - scan_qos_reliability:=${LIDAR_QOS_RELIABILITY:-best_effort} restart: unless-stopped + oled: + build: + context: . + dockerfile: oled/Dockerfile + platforms: + - linux/arm64 + image: raspbot_v2_oled:latest + network_mode: host + ipc: host + devices: + - /dev/i2c-1:/dev/i2c-1 + environment: + - ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-0} + restart: unless-stopped + webui: build: context: webui diff --git a/oled/Dockerfile b/oled/Dockerfile new file mode 100644 index 0000000..988f47d --- /dev/null +++ b/oled/Dockerfile @@ -0,0 +1,41 @@ +# syntax=docker/dockerfile:1 + +# ── Stage 1: build ──────────────────────────────────────────────────────────── +FROM ros:kilted AS builder + +SHELL ["/bin/bash", "-c"] + +RUN apt-get update && apt-get install -y --no-install-recommends \ + python3-colcon-common-extensions \ + && rm -rf /var/lib/apt/lists/* + +WORKDIR /ws +COPY oled/src/raspbot_oled/ src/raspbot_oled/ + +RUN source /opt/ros/${ROS_DISTRO}/setup.bash && \ + colcon build --packages-select raspbot_oled + +# ── Stage 2: runtime ────────────────────────────────────────────────────────── +FROM ros:kilted-ros-core + +RUN apt-get update && apt-get install -y --no-install-recommends \ + ros-${ROS_DISTRO}-rclpy \ + ros-${ROS_DISTRO}-sensor-msgs \ + python3-venv \ + python3-smbus \ + fonts-dejavu-core \ + && rm -rf /var/lib/apt/lists/* + +RUN python3 -m venv /opt/oled-venv --system-site-packages && \ + /opt/oled-venv/bin/pip install --no-cache-dir luma.oled psutil + +COPY --from=builder /ws/install /ws/install + +RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> /etc/bash.bashrc && \ + echo "source /ws/install/setup.bash" >> /etc/bash.bashrc + +COPY docker-entrypoint.sh /docker-entrypoint.sh +RUN chmod +x /docker-entrypoint.sh + +ENTRYPOINT ["/docker-entrypoint.sh"] +CMD ["/opt/oled-venv/bin/python3", "-m", "raspbot_oled.oled_node"] diff --git a/oled/src/raspbot_oled/package.xml b/oled/src/raspbot_oled/package.xml new file mode 100644 index 0000000..95c5471 --- /dev/null +++ b/oled/src/raspbot_oled/package.xml @@ -0,0 +1,16 @@ + + + + raspbot_oled + 0.0.1 + OLED dashboard display node for the Raspbot V2. + Matt + MIT + + rclpy + sensor_msgs + + + ament_python + + diff --git a/oled/src/raspbot_oled/raspbot_oled/__init__.py b/oled/src/raspbot_oled/raspbot_oled/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/oled/src/raspbot_oled/raspbot_oled/oled_node.py b/oled/src/raspbot_oled/raspbot_oled/oled_node.py new file mode 100644 index 0000000..4cf44a2 --- /dev/null +++ b/oled/src/raspbot_oled/raspbot_oled/oled_node.py @@ -0,0 +1,208 @@ +#!/usr/bin/env python3 +""" +OLED display node for the Raspbot V2. + +Renders a live dashboard on the I2C OLED screen showing: + - Robot hostname / IP address + - Ultrasonic range reading + - Camera pan / tilt angles + - CPU and memory usage + +Parameters +---------- +driver str default 'ssd1306' – OLED driver: ssd1306 | sh1106 +i2c_port int default 1 – I2C bus number +i2c_address int default 0x3C – I2C device address +width int default 128 +height int default 64 +rotate int default 0 – 0/1/2/3 = 0/90/180/270° +refresh_hz float default 2.0 – display update rate +data_timeout_s float default 5.0 – seconds before a reading shows '---' +""" + +import math +import socket +import time + +import psutil +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import JointState, Range + +DRIVERS = ('ssd1306', 'sh1106') + + +def _get_ip() -> str: + try: + s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + s.connect(('8.8.8.8', 80)) + ip = s.getsockname()[0] + s.close() + return ip + except Exception: + return '?.?.?.?' + + +def _load_font(size: int): + from PIL import ImageFont + candidates = [ + '/usr/share/fonts/truetype/dejavu/DejaVuSansMono.ttf', + '/usr/share/fonts/truetype/dejavu/DejaVuSans.ttf', + '/usr/share/fonts/truetype/liberation/LiberationMono-Regular.ttf', + ] + for path in candidates: + try: + return ImageFont.truetype(path, size) + except (IOError, OSError): + pass + return ImageFont.load_default() + + +class OledNode(Node): + + def __init__(self): + super().__init__('oled_display') + + self.declare_parameter('driver', 'ssd1306') + self.declare_parameter('i2c_port', 1) + self.declare_parameter('i2c_address', 0x3C) + self.declare_parameter('width', 128) + self.declare_parameter('height', 64) + self.declare_parameter('rotate', 0) + self.declare_parameter('refresh_hz', 2.0) + self.declare_parameter('data_timeout_s', 5.0) + + self._timeout = self.get_parameter('data_timeout_s').value + self._range: float | None = None + self._range_ts: float = 0.0 + self._joints: dict[str, float] = {} + self._joints_ts: float = 0.0 + + self.create_subscription(Range, '/ultrasonic/range', self._range_cb, 10) + self.create_subscription(JointState, '/joint_states', self._joints_cb, 10) + + self._device = self._init_display() + self._font = _load_font(10) + self._line_h = 12 # pixels per text row + + rate = self.get_parameter('refresh_hz').value + self.create_timer(1.0 / rate, self._render) + + self.get_logger().info('OLED display node started.') + + # ------------------------------------------------------------------ + # Display initialisation + # ------------------------------------------------------------------ + + def _init_display(self): + driver = self.get_parameter('driver').value + if driver not in DRIVERS: + self.get_logger().warn(f"Unknown driver '{driver}', falling back to ssd1306.") + driver = 'ssd1306' + + port = self.get_parameter('i2c_port').value + address = self.get_parameter('i2c_address').value + width = self.get_parameter('width').value + height = self.get_parameter('height').value + rotate = self.get_parameter('rotate').value + + try: + from luma.core.interface.serial import i2c as luma_i2c + from luma.oled.device import sh1106, ssd1306 + serial = luma_i2c(port=port, address=address) + cls = ssd1306 if driver == 'ssd1306' else sh1106 + device = cls(serial, width=width, height=height, rotate=rotate) + self.get_logger().info( + f"OLED {driver} initialised at I2C bus {port}, address 0x{address:02X}." + ) + return device + except Exception as e: + self.get_logger().error(f'Could not initialise OLED display: {e}') + return None + + # ------------------------------------------------------------------ + # Topic callbacks + # ------------------------------------------------------------------ + + def _range_cb(self, msg: Range) -> None: + self._range = msg.range + self._range_ts = time.monotonic() + + def _joints_cb(self, msg: JointState) -> None: + self._joints = dict(zip(msg.name, msg.position)) + self._joints_ts = time.monotonic() + + # ------------------------------------------------------------------ + # Rendering + # ------------------------------------------------------------------ + + def _render(self) -> None: + if self._device is None: + return + + now = time.monotonic() + + # Ultrasonic range + if self._range is not None and now - self._range_ts < self._timeout: + range_str = f'{self._range:.2f} m' + else: + range_str = '---' + + # Pan / tilt + if self._joints and now - self._joints_ts < self._timeout: + pan_deg = math.degrees(self._joints.get('pan', 0.0)) + tilt_deg = math.degrees(self._joints.get('tilt', 0.0)) + pt_str = f'P:{pan_deg:+.0f}° T:{tilt_deg:+.0f}°' + else: + pt_str = 'P:--- T:---' + + # System stats + cpu_pct = psutil.cpu_percent() + mem_pct = psutil.virtual_memory().percent + + ip = _get_ip() + + try: + from luma.core.render import canvas + with canvas(self._device) as draw: + y = 0 + draw.text((0, y), 'RASPBOT V2', font=self._font, fill='white') + y += self._line_h + draw.text((0, y), f'IP: {ip}', font=self._font, fill='white') + y += self._line_h + draw.text((0, y), f'Dist: {range_str}', font=self._font, fill='white') + y += self._line_h + draw.text((0, y), pt_str, font=self._font, fill='white') + y += self._line_h + draw.text((0, y), f'CPU:{cpu_pct:.0f}% Mem:{mem_pct:.0f}%', font=self._font, fill='white') + except Exception as e: + self.get_logger().error(f'Display render error: {e}') + + # ------------------------------------------------------------------ + # Shutdown + # ------------------------------------------------------------------ + + def destroy_node(self) -> None: + if self._device is not None: + try: + self._device.clear() + self._device.cleanup() + except Exception: + pass + super().destroy_node() + + +def main(args=None): + rclpy.init(args=args) + node = OledNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/oled/src/raspbot_oled/resource/raspbot_oled b/oled/src/raspbot_oled/resource/raspbot_oled new file mode 100644 index 0000000..e69de29 diff --git a/oled/src/raspbot_oled/setup.cfg b/oled/src/raspbot_oled/setup.cfg new file mode 100644 index 0000000..54eb2bd --- /dev/null +++ b/oled/src/raspbot_oled/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/raspbot_oled +[install] +install_scripts=$base/lib/raspbot_oled diff --git a/oled/src/raspbot_oled/setup.py b/oled/src/raspbot_oled/setup.py new file mode 100644 index 0000000..1555595 --- /dev/null +++ b/oled/src/raspbot_oled/setup.py @@ -0,0 +1,20 @@ +from setuptools import setup + +package_name = 'raspbot_oled' + +setup( + name=package_name, + version='0.0.1', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + entry_points={ + 'console_scripts': [ + 'oled_display = raspbot_oled.oled_node:main', + ], + }, +)