From 48bbf8cfef729828966706ca29bc95620a0051da Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Sun, 10 May 2026 11:36:49 -0400 Subject: [PATCH] added led indicator node --- requirements.txt | 1 + src/msgs/msg/LedStatus.msg | 10 ++ .../launch/led_indicator.launch.py | 17 ++++ .../led_indicator/led_indicator/__init__.py | 0 .../led_indicator/led_indicator_node.py | 94 +++++++++++++++++++ .../navigation/led_indicator/package.xml | 23 +++++ .../led_indicator/resource/led_indicator | 0 .../navigation/led_indicator/setup.cfg | 4 + .../navigation/led_indicator/setup.py | 28 ++++++ 9 files changed, 177 insertions(+) create mode 100644 src/msgs/msg/LedStatus.msg create mode 100644 src/subsystems/navigation/led_indicator/launch/led_indicator.launch.py create mode 100644 src/subsystems/navigation/led_indicator/led_indicator/__init__.py create mode 100644 src/subsystems/navigation/led_indicator/led_indicator/led_indicator_node.py create mode 100644 src/subsystems/navigation/led_indicator/package.xml create mode 100644 src/subsystems/navigation/led_indicator/resource/led_indicator create mode 100644 src/subsystems/navigation/led_indicator/setup.cfg create mode 100644 src/subsystems/navigation/led_indicator/setup.py diff --git a/requirements.txt b/requirements.txt index f4c6af3f..5b56cc5c 100644 --- a/requirements.txt +++ b/requirements.txt @@ -5,3 +5,4 @@ lark numpy<2.0 opencv-contrib-python==4.10.0.84 ahrs +pyserial diff --git a/src/msgs/msg/LedStatus.msg b/src/msgs/msg/LedStatus.msg new file mode 100644 index 00000000..64858567 --- /dev/null +++ b/src/msgs/msg/LedStatus.msg @@ -0,0 +1,10 @@ +uint8 CMD_SOLID = 1 +uint8 CMD_FLASH = 2 +uint8 CMD_PULSE = 3 +uint8 CMD_OFF = 255 + +uint8 cmd +uint8 r +uint8 g +uint8 b +uint8 param # rate in tenths-of-Hz for FLASH/PULSE; 0 = default (2 Hz) diff --git a/src/subsystems/navigation/led_indicator/launch/led_indicator.launch.py b/src/subsystems/navigation/led_indicator/launch/led_indicator.launch.py new file mode 100644 index 00000000..d3a0df70 --- /dev/null +++ b/src/subsystems/navigation/led_indicator/launch/led_indicator.launch.py @@ -0,0 +1,17 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription([ + Node( + package="led_indicator", + executable="led_indicator_node", + name="led_indicator", + output="screen", + parameters=[{ + "serial_port": "auto", + "baud_rate": 115200, + }], + ) + ]) diff --git a/src/subsystems/navigation/led_indicator/led_indicator/__init__.py b/src/subsystems/navigation/led_indicator/led_indicator/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/subsystems/navigation/led_indicator/led_indicator/led_indicator_node.py b/src/subsystems/navigation/led_indicator/led_indicator/led_indicator_node.py new file mode 100644 index 00000000..434f02a9 --- /dev/null +++ b/src/subsystems/navigation/led_indicator/led_indicator/led_indicator_node.py @@ -0,0 +1,94 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy + +from msgs.msg import LedStatus + +import serial +import serial.tools.list_ports + +_SYNC = 0xAA + + +def _packet(cmd: int, r: int = 0, g: int = 0, b: int = 0, param: int = 0) -> bytes: + xor = cmd ^ r ^ g ^ b ^ param + return bytes([_SYNC, cmd, r, g, b, param, xor]) + + +def _find_data_port() -> str | None: + ports = sorted( + p.device + for p in serial.tools.list_ports.comports() + if "trinkey" in (p.description or "").lower() + or "trinkey" in (p.product or "").lower() + or "adafruit" in (p.manufacturer or "").lower() + ) + return ports[-1] if ports else None + + +class LedIndicatorNode(Node): + def __init__(self): + super().__init__("led_indicator") + + self.declare_parameter("serial_port", "auto") + self.declare_parameter("baud_rate", 115200) + + port = self.get_parameter("serial_port").value + baud = self.get_parameter("baud_rate").value + + if port == "auto": + port = _find_data_port() + if port is None: + self.get_logger().warn( + "Trinkey data port not found — defaulting to /dev/ttyACM1. " + "Set 'serial_port' explicitly if wrong." + ) + port = "/dev/ttyACM1" + else: + self.get_logger().info(f"Auto-detected Trinkey data port: {port}") + + self._ser: serial.Serial | None = None + try: + self._ser = serial.Serial(port, baud, timeout=1, dsrdtr=False, rtscts=False) + self.get_logger().info(f"Connected to Pixel Trinkey on {port}") + except serial.SerialException as e: + self.get_logger().error(f"Could not open {port}: {e}. LED output disabled.") + + reliable_qos = QoSProfile(depth=1, reliability=ReliabilityPolicy.RELIABLE) + self.create_subscription(LedStatus, "/led_status", self._cb, reliable_qos) + + self._send(_packet(LedStatus.CMD_OFF)) + + def _cb(self, msg: LedStatus) -> None: + self._send(_packet(msg.cmd, msg.r, msg.g, msg.b, msg.param)) + + def _send(self, pkt: bytes) -> None: + if self._ser is None or not self._ser.is_open: + return + try: + self._ser.write(pkt) + except serial.SerialException as e: + self.get_logger().warn(f"Serial write failed: {e}", throttle_duration_sec=5.0) + + def destroy_node(self) -> None: + self._send(_packet(LedStatus.CMD_OFF)) + if self._ser and self._ser.is_open: + self._ser.close() + super().destroy_node() + + +def main(args=None): + rclpy.init(args=args) + node = LedIndicatorNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/subsystems/navigation/led_indicator/package.xml b/src/subsystems/navigation/led_indicator/package.xml new file mode 100644 index 00000000..171ce016 --- /dev/null +++ b/src/subsystems/navigation/led_indicator/package.xml @@ -0,0 +1,23 @@ + + + + led_indicator + 0.0.0 + LED status indicator for navigation mode (Red=autonomous, Blue=teleop, Flashing Green=arrived) + mdurrani + MIT + + rclpy + msgs + std_msgs + python3-serial + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/subsystems/navigation/led_indicator/resource/led_indicator b/src/subsystems/navigation/led_indicator/resource/led_indicator new file mode 100644 index 00000000..e69de29b diff --git a/src/subsystems/navigation/led_indicator/setup.cfg b/src/subsystems/navigation/led_indicator/setup.cfg new file mode 100644 index 00000000..fb84a867 --- /dev/null +++ b/src/subsystems/navigation/led_indicator/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/led_indicator +[install] +install_scripts=$base/lib/led_indicator diff --git a/src/subsystems/navigation/led_indicator/setup.py b/src/subsystems/navigation/led_indicator/setup.py new file mode 100644 index 00000000..53711e99 --- /dev/null +++ b/src/subsystems/navigation/led_indicator/setup.py @@ -0,0 +1,28 @@ +from setuptools import find_packages, setup +from glob import glob +import os + +package_name = "led_indicator" + +setup( + name=package_name, + version="0.0.0", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + (os.path.join("share", package_name, "launch"), glob("launch/*.py")), + ], + install_requires=["setuptools", "pyserial"], + zip_safe=True, + maintainer="mdurrani", + maintainer_email="mdurrani808@gmail.com", + description="LED status indicator via Adafruit Pixel Trinkey + NeoPixels", + license="MIT", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "led_indicator_node = led_indicator.led_indicator_node:main", + ], + }, +)