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:
@@ -111,7 +111,7 @@
|
||||
<child link="camera_pan_link"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<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>
|
||||
|
||||
<!-- Tilt joint (pitch, around Y) -->
|
||||
@@ -126,7 +126,7 @@
|
||||
<child link="camera_link"/>
|
||||
<axis xyz="0 1 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>
|
||||
|
||||
</robot>
|
||||
|
||||
@@ -54,6 +54,7 @@ async def lifespan(app: FastAPI):
|
||||
node = start_ros()
|
||||
node.on_joint_states_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
|
||||
stop_ros()
|
||||
|
||||
@@ -65,6 +66,10 @@ app = FastAPI(lifespan=lifespan)
|
||||
async def websocket_endpoint(ws: WebSocket):
|
||||
await manager.connect(ws)
|
||||
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:
|
||||
while True:
|
||||
data = await ws.receive_json()
|
||||
|
||||
+64
-17
@@ -1,11 +1,39 @@
|
||||
import threading
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
import rclpy
|
||||
from geometry_msgs.msg import Twist
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
|
||||
from sensor_msgs.msg import JointState, Range
|
||||
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):
|
||||
def __init__(self):
|
||||
@@ -16,22 +44,25 @@ class RobotBridgeNode(Node):
|
||||
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)
|
||||
self.create_subscription(JointState, '/joint_states', self._on_joint_states, 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_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:
|
||||
# print(f'Publishing cmd_vel: linear_x={linear_x}, angular_z={angular_z}')
|
||||
msg = Twist()
|
||||
msg.linear.x = float(linear_x)
|
||||
msg.angular.z = float(angular_z)
|
||||
try:
|
||||
self._cmd_vel_pub.publish(msg)
|
||||
except Exception as e:
|
||||
print('Failed to publish cmd_vel:', e)
|
||||
# self._cmd_vel_pub.publish(msg)
|
||||
self._cmd_vel_pub.publish(msg)
|
||||
|
||||
def publish_joint_command(self, pan: float, tilt: float) -> None:
|
||||
msg = JointState()
|
||||
@@ -41,17 +72,22 @@ class RobotBridgeNode(Node):
|
||||
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)
|
||||
msg = ColorRGBA(r=float(r), g=float(g), b=float(b), 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)
|
||||
self._led_effect_pub.publish(String(data=effect))
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Accessors
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
def get_joint_limits(self) -> dict | None:
|
||||
return self._joint_limits
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Subscribers
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
def _on_joint_states(self, msg: JointState) -> None:
|
||||
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:
|
||||
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
|
||||
_spin_thread: threading.Thread | None = None
|
||||
@@ -78,7 +125,7 @@ def start_ros() -> 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
|
||||
|
||||
|
||||
|
||||
@@ -7,6 +7,7 @@
|
||||
<style>
|
||||
*, *::before, *::after { box-sizing: border-box; margin: 0; padding: 0; }
|
||||
body { background: #111; color: #eee; font-family: monospace; overflow: hidden; }
|
||||
@media (max-width: 640px) { body { overflow-y: auto; } }
|
||||
</style>
|
||||
</head>
|
||||
<body>
|
||||
|
||||
+32
-14
@@ -6,37 +6,55 @@ import { VideoStream } from './components/VideoStream.jsx'
|
||||
import { useWebSocket } from './hooks/useWebSocket.js'
|
||||
|
||||
export default function App() {
|
||||
const [jointStates, setJointStates] = useState({})
|
||||
const [range, setRange] = useState(null)
|
||||
const [jointStates, setJointStates] = useState({})
|
||||
const [range, setRange] = useState(null)
|
||||
const [jointLimits, setJointLimits] = useState(null)
|
||||
|
||||
const send = useWebSocket((msg) => {
|
||||
if (msg.type === 'joint_states') setJointStates(msg.positions)
|
||||
if (msg.type === 'ultrasonic') setRange(msg.range)
|
||||
const { send, connected } = useWebSocket((msg) => {
|
||||
if (msg.type === 'joint_states') { console.log('[joint_states]', msg.positions); setJointStates(msg.positions) }
|
||||
if (msg.type === 'ultrasonic') setRange(msg.range)
|
||||
if (msg.type === 'joint_limits') setJointLimits(msg.limits)
|
||||
})
|
||||
|
||||
return (
|
||||
<div style={{ display: 'flex', flexDirection: 'column', height: '100vh', padding: 8, gap: 8 }}>
|
||||
<div className="app-layout">
|
||||
|
||||
{/* 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>
|
||||
{range !== null && (
|
||||
<span style={{ fontSize: 12, color: range < 0.3 ? '#f87171' : '#facc15' }}>
|
||||
▶ {range.toFixed(2)} m
|
||||
<div style={{ display: 'flex', alignItems: 'center', gap: 12 }}>
|
||||
{range !== null
|
||||
? <span style={{ fontSize: 12, color: range < 0.3 ? '#f87171' : '#facc15' }}>
|
||||
▶ {range.toFixed(2)} m
|
||||
</span>
|
||||
: <Waiting topic="/ultrasonic/range" />
|
||||
}
|
||||
<span style={{ fontSize: 11, color: connected ? '#4ade80' : '#f87171' }}>
|
||||
{connected ? '● ROS' : '○ connecting…'}
|
||||
</span>
|
||||
)}
|
||||
</div>
|
||||
</div>
|
||||
|
||||
{/* Video */}
|
||||
<div style={{ flexShrink: 0 }}>
|
||||
<div className="app-video">
|
||||
<VideoStream />
|
||||
</div>
|
||||
|
||||
{/* Controls */}
|
||||
<div style={{ display: 'flex', justifyContent: 'space-between', alignItems: 'flex-start', gap: 16, flex: 1 }}>
|
||||
<div className="controls-row">
|
||||
<RobotControls send={send} />
|
||||
<CameraControls send={send} jointStates={jointStates} />
|
||||
<CameraControls send={send} jointStates={jointStates} limits={jointLimits} />
|
||||
<LedControls send={send} />
|
||||
</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,60 +1,76 @@
|
||||
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)
|
||||
|
||||
export function CameraControls({ send, jointStates }) {
|
||||
const [pan, setPan] = useState(0)
|
||||
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 [tilt, setTilt] = useState(0)
|
||||
|
||||
const sendCmd = (p, t) => send({ type: 'joint_command', pan: p, tilt: t })
|
||||
|
||||
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 panLimits = limits?.pan
|
||||
const tiltLimits = limits?.tilt
|
||||
|
||||
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 }}>
|
||||
Camera
|
||||
</div>
|
||||
|
||||
{/* Pan */}
|
||||
<div style={row}>
|
||||
<div style={label}>
|
||||
<span>Pan</span>
|
||||
<span style={{ color: '#4ade80' }}>{toDeg(jointStates?.pan ?? pan)}°</span>
|
||||
</div>
|
||||
<input
|
||||
type="range"
|
||||
min={PAN_MIN} max={PAN_MAX} step={0.01}
|
||||
value={pan}
|
||||
style={{ accentColor: '#4ade80', width: '100%' }}
|
||||
onChange={(e) => { const v = +e.target.value; setPan(v); sendCmd(v, tilt) }}
|
||||
/>
|
||||
{panLimits
|
||||
? <input
|
||||
type="range"
|
||||
min={panLimits.lower} max={panLimits.upper} step={0.01}
|
||||
value={pan}
|
||||
style={{ accentColor: '#4ade80', width: '100%' }}
|
||||
onChange={(e) => { const v = +e.target.value; setPan(v); sendCmd(v, tilt) }}
|
||||
/>
|
||||
: <Waiting topic="/robot_description" />
|
||||
}
|
||||
</div>
|
||||
|
||||
{/* Tilt */}
|
||||
<div style={row}>
|
||||
<div style={label}>
|
||||
<span>Tilt</span>
|
||||
<span style={{ color: '#4ade80' }}>{toDeg(jointStates?.tilt ?? tilt)}°</span>
|
||||
</div>
|
||||
<input
|
||||
type="range"
|
||||
min={TILT_MIN} max={TILT_MAX} step={0.01}
|
||||
value={tilt}
|
||||
style={{ accentColor: '#4ade80', width: '100%' }}
|
||||
onChange={(e) => { const v = +e.target.value; setTilt(v); sendCmd(pan, v) }}
|
||||
/>
|
||||
{tiltLimits
|
||||
? <input
|
||||
type="range"
|
||||
min={tiltLimits.lower} max={tiltLimits.upper} step={0.01}
|
||||
value={tilt}
|
||||
style={{ accentColor: '#4ade80', width: '100%' }}
|
||||
onChange={(e) => { const v = +e.target.value; setTilt(v); sendCmd(pan, v) }}
|
||||
/>
|
||||
: <Waiting topic="/robot_description" />
|
||||
}
|
||||
</div>
|
||||
|
||||
<button
|
||||
disabled={!panLimits || !tiltLimits}
|
||||
onClick={() => { setPan(0); setTilt(0); sendCmd(0, 0) }}
|
||||
style={{
|
||||
background: '#2a2a2a', color: '#aaa', border: '1px solid #444',
|
||||
padding: '4px 8px', borderRadius: 4, cursor: 'pointer', fontSize: 12,
|
||||
opacity: (!panLimits || !tiltLimits) ? 0.4 : 1,
|
||||
}}
|
||||
>
|
||||
Centre
|
||||
|
||||
@@ -46,7 +46,7 @@ export function LedControls({ send }) {
|
||||
})
|
||||
|
||||
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 }}>
|
||||
LEDs
|
||||
</div>
|
||||
|
||||
@@ -72,7 +72,7 @@ export function VideoStream() {
|
||||
autoPlay
|
||||
playsInline
|
||||
muted
|
||||
style={{ width: '100%', maxHeight: '60vh', background: '#000', display: 'block' }}
|
||||
style={{ background: '#000' }}
|
||||
/>
|
||||
)
|
||||
}
|
||||
|
||||
@@ -1,17 +1,23 @@
|
||||
import { useCallback, useEffect, useRef } from 'react'
|
||||
import { useCallback, useEffect, useRef, useState } from 'react'
|
||||
|
||||
export function useWebSocket(onMessage) {
|
||||
const wsRef = useRef(null)
|
||||
const onMessageRef = useRef(onMessage)
|
||||
onMessageRef.current = onMessage
|
||||
|
||||
const [connected, setConnected] = useState(false)
|
||||
|
||||
useEffect(() => {
|
||||
const ws = new WebSocket(`ws://${window.location.host}/ws`)
|
||||
wsRef.current = ws
|
||||
|
||||
ws.onopen = () => setConnected(true)
|
||||
ws.onclose = () => setConnected(false)
|
||||
|
||||
ws.onmessage = (e) => {
|
||||
try { onMessageRef.current(JSON.parse(e.data)) } catch {}
|
||||
}
|
||||
|
||||
let cleanedUp = false
|
||||
ws.onerror = (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) {
|
||||
wsRef.current.send(JSON.stringify(data))
|
||||
}
|
||||
}, [])
|
||||
|
||||
return { send, connected }
|
||||
}
|
||||
|
||||
@@ -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,4 +1,5 @@
|
||||
import { createRoot } from 'react-dom/client'
|
||||
import './index.css'
|
||||
import App from './App.jsx'
|
||||
|
||||
createRoot(document.getElementById('root')).render(<App />)
|
||||
|
||||
Reference in New Issue
Block a user