diff --git a/jetson/ros2_ws/src/saltybot_pid_autotune/config/autotune_config.yaml b/jetson/ros2_ws/src/saltybot_pid_autotune/config/autotune_config.yaml new file mode 100644 index 0000000..7eb2cb6 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_pid_autotune/config/autotune_config.yaml @@ -0,0 +1,18 @@ +# PID auto-tuner configuration (Astrom-Hagglund relay method) + +pid_autotune: + ros__parameters: + # Relay magnitude for oscillation (m/s for balance control) + relay_magnitude: 0.3 + + # Safety: abort if robot tilt exceeds this angle (degrees) + tilt_safety_limit: 25.0 + + # Maximum duration for entire tuning process (seconds) + max_tune_duration: 120.0 + + # Number of complete oscillation cycles to measure + oscillation_count: 5 + + # Time to allow system to settle before starting relay (seconds) + settle_time: 2.0 diff --git a/jetson/ros2_ws/src/saltybot_pid_autotune/launch/pid_autotune.launch.py b/jetson/ros2_ws/src/saltybot_pid_autotune/launch/pid_autotune.launch.py new file mode 100644 index 0000000..af7ac70 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_pid_autotune/launch/pid_autotune.launch.py @@ -0,0 +1,36 @@ +"""Launch file for pid_autotune_node.""" + +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument +import os +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + """Generate launch description for PID auto-tuner.""" + # Package directory + pkg_dir = get_package_share_directory("saltybot_pid_autotune") + + # Parameters + config_file = os.path.join(pkg_dir, "config", "autotune_config.yaml") + + # Declare launch arguments + return LaunchDescription( + [ + DeclareLaunchArgument( + "config_file", + default_value=config_file, + description="Path to configuration YAML file", + ), + # PID auto-tuner node + Node( + package="saltybot_pid_autotune", + executable="pid_autotune_node", + name="pid_autotune", + output="screen", + parameters=[LaunchConfiguration("config_file")], + ), + ] + ) diff --git a/jetson/ros2_ws/src/saltybot_pid_autotune/package.xml b/jetson/ros2_ws/src/saltybot_pid_autotune/package.xml new file mode 100644 index 0000000..c856c22 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_pid_autotune/package.xml @@ -0,0 +1,30 @@ + + + + saltybot_pid_autotune + 0.1.0 + + PID auto-tuner for SaltyBot using Astrom-Hagglund relay feedback method. + Service-triggered relay oscillation measures ultimate gain (Ku) and ultimate + period (Tu), then computes Ziegler-Nichols PD gains. Safety abort on >25deg tilt. + + sl-controls + MIT + + rclpy + geometry_msgs + sensor_msgs + std_msgs + std_srvs + + ament_python + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_pid_autotune/resource/saltybot_pid_autotune b/jetson/ros2_ws/src/saltybot_pid_autotune/resource/saltybot_pid_autotune new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_pid_autotune/saltybot_pid_autotune/__init__.py b/jetson/ros2_ws/src/saltybot_pid_autotune/saltybot_pid_autotune/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_pid_autotune/saltybot_pid_autotune/pid_autotune_node.py b/jetson/ros2_ws/src/saltybot_pid_autotune/saltybot_pid_autotune/pid_autotune_node.py new file mode 100644 index 0000000..cd5bd95 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_pid_autotune/saltybot_pid_autotune/pid_autotune_node.py @@ -0,0 +1,335 @@ +#!/usr/bin/env python3 +"""PID auto-tuner for SaltyBot using Astrom-Hagglund relay feedback method. + +Service-triggered relay oscillation measures ultimate gain (Ku) and ultimate period +(Tu), then computes Ziegler-Nichols PD gains. Aborts on >25deg tilt for safety. + +Service: + /saltybot/autotune_pid (std_srvs/Trigger) - Start auto-tuning process + +Published topics: + /saltybot/autotune_info (std_msgs/String) - JSON with tuning progress/results + /saltybot/autotune_cmd_vel (geometry_msgs/Twist) - Relay control during tuning + +Subscribed topics: + /imu/data (sensor_msgs/Imu) - IMU for tilt safety check + /saltybot/balance_feedback (std_msgs/Float32) - Balance angle feedback for oscillation +""" + +import json +import math +import time +from enum import Enum +from typing import Optional, Tuple, List + +import rclpy +from rclpy.node import Node +from rclpy.service import Service +from geometry_msgs.msg import Twist +from sensor_msgs.msg import Imu +from std_msgs.msg import Float32, String +from std_srvs.srv import Trigger + + +class TuneState(Enum): + """State machine for auto-tuning process.""" + IDLE = 0 + TUNING = 1 + MEASURING = 2 + COMPLETE = 3 + ABORTED = 4 + + +class PIDAutotuneNode(Node): + """ROS2 node for automatic PID tuning via relay feedback.""" + + def __init__(self): + super().__init__("pid_autotune") + + # Parameters + self.declare_parameter("relay_magnitude", 0.3) # Relay on-off magnitude + self.declare_parameter("tilt_safety_limit", 25.0) # degrees + self.declare_parameter("max_tune_duration", 120.0) # seconds + self.declare_parameter("oscillation_count", 5) # Number of cycles to measure + self.declare_parameter("settle_time", 2.0) # Settle before starting relay + + self.relay_magnitude = self.get_parameter("relay_magnitude").value + self.tilt_safety_limit = self.get_parameter("tilt_safety_limit").value + self.max_tune_duration = self.get_parameter("max_tune_duration").value + self.oscillation_count = self.get_parameter("oscillation_count").value + self.settle_time = self.get_parameter("settle_time").value + + # State tracking + self.tune_state = TuneState.IDLE + self.last_imu_tilt = 0.0 + self.last_feedback = 0.0 + self.feedback_history: List[Tuple[float, float]] = [] # (time, feedback) + self.peaks: List[Tuple[float, float]] = [] # (time, peak_value) + self.start_time = None + + # Subscriptions + self.create_subscription(Imu, "/imu/data", self._on_imu, 10) + self.create_subscription( + Float32, "/saltybot/balance_feedback", self._on_feedback, 10 + ) + + # Publications + self.pub_cmd_vel = self.create_publisher(Twist, "/saltybot/autotune_cmd_vel", 10) + self.pub_info = self.create_publisher(String, "/saltybot/autotune_info", 10) + + # Service + self.srv_autotune = self.create_service( + Trigger, "/saltybot/autotune_pid", self._handle_autotune_request + ) + + self.get_logger().info( + "PID auto-tuner initialized. Service: /saltybot/autotune_pid\n" + f"Relay magnitude: {self.relay_magnitude}, " + f"Tilt safety limit: {self.tilt_safety_limit}°, " + f"Max duration: {self.max_tune_duration}s" + ) + + def _on_imu(self, msg: Imu) -> None: + """Extract tilt angle from IMU.""" + # Approximate tilt from acceleration (pitch angle) + # pitch = atan2(ax, sqrt(ay^2 + az^2)) + accel = msg.linear_acceleration + self.last_imu_tilt = math.degrees( + math.atan2(accel.x, math.sqrt(accel.y**2 + accel.z**2)) + ) + + def _on_feedback(self, msg: Float32) -> None: + """Record balance feedback for oscillation measurement.""" + now = self.get_clock().now().nanoseconds / 1e9 + self.last_feedback = msg.data + + # During tuning, record history + if self.tune_state in (TuneState.MEASURING,): + self.feedback_history.append((now, msg.data)) + + def _handle_autotune_request( + self, request: Trigger.Request, response: Trigger.Response + ) -> Trigger.Response: + """Handle service request to start auto-tuning.""" + if self.tune_state != TuneState.IDLE: + response.success = False + response.message = f"Already tuning (state: {self.tune_state.name})" + return response + + self.get_logger().info("Starting PID auto-tune...") + self.tune_state = TuneState.TUNING + self.feedback_history = [] + self.peaks = [] + self.start_time = self.get_clock().now().nanoseconds / 1e9 + + # Start tuning process + self._run_autotuning() + + response.success = self.tune_state == TuneState.COMPLETE + response.message = f"Tuning {self.tune_state.name}" + return response + + def _run_autotuning(self) -> None: + """Execute the complete auto-tuning sequence.""" + # Phase 1: Settle + self.get_logger().info("Phase 1: Settling...") + if not self._settle(): + self.tune_state = TuneState.ABORTED + self._publish_info( + {"status": "aborted", "reason": "Failed to settle before tuning"} + ) + return + + # Phase 2: Relay oscillation + self.get_logger().info("Phase 2: Starting relay oscillation...") + self.tune_state = TuneState.MEASURING + if not self._relay_oscillation(): + self.tune_state = TuneState.ABORTED + self._publish_info( + {"status": "aborted", "reason": "Safety limit exceeded or timeout"} + ) + return + + # Phase 3: Analysis + self.get_logger().info("Phase 3: Analyzing measurements...") + ku, tu = self._analyze_oscillation() + if ku is None or tu is None: + self.tune_state = TuneState.ABORTED + self._publish_info( + {"status": "aborted", "reason": "Insufficient oscillation data"} + ) + return + + # Phase 4: Compute gains + kp, kd = self._compute_ziegler_nichols(ku, tu) + + # Done + self.tune_state = TuneState.COMPLETE + self._publish_info( + { + "status": "complete", + "ku": ku, + "tu": tu, + "kp": kp, + "kd": kd, + } + ) + self.get_logger().info( + f"Auto-tune complete: Ku={ku:.4f}, Tu={tu:.2f}s, Kp={kp:.4f}, Kd={kd:.4f}" + ) + + def _settle(self) -> bool: + """Allow system to settle before starting relay.""" + settle_start = self.get_clock().now().nanoseconds / 1e9 + relay_off = Twist() # Zero command + + while True: + now = self.get_clock().now().nanoseconds / 1e9 + elapsed = now - settle_start + + # Safety check + if abs(self.last_imu_tilt) > self.tilt_safety_limit: + self.get_logger().error( + f"Safety abort: Tilt {self.last_imu_tilt:.1f}° > {self.tilt_safety_limit}°" + ) + return False + + # Timeout check + if elapsed > self.max_tune_duration: + self.get_logger().error("Settling timeout") + return False + + # Continue settling + if elapsed > self.settle_time: + return True + + self.pub_cmd_vel.publish(relay_off) + time.sleep(0.01) + + def _relay_oscillation(self) -> bool: + """Apply relay control to induce oscillation.""" + relay_on = Twist() + relay_on.linear.x = self.relay_magnitude + + relay_off = Twist() + + oscillation_start = self.get_clock().now().nanoseconds / 1e9 + relay_state = True # True = relay ON, False = relay OFF + last_switch = oscillation_start + cycles_measured = 0 + last_peak_value = None + + while cycles_measured < self.oscillation_count: + now = self.get_clock().now().nanoseconds / 1e9 + elapsed = now - oscillation_start + + # Safety checks + if abs(self.last_imu_tilt) > self.tilt_safety_limit: + self.get_logger().error( + f"Safety abort: Tilt {self.last_imu_tilt:.1f}° > {self.tilt_safety_limit}°" + ) + return False + + if elapsed > self.max_tune_duration: + self.get_logger().error("Relay oscillation timeout") + return False + + # Switch relay at zero crossings + if relay_state and self.last_feedback < 0: + relay_state = False + self.peaks.append((now, self.last_feedback)) + cycles_measured += 1 + last_switch = now + self.get_logger().debug( + f"Cycle {cycles_measured}: Peak at {now - oscillation_start:.2f}s = {self.last_feedback:.4f}" + ) + + elif not relay_state and self.last_feedback > 0: + relay_state = True + self.peaks.append((now, self.last_feedback)) + cycles_measured += 1 + last_switch = now + self.get_logger().debug( + f"Cycle {cycles_measured}: Peak at {now - oscillation_start:.2f}s = {self.last_feedback:.4f}" + ) + + # Publish relay command + cmd = relay_on if relay_state else relay_off + self.pub_cmd_vel.publish(cmd) + + # Report progress + self._publish_info( + { + "status": "measuring", + "cycles": cycles_measured, + "feedback": self.last_feedback, + "tilt": self.last_imu_tilt, + } + ) + + time.sleep(0.01) + + # Stop relay + self.pub_cmd_vel.publish(relay_off) + return len(self.peaks) >= self.oscillation_count + + def _analyze_oscillation(self) -> Tuple[Optional[float], Optional[float]]: + """Calculate Ku and Tu from measured peaks.""" + if len(self.peaks) < 3: + self.get_logger().error(f"Insufficient peaks: {len(self.peaks)}") + return None, None + + # Ku = 4*relay_magnitude / (pi * |peak|) + peak_values = [abs(p[1]) for p in self.peaks] + avg_peak = sum(peak_values) / len(peak_values) + ku = (4.0 * self.relay_magnitude) / (math.pi * avg_peak) + + # Tu = 2 * (time between peaks) - average period + peak_times = [p[0] for p in self.peaks] + time_diffs = [ + peak_times[i + 1] - peak_times[i] for i in range(len(peak_times) - 1) + ] + tu = 2.0 * (sum(time_diffs) / len(time_diffs)) + + self.get_logger().info( + f"Analysis: avg_peak={avg_peak:.4f}, Ku={ku:.4f}, Tu={tu:.2f}s" + ) + return ku, tu + + @staticmethod + def _compute_ziegler_nichols(ku: float, tu: float) -> Tuple[float, float]: + """Compute Ziegler-Nichols PD gains. + + For PD controller: + Kp = 0.6 * Ku + Kd = 0.075 * Ku * Tu (approximately Kp*Tu/8) + """ + kp = 0.6 * ku + kd = 0.075 * ku * tu + return kp, kd + + def _publish_info(self, data: dict) -> None: + """Publish tuning information as JSON.""" + info = { + "timestamp": self.get_clock().now().nanoseconds / 1e9, + "state": self.tune_state.name, + **data, + } + msg = String(data=json.dumps(info)) + self.pub_info.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + node = PIDAutotuneNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/jetson/ros2_ws/src/saltybot_pid_autotune/setup.cfg b/jetson/ros2_ws/src/saltybot_pid_autotune/setup.cfg new file mode 100644 index 0000000..6d192ce --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_pid_autotune/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/saltybot_pid_autotune +[egg_info] +tag_date = 0 diff --git a/jetson/ros2_ws/src/saltybot_pid_autotune/setup.py b/jetson/ros2_ws/src/saltybot_pid_autotune/setup.py new file mode 100644 index 0000000..6f43fd3 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_pid_autotune/setup.py @@ -0,0 +1,29 @@ +from setuptools import setup + +package_name = "saltybot_pid_autotune" + +setup( + name=package_name, + version="0.1.0", + packages=[package_name], + data_files=[ + ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]), + (f"share/{package_name}", ["package.xml"]), + (f"share/{package_name}/launch", ["launch/pid_autotune.launch.py"]), + (f"share/{package_name}/config", ["config/autotune_config.yaml"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="sl-controls", + maintainer_email="sl-controls@saltylab.local", + description=( + "PID auto-tuner: Astrom-Hagglund relay oscillation with Ziegler-Nichols gains" + ), + license="MIT", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "pid_autotune_node = saltybot_pid_autotune.pid_autotune_node:main", + ], + }, +) diff --git a/jetson/ros2_ws/src/saltybot_pid_autotune/test/__init__.py b/jetson/ros2_ws/src/saltybot_pid_autotune/test/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_pid_autotune/test/test_pid_autotune.py b/jetson/ros2_ws/src/saltybot_pid_autotune/test/test_pid_autotune.py new file mode 100644 index 0000000..83e82b1 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_pid_autotune/test/test_pid_autotune.py @@ -0,0 +1,314 @@ +"""Unit tests for pid_autotune_node.""" + +import pytest +import json +import math +from geometry_msgs.msg import Twist +from sensor_msgs.msg import Imu +from std_msgs.msg import Float32, String + +import rclpy + +# Import the node under test +from saltybot_pid_autotune.pid_autotune_node import ( + PIDAutotuneNode, + TuneState, +) + + +@pytest.fixture +def rclpy_fixture(): + """Initialize and cleanup rclpy.""" + rclpy.init() + yield + rclpy.shutdown() + + +@pytest.fixture +def node(rclpy_fixture): + """Create a PID autotune node instance.""" + node = PIDAutotuneNode() + yield node + node.destroy_node() + + +class TestPIDAutotuneNode: + """Test suite for PIDAutotuneNode.""" + + def test_node_initialization(self, node): + """Test that node initializes with correct defaults.""" + assert node.tune_state == TuneState.IDLE + assert node.relay_magnitude == 0.3 + assert node.tilt_safety_limit == 25.0 + assert node.max_tune_duration == 120.0 + assert node.oscillation_count == 5 + assert node.last_imu_tilt == 0.0 + assert node.last_feedback == 0.0 + assert len(node.feedback_history) == 0 + assert len(node.peaks) == 0 + + def test_imu_tilt_extraction(self, node): + """Test IMU tilt angle extraction.""" + msg = Imu() + # ax=1, ay=0, az=0 -> pitch = atan2(1, 0) = 90 degrees + msg.linear_acceleration.x = 1.0 + msg.linear_acceleration.y = 0.0 + msg.linear_acceleration.z = 0.0 + + node._on_imu(msg) + assert abs(node.last_imu_tilt - 90.0) < 1.0 + + def test_imu_small_tilt(self, node): + """Test small tilt angle extraction.""" + msg = Imu() + # Simulate 10 degree tilt + accel_x = math.sin(math.radians(10)) + accel_z = math.cos(math.radians(10)) + msg.linear_acceleration.x = accel_x + msg.linear_acceleration.y = 0.0 + msg.linear_acceleration.z = accel_z + + node._on_imu(msg) + assert abs(node.last_imu_tilt - 10.0) < 1.0 + + def test_feedback_recording(self, node): + """Test feedback value recording.""" + msg = Float32(data=0.5) + node._on_feedback(msg) + assert node.last_feedback == 0.5 + + def test_feedback_history_during_tuning(self, node): + """Test that feedback history is only recorded during MEASURING state.""" + # Not tuning yet + node._on_feedback(Float32(data=1.0)) + assert len(node.feedback_history) == 0 + + # Start measuring + node.tune_state = TuneState.MEASURING + node.start_time = node.get_clock().now().nanoseconds / 1e9 + + node._on_feedback(Float32(data=1.0)) + assert len(node.feedback_history) == 1 + + node._on_feedback(Float32(data=2.0)) + assert len(node.feedback_history) == 2 + + def test_ziegler_nichols_pd_gains(self, node): + """Test Ziegler-Nichols PD gain calculation.""" + # Example: Ku=2.0, Tu=1.0 + kp, kd = node._compute_ziegler_nichols(2.0, 1.0) + + # Kp = 0.6 * Ku = 0.6 * 2.0 = 1.2 + assert abs(kp - 1.2) < 1e-6 + + # Kd = 0.075 * Ku * Tu = 0.075 * 2.0 * 1.0 = 0.15 + assert abs(kd - 0.15) < 1e-6 + + def test_ziegler_nichols_high_gains(self, node): + """Test Z-N gains with higher Ku/Tu values.""" + kp, kd = node._compute_ziegler_nichols(3.0, 2.0) + + assert abs(kp - 1.8) < 1e-6 # 0.6 * 3.0 + assert abs(kd - 0.45) < 1e-6 # 0.075 * 3.0 * 2.0 + + def test_ultimate_gain_from_relay(self, node): + """Test Ku calculation from relay magnitude and peak.""" + # Ku = 4*relay_magnitude / (pi * |peak|) + relay_mag = 0.3 + peak_amplitude = 0.5 + + ku = (4.0 * relay_mag) / (math.pi * peak_amplitude) + + expected = (4.0 * 0.3) / (math.pi * 0.5) + assert abs(ku - expected) < 1e-6 + + def test_analyze_oscillation_insufficient_peaks(self, node): + """Test that analysis fails with insufficient peaks.""" + node.peaks = [(0.0, 0.5), (1.0, -0.5)] # Only 2 peaks + + ku, tu = node._analyze_oscillation() + assert ku is None + assert tu is None + + def test_analyze_oscillation_valid_data(self, node): + """Test oscillation analysis with valid peak data.""" + node.relay_magnitude = 0.2 + # Simulate 5 alternating peaks every ~1 second + node.peaks = [ + (0.0, -0.4), # Negative peak + (1.0, 0.4), # Positive peak + (2.0, -0.4), # Negative peak + (3.0, 0.4), # Positive peak + (4.0, -0.4), # Negative peak + ] + + ku, tu = node._analyze_oscillation() + + # Should compute meaningful values + assert ku is not None + assert tu is not None + assert ku > 0 + assert tu > 0 + + def test_tune_state_machine(self, node): + """Test state machine transitions.""" + assert node.tune_state == TuneState.IDLE + + # Invalid state for tuning (should reject) + node.tune_state = TuneState.TUNING + response = node._handle_autotune_request( + type("Request", (), {}), type("Response", (), {}) + ) + # Should fail because already tuning + # (actual test depends on response structure) + + def test_info_json_format(self, node): + """Test that info JSON is valid.""" + info = { + "status": "measuring", + "cycles": 3, + "feedback": 0.5, + "tilt": 5.0, + } + + json_str = json.dumps(info) + parsed = json.loads(json_str) + + assert parsed["status"] == "measuring" + assert parsed["cycles"] == 3 + assert abs(parsed["feedback"] - 0.5) < 1e-6 + assert abs(parsed["tilt"] - 5.0) < 1e-6 + + def test_tilt_safety_limit_check(self, node): + """Test that tilt exceeding limit is detected.""" + node.last_imu_tilt = 30.0 # Exceeds 25 degree limit + node.tilt_safety_limit = 25.0 + + # In actual settle/relay code, this would trigger abort + assert abs(node.last_imu_tilt) > node.tilt_safety_limit + + def test_tilt_safe_margin(self, node): + """Test normal tilt well within limits.""" + node.last_imu_tilt = 10.0 + node.tilt_safety_limit = 25.0 + + assert abs(node.last_imu_tilt) < node.tilt_safety_limit + + +class TestPIDAutotuneScenarios: + """Integration-style tests for realistic scenarios.""" + + def test_scenario_clean_oscillation(self, node): + """Scenario: perfect sinusoidal oscillation.""" + node.relay_magnitude = 0.25 + + # Simulate clean oscillation: 5 peaks, 1Hz frequency + node.peaks = [ + (0.0, -0.5), + (1.0, 0.5), + (2.0, -0.5), + (3.0, 0.5), + (4.0, -0.5), + ] + + ku, tu = node._analyze_oscillation() + + assert ku is not None + assert tu is not None + # Tu should be ~2 seconds (period of oscillation) + assert 1.8 < tu < 2.2 + + def test_scenario_damped_oscillation(self, node): + """Scenario: damped oscillation with decaying peaks.""" + node.relay_magnitude = 0.2 + + # Peaks decay over time + node.peaks = [ + (0.0, -0.6), + (1.0, 0.55), + (2.0, -0.5), + (3.0, 0.45), + (4.0, -0.4), + ] + + ku, tu = node._analyze_oscillation() + + # Should still work with average of peaks + assert ku is not None + assert tu is not None + + def test_scenario_high_frequency_oscillation(self, node): + """Scenario: fast oscillation (0.2Hz).""" + node.relay_magnitude = 0.3 + + # 5 second period = 0.2 Hz + node.peaks = [ + (0.0, -0.5), + (5.0, 0.5), + (10.0, -0.5), + (15.0, 0.5), + (20.0, -0.5), + ] + + ku, tu = node._analyze_oscillation() + + assert tu is not None + # Period should be ~10 seconds (5 second half-period) + assert 9.8 < tu < 10.2 + + def test_scenario_z_n_aggressive_tuning(self, node): + """Scenario: Z-N gains for aggressive tuning.""" + # High ultimate gain + kp, kd = node._compute_ziegler_nichols(ku=4.0, tu=0.5) + + assert kp == pytest.approx(2.4) # 0.6 * 4.0 + assert kd == pytest.approx(0.15) # 0.075 * 4.0 * 0.5 + + def test_scenario_z_n_conservative_tuning(self, node): + """Scenario: Z-N gains for conservative tuning.""" + # Low ultimate gain + kp, kd = node._compute_ziegler_nichols(ku=1.0, tu=2.0) + + assert kp == pytest.approx(0.6) # 0.6 * 1.0 + assert kd == pytest.approx(0.15) # 0.075 * 1.0 * 2.0 + + def test_scenario_tilt_safety_aborts(self, node): + """Scenario: tuning aborted due to tilt exceeding limit.""" + node.last_imu_tilt = 28.0 # Exceeds 25 degree limit + node.tilt_safety_limit = 25.0 + + # Safety check would catch this + should_abort = abs(node.last_imu_tilt) > node.tilt_safety_limit + assert should_abort is True + + def test_scenario_normal_balance_tuning(self, node): + """Scenario: typical balance tuning process.""" + # Starting state + assert node.tune_state == TuneState.IDLE + + # After settle + node.tune_state = TuneState.TUNING + assert node.tune_state == TuneState.TUNING + + # After relay measurement + node.tune_state = TuneState.MEASURING + node.peaks = [ + (0.0, -0.3), + (1.5, 0.3), + (3.0, -0.3), + (4.5, 0.3), + (6.0, -0.3), + ] + + ku, tu = node._analyze_oscillation() + assert ku is not None + assert tu is not None + + # Compute gains + kp, kd = node._compute_ziegler_nichols(ku, tu) + assert kp > 0 + assert kd > 0 + + # Complete + node.tune_state = TuneState.COMPLETE + assert node.tune_state == TuneState.COMPLETE diff --git a/ui/social-bot/src/App.jsx b/ui/social-bot/src/App.jsx index 0ff8dfb..e788b85 100644 --- a/ui/social-bot/src/App.jsx +++ b/ui/social-bot/src/App.jsx @@ -50,6 +50,9 @@ import { CameraViewer } from './components/CameraViewer.jsx'; // Event log (issue #192) import { EventLog } from './components/EventLog.jsx'; +// Joystick teleop (issue #212) +import JoystickTeleop from './components/JoystickTeleop.jsx'; + const TAB_GROUPS = [ { label: 'SOCIAL', @@ -210,7 +213,7 @@ export default function App() { {/* ── Content ── */} -
+
{activeTab === 'status' && } {activeTab === 'faces' && } {activeTab === 'conversation' && } @@ -221,7 +224,16 @@ export default function App() { {activeTab === 'battery' && } {activeTab === 'motors' && } {activeTab === 'map' && } - {activeTab === 'control' && } + {activeTab === 'control' && ( +
+
+ +
+
+ +
+
+ )} {activeTab === 'health' && } {activeTab === 'cameras' && } diff --git a/ui/social-bot/src/components/JoystickTeleop.jsx b/ui/social-bot/src/components/JoystickTeleop.jsx new file mode 100644 index 0000000..1051ade --- /dev/null +++ b/ui/social-bot/src/components/JoystickTeleop.jsx @@ -0,0 +1,339 @@ +/** + * JoystickTeleop.jsx — Virtual joystick for robot teleop + * + * Features: + * - Virtual thumbstick UI with visual feedback + * - Touch & mouse support + * - Publishes geometry_msgs/Twist to /cmd_vel + * - 10% deadzone for both axes + * - Max linear velocity: 0.5 m/s + * - Max angular velocity: 1.0 rad/s + * - Real-time velocity display + */ + +import { useEffect, useRef, useState } from 'react'; + +const DEADZONE = 0.1; // 10% +const MAX_LINEAR_VEL = 0.5; // m/s +const MAX_ANGULAR_VEL = 1.0; // rad/s +const RADIUS = 80; // pixels +const STICK_RADIUS = 25; // pixels + +function JoystickTeleop({ publish }) { + const canvasRef = useRef(null); + const containerRef = useRef(null); + const [joystickPos, setJoystickPos] = useState({ x: 0, y: 0 }); + const [isDragging, setIsDragging] = useState(false); + const [linearVel, setLinearVel] = useState(0); + const [angularVel, setAngularVel] = useState(0); + const velocityTimerRef = useRef(null); + + // Draw joystick on canvas + useEffect(() => { + const canvas = canvasRef.current; + if (!canvas) return; + + const ctx = canvas.getContext('2d'); + const rect = canvas.getBoundingClientRect(); + const centerX = canvas.width / 2; + const centerY = canvas.height / 2; + + // Clear canvas + ctx.fillStyle = '#1f2937'; + ctx.fillRect(0, 0, canvas.width, canvas.height); + + // Draw outer circle (background) + ctx.strokeStyle = '#374151'; + ctx.lineWidth = 2; + ctx.beginPath(); + ctx.arc(centerX, centerY, RADIUS, 0, Math.PI * 2); + ctx.stroke(); + + // Draw crosshair + ctx.strokeStyle = '#4b5563'; + ctx.lineWidth = 1; + ctx.beginPath(); + ctx.moveTo(centerX - 30, centerY); + ctx.lineTo(centerX + 30, centerY); + ctx.moveTo(centerX, centerY - 30); + ctx.lineTo(centerX, centerY + 30); + ctx.stroke(); + + // Draw deadzone indicator + ctx.strokeStyle = '#6b7280'; + ctx.lineWidth = 1; + ctx.setLineDash([2, 2]); + ctx.beginPath(); + ctx.arc(centerX, centerY, RADIUS * DEADZONE, 0, Math.PI * 2); + ctx.stroke(); + ctx.setLineDash([]); + + // Draw stick + const stickX = centerX + joystickPos.x * RADIUS; + const stickY = centerY + joystickPos.y * RADIUS; + + ctx.fillStyle = isDragging ? '#06b6d4' : '#0891b2'; + ctx.beginPath(); + ctx.arc(stickX, stickY, STICK_RADIUS, 0, Math.PI * 2); + ctx.fill(); + + // Draw direction indicator (arrow) + if (Math.abs(joystickPos.x) > 0.01 || Math.abs(joystickPos.y) > 0.01) { + const angle = Math.atan2(-joystickPos.y, joystickPos.x); + const arrowLen = 40; + const arrowX = centerX + Math.cos(angle) * arrowLen; + const arrowY = centerY - Math.sin(angle) * arrowLen; + + ctx.strokeStyle = '#06b6d4'; + ctx.lineWidth = 2; + ctx.beginPath(); + ctx.moveTo(centerX, centerY); + ctx.lineTo(arrowX, arrowY); + ctx.stroke(); + } + }, [joystickPos, isDragging]); + + // Handle joystick input + const handleInput = (clientX, clientY) => { + const canvas = canvasRef.current; + if (!canvas) return; + + const rect = canvas.getBoundingClientRect(); + const centerX = canvas.width / 2; + const centerY = canvas.height / 2; + + // Calculate relative position + let x = (clientX - rect.left - centerX) / RADIUS; + let y = (clientY - rect.top - centerY) / RADIUS; + + // Normalize to unit circle + const distance = Math.sqrt(x * x + y * y); + if (distance > 1) { + x /= distance; + y /= distance; + } + + // Apply deadzone + const deadzoneMagnitude = Math.sqrt(x * x + y * y); + let adjustedX = x; + let adjustedY = y; + + if (deadzoneMagnitude < DEADZONE) { + adjustedX = 0; + adjustedY = 0; + } else { + // Scale values above deadzone + const scale = (deadzoneMagnitude - DEADZONE) / (1 - DEADZONE); + adjustedX = (x / deadzoneMagnitude) * scale; + adjustedY = (y / deadzoneMagnitude) * scale; + } + + setJoystickPos({ x: adjustedX, y: adjustedY }); + + // Calculate velocities + // X-axis (left-right) = angular velocity + // Y-axis (up-down) = linear velocity (negative because canvas Y is inverted) + const linear = -adjustedY * MAX_LINEAR_VEL; + const angular = adjustedX * MAX_ANGULAR_VEL; + + setLinearVel(linear); + setAngularVel(angular); + + // Publish Twist message + if (publish) { + const twist = { + linear: { x: linear, y: 0, z: 0 }, + angular: { x: 0, y: 0, z: angular } + }; + publish('/cmd_vel', 'geometry_msgs/Twist', twist); + } + }; + + const handleMouseDown = (e) => { + setIsDragging(true); + handleInput(e.clientX, e.clientY); + }; + + const handleMouseMove = (e) => { + if (isDragging) { + handleInput(e.clientX, e.clientY); + } + }; + + const handleMouseUp = () => { + setIsDragging(false); + setJoystickPos({ x: 0, y: 0 }); + setLinearVel(0); + setAngularVel(0); + + if (publish) { + const twist = { + linear: { x: 0, y: 0, z: 0 }, + angular: { x: 0, y: 0, z: 0 } + }; + publish('/cmd_vel', 'geometry_msgs/Twist', twist); + } + }; + + // Touch support + const handleTouchStart = (e) => { + setIsDragging(true); + const touch = e.touches[0]; + handleInput(touch.clientX, touch.clientY); + }; + + const handleTouchMove = (e) => { + if (isDragging) { + e.preventDefault(); + const touch = e.touches[0]; + handleInput(touch.clientX, touch.clientY); + } + }; + + const handleTouchEnd = () => { + setIsDragging(false); + setJoystickPos({ x: 0, y: 0 }); + setLinearVel(0); + setAngularVel(0); + + if (publish) { + const twist = { + linear: { x: 0, y: 0, z: 0 }, + angular: { x: 0, y: 0, z: 0 } + }; + publish('/cmd_vel', 'geometry_msgs/Twist', twist); + } + }; + + useEffect(() => { + const canvas = canvasRef.current; + if (!canvas) return; + + canvas.addEventListener('mousedown', handleMouseDown); + window.addEventListener('mousemove', handleMouseMove); + window.addEventListener('mouseup', handleMouseUp); + canvas.addEventListener('touchstart', handleTouchStart); + canvas.addEventListener('touchmove', handleTouchMove); + canvas.addEventListener('touchend', handleTouchEnd); + + return () => { + canvas.removeEventListener('mousedown', handleMouseDown); + window.removeEventListener('mousemove', handleMouseMove); + window.removeEventListener('mouseup', handleMouseUp); + canvas.removeEventListener('touchstart', handleTouchStart); + canvas.removeEventListener('touchmove', handleTouchMove); + canvas.removeEventListener('touchend', handleTouchEnd); + }; + }, [isDragging]); + + // Cleanup on unmount + useEffect(() => { + return () => { + if (velocityTimerRef.current) { + clearTimeout(velocityTimerRef.current); + } + // Send stop command on unmount + if (publish) { + const twist = { + linear: { x: 0, y: 0, z: 0 }, + angular: { x: 0, y: 0, z: 0 } + }; + publish('/cmd_vel', 'geometry_msgs/Twist', twist); + } + }; + }, [publish]); + + return ( +
+
+
+ JOYSTICK TELEOP +
+ +
+ {/* Joystick canvas */} +
+ +
+ + {/* Controls and info */} +
+ {/* Velocity displays */} +
+
+
LINEAR VELOCITY
+
+ + {linearVel.toFixed(3)} + + m/s +
+
+ Range: -0.500 to +0.500 m/s +
+
+
+ +
+ +
+
+
ANGULAR VELOCITY
+
+ + {angularVel.toFixed(3)} + + rad/s +
+
+ Range: -1.000 to +1.000 rad/s +
+
+
+ +
+ + {/* Instructions */} +
+
CONTROLS:
+
▲ Forward / ▼ Backward
+
◄ Rotate Left / ► Rotate Right
+
Deadzone: 10%
+
+ + {/* Status indicators */} +
+
+
+ {isDragging ? 'ACTIVE' : 'READY'} +
+
+
+
+
+ + {/* Topic info */} +
+
+ Publishing to: /cmd_vel + geometry_msgs/Twist +
+
+
+ ); +} + +export default JoystickTeleop;