Read constraints from the published URDF rather than hard code them.

Also make the webui more responsive for small screens.
This commit is contained in:
2026-05-01 14:20:59 +00:00
parent ee71c80edd
commit 16ee4b8296
11 changed files with 221 additions and 60 deletions
@@ -111,7 +111,7 @@
<child link="camera_pan_link"/> <child link="camera_pan_link"/>
<axis xyz="0 0 1"/> <axis xyz="0 0 1"/>
<origin xyz="0 0 0.015" rpy="0 0 0"/> <origin xyz="0 0 0.015" rpy="0 0 0"/>
<limit lower="-1.0472" upper="1.5708" effort="1.0" velocity="1.0"/> <limit lower="-1.5708" upper="1.5708" effort="1.0" velocity="1.0"/>
</joint> </joint>
<!-- Tilt joint (pitch, around Y) --> <!-- Tilt joint (pitch, around Y) -->
@@ -126,7 +126,7 @@
<child link="camera_link"/> <child link="camera_link"/>
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
<origin xyz="0.02 0 0" rpy="0 0 0"/> <origin xyz="0.02 0 0" rpy="0 0 0"/>
<limit lower="-1.5708" upper="0.7854" effort="1.0" velocity="1.0"/> <limit lower="-1.0472" upper="0.7854" effort="1.0" velocity="1.0"/>
</joint> </joint>
</robot> </robot>
+5
View File
@@ -54,6 +54,7 @@ async def lifespan(app: FastAPI):
node = start_ros() node = start_ros()
node.on_joint_states_callbacks.append(manager.broadcast_from_thread) node.on_joint_states_callbacks.append(manager.broadcast_from_thread)
node.on_ultrasonic_callbacks.append(manager.broadcast_from_thread) node.on_ultrasonic_callbacks.append(manager.broadcast_from_thread)
node.on_joint_limits_callbacks.append(manager.broadcast_from_thread)
yield yield
stop_ros() stop_ros()
@@ -65,6 +66,10 @@ app = FastAPI(lifespan=lifespan)
async def websocket_endpoint(ws: WebSocket): async def websocket_endpoint(ws: WebSocket):
await manager.connect(ws) await manager.connect(ws)
node = get_node() node = get_node()
# Send cached joint limits immediately so the client doesn't wait for the next publish
cached_limits = node.get_joint_limits()
if cached_limits:
await ws.send_json({'type': 'joint_limits', 'limits': cached_limits})
try: try:
while True: while True:
data = await ws.receive_json() data = await ws.receive_json()
+61 -14
View File
@@ -1,11 +1,39 @@
import threading import threading
import xml.etree.ElementTree as ET
import rclpy import rclpy
from geometry_msgs.msg import Twist from geometry_msgs.msg import Twist
from rclpy.node import Node from rclpy.node import Node
from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
from sensor_msgs.msg import JointState, Range from sensor_msgs.msg import JointState, Range
from std_msgs.msg import ColorRGBA, String from std_msgs.msg import ColorRGBA, String
_TRANSIENT_LOCAL_QOS = QoSProfile(
reliability=ReliabilityPolicy.RELIABLE,
durability=DurabilityPolicy.TRANSIENT_LOCAL,
depth=1,
)
def _parse_joint_limits(urdf_xml: str) -> dict:
"""Extract lower/upper limits for pan and tilt joints from a URDF string."""
try:
root = ET.fromstring(urdf_xml)
limits = {}
for joint in root.findall('joint'):
name = joint.get('name')
if name in ('pan', 'tilt'):
el = joint.find('limit')
if el is not None:
limits[name] = {
'lower': float(el.get('lower', 0)),
'upper': float(el.get('upper', 0)),
}
return limits
except Exception as e:
print(f'Failed to parse URDF joint limits: {e}')
return {}
class RobotBridgeNode(Node): class RobotBridgeNode(Node):
def __init__(self): def __init__(self):
@@ -18,20 +46,23 @@ class RobotBridgeNode(Node):
self.create_subscription(JointState, '/joint_states', self._on_joint_states, 10) self.create_subscription(JointState, '/joint_states', self._on_joint_states, 10)
self.create_subscription(Range, '/ultrasonic/range', self._on_ultrasonic, 10) self.create_subscription(Range, '/ultrasonic/range', self._on_ultrasonic, 10)
self.create_subscription(String, '/robot_description', self._on_robot_description,
_TRANSIENT_LOCAL_QOS)
self.on_joint_states_callbacks: list = [] self.on_joint_states_callbacks: list = []
self.on_ultrasonic_callbacks: list = [] self.on_ultrasonic_callbacks: list = []
self.on_joint_limits_callbacks: list = []
self._joint_limits: dict | None = None
# ------------------------------------------------------------------
# Publishers
# ------------------------------------------------------------------
def publish_cmd_vel(self, linear_x: float, angular_z: float) -> None: def publish_cmd_vel(self, linear_x: float, angular_z: float) -> None:
# print(f'Publishing cmd_vel: linear_x={linear_x}, angular_z={angular_z}')
msg = Twist() msg = Twist()
msg.linear.x = float(linear_x) msg.linear.x = float(linear_x)
msg.angular.z = float(angular_z) msg.angular.z = float(angular_z)
try:
self._cmd_vel_pub.publish(msg) self._cmd_vel_pub.publish(msg)
except Exception as e:
print('Failed to publish cmd_vel:', e)
# self._cmd_vel_pub.publish(msg)
def publish_joint_command(self, pan: float, tilt: float) -> None: def publish_joint_command(self, pan: float, tilt: float) -> None:
msg = JointState() msg = JointState()
@@ -41,17 +72,22 @@ class RobotBridgeNode(Node):
self._joint_cmd_pub.publish(msg) self._joint_cmd_pub.publish(msg)
def publish_led_color(self, r: float, g: float, b: float, a: float) -> None: def publish_led_color(self, r: float, g: float, b: float, a: float) -> None:
msg = ColorRGBA() msg = ColorRGBA(r=float(r), g=float(g), b=float(b), a=float(a))
msg.r = float(r)
msg.g = float(g)
msg.b = float(b)
msg.a = float(a)
self._led_color_pub.publish(msg) self._led_color_pub.publish(msg)
def publish_led_effect(self, effect: str) -> None: def publish_led_effect(self, effect: str) -> None:
msg = String() self._led_effect_pub.publish(String(data=effect))
msg.data = effect
self._led_effect_pub.publish(msg) # ------------------------------------------------------------------
# Accessors
# ------------------------------------------------------------------
def get_joint_limits(self) -> dict | None:
return self._joint_limits
# ------------------------------------------------------------------
# Subscribers
# ------------------------------------------------------------------
def _on_joint_states(self, msg: JointState) -> None: def _on_joint_states(self, msg: JointState) -> None:
data = {'type': 'joint_states', 'positions': dict(zip(msg.name, msg.position))} data = {'type': 'joint_states', 'positions': dict(zip(msg.name, msg.position))}
@@ -63,6 +99,17 @@ class RobotBridgeNode(Node):
for cb in self.on_ultrasonic_callbacks: for cb in self.on_ultrasonic_callbacks:
cb(data) cb(data)
def _on_robot_description(self, msg: String) -> None:
limits = _parse_joint_limits(msg.data)
self.get_logger().info(f'Received /robot_description, parsed limits: {limits}')
if not limits:
self.get_logger().warn('No pan/tilt limits found in URDF — check joint names.')
return
self._joint_limits = limits
data = {'type': 'joint_limits', 'limits': limits}
for cb in self.on_joint_limits_callbacks:
cb(data)
_node: RobotBridgeNode | None = None _node: RobotBridgeNode | None = None
_spin_thread: threading.Thread | None = None _spin_thread: threading.Thread | None = None
@@ -78,7 +125,7 @@ def start_ros() -> RobotBridgeNode:
def get_node() -> RobotBridgeNode: def get_node() -> RobotBridgeNode:
assert _node is not None, "ROS node not started" assert _node is not None, 'ROS node not started'
return _node return _node
+1
View File
@@ -7,6 +7,7 @@
<style> <style>
*, *::before, *::after { box-sizing: border-box; margin: 0; padding: 0; } *, *::before, *::after { box-sizing: border-box; margin: 0; padding: 0; }
body { background: #111; color: #eee; font-family: monospace; overflow: hidden; } body { background: #111; color: #eee; font-family: monospace; overflow: hidden; }
@media (max-width: 640px) { body { overflow-y: auto; } }
</style> </style>
</head> </head>
<body> <body>
+28 -10
View File
@@ -8,35 +8,53 @@ import { useWebSocket } from './hooks/useWebSocket.js'
export default function App() { export default function App() {
const [jointStates, setJointStates] = useState({}) const [jointStates, setJointStates] = useState({})
const [range, setRange] = useState(null) const [range, setRange] = useState(null)
const [jointLimits, setJointLimits] = useState(null)
const send = useWebSocket((msg) => { const { send, connected } = useWebSocket((msg) => {
if (msg.type === 'joint_states') setJointStates(msg.positions) if (msg.type === 'joint_states') { console.log('[joint_states]', msg.positions); setJointStates(msg.positions) }
if (msg.type === 'ultrasonic') setRange(msg.range) if (msg.type === 'ultrasonic') setRange(msg.range)
if (msg.type === 'joint_limits') setJointLimits(msg.limits)
}) })
return ( return (
<div style={{ display: 'flex', flexDirection: 'column', height: '100vh', padding: 8, gap: 8 }}> <div className="app-layout">
{/* Header */} {/* Header */}
<div style={{ display: 'flex', alignItems: 'center', justifyContent: 'space-between', flexShrink: 0 }}> <div className="app-header">
<span style={{ fontSize: 13, color: '#4ade80', letterSpacing: 2 }}>RASPBOT</span> <span style={{ fontSize: 13, color: '#4ade80', letterSpacing: 2 }}>RASPBOT</span>
{range !== null && ( <div style={{ display: 'flex', alignItems: 'center', gap: 12 }}>
<span style={{ fontSize: 12, color: range < 0.3 ? '#f87171' : '#facc15' }}> {range !== null
? <span style={{ fontSize: 12, color: range < 0.3 ? '#f87171' : '#facc15' }}>
&#9654; {range.toFixed(2)} m &#9654; {range.toFixed(2)} m
</span> </span>
)} : <Waiting topic="/ultrasonic/range" />
}
<span style={{ fontSize: 11, color: connected ? '#4ade80' : '#f87171' }}>
{connected ? '● ROS' : '○ connecting…'}
</span>
</div>
</div> </div>
{/* Video */} {/* Video */}
<div style={{ flexShrink: 0 }}> <div className="app-video">
<VideoStream /> <VideoStream />
</div> </div>
{/* Controls */} {/* Controls */}
<div style={{ display: 'flex', justifyContent: 'space-between', alignItems: 'flex-start', gap: 16, flex: 1 }}> <div className="controls-row">
<RobotControls send={send} /> <RobotControls send={send} />
<CameraControls send={send} jointStates={jointStates} /> <CameraControls send={send} jointStates={jointStates} limits={jointLimits} />
<LedControls send={send} /> <LedControls send={send} />
</div> </div>
</div> </div>
) )
} }
function Waiting({ topic }) {
return (
<span style={{ fontSize: 11, color: '#555', fontStyle: 'italic' }}>
waiting for <span style={{ fontFamily: 'monospace', color: '#666' }}>{topic}</span>
</span>
)
}
@@ -1,13 +1,16 @@
import { useState } from 'react' import { useState } from 'react'
const PAN_MIN = -Math.PI / 3 // -60°
const PAN_MAX = Math.PI / 2 // +90°
const TILT_MIN = -Math.PI / 2 // -90°
const TILT_MAX = Math.PI / 4 // +45°
const toDeg = (r) => Math.round(r * 180 / Math.PI) const toDeg = (r) => Math.round(r * 180 / Math.PI)
export function CameraControls({ send, jointStates }) { function Waiting({ topic }) {
return (
<div style={{ fontSize: 11, color: '#555', fontStyle: 'italic', padding: '4px 0' }}>
waiting for <span style={{ fontFamily: 'monospace', color: '#666' }}>{topic}</span>
</div>
)
}
export function CameraControls({ send, jointStates, limits }) {
const [pan, setPan] = useState(0) const [pan, setPan] = useState(0)
const [tilt, setTilt] = useState(0) const [tilt, setTilt] = useState(0)
@@ -16,45 +19,58 @@ export function CameraControls({ send, jointStates }) {
const row = { display: 'flex', flexDirection: 'column', gap: 4 } const row = { display: 'flex', flexDirection: 'column', gap: 4 }
const label = { fontSize: 12, color: '#888', display: 'flex', justifyContent: 'space-between' } const label = { fontSize: 12, color: '#888', display: 'flex', justifyContent: 'space-between' }
const panLimits = limits?.pan
const tiltLimits = limits?.tilt
return ( return (
<div style={{ display: 'flex', flexDirection: 'column', gap: 10, minWidth: 180 }}> <div className="control-panel" style={{ display: 'flex', flexDirection: 'column', gap: 10 }}>
<div style={{ fontSize: 12, color: '#555', borderBottom: '1px solid #2a2a2a', paddingBottom: 4 }}> <div style={{ fontSize: 12, color: '#555', borderBottom: '1px solid #2a2a2a', paddingBottom: 4 }}>
Camera Camera
</div> </div>
{/* Pan */}
<div style={row}> <div style={row}>
<div style={label}> <div style={label}>
<span>Pan</span> <span>Pan</span>
<span style={{ color: '#4ade80' }}>{toDeg(jointStates?.pan ?? pan)}°</span> <span style={{ color: '#4ade80' }}>{toDeg(jointStates?.pan ?? pan)}°</span>
</div> </div>
<input {panLimits
? <input
type="range" type="range"
min={PAN_MIN} max={PAN_MAX} step={0.01} min={panLimits.lower} max={panLimits.upper} step={0.01}
value={pan} value={pan}
style={{ accentColor: '#4ade80', width: '100%' }} style={{ accentColor: '#4ade80', width: '100%' }}
onChange={(e) => { const v = +e.target.value; setPan(v); sendCmd(v, tilt) }} onChange={(e) => { const v = +e.target.value; setPan(v); sendCmd(v, tilt) }}
/> />
: <Waiting topic="/robot_description" />
}
</div> </div>
{/* Tilt */}
<div style={row}> <div style={row}>
<div style={label}> <div style={label}>
<span>Tilt</span> <span>Tilt</span>
<span style={{ color: '#4ade80' }}>{toDeg(jointStates?.tilt ?? tilt)}°</span> <span style={{ color: '#4ade80' }}>{toDeg(jointStates?.tilt ?? tilt)}°</span>
</div> </div>
<input {tiltLimits
? <input
type="range" type="range"
min={TILT_MIN} max={TILT_MAX} step={0.01} min={tiltLimits.lower} max={tiltLimits.upper} step={0.01}
value={tilt} value={tilt}
style={{ accentColor: '#4ade80', width: '100%' }} style={{ accentColor: '#4ade80', width: '100%' }}
onChange={(e) => { const v = +e.target.value; setTilt(v); sendCmd(pan, v) }} onChange={(e) => { const v = +e.target.value; setTilt(v); sendCmd(pan, v) }}
/> />
: <Waiting topic="/robot_description" />
}
</div> </div>
<button <button
disabled={!panLimits || !tiltLimits}
onClick={() => { setPan(0); setTilt(0); sendCmd(0, 0) }} onClick={() => { setPan(0); setTilt(0); sendCmd(0, 0) }}
style={{ style={{
background: '#2a2a2a', color: '#aaa', border: '1px solid #444', background: '#2a2a2a', color: '#aaa', border: '1px solid #444',
padding: '4px 8px', borderRadius: 4, cursor: 'pointer', fontSize: 12, padding: '4px 8px', borderRadius: 4, cursor: 'pointer', fontSize: 12,
opacity: (!panLimits || !tiltLimits) ? 0.4 : 1,
}} }}
> >
Centre Centre
@@ -46,7 +46,7 @@ export function LedControls({ send }) {
}) })
return ( return (
<div style={{ display: 'flex', flexDirection: 'column', gap: 10, minWidth: 180 }}> <div className="control-panel" style={{ display: 'flex', flexDirection: 'column', gap: 10 }}>
<div style={{ fontSize: 12, color: '#555', borderBottom: '1px solid #2a2a2a', paddingBottom: 4 }}> <div style={{ fontSize: 12, color: '#555', borderBottom: '1px solid #2a2a2a', paddingBottom: 4 }}>
LEDs LEDs
</div> </div>
@@ -72,7 +72,7 @@ export function VideoStream() {
autoPlay autoPlay
playsInline playsInline
muted muted
style={{ width: '100%', maxHeight: '60vh', background: '#000', display: 'block' }} style={{ background: '#000' }}
/> />
) )
} }
+10 -2
View File
@@ -1,17 +1,23 @@
import { useCallback, useEffect, useRef } from 'react' import { useCallback, useEffect, useRef, useState } from 'react'
export function useWebSocket(onMessage) { export function useWebSocket(onMessage) {
const wsRef = useRef(null) const wsRef = useRef(null)
const onMessageRef = useRef(onMessage) const onMessageRef = useRef(onMessage)
onMessageRef.current = onMessage onMessageRef.current = onMessage
const [connected, setConnected] = useState(false)
useEffect(() => { useEffect(() => {
const ws = new WebSocket(`ws://${window.location.host}/ws`) const ws = new WebSocket(`ws://${window.location.host}/ws`)
wsRef.current = ws wsRef.current = ws
ws.onopen = () => setConnected(true)
ws.onclose = () => setConnected(false)
ws.onmessage = (e) => { ws.onmessage = (e) => {
try { onMessageRef.current(JSON.parse(e.data)) } catch {} try { onMessageRef.current(JSON.parse(e.data)) } catch {}
} }
let cleanedUp = false let cleanedUp = false
ws.onerror = (e) => { ws.onerror = (e) => {
if (!cleanedUp) console.error('WebSocket error', e) if (!cleanedUp) console.error('WebSocket error', e)
@@ -23,9 +29,11 @@ export function useWebSocket(onMessage) {
} }
}, []) }, [])
return useCallback((data) => { const send = useCallback((data) => {
if (wsRef.current?.readyState === WebSocket.OPEN) { if (wsRef.current?.readyState === WebSocket.OPEN) {
wsRef.current.send(JSON.stringify(data)) wsRef.current.send(JSON.stringify(data))
} }
}, []) }, [])
return { send, connected }
} }
+65
View File
@@ -0,0 +1,65 @@
/* ── App shell ────────────────────────────────────────────────────────────── */
.app-layout {
display: flex;
flex-direction: column;
height: 100vh;
padding: 8px;
gap: 8px;
overflow: hidden;
}
.app-header {
display: flex;
align-items: center;
justify-content: space-between;
flex-shrink: 0;
}
.app-video {
flex-shrink: 0;
}
.app-video video {
width: 100%;
max-height: 55vh;
background: #000;
display: block;
}
/* ── Controls row ─────────────────────────────────────────────────────────── */
.controls-row {
display: flex;
justify-content: space-between;
align-items: flex-start;
gap: 16px;
flex: 1;
min-height: 0;
}
.control-panel {
min-width: 160px;
}
/* ── Mobile ───────────────────────────────────────────────────────────────── */
@media (max-width: 640px) {
.app-layout {
height: auto;
min-height: 100dvh;
overflow-y: auto;
}
.app-video video {
max-height: 52vw; /* landscape-ish crop on portrait phone */
}
.controls-row {
flex-direction: column;
flex: none;
align-items: stretch;
}
.control-panel {
width: 100%;
min-width: unset;
}
}
+1
View File
@@ -1,4 +1,5 @@
import { createRoot } from 'react-dom/client' import { createRoot } from 'react-dom/client'
import './index.css'
import App from './App.jsx' import App from './App.jsx'
createRoot(document.getElementById('root')).render(<App />) createRoot(document.getElementById('root')).render(<App />)