Add contol node for the LEDs
This commit is contained in:
@@ -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',
|
||||
),
|
||||
|
||||
])
|
||||
|
||||
@@ -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()
|
||||
@@ -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',
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -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:
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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() {
|
||||
<div style={{ display: 'flex', justifyContent: 'space-between', alignItems: 'flex-start', gap: 16, flex: 1 }}>
|
||||
<RobotControls send={send} />
|
||||
<CameraControls send={send} jointStates={jointStates} />
|
||||
<LedControls send={send} />
|
||||
</div>
|
||||
</div>
|
||||
)
|
||||
|
||||
@@ -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 (
|
||||
<div style={{ display: 'flex', flexDirection: 'column', gap: 10, minWidth: 180 }}>
|
||||
<div style={{ fontSize: 12, color: '#555', borderBottom: '1px solid #2a2a2a', paddingBottom: 4 }}>
|
||||
LEDs
|
||||
</div>
|
||||
|
||||
{/* Solid colours */}
|
||||
<div>
|
||||
<div style={sectionLabel}>Colour</div>
|
||||
<div style={{ display: 'flex', flexWrap: 'wrap', gap: 5 }}>
|
||||
{COLOURS.map((c, i) => (
|
||||
<button
|
||||
key={c.label}
|
||||
title={c.label}
|
||||
onClick={() => sendColour(i)}
|
||||
style={{
|
||||
width: 22, height: 22, borderRadius: 4, cursor: 'pointer', border: 'none',
|
||||
background: swatch(c.r, c.g, c.b),
|
||||
outline: active === `colour:${i}` ? '2px solid #4ade80' : '2px solid transparent',
|
||||
outlineOffset: 2,
|
||||
}}
|
||||
/>
|
||||
))}
|
||||
<button onClick={sendOff} title="Off" style={{
|
||||
width: 22, height: 22, borderRadius: 4, cursor: 'pointer',
|
||||
background: '#111', border: '1px solid #444',
|
||||
outline: active === null ? '2px solid #4ade80' : '2px solid transparent',
|
||||
outlineOffset: 2, fontSize: 10, color: '#666',
|
||||
}}>✕</button>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
{/* Effects */}
|
||||
<div>
|
||||
<div style={sectionLabel}>Effect</div>
|
||||
<div style={{ display: 'flex', flexWrap: 'wrap', gap: 4 }}>
|
||||
{EFFECTS.map((e) => (
|
||||
<button key={e} onClick={() => sendEffect(e)} style={btn(active === `effect:${e}`)}>
|
||||
{e.replace('_', ' ')}
|
||||
</button>
|
||||
))}
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
)
|
||||
}
|
||||
Reference in New Issue
Block a user