diff --git a/jetson/ros2_ws/src/saltybot_battery_speed_limiter/config/battery_limiter_config.yaml b/jetson/ros2_ws/src/saltybot_battery_speed_limiter/config/battery_limiter_config.yaml new file mode 100644 index 0000000..9d1b920 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_battery_speed_limiter/config/battery_limiter_config.yaml @@ -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 \ No newline at end of file diff --git a/jetson/ros2_ws/src/saltybot_battery_speed_limiter/launch/battery_speed_limiter.launch.py b/jetson/ros2_ws/src/saltybot_battery_speed_limiter/launch/battery_speed_limiter.launch.py new file mode 100644 index 0000000..54c5060 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_battery_speed_limiter/launch/battery_speed_limiter.launch.py @@ -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, + ]) \ No newline at end of file diff --git a/jetson/ros2_ws/src/saltybot_battery_speed_limiter/package.xml b/jetson/ros2_ws/src/saltybot_battery_speed_limiter/package.xml new file mode 100644 index 0000000..6f1b880 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_battery_speed_limiter/package.xml @@ -0,0 +1,28 @@ + + + + saltybot_battery_speed_limiter + 0.1.0 + Battery-aware speed limiter that adjusts robot speed based on battery state to preserve charge + + SaltyBot Controls + MIT + + SaltyBot Controls Team + + ament_cmake + ament_cmake_python + + rclpy + std_msgs + sensor_msgs + + ament_copyright + ament_flake8 + ament_pep257 + pytest + + + ament_python + + \ No newline at end of file diff --git a/jetson/ros2_ws/src/saltybot_battery_speed_limiter/resource/saltybot_battery_speed_limiter b/jetson/ros2_ws/src/saltybot_battery_speed_limiter/resource/saltybot_battery_speed_limiter new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_battery_speed_limiter/saltybot_battery_speed_limiter/__init__.py b/jetson/ros2_ws/src/saltybot_battery_speed_limiter/saltybot_battery_speed_limiter/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_battery_speed_limiter/saltybot_battery_speed_limiter/battery_speed_limiter_node.py b/jetson/ros2_ws/src/saltybot_battery_speed_limiter/saltybot_battery_speed_limiter/battery_speed_limiter_node.py new file mode 100644 index 0000000..99fae39 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_battery_speed_limiter/saltybot_battery_speed_limiter/battery_speed_limiter_node.py @@ -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() \ No newline at end of file diff --git a/jetson/ros2_ws/src/saltybot_battery_speed_limiter/setup.cfg b/jetson/ros2_ws/src/saltybot_battery_speed_limiter/setup.cfg new file mode 100644 index 0000000..8736817 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_battery_speed_limiter/setup.cfg @@ -0,0 +1,5 @@ +[develop] +script_dir=$base/lib/saltybot_battery_speed_limiter +[egg_info] +tag_build = +tag_date = 0 \ No newline at end of file diff --git a/jetson/ros2_ws/src/saltybot_battery_speed_limiter/setup.py b/jetson/ros2_ws/src/saltybot_battery_speed_limiter/setup.py new file mode 100644 index 0000000..d649173 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_battery_speed_limiter/setup.py @@ -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', + ], + }, +) \ No newline at end of file diff --git a/jetson/ros2_ws/src/saltybot_battery_speed_limiter/test/__init__.py b/jetson/ros2_ws/src/saltybot_battery_speed_limiter/test/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_battery_speed_limiter/test/test_battery_speed_limiter.py b/jetson/ros2_ws/src/saltybot_battery_speed_limiter/test/test_battery_speed_limiter.py new file mode 100644 index 0000000..0d7b3c5 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_battery_speed_limiter/test/test_battery_speed_limiter.py @@ -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 diff --git a/ui/social-bot/src/App.jsx b/ui/social-bot/src/App.jsx index 650772a..716c6f4 100644 --- a/ui/social-bot/src/App.jsx +++ b/ui/social-bot/src/App.jsx @@ -71,6 +71,9 @@ import { TempGauge } from './components/TempGauge.jsx'; // Node list viewer import { NodeList } from './components/NodeList.jsx'; +// Gamepad teleoperation (issue #319) +import { Teleop } from './components/Teleop.jsx'; + const TAB_GROUPS = [ { label: 'SOCIAL', @@ -264,16 +267,7 @@ export default function App() { {activeTab === 'motor-current-graph' && } {activeTab === 'thermal' && } {activeTab === 'map' && } - {activeTab === 'control' && ( -
-
- -
-
- -
-
- )} + {activeTab === 'control' && } {activeTab === 'health' && } {activeTab === 'cameras' && } diff --git a/ui/social-bot/src/components/Teleop.jsx b/ui/social-bot/src/components/Teleop.jsx new file mode 100644 index 0000000..9b245d7 --- /dev/null +++ b/ui/social-bot/src/components/Teleop.jsx @@ -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 ( +
+
{label}
+ +
+ X: {position.x.toFixed(2)} Y: {position.y.toFixed(2)} +
+
+ ); +} + +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 ( +
+ {/* Status bar */} +
+
+
+ {isEstopped ? '🛑 E-STOP ACTIVE' : '⚡ TELEOP READY'} +
+ +
+ + {/* Velocity display */} +
+
+
LINEAR
+
+ {linearVel.toFixed(2)} m/s +
+
+
+
ANGULAR
+
+ {angularVel.toFixed(2)} rad/s +
+
+
+
+ + {/* Gamepad area */} +
+ + +
+ + {/* Speed limiter */} +
+
+
+ SPEED LIMITER +
+
+ {(speedLimit * 100).toFixed(0)}% +
+
+ setSpeedLimit(parseFloat(e.target.value))} + disabled={isEstopped} + className="w-full cursor-pointer" + style={{ + accentColor: isEstopped ? '#6b7280' : '#06b6d4', + }} + /> +
+
0%
+
50%
+
100%
+
+
+ + {/* Control info */} +
+
KEYBOARD FALLBACK
+
+
+ W/S: Forward/Back +
+
+ A/D: Turn Left/Right +
+
+
+ + {/* Topic info */} +
+
+ Topic: + /cmd_vel (geometry_msgs/Twist) +
+
+ Max Linear: + {MAX_LINEAR_VELOCITY} m/s +
+
+ Max Angular: + {MAX_ANGULAR_VELOCITY} rad/s +
+
+ Deadzone: + {(DEADZONE * 100).toFixed(0)}% +
+
+
+ ); +}