diff --git a/robot/src/raspbot_v2/urdf/raspbot_v2.urdf.xacro b/robot/src/raspbot_v2/urdf/raspbot_v2.urdf.xacro index 72e63d0..699d35a 100644 --- a/robot/src/raspbot_v2/urdf/raspbot_v2.urdf.xacro +++ b/robot/src/raspbot_v2/urdf/raspbot_v2.urdf.xacro @@ -111,7 +111,7 @@ - + @@ -126,7 +126,7 @@ - + diff --git a/webui/backend/main.py b/webui/backend/main.py index 22c53ca..62cba13 100644 --- a/webui/backend/main.py +++ b/webui/backend/main.py @@ -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() diff --git a/webui/backend/ros_node.py b/webui/backend/ros_node.py index c74bf17..acef017 100644 --- a/webui/backend/ros_node.py +++ b/webui/backend/ros_node.py @@ -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 diff --git a/webui/frontend/index.html b/webui/frontend/index.html index 0d53b52..bb789f5 100644 --- a/webui/frontend/index.html +++ b/webui/frontend/index.html @@ -7,6 +7,7 @@ diff --git a/webui/frontend/src/App.jsx b/webui/frontend/src/App.jsx index dc557bb..1856126 100644 --- a/webui/frontend/src/App.jsx +++ b/webui/frontend/src/App.jsx @@ -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 ( -
+
+ {/* Header */} -
+
RASPBOT - {range !== null && ( - - ▶ {range.toFixed(2)} m +
+ {range !== null + ? + ▶ {range.toFixed(2)} m + + : + } + + {connected ? '● ROS' : '○ connecting…'} - )} +
{/* Video */} -
+
{/* Controls */} -
+
- +
+
) } + +function Waiting({ topic }) { + return ( + + waiting for {topic} + + ) +} diff --git a/webui/frontend/src/components/CameraControls.jsx b/webui/frontend/src/components/CameraControls.jsx index ba8a3f2..9da2ff1 100644 --- a/webui/frontend/src/components/CameraControls.jsx +++ b/webui/frontend/src/components/CameraControls.jsx @@ -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 ( +
+ waiting for {topic} +
+ ) +} + +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 ( -
+
Camera
+ {/* Pan */}
Pan {toDeg(jointStates?.pan ?? pan)}°
- { const v = +e.target.value; setPan(v); sendCmd(v, tilt) }} - /> + {panLimits + ? { const v = +e.target.value; setPan(v); sendCmd(v, tilt) }} + /> + : + }
+ {/* Tilt */}
Tilt {toDeg(jointStates?.tilt ?? tilt)}°
- { const v = +e.target.value; setTilt(v); sendCmd(pan, v) }} - /> + {tiltLimits + ? { const v = +e.target.value; setTilt(v); sendCmd(pan, v) }} + /> + : + }