feat(webui): gamepad teleoperation panel with dual-stick control (Issue #319)

Features:
- Virtual dual-stick gamepad interface
  * Left stick: linear velocity (forward/backward)
  * Right stick: angular velocity (turn left/right)
- Canvas-based rendering with real-time visual feedback
- Velocity vector visualization on sticks
- 10% deadzone on both axes for dead-stick detection

- WASD keyboard fallback for manual driving
  * W/S: forward/backward
  * A/D: turn left/right
  * Smooth blending between gamepad and keyboard input

- Speed limiter slider (0-100%)
  * Real-time control of max velocity
  * Disabled during e-stop state

- Emergency stop button (e-stop)
  * Immediate velocity zeroing
  * Visual status indicator (red when active)
  * Prevents accidental motion during e-stop

- ROS Integration
  * Publishes geometry_msgs/Twist to /cmd_vel
  * Max linear velocity: 0.5 m/s
  * Max angular velocity: 1.0 rad/s
  * 50ms update rate

- UI Features
  * Real-time velocity display (m/s and rad/s)
  * Status bar with control mode indicator
  * Speed limit percentage display
  * Control info with keyboard fallback guide

Integration:
- Replaces previous split-view control layout
- Enhanced TELEMETRY/control tab
- Comprehensive gamepad + keyboard control system

Build: 120 modules, no errors

Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
This commit is contained in:
sl-webui 2026-03-03 00:43:47 -05:00
parent ffc69a05c0
commit 23e05a634f
12 changed files with 977 additions and 10 deletions

View File

@ -0,0 +1,23 @@
# Battery Speed Limiter Configuration
battery_speed_limiter:
ros__parameters:
# Battery percentage thresholds
threshold_full_1: 100 # Upper bound for full speed
threshold_full_2: 50 # Lower bound for full speed
threshold_reduced_1: 50 # Upper bound for reduced speed
threshold_reduced_2: 25 # Lower bound for reduced speed
threshold_critical_1: 25 # Upper bound for critical speed
threshold_critical_2: 15 # Lower bound for critical speed
threshold_stop: 15 # Below this: stop
# Speed limit factors
speed_factor_full: 1.0 # 100-50%: full speed
speed_factor_reduced: 0.7 # 50-25%: 70% speed
speed_factor_critical: 0.4 # 25-15%: 40% speed
speed_factor_stop: 0.0 # <15%: stop
# Publishing frequency (Hz)
publish_frequency: 10
# Enable/disable limiting
enable_limiting: true

View File

@ -0,0 +1,28 @@
import os
from launch import LaunchDescription
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import PathJoinSubstitution
def generate_launch_description():
config_dir = PathJoinSubstitution(
[FindPackageShare('saltybot_battery_speed_limiter'), 'config']
)
config_file = PathJoinSubstitution([config_dir, 'battery_limiter_config.yaml'])
battery_limiter = Node(
package='saltybot_battery_speed_limiter',
executable='battery_speed_limiter_node',
name='battery_speed_limiter',
output='screen',
parameters=[config_file],
remappings=[
('/saltybot/battery_state', '/saltybot/battery_state'),
('/saltybot/speed_limit_factor', '/saltybot/speed_limit_factor'),
],
)
return LaunchDescription([
battery_limiter,
])

View File

@ -0,0 +1,28 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>saltybot_battery_speed_limiter</name>
<version>0.1.0</version>
<description>Battery-aware speed limiter that adjusts robot speed based on battery state to preserve charge</description>
<maintainer email="sl-controls@saltybot.local">SaltyBot Controls</maintainer>
<license>MIT</license>
<author email="sl-controls@saltybot.local">SaltyBot Controls Team</author>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>ament_cmake_python</buildtool_depend>
<depend>rclpy</depend>
<depend>std_msgs</depend>
<depend>sensor_msgs</depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>

View File

@ -0,0 +1,152 @@
#!/usr/bin/env python3
"""Battery-aware speed limiter for SaltyBot.
Subscribes to /saltybot/battery_state and publishes a speed limit factor
on /saltybot/speed_limit_factor based on remaining battery percentage.
Thresholds:
100-50%: 1.0 (full speed)
50-25%: 0.7 (70% speed)
25-15%: 0.4 (40% speed)
<15%: 0.0 (stop, preserve battery)
"""
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32
from sensor_msgs.msg import BatteryState
class BatterySpeedLimiterNode(Node):
"""ROS2 node for battery-aware speed limiting."""
def __init__(self):
super().__init__('battery_speed_limiter')
# Parameters
self.declare_parameter('threshold_full_1', 100) # 100%
self.declare_parameter('threshold_full_2', 50) # 50%
self.declare_parameter('threshold_reduced_1', 50) # 50%
self.declare_parameter('threshold_reduced_2', 25) # 25%
self.declare_parameter('threshold_critical_1', 25) # 25%
self.declare_parameter('threshold_critical_2', 15) # 15%
self.declare_parameter('threshold_stop', 15) # <15%
self.declare_parameter('speed_factor_full', 1.0)
self.declare_parameter('speed_factor_reduced', 0.7)
self.declare_parameter('speed_factor_critical', 0.4)
self.declare_parameter('speed_factor_stop', 0.0)
self.declare_parameter('publish_frequency', 10)
self.declare_parameter('enable_limiting', True)
# Read parameters
self.threshold_full_1 = self.get_parameter('threshold_full_1').value
self.threshold_full_2 = self.get_parameter('threshold_full_2').value
self.threshold_reduced_1 = self.get_parameter('threshold_reduced_1').value
self.threshold_reduced_2 = self.get_parameter('threshold_reduced_2').value
self.threshold_critical_1 = self.get_parameter('threshold_critical_1').value
self.threshold_critical_2 = self.get_parameter('threshold_critical_2').value
self.threshold_stop = self.get_parameter('threshold_stop').value
self.speed_factor_full = self.get_parameter('speed_factor_full').value
self.speed_factor_reduced = self.get_parameter('speed_factor_reduced').value
self.speed_factor_critical = self.get_parameter('speed_factor_critical').value
self.speed_factor_stop = self.get_parameter('speed_factor_stop').value
publish_frequency = self.get_parameter('publish_frequency').value
self.enable_limiting = self.get_parameter('enable_limiting').value
# Current battery percentage
self.current_battery_percent = 100.0
# Subscription to battery state
self.sub_battery = self.create_subscription(
BatteryState, '/saltybot/battery_state', self._on_battery_state, 10
)
# Publisher for speed limit factor
self.pub_speed_limit = self.create_publisher(
Float32, '/saltybot/speed_limit_factor', 10
)
# Timer for publishing at fixed frequency
period = 1.0 / publish_frequency
self.timer = self.create_timer(period, self._timer_callback)
self.get_logger().info(
f"Battery speed limiter initialized. "
f"Thresholds: Full={self.threshold_full_2}%, "
f"Reduced={self.threshold_reduced_2}%, "
f"Critical={self.threshold_critical_2}%, "
f"Stop={self.threshold_stop}%"
)
def _on_battery_state(self, msg: BatteryState) -> None:
"""Callback for battery state messages.
Extract percentage from BatteryState message.
Handles both direct percentage field and calculated percentage.
"""
# Try to get percentage from message
if msg.percentage >= 0:
self.current_battery_percent = msg.percentage * 100.0
else:
# Calculate from voltage if percentage not available
# Standard LiPo cells: 3V min, 4.2V max per cell
# Assume 2S (8.4V max) or 3S (12.6V max) pack
# This is a fallback - ideally percentage is provided
if msg.voltage > 0:
# Normalize to 0-1 based on typical LiPo range
min_voltage = 6.0 # 2S minimum
max_voltage = 8.4 # 2S maximum
normalized = (msg.voltage - min_voltage) / (max_voltage - min_voltage)
self.current_battery_percent = max(0.0, min(100.0, normalized * 100.0))
def _calculate_speed_limit(self, battery_percent: float) -> float:
"""Calculate speed limit factor based on battery percentage.
Args:
battery_percent: Battery state as percentage (0-100)
Returns:
Speed limit factor (0.0-1.0)
"""
if not self.enable_limiting:
return 1.0
# Apply thresholds
if battery_percent >= self.threshold_full_2:
# 100-50%: Full speed
return self.speed_factor_full
elif battery_percent >= self.threshold_reduced_2:
# 50-25%: Reduced speed (70%)
return self.speed_factor_reduced
elif battery_percent >= self.threshold_critical_2:
# 25-15%: Critical speed (40%)
return self.speed_factor_critical
else:
# <15%: Stop to preserve battery
return self.speed_factor_stop
def _timer_callback(self) -> None:
"""Periodically publish speed limit factor based on battery state."""
speed_limit = self._calculate_speed_limit(self.current_battery_percent)
msg = Float32(data=speed_limit)
self.pub_speed_limit.publish(msg)
def main(args=None):
rclpy.init(args=args)
node = BatterySpeedLimiterNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

View File

@ -0,0 +1,5 @@
[develop]
script_dir=$base/lib/saltybot_battery_speed_limiter
[egg_info]
tag_build =
tag_date = 0

View File

@ -0,0 +1,34 @@
from setuptools import setup, find_packages
package_name = 'saltybot_battery_speed_limiter'
setup(
name=package_name,
version='0.1.0',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages',
['resource/saltybot_battery_speed_limiter']),
('share/' + package_name, ['package.xml']),
('share/' + package_name + '/config', ['config/battery_limiter_config.yaml']),
('share/' + package_name + '/launch', ['launch/battery_speed_limiter.launch.py']),
],
install_requires=['setuptools'],
zip_safe=True,
author='SaltyBot Controls',
author_email='sl-controls@saltybot.local',
maintainer='SaltyBot Controls',
maintainer_email='sl-controls@saltybot.local',
keywords=['ROS2', 'battery', 'speed', 'limiter'],
classifiers=[
'Intended Audience :: Developers',
'License :: OSI Approved :: MIT License',
'Programming Language :: Python :: 3',
'Topic :: Software Development',
],
entry_points={
'console_scripts': [
'battery_speed_limiter_node=saltybot_battery_speed_limiter.battery_speed_limiter_node:main',
],
},
)

View File

@ -0,0 +1,319 @@
"""Unit tests for battery speed limiter node."""
import pytest
from sensor_msgs.msg import BatteryState
from std_msgs.msg import Float32
import rclpy
from saltybot_battery_speed_limiter.battery_speed_limiter_node import (
BatterySpeedLimiterNode,
)
@pytest.fixture
def rclpy_fixture():
"""Initialize and cleanup rclpy."""
rclpy.init()
yield
rclpy.shutdown()
@pytest.fixture
def node(rclpy_fixture):
"""Create a battery speed limiter node instance."""
node = BatterySpeedLimiterNode()
yield node
node.destroy_node()
class TestBatterySpeedLimiterNode:
"""Test suite for BatterySpeedLimiterNode."""
def test_node_initialization(self, node):
"""Test that node initializes with correct defaults."""
assert node.current_battery_percent == 100.0
assert node.enable_limiting is True
assert node.speed_factor_full == 1.0
assert node.speed_factor_reduced == 0.7
assert node.speed_factor_critical == 0.4
assert node.speed_factor_stop == 0.0
def test_battery_state_subscription(self, node):
"""Test battery state subscription updates percentage."""
battery_msg = BatteryState()
battery_msg.percentage = 0.75 # 75% (0-1 range)
node._on_battery_state(battery_msg)
assert node.current_battery_percent == 75.0
def test_battery_percentage_conversion(self, node):
"""Test percentage conversion from 0-1 to 0-100 range."""
battery_msg = BatteryState()
battery_msg.percentage = 0.5 # 50%
node._on_battery_state(battery_msg)
assert node.current_battery_percent == 50.0
def test_battery_zero_percent(self, node):
"""Test zero battery percentage."""
battery_msg = BatteryState()
battery_msg.percentage = 0.0
node._on_battery_state(battery_msg)
assert node.current_battery_percent == 0.0
def test_battery_full_percent(self, node):
"""Test full battery percentage."""
battery_msg = BatteryState()
battery_msg.percentage = 1.0
node._on_battery_state(battery_msg)
assert node.current_battery_percent == 100.0
def test_speed_limit_threshold_full_100_percent(self, node):
"""Test speed limit at 100% battery (full speed)."""
speed_limit = node._calculate_speed_limit(100.0)
assert speed_limit == 1.0
def test_speed_limit_threshold_full_75_percent(self, node):
"""Test speed limit at 75% battery (full speed range)."""
speed_limit = node._calculate_speed_limit(75.0)
assert speed_limit == 1.0
def test_speed_limit_threshold_full_50_percent(self, node):
"""Test speed limit at 50% battery (boundary)."""
speed_limit = node._calculate_speed_limit(50.0)
assert speed_limit == 1.0
def test_speed_limit_threshold_reduced_49_percent(self, node):
"""Test speed limit at 49% battery (just below full threshold)."""
speed_limit = node._calculate_speed_limit(49.0)
assert speed_limit == 0.7
def test_speed_limit_threshold_reduced_37_percent(self, node):
"""Test speed limit at 37% battery (reduced range)."""
speed_limit = node._calculate_speed_limit(37.0)
assert speed_limit == 0.7
def test_speed_limit_threshold_reduced_25_percent(self, node):
"""Test speed limit at 25% battery (boundary)."""
speed_limit = node._calculate_speed_limit(25.0)
assert speed_limit == 0.7
def test_speed_limit_threshold_critical_24_percent(self, node):
"""Test speed limit at 24% battery (just below reduced threshold)."""
speed_limit = node._calculate_speed_limit(24.0)
assert speed_limit == 0.4
def test_speed_limit_threshold_critical_20_percent(self, node):
"""Test speed limit at 20% battery (critical range)."""
speed_limit = node._calculate_speed_limit(20.0)
assert speed_limit == 0.4
def test_speed_limit_threshold_critical_15_percent(self, node):
"""Test speed limit at 15% battery (boundary)."""
speed_limit = node._calculate_speed_limit(15.0)
assert speed_limit == 0.4
def test_speed_limit_threshold_stop_14_percent(self, node):
"""Test speed limit at 14% battery (just below critical threshold)."""
speed_limit = node._calculate_speed_limit(14.0)
assert speed_limit == 0.0
def test_speed_limit_threshold_stop_5_percent(self, node):
"""Test speed limit at 5% battery (stop range)."""
speed_limit = node._calculate_speed_limit(5.0)
assert speed_limit == 0.0
def test_speed_limit_threshold_stop_0_percent(self, node):
"""Test speed limit at 0% battery (empty)."""
speed_limit = node._calculate_speed_limit(0.0)
assert speed_limit == 0.0
def test_speed_limit_limiting_disabled(self, node):
"""Test that limiting can be disabled."""
node.enable_limiting = False
# Even at critical battery, should return full speed
speed_limit = node._calculate_speed_limit(10.0)
assert speed_limit == 1.0
def test_speed_limit_limiting_enabled(self, node):
"""Test that limiting is enabled by default."""
node.enable_limiting = True
speed_limit = node._calculate_speed_limit(10.0)
assert speed_limit == 0.0
def test_threshold_transitions_high_to_low(self, node):
"""Test transitions from high to low battery levels."""
# Start at 100%
assert node._calculate_speed_limit(100.0) == 1.0
# Transition to reduced at 49%
assert node._calculate_speed_limit(49.0) == 0.7
# Transition to critical at 24%
assert node._calculate_speed_limit(24.0) == 0.4
# Transition to stop at 14%
assert node._calculate_speed_limit(14.0) == 0.0
def test_threshold_transitions_low_to_high(self, node):
"""Test transitions from low to high battery levels (charging)."""
# Start at 0%
assert node._calculate_speed_limit(0.0) == 0.0
# Transition to critical at 15%
assert node._calculate_speed_limit(15.0) == 0.4
# Transition to reduced at 25%
assert node._calculate_speed_limit(25.0) == 0.7
# Transition to full at 50%
assert node._calculate_speed_limit(50.0) == 1.0
def test_battery_state_with_voltage_fallback(self, node):
"""Test battery state extraction when percentage not available."""
battery_msg = BatteryState()
battery_msg.percentage = -1.0 # Invalid percentage
battery_msg.voltage = 8.0 # 2S LiPo approximate mid-range
node._on_battery_state(battery_msg)
# Should calculate from voltage
assert 0 <= node.current_battery_percent <= 100
def test_timer_callback_publishes(self, node):
"""Test that timer callback executes without error."""
node._timer_callback()
# Should execute without crashing
def test_speed_limit_continuity(self, node):
"""Test that speed limits are continuous across range."""
for percent in range(0, 101, 5):
speed_limit = node._calculate_speed_limit(float(percent))
assert 0.0 <= speed_limit <= 1.0
def test_custom_threshold_values(self, node):
"""Test that custom threshold parameters are respected."""
# Modify thresholds
node.threshold_full_2 = 60
node.threshold_reduced_2 = 30
node.threshold_critical_2 = 10
# Test new boundaries
assert node._calculate_speed_limit(60.0) == 1.0
assert node._calculate_speed_limit(59.0) == 0.7
assert node._calculate_speed_limit(30.0) == 0.7
assert node._calculate_speed_limit(29.0) == 0.4
assert node._calculate_speed_limit(10.0) == 0.4
assert node._calculate_speed_limit(9.0) == 0.0
def test_custom_speed_factors(self, node):
"""Test that custom speed factor parameters are respected."""
# Modify speed factors
node.speed_factor_full = 1.0
node.speed_factor_reduced = 0.5
node.speed_factor_critical = 0.25
node.speed_factor_stop = 0.0
assert node._calculate_speed_limit(100.0) == 1.0
assert node._calculate_speed_limit(50.0) == 0.5
assert node._calculate_speed_limit(25.0) == 0.25
assert node._calculate_speed_limit(10.0) == 0.0
class TestBatterySpeedLimiterScenarios:
"""Integration-style tests for realistic scenarios."""
def test_scenario_normal_operation_high_battery(self, node):
"""Scenario: Normal operation with high battery."""
battery_msg = BatteryState()
battery_msg.percentage = 0.85 # 85%
node._on_battery_state(battery_msg)
speed_limit = node._calculate_speed_limit(node.current_battery_percent)
assert speed_limit == 1.0
def test_scenario_moderate_battery_level(self, node):
"""Scenario: Moderate battery level requires reduced speed."""
battery_msg = BatteryState()
battery_msg.percentage = 0.4 # 40%
node._on_battery_state(battery_msg)
speed_limit = node._calculate_speed_limit(node.current_battery_percent)
assert speed_limit == 0.7
def test_scenario_critical_battery_level(self, node):
"""Scenario: Critical battery level requires restricted speed."""
battery_msg = BatteryState()
battery_msg.percentage = 0.2 # 20%
node._on_battery_state(battery_msg)
speed_limit = node._calculate_speed_limit(node.current_battery_percent)
assert speed_limit == 0.4
def test_scenario_near_empty_battery(self, node):
"""Scenario: Battery near empty, speed stopped to preserve charge."""
battery_msg = BatteryState()
battery_msg.percentage = 0.08 # 8%
node._on_battery_state(battery_msg)
speed_limit = node._calculate_speed_limit(node.current_battery_percent)
assert speed_limit == 0.0
def test_scenario_battery_charge_cycle(self, node):
"""Scenario: Battery through complete charge/discharge cycle."""
battery_percentages = [5, 10, 15, 20, 25, 30, 40, 50, 60, 75, 90, 100]
expected_factors = [0.0, 0.0, 0.4, 0.4, 0.7, 0.7, 0.7, 1.0, 1.0, 1.0, 1.0, 1.0]
for percent, expected_factor in zip(battery_percentages, expected_factors):
speed_limit = node._calculate_speed_limit(float(percent))
assert speed_limit == expected_factor
def test_scenario_continuous_battery_update(self, node):
"""Scenario: Continuous battery state updates."""
for percent in range(100, 0, -5):
battery_msg = BatteryState()
battery_msg.percentage = percent / 100.0
node._on_battery_state(battery_msg)
speed_limit = node._calculate_speed_limit(node.current_battery_percent)
# Should always return valid factor
assert 0.0 <= speed_limit <= 1.0
def test_scenario_rapid_threshold_crossing(self, node):
"""Scenario: Rapid crossing of speed limit thresholds."""
thresholds = [100, 50, 49, 25, 24, 15, 14, 0]
for percent in thresholds:
speed_limit = node._calculate_speed_limit(float(percent))
assert 0.0 <= speed_limit <= 1.0
def test_scenario_hysteresis_behavior(self, node):
"""Test behavior near threshold boundaries."""
# Test just below and above each threshold
test_cases = [
(50.1, 1.0), # Just above 50% boundary (full speed)
(49.9, 0.7), # Just below 50% boundary (reduced)
(25.1, 0.7), # Just above 25% boundary (reduced)
(24.9, 0.4), # Just below 25% boundary (critical)
(15.1, 0.4), # Just above 15% boundary (critical)
(14.9, 0.0), # Just below 15% boundary (stop)
]
for percent, expected_factor in test_cases:
speed_limit = node._calculate_speed_limit(percent)
assert speed_limit == expected_factor

View File

@ -71,6 +71,9 @@ import { TempGauge } from './components/TempGauge.jsx';
// Node list viewer // Node list viewer
import { NodeList } from './components/NodeList.jsx'; import { NodeList } from './components/NodeList.jsx';
// Gamepad teleoperation (issue #319)
import { Teleop } from './components/Teleop.jsx';
const TAB_GROUPS = [ const TAB_GROUPS = [
{ {
label: 'SOCIAL', label: 'SOCIAL',
@ -264,16 +267,7 @@ export default function App() {
{activeTab === 'motor-current-graph' && <MotorCurrentGraph subscribe={subscribe} />} {activeTab === 'motor-current-graph' && <MotorCurrentGraph subscribe={subscribe} />}
{activeTab === 'thermal' && <TempGauge subscribe={subscribe} />} {activeTab === 'thermal' && <TempGauge subscribe={subscribe} />}
{activeTab === 'map' && <MapViewer subscribe={subscribe} />} {activeTab === 'map' && <MapViewer subscribe={subscribe} />}
{activeTab === 'control' && ( {activeTab === 'control' && <Teleop publish={publishFn} />}
<div className="flex flex-col h-full gap-4">
<div className="flex-1 overflow-y-auto">
<ControlMode subscribe={subscribe} />
</div>
<div className="flex-1 overflow-y-auto">
<JoystickTeleop publish={publishFn} />
</div>
</div>
)}
{activeTab === 'health' && <SystemHealth subscribe={subscribe} />} {activeTab === 'health' && <SystemHealth subscribe={subscribe} />}
{activeTab === 'cameras' && <CameraViewer subscribe={subscribe} />} {activeTab === 'cameras' && <CameraViewer subscribe={subscribe} />}

View File

@ -0,0 +1,384 @@
/**
* Teleop.jsx Gamepad and keyboard teleoperation controller
*
* Features:
* - Virtual dual-stick gamepad (left=linear, right=angular velocity)
* - WASD keyboard fallback for manual driving
* - Speed limiter slider for safe operation
* - E-stop button for emergency stop
* - Real-time velocity display (m/s and rad/s)
* - Publishes geometry_msgs/Twist to /cmd_vel
* - Visual feedback with stick position and velocity vectors
* - 10% deadzone on both axes
*/
import { useEffect, useRef, useState } from 'react';
const MAX_LINEAR_VELOCITY = 0.5; // m/s
const MAX_ANGULAR_VELOCITY = 1.0; // rad/s
const DEADZONE = 0.1; // 10% deadzone
const STICK_UPDATE_RATE = 50; // ms
function VirtualStick({
position,
onMove,
label,
color,
maxValue = 1.0,
}) {
const canvasRef = useRef(null);
const containerRef = useRef(null);
const isDraggingRef = useRef(false);
// Draw stick
useEffect(() => {
const canvas = canvasRef.current;
if (!canvas) return;
const ctx = canvas.getContext('2d');
const width = canvas.width;
const height = canvas.height;
const centerX = width / 2;
const centerY = height / 2;
const baseRadius = Math.min(width, height) * 0.35;
const knobRadius = Math.min(width, height) * 0.15;
// Clear canvas
ctx.fillStyle = '#1f2937';
ctx.fillRect(0, 0, width, height);
// Draw base circle
ctx.strokeStyle = '#374151';
ctx.lineWidth = 2;
ctx.beginPath();
ctx.arc(centerX, centerY, baseRadius, 0, Math.PI * 2);
ctx.stroke();
// Draw center crosshair
ctx.strokeStyle = '#4b5563';
ctx.lineWidth = 1;
ctx.beginPath();
ctx.moveTo(centerX - baseRadius * 0.3, centerY);
ctx.lineTo(centerX + baseRadius * 0.3, centerY);
ctx.moveTo(centerX, centerY - baseRadius * 0.3);
ctx.lineTo(centerX, centerY + baseRadius * 0.3);
ctx.stroke();
// Draw deadzone circle
ctx.strokeStyle = '#4b5563';
ctx.lineWidth = 1;
ctx.globalAlpha = 0.5;
ctx.beginPath();
ctx.arc(centerX, centerY, baseRadius * DEADZONE, 0, Math.PI * 2);
ctx.stroke();
ctx.globalAlpha = 1.0;
// Draw knob at current position
const knobX = centerX + (position.x / maxValue) * baseRadius;
const knobY = centerY - (position.y / maxValue) * baseRadius;
// Knob shadow
ctx.fillStyle = 'rgba(0, 0, 0, 0.3)';
ctx.beginPath();
ctx.arc(knobX + 2, knobY + 2, knobRadius, 0, Math.PI * 2);
ctx.fill();
// Knob
ctx.fillStyle = color;
ctx.beginPath();
ctx.arc(knobX, knobY, knobRadius, 0, Math.PI * 2);
ctx.fill();
// Knob border
ctx.strokeStyle = '#fff';
ctx.lineWidth = 2;
ctx.beginPath();
ctx.arc(knobX, knobY, knobRadius, 0, Math.PI * 2);
ctx.stroke();
// Draw velocity vector
if (Math.abs(position.x) > DEADZONE || Math.abs(position.y) > DEADZONE) {
ctx.strokeStyle = color;
ctx.lineWidth = 2;
ctx.globalAlpha = 0.7;
ctx.beginPath();
ctx.moveTo(centerX, centerY);
ctx.lineTo(knobX, knobY);
ctx.stroke();
ctx.globalAlpha = 1.0;
}
}, [position, color, maxValue]);
const handlePointerDown = (e) => {
isDraggingRef.current = true;
updateStickPosition(e);
};
const handlePointerMove = (e) => {
if (!isDraggingRef.current) return;
updateStickPosition(e);
};
const handlePointerUp = () => {
isDraggingRef.current = false;
onMove({ x: 0, y: 0 });
};
const updateStickPosition = (e) => {
const canvas = canvasRef.current;
const rect = canvas.getBoundingClientRect();
const centerX = rect.width / 2;
const centerY = rect.height / 2;
const baseRadius = Math.min(rect.width, rect.height) * 0.35;
const x = e.clientX - rect.left - centerX;
const y = -(e.clientY - rect.top - centerY);
const magnitude = Math.sqrt(x * x + y * y);
const angle = Math.atan2(y, x);
let clampedMagnitude = Math.min(magnitude, baseRadius) / baseRadius;
// Apply deadzone
if (clampedMagnitude < DEADZONE) {
clampedMagnitude = 0;
}
onMove({
x: Math.cos(angle) * clampedMagnitude,
y: Math.sin(angle) * clampedMagnitude,
});
};
return (
<div ref={containerRef} className="flex flex-col items-center gap-2">
<div className="text-xs font-bold text-gray-400 tracking-widest">{label}</div>
<canvas
ref={canvasRef}
width={160}
height={160}
className="border-2 border-gray-800 rounded-lg bg-gray-900 cursor-grab active:cursor-grabbing"
onPointerDown={handlePointerDown}
onPointerMove={handlePointerMove}
onPointerUp={handlePointerUp}
onPointerLeave={handlePointerUp}
style={{ touchAction: 'none' }}
/>
<div className="text-xs text-gray-500 font-mono">
X: {position.x.toFixed(2)} Y: {position.y.toFixed(2)}
</div>
</div>
);
}
export function Teleop({ publish }) {
const [leftStick, setLeftStick] = useState({ x: 0, y: 0 });
const [rightStick, setRightStick] = useState({ x: 0, y: 0 });
const [speedLimit, setSpeedLimit] = useState(1.0);
const [isEstopped, setIsEstopped] = useState(false);
const [linearVel, setLinearVel] = useState(0);
const [angularVel, setAngularVel] = useState(0);
const keysPressed = useRef({});
const publishIntervalRef = useRef(null);
// Keyboard handling
useEffect(() => {
const handleKeyDown = (e) => {
const key = e.key.toLowerCase();
if (['w', 'a', 's', 'd', ' '].includes(key)) {
keysPressed.current[key] = true;
e.preventDefault();
}
};
const handleKeyUp = (e) => {
const key = e.key.toLowerCase();
if (['w', 'a', 's', 'd', ' '].includes(key)) {
keysPressed.current[key] = false;
e.preventDefault();
}
};
window.addEventListener('keydown', handleKeyDown);
window.addEventListener('keyup', handleKeyUp);
return () => {
window.removeEventListener('keydown', handleKeyDown);
window.removeEventListener('keyup', handleKeyUp);
};
}, []);
// Calculate velocities from input
useEffect(() => {
const interval = setInterval(() => {
let linear = leftStick.y;
let angular = rightStick.x;
// WASD fallback
if (keysPressed.current['w']) linear = Math.min(1, linear + 0.5);
if (keysPressed.current['s']) linear = Math.max(-1, linear - 0.5);
if (keysPressed.current['d']) angular = Math.min(1, angular + 0.5);
if (keysPressed.current['a']) angular = Math.max(-1, angular - 0.5);
// Clamp to [-1, 1]
linear = Math.max(-1, Math.min(1, linear));
angular = Math.max(-1, Math.min(1, angular));
// Apply speed limit
const speedFactor = isEstopped ? 0 : speedLimit;
const finalLinear = linear * MAX_LINEAR_VELOCITY * speedFactor;
const finalAngular = angular * MAX_ANGULAR_VELOCITY * speedFactor;
setLinearVel(finalLinear);
setAngularVel(finalAngular);
// Publish Twist
if (publish) {
publish('/cmd_vel', 'geometry_msgs/Twist', {
linear: { x: finalLinear, y: 0, z: 0 },
angular: { x: 0, y: 0, z: finalAngular },
});
}
}, STICK_UPDATE_RATE);
return () => clearInterval(interval);
}, [leftStick, rightStick, speedLimit, isEstopped, publish]);
const handleEstop = () => {
setIsEstopped(!isEstopped);
// Publish stop immediately
if (publish) {
publish('/cmd_vel', 'geometry_msgs/Twist', {
linear: { x: 0, y: 0, z: 0 },
angular: { x: 0, y: 0, z: 0 },
});
}
};
return (
<div className="flex flex-col h-full space-y-3">
{/* Status bar */}
<div className={`rounded-lg border p-3 space-y-2 ${
isEstopped
? 'bg-red-950 border-red-900'
: 'bg-gray-950 border-cyan-950'
}`}>
<div className="flex justify-between items-center">
<div className={`text-xs font-bold tracking-widest ${
isEstopped ? 'text-red-700' : 'text-cyan-700'
}`}>
{isEstopped ? '🛑 E-STOP ACTIVE' : '⚡ TELEOP READY'}
</div>
<button
onClick={handleEstop}
className={`px-3 py-1 text-xs font-bold rounded border transition-colors ${
isEstopped
? 'bg-green-950 border-green-800 text-green-400 hover:bg-green-900'
: 'bg-red-950 border-red-800 text-red-400 hover:bg-red-900'
}`}
>
{isEstopped ? 'RESUME' : 'E-STOP'}
</button>
</div>
{/* Velocity display */}
<div className="grid grid-cols-2 gap-2">
<div className="bg-gray-900 rounded p-2">
<div className="text-gray-600 text-xs">LINEAR</div>
<div className="text-lg font-mono text-cyan-300">
{linearVel.toFixed(2)} m/s
</div>
</div>
<div className="bg-gray-900 rounded p-2">
<div className="text-gray-600 text-xs">ANGULAR</div>
<div className="text-lg font-mono text-amber-300">
{angularVel.toFixed(2)} rad/s
</div>
</div>
</div>
</div>
{/* Gamepad area */}
<div className="flex-1 bg-gray-950 rounded-lg border border-cyan-950 p-4 flex justify-center items-center gap-8">
<VirtualStick
position={leftStick}
onMove={setLeftStick}
label="LEFT — LINEAR"
color="#10b981"
maxValue={1.0}
/>
<VirtualStick
position={rightStick}
onMove={setRightStick}
label="RIGHT — ANGULAR"
color="#f59e0b"
maxValue={1.0}
/>
</div>
{/* Speed limiter */}
<div className="bg-gray-950 rounded-lg border border-cyan-950 p-3 space-y-2">
<div className="flex justify-between items-center">
<div className="text-cyan-700 text-xs font-bold tracking-widest">
SPEED LIMITER
</div>
<div className="text-gray-400 text-xs font-mono">
{(speedLimit * 100).toFixed(0)}%
</div>
</div>
<input
type="range"
min="0"
max="1"
step="0.05"
value={speedLimit}
onChange={(e) => setSpeedLimit(parseFloat(e.target.value))}
disabled={isEstopped}
className="w-full cursor-pointer"
style={{
accentColor: isEstopped ? '#6b7280' : '#06b6d4',
}}
/>
<div className="grid grid-cols-3 gap-2 text-xs text-gray-600">
<div className="text-center">0%</div>
<div className="text-center">50%</div>
<div className="text-center">100%</div>
</div>
</div>
{/* Control info */}
<div className="bg-gray-950 rounded border border-gray-800 p-2 text-xs text-gray-600 space-y-1">
<div className="font-bold text-cyan-700 mb-2">KEYBOARD FALLBACK</div>
<div className="grid grid-cols-2 gap-2">
<div>
<span className="text-gray-500">W/S:</span> <span className="text-gray-400 font-mono">Forward/Back</span>
</div>
<div>
<span className="text-gray-500">A/D:</span> <span className="text-gray-400 font-mono">Turn Left/Right</span>
</div>
</div>
</div>
{/* Topic info */}
<div className="bg-gray-950 rounded border border-gray-800 p-2 text-xs text-gray-600 space-y-1">
<div className="flex justify-between">
<span>Topic:</span>
<span className="text-gray-500">/cmd_vel (geometry_msgs/Twist)</span>
</div>
<div className="flex justify-between">
<span>Max Linear:</span>
<span className="text-gray-500">{MAX_LINEAR_VELOCITY} m/s</span>
</div>
<div className="flex justify-between">
<span>Max Angular:</span>
<span className="text-gray-500">{MAX_ANGULAR_VELOCITY} rad/s</span>
</div>
<div className="flex justify-between">
<span>Deadzone:</span>
<span className="text-gray-500">{(DEADZONE * 100).toFixed(0)}%</span>
</div>
</div>
</div>
);
}