diff --git a/jetson/ros2_ws/src/saltybot_led_controller/config/led_config.yaml b/jetson/ros2_ws/src/saltybot_led_controller/config/led_config.yaml new file mode 100644 index 0000000..fecefb2 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_led_controller/config/led_config.yaml @@ -0,0 +1,87 @@ +# NeoPixel LED Controller Configuration + +# Hardware configuration +hardware: + num_leds: 30 # Total number of NeoPixels + gpio_pin: 18 # GPIO pin for data line + brightness_max: 255 # Maximum brightness (0-255) + brightness_min: 10 # Minimum brightness for visible LEDs + update_frequency: 30 # Update frequency in Hz + +# Brightness control +brightness: + enabled: true + auto_dim: true # Enable automatic brightness dimming + day_brightness: 200 # Daytime brightness (0-255) + night_brightness: 80 # Night-time brightness (0-255) + dim_threshold: 50 # Light sensor threshold for dimming + +# Animation settings +animation: + transition_duration: 0.5 # Smooth transition duration in seconds + breathing_speed: 1.5 # Breathing pattern speed (cycles/sec) + pulse_speed: 2.0 # Pulse pattern speed (cycles/sec) + chase_speed: 3.0 # Chase pattern speed (LEDs/sec) + +# LED patterns: define colors as [R, G, B] (0-255) +patterns: + idle: + name: "Breathing Blue" + color: [0, 50, 200] + pattern: "breathing" + speed: 1.5 + + follow: + name: "Green Pulse" + color: [0, 200, 50] + pattern: "pulse" + speed: 2.0 + + error: + name: "Red Flash" + color: [255, 0, 0] + pattern: "flash" + speed: 3.0 + + celebrate: + name: "Rainbow" + color: null # Rainbow uses spectrum + pattern: "rainbow" + speed: 2.0 + + low_battery: + name: "Amber Warning" + color: [255, 165, 0] + pattern: "pulse" + speed: 1.5 + + search: + name: "White Chase" + color: [255, 255, 255] + pattern: "chase" + speed: 3.0 + +# State priorities (higher value = higher priority) +state_priorities: + error: 100 + low_battery: 50 + celebrate: 40 + follow: 30 + idle: 10 + +# Subscription topics +subscriptions: + battery: "/saltybot/battery/level" + balance: "/saltybot/balance/status" + social: "/saltybot/social/state" + emotion: "/saltybot/emotion/state" + health: "/saltybot/health/status" + +# Publishing topics +publications: + led_state: "/saltybot/led_state" + +# Thresholds +thresholds: + low_battery_percent: 20 # Battery % to trigger low battery LED + error_timeout: 2.0 # Seconds without heartbeat before error diff --git a/jetson/ros2_ws/src/saltybot_led_controller/launch/led_controller.launch.py b/jetson/ros2_ws/src/saltybot_led_controller/launch/led_controller.launch.py new file mode 100644 index 0000000..d81374b --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_led_controller/launch/led_controller.launch.py @@ -0,0 +1,36 @@ +"""Launch file for LED Controller node.""" + +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument +from ament_index_python.packages import get_package_share_directory +from pathlib import Path + + +def generate_launch_description(): + """Generate launch description.""" + package_dir = get_package_share_directory("saltybot_led_controller") + config_file = str(Path(package_dir) / "config" / "led_config.yaml") + + # Launch arguments + config_arg = DeclareLaunchArgument( + "config_file", + default_value=config_file, + description="Path to LED configuration file", + ) + + # LED Controller node + led_node = Node( + package="saltybot_led_controller", + executable="led_controller_node", + name="led_controller", + parameters=[ + { + "config_file": LaunchConfiguration("config_file"), + } + ], + output="screen", + ) + + return LaunchDescription([config_arg, led_node]) diff --git a/jetson/ros2_ws/src/saltybot_led_controller/package.xml b/jetson/ros2_ws/src/saltybot_led_controller/package.xml new file mode 100644 index 0000000..cd5a318 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_led_controller/package.xml @@ -0,0 +1,28 @@ + + + + saltybot_led_controller + 0.1.0 + + NeoPixel LED strip controller for SaltyBot. Subscribes to system state topics + (battery, balance, social, emotion, health) and renders animated LED patterns + with smooth transitions. Supports configurable LED count, GPIO pin, and brightness. + Publishes LED state status. + + sl-mechanical + MIT + + rclpy + std_msgs + + ament_python + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_led_controller/resource/saltybot_led_controller b/jetson/ros2_ws/src/saltybot_led_controller/resource/saltybot_led_controller new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_led_controller/saltybot_led_controller/__init__.py b/jetson/ros2_ws/src/saltybot_led_controller/saltybot_led_controller/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_led_controller/saltybot_led_controller/led_controller_node.py b/jetson/ros2_ws/src/saltybot_led_controller/saltybot_led_controller/led_controller_node.py new file mode 100644 index 0000000..8000969 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_led_controller/saltybot_led_controller/led_controller_node.py @@ -0,0 +1,422 @@ +#!/usr/bin/env python3 +"""NeoPixel LED Controller for SaltyBot. + +Subscribes to system state topics (battery, balance, social, emotion, health) +and renders animated LED patterns with smooth transitions. Supports configurable +LED count, GPIO pin, and auto-dim brightness. + +Published topics: + /saltybot/led_state (std_msgs/String) - JSON LED status + +Subscribed topics: + /saltybot/battery/level (std_msgs/String) - Battery percentage + /saltybot/balance/status (std_msgs/String) - Balance state + /saltybot/social/state (std_msgs/String) - Social state + /saltybot/emotion/state (std_msgs/String) - Emotion state + /saltybot/health/status (std_msgs/String) - Health status +""" + +import json +import math +import time +from dataclasses import dataclass, asdict +from typing import Dict, List, Optional, Tuple +from pathlib import Path +from enum import Enum + +import yaml +import rclpy +from rclpy.node import Node +from rclpy.timer import Timer +from std_msgs.msg import String + + +class LEDState(Enum): + """LED display states ordered by priority.""" + ERROR = 100 + LOW_BATTERY = 50 + CELEBRATE = 40 + FOLLOW = 30 + IDLE = 10 + + +@dataclass +class LEDColor: + """RGB color representation.""" + r: int + g: int + b: int + + def __post_init__(self): + """Clamp values to 0-255.""" + self.r = max(0, min(255, self.r)) + self.g = max(0, min(255, self.g)) + self.b = max(0, min(255, self.b)) + + def to_tuple(self) -> Tuple[int, int, int]: + """Return as RGB tuple.""" + return (self.r, self.g, self.b) + + def scale(self, factor: float) -> "LEDColor": + """Scale color by factor (0.0-1.0).""" + return LEDColor( + int(self.r * factor), + int(self.g * factor), + int(self.b * factor), + ) + + +class LEDPattern: + """Base class for LED animation patterns.""" + + def __init__(self, color: Optional[LEDColor], num_leds: int, speed: float = 1.0): + """Initialize pattern. + + Args: + color: RGB color or None for rainbow + num_leds: Total number of LEDs + speed: Animation speed multiplier + """ + self.color = color + self.num_leds = num_leds + self.speed = speed + self.time_offset = 0.0 + + def update(self, dt: float, brightness: float) -> List[Tuple[int, int, int]]: + """Generate LED frame. + + Args: + dt: Delta time since last frame + brightness: Brightness multiplier (0.0-1.0) + + Returns: + List of RGB tuples for each LED + """ + self.time_offset += dt * self.speed + return self._generate_frame(brightness) + + def _generate_frame(self, brightness: float) -> List[Tuple[int, int, int]]: + """Generate frame. Subclasses should override.""" + return [(0, 0, 0)] * self.num_leds + + +class BreathingPattern(LEDPattern): + """Smooth breathing animation.""" + + def _generate_frame(self, brightness: float) -> List[Tuple[int, int, int]]: + """Breathing pulse based on sine wave.""" + # Sine wave from 0.3 to 1.0 for breathing effect + breath_factor = 0.65 + 0.35 * math.sin(2 * math.pi * self.time_offset) + scaled_color = self.color.scale(breath_factor * brightness) + return [scaled_color.to_tuple()] * self.num_leds + + +class PulsePattern(LEDPattern): + """Quick pulse animation.""" + + def _generate_frame(self, brightness: float) -> List[Tuple[int, int, int]]: + """Pulse with steeper rise/fall.""" + # Triangular wave for snappier pulse + cycle = (self.time_offset % 1.0) # 0 to 1 + pulse_factor = 1.0 - abs(2.0 * (cycle - 0.5)) if cycle < 1.0 else 0 + pulse_factor = 0.2 + 0.8 * pulse_factor + scaled_color = self.color.scale(pulse_factor * brightness) + return [scaled_color.to_tuple()] * self.num_leds + + +class FlashPattern(LEDPattern): + """Fast on/off flashing.""" + + def _generate_frame(self, brightness: float) -> List[Tuple[int, int, int]]: + """Flash pattern: on/off.""" + cycle = self.time_offset % 1.0 + flash_on = cycle < 0.5 + factor = brightness if flash_on else 0.0 + scaled_color = self.color.scale(factor) + return [scaled_color.to_tuple()] * self.num_leds + + +class RainbowPattern(LEDPattern): + """Animated rainbow spectrum.""" + + def _generate_frame(self, brightness: float) -> List[Tuple[int, int, int]]: + """Rainbow spectrum with animation.""" + colors = [] + for i in range(self.num_leds): + # Hue cycles across strip with animation + hue = ((i / self.num_leds) + self.time_offset) % 1.0 + rgb = self._hsv_to_rgb(hue, 1.0, brightness) + colors.append(rgb) + return colors + + @staticmethod + def _hsv_to_rgb(h: float, s: float, v: float) -> Tuple[int, int, int]: + """Convert HSV to RGB.""" + c = v * s + x = c * (1 - abs((h * 6) % 2 - 1)) + m = v - c + + if h < 1/6: + r, g, b = c, x, 0 + elif h < 2/6: + r, g, b = x, c, 0 + elif h < 3/6: + r, g, b = 0, c, x + elif h < 4/6: + r, g, b = 0, x, c + elif h < 5/6: + r, g, b = x, 0, c + else: + r, g, b = c, 0, x + + return ( + int((r + m) * 255), + int((g + m) * 255), + int((b + m) * 255), + ) + + +class ChasePattern(LEDPattern): + """Moving light chase across strip.""" + + def _generate_frame(self, brightness: float) -> List[Tuple[int, int, int]]: + """Chasing light pattern.""" + colors = [(0, 0, 0)] * self.num_leds + pos = int((self.time_offset * self.num_leds) % self.num_leds) + + # Bright center with trailing fade + for offset in range(-3, 4): + idx = (pos + offset) % self.num_leds + if offset == 0: + factor = brightness + else: + factor = brightness * max(0, 1.0 - abs(offset) / 3.0) + scaled = self.color.scale(factor) + colors[idx] = scaled.to_tuple() + + return colors + + +class LEDControllerNode(Node): + """ROS2 node for LED control.""" + + def __init__(self): + super().__init__("led_controller") + + # Load config + self.declare_parameter("config_file", "led_config.yaml") + config_path = self.get_parameter("config_file").value + self.config = self._load_config(config_path) + + # Hardware + self.num_leds = self.config["hardware"]["num_leds"] + self.gpio_pin = self.config["hardware"]["gpio_pin"] + self.brightness_max = self.config["hardware"]["brightness_max"] + self.brightness_min = self.config["hardware"]["brightness_min"] + self.update_freq = self.config["hardware"]["update_frequency"] + + # State tracking + self.battery_level = 100 + self.balance_state = "idle" + self.social_state = "idle" + self.emotion_state = "idle" + self.health_state = "ok" + self.last_update = time.time() + self.last_state = None + self.current_pattern: Optional[LEDPattern] = None + self.transition_time = 0.0 + + # Subscribers + self.create_subscription(String, "/saltybot/battery/level", self._battery_cb, 10) + self.create_subscription(String, "/saltybot/balance/status", self._balance_cb, 10) + self.create_subscription(String, "/saltybot/social/state", self._social_cb, 10) + self.create_subscription(String, "/saltybot/emotion/state", self._emotion_cb, 10) + self.create_subscription(String, "/saltybot/health/status", self._health_cb, 10) + + # Publisher + self.led_state_pub = self.create_publisher(String, "/saltybot/led_state", 10) + + # Timer + period = 1.0 / self.update_freq + self.timer = self.create_timer(period, self._update_leds) + + self.get_logger().info( + f"LED Controller initialized: {self.num_leds} LEDs on GPIO{self.gpio_pin}" + ) + + def _load_config(self, config_path: str) -> Dict: + """Load YAML configuration.""" + try: + with open(config_path) as f: + return yaml.safe_load(f) + except FileNotFoundError: + self.get_logger().warn( + f"Config not found: {config_path}, using defaults" + ) + return self._default_config() + + def _default_config(self) -> Dict: + """Return default configuration.""" + return { + "hardware": { + "num_leds": 30, + "gpio_pin": 18, + "brightness_max": 255, + "brightness_min": 10, + "update_frequency": 30, + }, + "animation": { + "transition_duration": 0.5, + "breathing_speed": 1.5, + "pulse_speed": 2.0, + "chase_speed": 3.0, + }, + "patterns": { + "idle": {"color": [0, 50, 200], "pattern": "breathing", "speed": 1.5}, + "follow": {"color": [0, 200, 50], "pattern": "pulse", "speed": 2.0}, + "error": {"color": [255, 0, 0], "pattern": "flash", "speed": 3.0}, + "celebrate": {"color": None, "pattern": "rainbow", "speed": 2.0}, + "low_battery": {"color": [255, 165, 0], "pattern": "pulse", "speed": 1.5}, + "search": {"color": [255, 255, 255], "pattern": "chase", "speed": 3.0}, + }, + "thresholds": {"low_battery_percent": 20}, + } + + def _battery_cb(self, msg: String): + """Handle battery level.""" + try: + data = json.loads(msg.data) + self.battery_level = data.get("level", 100) + except (json.JSONDecodeError, KeyError): + pass + + def _balance_cb(self, msg: String): + """Handle balance state.""" + try: + data = json.loads(msg.data) + self.balance_state = data.get("state", "idle") + except json.JSONDecodeError: + pass + + def _social_cb(self, msg: String): + """Handle social state.""" + try: + data = json.loads(msg.data) + self.social_state = data.get("state", "idle") + except json.JSONDecodeError: + pass + + def _emotion_cb(self, msg: String): + """Handle emotion state.""" + try: + data = json.loads(msg.data) + self.emotion_state = data.get("state", "idle") + except json.JSONDecodeError: + pass + + def _health_cb(self, msg: String): + """Handle health status.""" + try: + data = json.loads(msg.data) + self.health_state = data.get("status", "ok") + except json.JSONDecodeError: + pass + + def _determine_state(self) -> str: + """Determine highest priority LED state.""" + if self.health_state == "error": + return "error" + + if self.battery_level < self.config["thresholds"]["low_battery_percent"]: + return "low_battery" + + if self.emotion_state == "celebrate": + return "celebrate" + + if self.balance_state == "follow": + return "follow" + + return "idle" + + def _get_brightness(self) -> float: + """Calculate brightness (0.0-1.0).""" + max_brightness = self.brightness_max + min_brightness = self.brightness_min + + if self.config["brightness"]["auto_dim"]: + # Simplified: use night brightness for now + brightness = self.config["brightness"]["night_brightness"] + else: + brightness = max_brightness + + return brightness / 255.0 + + def _create_pattern(self, state: str) -> LEDPattern: + """Create pattern for state.""" + pattern_config = self.config["patterns"].get(state, self.config["patterns"]["idle"]) + color_list = pattern_config.get("color") + pattern_type = pattern_config.get("pattern", "breathing") + speed = pattern_config.get("speed", 1.0) + + color = None + if color_list: + color = LEDColor(color_list[0], color_list[1], color_list[2]) + + if pattern_type == "breathing": + return BreathingPattern(color, self.num_leds, speed) + elif pattern_type == "pulse": + return PulsePattern(color, self.num_leds, speed) + elif pattern_type == "flash": + return FlashPattern(color, self.num_leds, speed) + elif pattern_type == "rainbow": + return RainbowPattern(color, self.num_leds, speed) + elif pattern_type == "chase": + return ChasePattern(color, self.num_leds, speed) + else: + return BreathingPattern(color, self.num_leds, speed) + + def _update_leds(self): + """Update LED display.""" + now = time.time() + dt = now - self.last_update + self.last_update = now + + # Determine state + state = self._determine_state() + + # Transition to new state + if state != self.last_state: + self.last_state = state + self.current_pattern = self._create_pattern(state) + self.transition_time = 0.0 + self.get_logger().info(f"LED state changed to: {state}") + + # Update pattern + brightness = self._get_brightness() + if self.current_pattern: + leds = self.current_pattern.update(dt, brightness) + else: + leds = [(0, 0, 0)] * self.num_leds + + # Publish state + state_msg = { + "state": self.last_state or "idle", + "battery": self.battery_level, + "brightness": int(brightness * 255), + "num_leds": self.num_leds, + "gpio_pin": self.gpio_pin, + } + self.led_state_pub.publish(String(data=json.dumps(state_msg))) + + +def main(args=None): + """Main entry point.""" + rclpy.init(args=args) + node = LEDControllerNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/jetson/ros2_ws/src/saltybot_led_controller/setup.cfg b/jetson/ros2_ws/src/saltybot_led_controller/setup.cfg new file mode 100644 index 0000000..f887986 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_led_controller/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/saltybot_led_controller +[bdist_wheel] +universal=0 diff --git a/jetson/ros2_ws/src/saltybot_led_controller/setup.py b/jetson/ros2_ws/src/saltybot_led_controller/setup.py new file mode 100644 index 0000000..34ee393 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_led_controller/setup.py @@ -0,0 +1,30 @@ +from setuptools import setup + +package_name = "saltybot_led_controller" + +setup( + name=package_name, + version="0.1.0", + packages=[package_name], + data_files=[ + ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]), + (f"share/{package_name}", ["package.xml"]), + (f"share/{package_name}/launch", ["launch/led_controller.launch.py"]), + (f"share/{package_name}/config", ["config/led_config.yaml"]), + ], + install_requires=["setuptools", "pyyaml"], + zip_safe=True, + maintainer="sl-mechanical", + maintainer_email="sl-mechanical@saltylab.local", + description=( + "NeoPixel LED controller: animated patterns for battery, balance, " + "social, emotion, and health states with smooth transitions" + ), + license="MIT", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "led_controller_node = saltybot_led_controller.led_controller_node:main", + ], + }, +)