diff --git a/robot/src/raspbot_v2/launch/robot.launch.py b/robot/src/raspbot_v2/launch/robot.launch.py index 014ff8f..732b562 100644 --- a/robot/src/raspbot_v2/launch/robot.launch.py +++ b/robot/src/raspbot_v2/launch/robot.launch.py @@ -30,10 +30,10 @@ def generate_launch_description(): description='Ultrasonic sensor publish rate (Hz)'), # ── Camera orientation arguments ────────────────────────────────── - DeclareLaunchArgument('pan_center_deg', default_value='90.0', - description='Pan angle at startup and shutdown (degrees)'), - DeclareLaunchArgument('tilt_center_deg', default_value='60.0', - description='Tilt angle at startup and shutdown (degrees)'), + DeclareLaunchArgument('pan_center_deg', default_value='0.0', + description='Pan angle at startup and shutdown (degrees, ROS convention)'), + DeclareLaunchArgument('tilt_center_deg', default_value='-15.0', + description='Tilt angle at startup and shutdown (degrees, ROS convention)'), # ── TF / robot description ──────────────────────────────────────── Node( @@ -95,4 +95,11 @@ def generate_launch_description(): output='screen', ), + Node( + package='raspbot_v2', + executable='led_controller', + name='led_controller', + output='screen', + ), + ]) diff --git a/robot/src/raspbot_v2/raspbot_v2/led_node.py b/robot/src/raspbot_v2/raspbot_v2/led_node.py new file mode 100644 index 0000000..1b3f549 --- /dev/null +++ b/robot/src/raspbot_v2/raspbot_v2/led_node.py @@ -0,0 +1,136 @@ +#!/usr/bin/env python3 +""" +LED bar controller node. + +Controls the 14-LED WS2812 bar on the Raspbot via two topics: + +Topics +------ +/led/color (std_msgs/ColorRGBA) + Set all LEDs to a solid RGB colour. r/g/b are 0.0–1.0; a=0.0 turns + the bar off. Publishing this topic also cancels any running effect. + +/led/effect (std_msgs/String) + Start a named light effect (runs until a new command arrives): + river | breathing | gradient | random_running | starlight | off + +Parameters +---------- +effect_speed float default 0.05 – delay between effect frames (s) +effect_color int default 0 – colour index for breathing effect + 0=red 1=green 2=blue 3=yellow + 4=purple 5=cyan 6=white +""" + +import threading + +import rclpy +from rclpy.node import Node +from std_msgs.msg import ColorRGBA, String + +VALID_EFFECTS = {'river', 'breathing', 'gradient', 'random_running', 'starlight', 'off'} + + +class LedNode(Node): + + def __init__(self): + super().__init__('led_controller') + + self.declare_parameter('effect_speed', 0.05) + self.declare_parameter('effect_color', 0) + + from raspbot_v2_interface.Raspbot_Lib import Raspbot, LightShow + self._Raspbot = Raspbot + self._LightShow = LightShow + self._bot = Raspbot() + + self._light_show: object | None = None + self._effect_thread: threading.Thread | None = None + + self.create_subscription(ColorRGBA, 'led/color', self._color_cb, 10) + self.create_subscription(String, 'led/effect', self._effect_cb, 10) + + self._bot.Ctrl_WQ2812_ALL(0, 0) + self.get_logger().info('LED controller started (14 LEDs, bar off).') + + # ------------------------------------------------------------------ + # Callbacks + # ------------------------------------------------------------------ + + def _color_cb(self, msg: ColorRGBA) -> None: + self._stop_effect() + if msg.a == 0.0: + self._bot.Ctrl_WQ2812_ALL(0, 0) + else: + r = max(0, min(255, int(msg.r * 255))) + g = max(0, min(255, int(msg.g * 255))) + b = max(0, min(255, int(msg.b * 255))) + self._bot.Ctrl_WQ2812_brightness_ALL(r, g, b) + + def _effect_cb(self, msg: String) -> None: + effect = msg.data.strip() + if effect not in VALID_EFFECTS: + self.get_logger().warn( + f"Unknown effect '{effect}'. Valid: {', '.join(sorted(VALID_EFFECTS))}" + ) + return + + self._stop_effect() + + if effect == 'off': + self._bot.Ctrl_WQ2812_ALL(0, 0) + return + + speed = self.get_parameter('effect_speed').value + color = self.get_parameter('effect_color').value + + ls = self._LightShow() + self._light_show = ls + + def _run(): + ls.execute_effect(effect, ls.MAX_TIME, speed, color) + + self._effect_thread = threading.Thread(target=_run, daemon=True, name=f'led_{effect}') + self._effect_thread.start() + self.get_logger().info(f"LED effect '{effect}' started.") + + # ------------------------------------------------------------------ + # Helpers + # ------------------------------------------------------------------ + + def _stop_effect(self) -> None: + if self._light_show is not None: + self._light_show.stop() + if self._effect_thread is not None and self._effect_thread.is_alive(): + self._effect_thread.join(timeout=2.0) + self._light_show = None + self._effect_thread = None + + # ------------------------------------------------------------------ + # Shutdown + # ------------------------------------------------------------------ + + def destroy_node(self) -> None: + self.get_logger().info('Shutting down LED controller — turning off bar.') + self._stop_effect() + try: + self._bot.Ctrl_WQ2812_ALL(0, 0) + except Exception: + pass + super().destroy_node() + + +def main(args=None): + rclpy.init(args=args) + node = LedNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/robot/src/raspbot_v2/setup.py b/robot/src/raspbot_v2/setup.py index f4bf635..e7c3816 100644 --- a/robot/src/raspbot_v2/setup.py +++ b/robot/src/raspbot_v2/setup.py @@ -19,6 +19,7 @@ setup( 'motor_controller = raspbot_v2.motor_controller_node:main', 'camera_orientation = raspbot_v2.camera_orientation_node:main', 'ultrasonic = raspbot_v2.ultrasonic_node:main', + 'led_controller = raspbot_v2.led_node:main', ], }, ) \ No newline at end of file diff --git a/webui/backend/main.py b/webui/backend/main.py index 412ea26..22c53ca 100644 --- a/webui/backend/main.py +++ b/webui/backend/main.py @@ -73,6 +73,10 @@ async def websocket_endpoint(ws: WebSocket): node.publish_cmd_vel(data.get('linear_x', 0.0), data.get('angular_z', 0.0)) elif msg_type == 'joint_command': node.publish_joint_command(data.get('pan', 0.0), data.get('tilt', 0.0)) + elif msg_type == 'led_color': + node.publish_led_color(data.get('r', 0.0), data.get('g', 0.0), data.get('b', 0.0), data.get('a', 1.0)) + elif msg_type == 'led_effect': + node.publish_led_effect(data.get('effect', 'off')) except WebSocketDisconnect: pass except Exception as e: diff --git a/webui/backend/ros_node.py b/webui/backend/ros_node.py index d1d072d..c74bf17 100644 --- a/webui/backend/ros_node.py +++ b/webui/backend/ros_node.py @@ -4,6 +4,7 @@ import rclpy from geometry_msgs.msg import Twist from rclpy.node import Node from sensor_msgs.msg import JointState, Range +from std_msgs.msg import ColorRGBA, String class RobotBridgeNode(Node): @@ -12,6 +13,8 @@ class RobotBridgeNode(Node): self._cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10) self._joint_cmd_pub = self.create_publisher(JointState, '/joint_command', 10) + self._led_color_pub = self.create_publisher(ColorRGBA, '/led/color', 10) + self._led_effect_pub = self.create_publisher(String, '/led/effect', 10) self.create_subscription(JointState, '/joint_states', self._on_joint_states, 10) self.create_subscription(Range, '/ultrasonic/range', self._on_ultrasonic, 10) @@ -37,6 +40,19 @@ class RobotBridgeNode(Node): msg.position = [float(pan), float(tilt)] self._joint_cmd_pub.publish(msg) + def publish_led_color(self, r: float, g: float, b: float, a: float) -> None: + msg = ColorRGBA() + msg.r = float(r) + msg.g = float(g) + msg.b = float(b) + msg.a = float(a) + self._led_color_pub.publish(msg) + + def publish_led_effect(self, effect: str) -> None: + msg = String() + msg.data = effect + self._led_effect_pub.publish(msg) + def _on_joint_states(self, msg: JointState) -> None: data = {'type': 'joint_states', 'positions': dict(zip(msg.name, msg.position))} for cb in self.on_joint_states_callbacks: diff --git a/webui/frontend/src/App.jsx b/webui/frontend/src/App.jsx index fdcb747..dc557bb 100644 --- a/webui/frontend/src/App.jsx +++ b/webui/frontend/src/App.jsx @@ -1,5 +1,6 @@ import { useState } from 'react' import { CameraControls } from './components/CameraControls.jsx' +import { LedControls } from './components/LedControls.jsx' import { RobotControls } from './components/RobotControls.jsx' import { VideoStream } from './components/VideoStream.jsx' import { useWebSocket } from './hooks/useWebSocket.js' @@ -34,6 +35,7 @@ export default function App() {
+
) diff --git a/webui/frontend/src/components/LedControls.jsx b/webui/frontend/src/components/LedControls.jsx new file mode 100644 index 0000000..d1dc57d --- /dev/null +++ b/webui/frontend/src/components/LedControls.jsx @@ -0,0 +1,93 @@ +import { useState } from 'react' + +const COLOURS = [ + { label: 'Red', r: 1, g: 0, b: 0 }, + { label: 'Green', r: 0, g: 1, b: 0 }, + { label: 'Blue', r: 0, g: 0, b: 1 }, + { label: 'Yellow', r: 1, g: 1, b: 0 }, + { label: 'Purple', r: 1, g: 0, b: 1 }, + { label: 'Cyan', r: 0, g: 1, b: 1 }, + { label: 'White', r: 1, g: 1, b: 1 }, +] + +const EFFECTS = ['river', 'breathing', 'gradient', 'random_running', 'starlight'] + +const swatch = (r, g, b) => + `rgb(${Math.round(r * 255)},${Math.round(g * 255)},${Math.round(b * 255)})` + +export function LedControls({ send }) { + const [active, setActive] = useState(null) // null=off, 'colour:N', 'effect:name' + + function sendColour(idx) { + const c = COLOURS[idx] + send({ type: 'led_color', r: c.r, g: c.g, b: c.b, a: 1.0 }) + setActive(`colour:${idx}`) + } + + function sendEffect(effect) { + send({ type: 'led_effect', effect }) + setActive(`effect:${effect}`) + } + + function sendOff() { + send({ type: 'led_color', r: 0, g: 0, b: 0, a: 0.0 }) + setActive(null) + } + + const sectionLabel = { + fontSize: 11, color: '#666', marginBottom: 4, textTransform: 'uppercase', letterSpacing: 1, + } + + const btn = (isActive) => ({ + background: isActive ? '#4ade80' : '#2a2a2a', + color: isActive ? '#000' : '#aaa', + border: '1px solid ' + (isActive ? '#4ade80' : '#444'), + padding: '3px 7px', borderRadius: 4, cursor: 'pointer', fontSize: 11, + }) + + return ( +
+
+ LEDs +
+ + {/* Solid colours */} +
+
Colour
+
+ {COLOURS.map((c, i) => ( + +
+
+ + {/* Effects */} +
+
Effect
+
+ {EFFECTS.map((e) => ( + + ))} +
+
+
+ ) +}