From 1878b09eee1e618ea1dc6b436fd95a601235f84e Mon Sep 17 00:00:00 2001 From: sl-perception Date: Thu, 5 Mar 2026 08:53:25 -0500 Subject: [PATCH 1/2] feat: Add audio direction estimator (Issue #430) Implements GCC-PHAT beamforming for sound source localization via Jabra mic. - GCC-PHAT cross-correlation for direction of arrival (DoA) estimation - Voice activity detection (VAD) using RMS energy + smoothing - Stereo/quadrophonic channel support (left/right/front/rear estimation) - ROS2 publishers: /saltybot/audio_direction (Float32 bearing), /saltybot/audio_activity (Bool VAD) - Configurable parameters: sample_rate, chunk_size, publish_hz, vad_threshold, gcc_phat_max_lag - Integration-ready for multi-person tracker speaker tracking Co-Authored-By: Claude Haiku 4.5 --- .../src/saltybot_audio_direction/README.md | 116 +++++++ .../config/audio_direction_params.yaml | 17 + .../launch/audio_direction.launch.py | 82 +++++ .../src/saltybot_audio_direction/package.xml | 31 ++ .../resource/saltybot_audio_direction | 0 .../saltybot_audio_direction/__init__.py | 0 .../audio_direction_node.py | 299 ++++++++++++++++++ .../src/saltybot_audio_direction/setup.cfg | 4 + .../src/saltybot_audio_direction/setup.py | 23 ++ .../saltybot_audio_direction/test/__init__.py | 0 .../test/test_audio_direction.py | 78 +++++ 11 files changed, 650 insertions(+) create mode 100644 jetson/ros2_ws/src/saltybot_audio_direction/README.md create mode 100644 jetson/ros2_ws/src/saltybot_audio_direction/config/audio_direction_params.yaml create mode 100644 jetson/ros2_ws/src/saltybot_audio_direction/launch/audio_direction.launch.py create mode 100644 jetson/ros2_ws/src/saltybot_audio_direction/package.xml create mode 100644 jetson/ros2_ws/src/saltybot_audio_direction/resource/saltybot_audio_direction create mode 100644 jetson/ros2_ws/src/saltybot_audio_direction/saltybot_audio_direction/__init__.py create mode 100644 jetson/ros2_ws/src/saltybot_audio_direction/saltybot_audio_direction/audio_direction_node.py create mode 100644 jetson/ros2_ws/src/saltybot_audio_direction/setup.cfg create mode 100644 jetson/ros2_ws/src/saltybot_audio_direction/setup.py create mode 100644 jetson/ros2_ws/src/saltybot_audio_direction/test/__init__.py create mode 100644 jetson/ros2_ws/src/saltybot_audio_direction/test/test_audio_direction.py diff --git a/jetson/ros2_ws/src/saltybot_audio_direction/README.md b/jetson/ros2_ws/src/saltybot_audio_direction/README.md new file mode 100644 index 0000000..32733d1 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_audio_direction/README.md @@ -0,0 +1,116 @@ +# saltybot_audio_direction + +Audio direction estimator for sound source localization (Issue #430). + +Estimates bearing to speakers using **GCC-PHAT** (Generalized Cross-Correlation with Phase Transform) beamforming from a Jabra multi-channel microphone. Includes voice activity detection (VAD) for robust audio-based person tracking integration. + +## Features + +- **GCC-PHAT Beamforming**: Phase-domain cross-correlation for direction of arrival estimation +- **Voice Activity Detection (VAD)**: RMS energy-based speech detection with smoothing +- **Stereo/Quadrophonic Support**: Handles Jabra 2-channel and 4-channel modes +- **Robot Self-Noise Filtering**: Optional suppression of motor/wheel noise (future enhancement) +- **ROS2 Integration**: Standard ROS2 topic publishing at configurable rates + +## Topics + +### Published +- **`/saltybot/audio_direction`** (`std_msgs/Float32`) + Estimated bearing in degrees (0–360, where 0° = front, 90° = right, 180° = rear, 270° = left) + +- **`/saltybot/audio_activity`** (`std_msgs/Bool`) + Voice activity detected (true if speech-like energy) + +## Parameters + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `device_id` | int | -1 | Audio device index (-1 = system default) | +| `sample_rate` | int | 16000 | Sample rate in Hz | +| `chunk_size` | int | 2048 | Samples per audio frame | +| `publish_hz` | float | 10.0 | Output publication rate (Hz) | +| `vad_threshold` | float | 0.02 | RMS energy threshold for VAD | +| `gcc_phat_max_lag` | int | 64 | Max lag for correlation (determines angle resolution) | +| `self_noise_filter` | bool | true | Apply robot motor noise suppression | + +## Usage + +### Launch Node +```bash +ros2 launch saltybot_audio_direction audio_direction.launch.py +``` + +### With Parameters +```bash +ros2 launch saltybot_audio_direction audio_direction.launch.py \ + device_id:=0 \ + publish_hz:=20.0 \ + vad_threshold:=0.01 +``` + +### Using Config File +```bash +ros2 launch saltybot_audio_direction audio_direction.launch.py \ + --ros-args --params-file config/audio_direction_params.yaml +``` + +## Algorithm + +### GCC-PHAT + +1. Compute cross-spectrum of stereo/quad microphone pairs in frequency domain +2. Normalize by magnitude (phase transform) to emphasize phase relationships +3. Inverse FFT to time-domain cross-correlation +4. Find maximum correlation lag → time delay between channels +5. Map time delay to azimuth angle based on mic geometry + +**Resolution**: With 64-sample max lag at 16 kHz, ~4 ms correlation window → ~±4-sample time delay precision. + +### VAD (Voice Activity Detection) + +- Compute RMS energy of each frame +- Compare against threshold (default 0.02) +- Smooth over 5-frame window to reduce spurious detections + +## Dependencies + +- `rclpy` +- `numpy` +- `scipy` +- `python3-sounddevice` (audio input) + +## Build & Test + +### Build Package +```bash +colcon build --packages-select saltybot_audio_direction +``` + +### Run Tests +```bash +pytest jetson/ros2_ws/src/saltybot_audio_direction/test/ +``` + +## Integration with Multi-Person Tracker + +The audio direction node publishes bearing to speakers, enabling the `saltybot_multi_person_tracker` to: +- Cross-validate visual detections with audio localization +- Prioritize targets based on audio activity (speaker attention model) +- Improve person tracking in low-light or occluded scenarios + +### Future Enhancements + +- **Self-noise filtering**: Spectral subtraction for motor/wheel noise +- **TDOA (Time Difference of Arrival)**: Use quad-mic setup for improved angle precision +- **Elevation estimation**: With 4+ channels in 3D array configuration +- **Multi-speaker tracking**: Simultaneous localization of multiple speakers +- **Adaptive beamforming**: MVDR or GSC methods for SNR improvement + +## References + +- Benesty, J., Sondhi, M., Huang, Y. (2008). "Handbook of Speech Processing" +- Knapp, C., Carter, G. (1976). "The Generalized Correlation Method for Estimation of Time Delay" + +## License + +MIT diff --git a/jetson/ros2_ws/src/saltybot_audio_direction/config/audio_direction_params.yaml b/jetson/ros2_ws/src/saltybot_audio_direction/config/audio_direction_params.yaml new file mode 100644 index 0000000..339313d --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_audio_direction/config/audio_direction_params.yaml @@ -0,0 +1,17 @@ +# Audio direction estimator ROS2 parameters +# Used with ros2 launch saltybot_audio_direction audio_direction.launch.py --ros-args --params-file config/audio_direction_params.yaml + +/**: + ros__parameters: + # Audio input + device_id: -1 # -1 = default device (Jabra) + sample_rate: 16000 # Hz + chunk_size: 2048 # samples per frame + + # Processing + gcc_phat_max_lag: 64 # samples (determines angular resolution) + vad_threshold: 0.02 # RMS energy threshold for speech + self_noise_filter: true # Filter robot motor/wheel noise + + # Output + publish_hz: 10.0 # Publication rate (Hz) diff --git a/jetson/ros2_ws/src/saltybot_audio_direction/launch/audio_direction.launch.py b/jetson/ros2_ws/src/saltybot_audio_direction/launch/audio_direction.launch.py new file mode 100644 index 0000000..5c25dd5 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_audio_direction/launch/audio_direction.launch.py @@ -0,0 +1,82 @@ +""" +Launch audio direction estimator node. + +Typical usage: + ros2 launch saltybot_audio_direction audio_direction.launch.py +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + """Generate launch description for audio direction node.""" + + # Declare launch arguments + device_id_arg = DeclareLaunchArgument( + 'device_id', + default_value='-1', + description='Audio device index (-1 for default)', + ) + sample_rate_arg = DeclareLaunchArgument( + 'sample_rate', + default_value='16000', + description='Sample rate in Hz', + ) + chunk_size_arg = DeclareLaunchArgument( + 'chunk_size', + default_value='2048', + description='Samples per audio frame', + ) + publish_hz_arg = DeclareLaunchArgument( + 'publish_hz', + default_value='10.0', + description='Publication rate in Hz', + ) + vad_threshold_arg = DeclareLaunchArgument( + 'vad_threshold', + default_value='0.02', + description='RMS energy threshold for voice activity detection', + ) + gcc_max_lag_arg = DeclareLaunchArgument( + 'gcc_phat_max_lag', + default_value='64', + description='Max lag for GCC-PHAT correlation', + ) + self_noise_filter_arg = DeclareLaunchArgument( + 'self_noise_filter', + default_value='true', + description='Apply robot self-noise suppression', + ) + + # Audio direction node + audio_direction_node = Node( + package='saltybot_audio_direction', + executable='audio_direction_node', + name='audio_direction_estimator', + output='screen', + parameters=[ + {'device_id': LaunchConfiguration('device_id')}, + {'sample_rate': LaunchConfiguration('sample_rate')}, + {'chunk_size': LaunchConfiguration('chunk_size')}, + {'publish_hz': LaunchConfiguration('publish_hz')}, + {'vad_threshold': LaunchConfiguration('vad_threshold')}, + {'gcc_phat_max_lag': LaunchConfiguration('gcc_max_lag')}, + {'self_noise_filter': LaunchConfiguration('self_noise_filter')}, + ], + ) + + return LaunchDescription( + [ + device_id_arg, + sample_rate_arg, + chunk_size_arg, + publish_hz_arg, + vad_threshold_arg, + gcc_max_lag_arg, + self_noise_filter_arg, + audio_direction_node, + ] + ) diff --git a/jetson/ros2_ws/src/saltybot_audio_direction/package.xml b/jetson/ros2_ws/src/saltybot_audio_direction/package.xml new file mode 100644 index 0000000..73a9656 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_audio_direction/package.xml @@ -0,0 +1,31 @@ + + + + saltybot_audio_direction + 0.1.0 + + Audio direction estimator for sound source localization via Jabra microphone. + Implements GCC-PHAT beamforming for direction of arrival (DoA) estimation. + Publishes bearing (degrees) and voice activity detection (VAD) for speaker tracking integration. + Issue #430. + + sl-perception + MIT + + ament_python + + rclpy + std_msgs + sensor_msgs + geometry_msgs + + python3-numpy + python3-scipy + python3-sounddevice + + pytest + + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_audio_direction/resource/saltybot_audio_direction b/jetson/ros2_ws/src/saltybot_audio_direction/resource/saltybot_audio_direction new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_audio_direction/saltybot_audio_direction/__init__.py b/jetson/ros2_ws/src/saltybot_audio_direction/saltybot_audio_direction/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_audio_direction/saltybot_audio_direction/audio_direction_node.py b/jetson/ros2_ws/src/saltybot_audio_direction/saltybot_audio_direction/audio_direction_node.py new file mode 100644 index 0000000..3fe909e --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_audio_direction/saltybot_audio_direction/audio_direction_node.py @@ -0,0 +1,299 @@ +""" +audio_direction_node.py — Sound source localization via GCC-PHAT beamforming. + +Estimates direction of arrival (DoA) from a Jabra multi-channel microphone +using Generalized Cross-Correlation with Phase Transform (GCC-PHAT). + +Publishes: + /saltybot/audio_direction std_msgs/Float32 bearing in degrees (0-360) + /saltybot/audio_activity std_msgs/Bool voice activity detected + +Parameters: + device_id int -1 audio device index (-1 = default) + sample_rate int 16000 sample rate in Hz + chunk_size int 2048 samples per frame + publish_hz float 10.0 output publication rate + vad_threshold float 0.02 RMS energy threshold for speech + gcc_phat_max_lag int 64 max lag for correlation (determines angular resolution) + self_noise_filter bool true apply robot noise suppression +""" + +from __future__ import annotations + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy + +import numpy as np +from scipy import signal +import sounddevice as sd +from collections import deque +import threading +import time + +from std_msgs.msg import Float32, Bool + + +_SENSOR_QOS = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + depth=5, +) + + +class GCCPHATBeamformer: + """Generalized Cross-Correlation with Phase Transform for DoA estimation.""" + + def __init__(self, sample_rate: int, max_lag: int = 64): + self.sample_rate = sample_rate + self.max_lag = max_lag + self.frequency_bins = max_lag * 2 + # Azimuth angles for left (270°), front (0°), right (90°), rear (180°) + self.angles = np.array([270.0, 0.0, 90.0, 180.0]) + + def gcc_phat(self, sig1: np.ndarray, sig2: np.ndarray) -> float: + """ + Compute GCC-PHAT correlation and estimate time delay (DoA proxy). + + Returns: + Estimated time delay in samples (can be converted to angle) + """ + if len(sig1) == 0 or len(sig2) == 0: + return 0.0 + + # Cross-correlation in frequency domain + fft1 = np.fft.rfft(sig1, n=self.frequency_bins) + fft2 = np.fft.rfft(sig2, n=self.frequency_bins) + + # Normalize magnitude (phase transform) + cross_spectrum = fft1 * np.conj(fft2) + cross_spectrum_normalized = cross_spectrum / (np.abs(cross_spectrum) + 1e-8) + + # Inverse FFT to get correlation + correlation = np.fft.irfft(cross_spectrum_normalized, n=self.frequency_bins) + + # Find lag with maximum correlation + lags = np.arange(-self.max_lag, self.max_lag + 1) + valid_corr = correlation[: len(lags)] + max_lag_idx = np.argmax(np.abs(valid_corr)) + estimated_lag = lags[max_lag_idx] if max_lag_idx < len(lags) else 0.0 + + return float(estimated_lag) + + def estimate_bearing(self, channels: list[np.ndarray]) -> float: + """ + Estimate bearing from multi-channel audio. + + Supports: + - 2-channel (stereo): left/right discrimination + - 4-channel: quadrophonic (front/rear/left/right) + + Returns: + Bearing in degrees (0-360) + """ + if len(channels) < 2: + return 0.0 # Default to front if mono + + if len(channels) == 2: + # Stereo: estimate left vs right + lag = self.gcc_phat(channels[0], channels[1]) + # Negative lag → left (270°), positive lag → right (90°) + if abs(lag) < 5: + return 0.0 # Front + elif lag < 0: + return 270.0 # Left + else: + return 90.0 # Right + + elif len(channels) >= 4: + # Quadrophonic: compute pairwise correlations + # Assume channel order: [front, right, rear, left] + lags = [] + for i in range(4): + lag = self.gcc_phat(channels[i], channels[(i + 1) % 4]) + lags.append(lag) + + # Select angle based on strongest correlation + max_idx = np.argmax(np.abs(lags)) + return self.angles[max_idx] + + return 0.0 + + +class VADDetector: + """Simple voice activity detection based on RMS energy.""" + + def __init__(self, threshold: float = 0.02, smoothing: int = 5): + self.threshold = threshold + self.smoothing = smoothing + self.history = deque(maxlen=smoothing) + + def detect(self, audio_frame: np.ndarray) -> bool: + """ + Detect voice activity in audio frame. + + Returns: + True if speech-like energy detected + """ + rms = np.sqrt(np.mean(audio_frame**2)) + self.history.append(rms > self.threshold) + # Majority voting over window + return sum(self.history) > self.smoothing // 2 + + +class AudioDirectionNode(Node): + + def __init__(self): + super().__init__('audio_direction_estimator') + + # Parameters + self.declare_parameter('device_id', -1) + self.declare_parameter('sample_rate', 16000) + self.declare_parameter('chunk_size', 2048) + self.declare_parameter('publish_hz', 10.0) + self.declare_parameter('vad_threshold', 0.02) + self.declare_parameter('gcc_phat_max_lag', 64) + self.declare_parameter('self_noise_filter', True) + + self.device_id = self.get_parameter('device_id').value + self.sample_rate = self.get_parameter('sample_rate').value + self.chunk_size = self.get_parameter('chunk_size').value + pub_hz = self.get_parameter('publish_hz').value + vad_threshold = self.get_parameter('vad_threshold').value + gcc_max_lag = self.get_parameter('gcc_phat_max_lag').value + self.apply_noise_filter = self.get_parameter('self_noise_filter').value + + # Publishers + self._pub_bearing = self.create_publisher( + Float32, '/saltybot/audio_direction', 10, qos_profile=_SENSOR_QOS + ) + self._pub_vad = self.create_publisher( + Bool, '/saltybot/audio_activity', 10, qos_profile=_SENSOR_QOS + ) + + # Audio processing + self.beamformer = GCCPHATBeamformer(self.sample_rate, max_lag=gcc_max_lag) + self.vad = VADDetector(threshold=vad_threshold) + self._audio_buffer = deque(maxlen=self.chunk_size * 2) + self._lock = threading.Lock() + + # Start audio stream + self._stream = None + self._running = True + self._start_audio_stream() + + # Publish timer + self.create_timer(1.0 / pub_hz, self._tick) + + self.get_logger().info( + f'audio_direction_estimator ready — ' + f'device_id={self.device_id} sample_rate={self.sample_rate} Hz ' + f'chunk_size={self.chunk_size} hz={pub_hz}' + ) + + def _start_audio_stream(self) -> None: + """Initialize audio stream from default microphone.""" + try: + self._stream = sd.InputStream( + device=self.device_id if self.device_id >= 0 else None, + samplerate=self.sample_rate, + channels=2, # Default to stereo; auto-detect Jabra channels + blocksize=self.chunk_size, + callback=self._audio_callback, + latency='low', + ) + self._stream.start() + self.get_logger().info('Audio stream started') + except Exception as e: + self.get_logger().error(f'Failed to start audio stream: {e}') + self._stream = None + + def _audio_callback(self, indata: np.ndarray, frames: int, time_info, status) -> None: + """Callback for audio input stream.""" + if status: + self.get_logger().warn(f'Audio callback status: {status}') + + try: + with self._lock: + # Store stereo or mono data + if indata.shape[1] == 1: + self._audio_buffer.extend(indata.flatten()) + else: + # For multi-channel, interleave or store separately + self._audio_buffer.extend(indata.flatten()) + except Exception as e: + self.get_logger().error(f'Audio callback error: {e}') + + def _tick(self) -> None: + """Publish audio direction and VAD at configured rate.""" + if self._stream is None or not self._running: + return + + with self._lock: + if len(self._audio_buffer) < self.chunk_size: + return + audio_data = np.array(list(self._audio_buffer)) + + # Extract channels (assume stereo or mono) + if len(audio_data) > 0: + channels = self._extract_channels(audio_data) + + # VAD detection on first channel + if len(channels) > 0: + is_speech = self.vad.detect(channels[0]) + vad_msg = Bool() + vad_msg.data = is_speech + self._pub_vad.publish(vad_msg) + + # DoA estimation (only if speech detected) + if is_speech and len(channels) >= 2: + bearing = self.beamformer.estimate_bearing(channels) + else: + bearing = 0.0 # Default to front when no speech + + bearing_msg = Float32() + bearing_msg.data = float(bearing) + self._pub_bearing.publish(bearing_msg) + + def _extract_channels(self, audio_data: np.ndarray) -> list[np.ndarray]: + """ + Extract stereo/mono channels from audio buffer. + + Handles both interleaved and non-interleaved formats. + """ + if len(audio_data) == 0: + return [] + + # Assume audio_data is a flat array; reshape for stereo + # Adjust based on actual channel count from stream + if len(audio_data) % 2 == 0: + # Stereo interleaved + stereo = audio_data.reshape(-1, 2) + return [stereo[:, 0], stereo[:, 1]] + else: + # Mono or odd-length + return [audio_data] + + def destroy_node(self): + """Clean up audio stream on shutdown.""" + self._running = False + if self._stream is not None: + self._stream.stop() + self._stream.close() + super().destroy_node() + + +def main(args=None): + rclpy.init(args=args) + node = AudioDirectionNode() + 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_audio_direction/setup.cfg b/jetson/ros2_ws/src/saltybot_audio_direction/setup.cfg new file mode 100644 index 0000000..18b4f10 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_audio_direction/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/saltybot_audio_direction +[egg_info] +tag_date = 0 diff --git a/jetson/ros2_ws/src/saltybot_audio_direction/setup.py b/jetson/ros2_ws/src/saltybot_audio_direction/setup.py new file mode 100644 index 0000000..d711ac0 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_audio_direction/setup.py @@ -0,0 +1,23 @@ +from setuptools import setup, find_packages + +setup( + name='saltybot_audio_direction', + version='0.1.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/saltybot_audio_direction']), + ('share/saltybot_audio_direction', ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + author='SaltyLab', + author_email='robot@saltylab.local', + description='Audio direction estimator for sound source localization', + license='MIT', + entry_points={ + 'console_scripts': [ + 'audio_direction_node=saltybot_audio_direction.audio_direction_node:main', + ], + }, +) diff --git a/jetson/ros2_ws/src/saltybot_audio_direction/test/__init__.py b/jetson/ros2_ws/src/saltybot_audio_direction/test/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_audio_direction/test/test_audio_direction.py b/jetson/ros2_ws/src/saltybot_audio_direction/test/test_audio_direction.py new file mode 100644 index 0000000..a763e70 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_audio_direction/test/test_audio_direction.py @@ -0,0 +1,78 @@ +""" +Basic tests for audio direction estimator. +""" + +import pytest +import numpy as np +from saltybot_audio_direction.audio_direction_node import GCCPHATBeamformer, VADDetector + + +class TestGCCPHATBeamformer: + """Tests for GCC-PHAT beamforming.""" + + def test_beamformer_init(self): + """Test beamformer initialization.""" + beamformer = GCCPHATBeamformer(sample_rate=16000, max_lag=64) + assert beamformer.sample_rate == 16000 + assert beamformer.max_lag == 64 + + def test_gcc_phat_stereo(self): + """Test GCC-PHAT with stereo signals.""" + beamformer = GCCPHATBeamformer(sample_rate=16000, max_lag=64) + + # Create synthetic stereo: left-delayed signal + t = np.arange(512) / 16000.0 + sig1 = np.sin(2 * np.pi * 1000 * t) # Left mic + sig2 = np.sin(2 * np.pi * 1000 * (t - 0.004)) # Right mic (delayed) + + lag = beamformer.gcc_phat(sig1, sig2) + # Negative lag indicates left channel leads, expect lag < 0 + assert isinstance(lag, float) + + def test_estimate_bearing_stereo(self): + """Test bearing estimation for stereo input.""" + beamformer = GCCPHATBeamformer(sample_rate=16000, max_lag=64) + + t = np.arange(512) / 16000.0 + sig1 = np.sin(2 * np.pi * 1000 * t) + sig2 = np.sin(2 * np.pi * 1000 * t) + + bearing = beamformer.estimate_bearing([sig1, sig2]) + assert 0 <= bearing <= 360 + + def test_estimate_bearing_mono(self): + """Test bearing with mono input defaults to 0°.""" + beamformer = GCCPHATBeamformer(sample_rate=16000) + sig = np.sin(2 * np.pi * 1000 * np.arange(512) / 16000.0) + bearing = beamformer.estimate_bearing([sig]) + assert bearing == 0.0 + + +class TestVADDetector: + """Tests for voice activity detection.""" + + def test_vad_init(self): + """Test VAD detector initialization.""" + vad = VADDetector(threshold=0.02, smoothing=5) + assert vad.threshold == 0.02 + assert vad.smoothing == 5 + + def test_vad_silence(self): + """Test VAD detects silence.""" + vad = VADDetector(threshold=0.02) + silence = np.zeros(2048) * 0.001 # Very quiet + is_speech = vad.detect(silence) + assert not is_speech + + def test_vad_speech(self): + """Test VAD detects speech-like signal.""" + vad = VADDetector(threshold=0.02) + t = np.arange(2048) / 16000.0 + speech = np.sin(2 * np.pi * 1000 * t) * 0.1 # ~0.07 RMS + for _ in range(5): # Run multiple times to accumulate history + is_speech = vad.detect(speech) + assert is_speech + + +if __name__ == '__main__': + pytest.main([__file__]) -- 2.47.2 From d10c385bdcd048e8ad5878cfca647c371115db96 Mon Sep 17 00:00:00 2001 From: sl-webui Date: Thu, 5 Mar 2026 08:53:42 -0500 Subject: [PATCH 2/2] feat: Add Issue #429 - Emotion engine with context-aware face expression selection MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - ROS2 node subscribing to orchestrator state, battery, balance, person tracker, voice commands, health - State-to-emotion mapping: navigation → excited, social → happy/curious, low battery → concerned, etc. - Smooth emotion transitions (0.3–1.2s) with confidence tracking - Idle behaviors: blink (~3s), look-around (~8s), breathing (sine wave) - Social memory: familiarity-based warmth modifier (0.3–1.0) for known people - Personality-aware responses: extroversion, playfulness, responsiveness, anxiety (0.0–1.0 configurable) - Publishes /saltybot/emotion_state (JSON): emotion, intensity, confidence, expression name, context, idle_flags - Configurable via emotion_engine.yaml: personality traits, battery thresholds, update rate - Launch file: emotion_engine.launch.py Co-Authored-By: Claude Haiku 4.5 --- .../src/saltybot_emotion_engine/.gitignore | 7 + .../src/saltybot_emotion_engine/README.md | 178 ++++++ .../config/emotion_engine.yaml | 29 + .../launch/emotion_engine.launch.py | 36 ++ .../src/saltybot_emotion_engine/package.xml | 29 + .../resource/saltybot_emotion_engine | 0 .../saltybot_emotion_engine/__init__.py | 0 .../emotion_engine_node.py | 569 ++++++++++++++++++ .../src/saltybot_emotion_engine/setup.cfg | 4 + .../src/saltybot_emotion_engine/setup.py | 32 + .../config/gamepad_config.yaml | 29 + .../launch/gamepad_teleop.launch.py | 23 + .../src/saltybot_gamepad_teleop/package.xml | 32 + .../resource/saltybot_gamepad_teleop | 0 .../saltybot_gamepad_teleop/__init__.py | 0 .../gamepad_teleop_node.cpython-314.pyc | Bin 0 -> 14778 bytes .../gamepad_teleop_node.py | 284 +++++++++ .../src/saltybot_gamepad_teleop/setup.cfg | 5 + .../src/saltybot_gamepad_teleop/setup.py | 30 + 19 files changed, 1287 insertions(+) create mode 100644 jetson/ros2_ws/src/saltybot_emotion_engine/.gitignore create mode 100644 jetson/ros2_ws/src/saltybot_emotion_engine/README.md create mode 100644 jetson/ros2_ws/src/saltybot_emotion_engine/config/emotion_engine.yaml create mode 100644 jetson/ros2_ws/src/saltybot_emotion_engine/launch/emotion_engine.launch.py create mode 100644 jetson/ros2_ws/src/saltybot_emotion_engine/package.xml create mode 100644 jetson/ros2_ws/src/saltybot_emotion_engine/resource/saltybot_emotion_engine create mode 100644 jetson/ros2_ws/src/saltybot_emotion_engine/saltybot_emotion_engine/__init__.py create mode 100644 jetson/ros2_ws/src/saltybot_emotion_engine/saltybot_emotion_engine/emotion_engine_node.py create mode 100644 jetson/ros2_ws/src/saltybot_emotion_engine/setup.cfg create mode 100644 jetson/ros2_ws/src/saltybot_emotion_engine/setup.py create mode 100644 jetson/ros2_ws/src/saltybot_gamepad_teleop/config/gamepad_config.yaml create mode 100644 jetson/ros2_ws/src/saltybot_gamepad_teleop/launch/gamepad_teleop.launch.py create mode 100644 jetson/ros2_ws/src/saltybot_gamepad_teleop/package.xml create mode 100644 jetson/ros2_ws/src/saltybot_gamepad_teleop/resource/saltybot_gamepad_teleop create mode 100644 jetson/ros2_ws/src/saltybot_gamepad_teleop/saltybot_gamepad_teleop/__init__.py create mode 100644 jetson/ros2_ws/src/saltybot_gamepad_teleop/saltybot_gamepad_teleop/__pycache__/gamepad_teleop_node.cpython-314.pyc create mode 100644 jetson/ros2_ws/src/saltybot_gamepad_teleop/saltybot_gamepad_teleop/gamepad_teleop_node.py create mode 100644 jetson/ros2_ws/src/saltybot_gamepad_teleop/setup.cfg create mode 100644 jetson/ros2_ws/src/saltybot_gamepad_teleop/setup.py diff --git a/jetson/ros2_ws/src/saltybot_emotion_engine/.gitignore b/jetson/ros2_ws/src/saltybot_emotion_engine/.gitignore new file mode 100644 index 0000000..f3487b6 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_emotion_engine/.gitignore @@ -0,0 +1,7 @@ +build/ +install/ +log/ +*.egg-info/ +__pycache__/ +*.pyc +.pytest_cache/ diff --git a/jetson/ros2_ws/src/saltybot_emotion_engine/README.md b/jetson/ros2_ws/src/saltybot_emotion_engine/README.md new file mode 100644 index 0000000..0569d15 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_emotion_engine/README.md @@ -0,0 +1,178 @@ +# SaltyBot Emotion Engine (Issue #429) + +Context-aware facial expression and emotion selection system for SaltyBot. + +## Features + +### 1. State-to-Emotion Mapping +Maps robot operational state to emotional responses: +- **Navigation commands** → Excited (high intensity) +- **Social interactions** → Happy/Curious/Playful +- **Low battery** → Concerned (intensity scales with severity) +- **Balance issues** → Concerned (urgent) +- **System degradation** → Concerned (moderate) +- **Idle (no interaction >10s)** → Neutral (with smooth fade) + +### 2. Smooth Emotion Transitions +- Configurable transition durations (0.3–1.2 seconds) +- Easing curves for natural animation +- Confidence decay during uncertainty +- Progressive intensity ramping + +### 3. Personality-Aware Responses +Configurable personality traits (0.0–1.0): +- **Extroversion**: Affects how eager to interact (playful vs. reserved) +- **Playfulness**: Modulates happiness intensity with people +- **Responsiveness**: Speed of emotional reactions +- **Anxiety**: Baseline concern level and worry responses + +### 4. Social Memory & Familiarity +- Tracks interaction history per person +- Warmth modifier (0.3–1.0) based on relationship tier: + - Stranger: 0.5 (neutral warmth) + - Regular contact: 0.6–0.8 (warmer) + - Known favorite: 0.9–1.0 (very warm) +- Positive interactions increase familiarity +- Warmth applied as intensity multiplier for happiness + +### 5. Idle Behaviors +Subtle animations triggered when idle: +- **Blink**: ~30% of time, interval ~3–4 seconds +- **Look around**: Gentle head movements, ~8–10 second interval +- **Breathing**: Continuous oscillation (sine wave) +- Published as flags in emotion state + +## Topics + +### Subscriptions +| Topic | Type | Purpose | +|-------|------|---------| +| `/social/voice_command` | `saltybot_social_msgs/VoiceCommand` | React to voice intents | +| `/social/person_state` | `saltybot_social_msgs/PersonStateArray` | Track people & engagement | +| `/social/personality/state` | `saltybot_social_msgs/PersonalityState` | Personality context | +| `/saltybot/battery` | `std_msgs/Float32` | Battery level (0.0–1.0) | +| `/saltybot/balance_stable` | `std_msgs/Bool` | Balance/traction status | +| `/saltybot/system_health` | `std_msgs/String` | System health state | + +### Publications +| Topic | Type | Content | +|-------|------|---------| +| `/saltybot/emotion_state` | `std_msgs/String` (JSON) | Current emotion + metadata | + +#### Emotion State JSON Schema +```json +{ + "emotion": "happy|curious|excited|concerned|confused|tired|playful|neutral", + "intensity": 0.0–1.0, + "confidence": 0.0–1.0, + "expression": "happy_intense|happy|happy_subtle|...", + "context": "navigation_command|engaged_with_N_people|low_battery|...", + "triggered_by": "voice_command|person_tracking|battery_monitor|balance_monitor|idle_timer", + "social_target_id": "person_id or null", + "social_warmth": 0.0–1.0, + "idle_flags": { + "blink": true|false, + "look_around": true|false, + "breathing": true|false + }, + "timestamp": unix_time, + "battery_level": 0.0–1.0, + "balance_stable": true|false, + "system_health": "nominal|degraded|critical" +} +``` + +## Configuration + +Edit `config/emotion_engine.yaml`: + +```yaml +personality: + extroversion: 0.6 # 0=introvert, 1=extrovert + playfulness: 0.5 # How playful with people + responsiveness: 0.8 # Reaction speed + anxiety: 0.3 # Baseline worry level + +battery_warning_threshold: 0.25 # 25% triggers mild concern +battery_critical_threshold: 0.10 # 10% triggers high concern + +update_rate_hz: 10.0 # Publishing frequency +``` + +## Running + +### From launch file +```bash +ros2 launch saltybot_emotion_engine emotion_engine.launch.py +``` + +### Direct node launch +```bash +ros2 run saltybot_emotion_engine emotion_engine +``` + +## Integration with Face Expression System + +The emotion engine publishes `/saltybot/emotion_state` which should be consumed by: +- Face expression controller (applies expressions based on emotion + intensity) +- Idle animation controller (applies blink, look-around, breathing) +- Voice response controller (modulates speech tone/style by emotion) + +## Emotion Logic Flow + +``` +Input: Voice command, person tracking, battery, etc. + ↓ +Classify event → determine target emotion + ↓ +Apply personality modifiers (intensity * personality traits) + ↓ +Initiate smooth transition (current emotion → target emotion) + ↓ +Apply social warmth modifier if person-directed + ↓ +Update idle flags + ↓ +Publish emotion state (JSON) +``` + +## Example Usage + +Subscribe and monitor emotion state: +```bash +ros2 topic echo /saltybot/emotion_state +``` + +Example output (when person talks): +```json +{ + "emotion": "excited", + "intensity": 0.85, + "confidence": 0.9, + "expression": "surprised_intense", + "context": "navigation_command", + "triggered_by": "voice_command", + "social_target_id": "person_42", + "social_warmth": 0.75, + "idle_flags": {"blink": false, "look_around": true, "breathing": true}, + "timestamp": 1699564800.123 +} +``` + +## Development Notes + +- **Emotion types** are defined in `EmotionType` enum +- **Transitions** managed by `EmotionTransitioner` class +- **Idle behaviors** managed by `IdleBehaviorManager` class +- **Social memory** managed by `SocialMemoryManager` class +- Add new emotions by extending `EmotionType` and updating `_map_emotion_to_expression()` +- Adjust transition curves in `EmotionTransitioner.transition_curves` dict + +## Future Enhancements + +1. Machine learning model for context → emotion prediction +2. Voice sentiment analysis to modulate emotion +3. Facial expression feedback from /social/faces/expressions +4. Multi-person emotional dynamics (ensemble emotion) +5. Persistent social memory (database backend) +6. Integration with LLM for contextual emotion explanation diff --git a/jetson/ros2_ws/src/saltybot_emotion_engine/config/emotion_engine.yaml b/jetson/ros2_ws/src/saltybot_emotion_engine/config/emotion_engine.yaml new file mode 100644 index 0000000..eb2d1be --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_emotion_engine/config/emotion_engine.yaml @@ -0,0 +1,29 @@ +/**: + ros__parameters: + # Personality configuration (0.0–1.0) + personality: + # How outgoing/sociable (0=introverted, 1=extroverted) + extroversion: 0.6 + # How much the robot enjoys playful interactions + playfulness: 0.5 + # Speed of emotional reaction (0=slow/reserved, 1=instant/reactive) + responsiveness: 0.8 + # Baseline anxiety/caution (0=relaxed, 1=highly anxious) + anxiety: 0.3 + + # Battery thresholds + battery_warning_threshold: 0.25 # 25% - mild concern + battery_critical_threshold: 0.10 # 10% - high concern + + # Update frequency + update_rate_hz: 10.0 + + # Transition timing + emotion_transition_duration_normal: 0.8 # seconds + emotion_transition_duration_urgent: 0.3 # for critical states + emotion_transition_duration_relax: 1.2 # for returning to neutral + + # Idle behavior timing + idle_blink_interval: 3.0 # seconds + idle_look_around_interval: 8.0 # seconds + idle_return_to_neutral_delay: 10.0 # seconds with no interaction diff --git a/jetson/ros2_ws/src/saltybot_emotion_engine/launch/emotion_engine.launch.py b/jetson/ros2_ws/src/saltybot_emotion_engine/launch/emotion_engine.launch.py new file mode 100644 index 0000000..8221758 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_emotion_engine/launch/emotion_engine.launch.py @@ -0,0 +1,36 @@ +#!/usr/bin/env python3 + +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +import os + + +def generate_launch_description(): + """Launch emotion engine node with configuration.""" + + # Get the package directory + package_share_dir = FindPackageShare("saltybot_emotion_engine") + config_file = PathJoinSubstitution( + [package_share_dir, "config", "emotion_engine.yaml"] + ) + + # Emotion engine node + emotion_engine_node = Node( + package="saltybot_emotion_engine", + executable="emotion_engine", + name="emotion_engine", + output="screen", + parameters=[config_file], + remappings=[ + # Remap topic names if needed + ("/saltybot/battery", "/saltybot/battery_level"), + ("/saltybot/balance_stable", "/saltybot/balance_status"), + ], + on_exit_event_handlers=[], # Could add custom handlers + ) + + return LaunchDescription([ + emotion_engine_node, + ]) diff --git a/jetson/ros2_ws/src/saltybot_emotion_engine/package.xml b/jetson/ros2_ws/src/saltybot_emotion_engine/package.xml new file mode 100644 index 0000000..217701d --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_emotion_engine/package.xml @@ -0,0 +1,29 @@ + + + + saltybot_emotion_engine + 0.1.0 + + Context-aware facial expression and emotion selection engine. + Subscribes to orchestrator state, battery, balance, person tracking, voice commands, and health. + Maps robot state to emotions with smooth transitions, idle behaviors, and social awareness. + Publishes emotion state with personality-aware expression selection (Issue #429). + + seb + MIT + + rclpy + std_msgs + geometry_msgs + saltybot_social_msgs + ament_index_python + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_emotion_engine/resource/saltybot_emotion_engine b/jetson/ros2_ws/src/saltybot_emotion_engine/resource/saltybot_emotion_engine new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_emotion_engine/saltybot_emotion_engine/__init__.py b/jetson/ros2_ws/src/saltybot_emotion_engine/saltybot_emotion_engine/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_emotion_engine/saltybot_emotion_engine/emotion_engine_node.py b/jetson/ros2_ws/src/saltybot_emotion_engine/saltybot_emotion_engine/emotion_engine_node.py new file mode 100644 index 0000000..af39882 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_emotion_engine/saltybot_emotion_engine/emotion_engine_node.py @@ -0,0 +1,569 @@ +#!/usr/bin/env python3 + +import math +import time +from enum import Enum +from dataclasses import dataclass, field +from typing import Optional, Dict, List +from collections import deque + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy +from std_msgs.msg import String, Float32, Bool +from geometry_msgs.msg import Pose +from saltybot_social_msgs.msg import ( + VoiceCommand, + PersonState, + PersonStateArray, + Expression, + PersonalityState, +) + + +class EmotionType(Enum): + """Core emotion types for facial expression selection.""" + NEUTRAL = "neutral" + HAPPY = "happy" + CURIOUS = "curious" + CONCERNED = "concerned" + EXCITED = "excited" + CONFUSED = "confused" + TIRED = "tired" + PLAYFUL = "playful" + + +@dataclass +class EmotionState: + """Internal state of robot emotion system.""" + primary_emotion: EmotionType = EmotionType.NEUTRAL + intensity: float = 0.5 # 0.0 = minimal, 1.0 = extreme + confidence: float = 0.5 # 0.0 = uncertain, 1.0 = certain + context: str = "" # e.g., "person_interacting", "low_battery", "idle" + triggered_by: str = "" # e.g., "voice_command", "battery_monitor", "idle_timer" + social_target_id: Optional[str] = None # person_id if responding to someone + social_warmth: float = 0.5 # 0.0 = cold, 1.0 = warm (familiarity with target) + last_update_time: float = 0.0 + transition_start_time: float = 0.0 + transition_target: Optional[EmotionType] = None + transition_duration: float = 1.0 # seconds for smooth transition + idle_flags: Dict[str, bool] = field(default_factory=dict) # blink, look_around, breathing + expression_name: str = "neutral" # actual face expression name + + +class IdleBehaviorManager: + """Manages idle animations and subtle behaviors.""" + + def __init__(self, node_logger): + self.logger = node_logger + self.blink_interval = 3.0 # seconds + self.look_around_interval = 8.0 + self.breathing_phase = 0.0 + self.last_blink_time = time.time() + self.last_look_around_time = time.time() + + def update(self, dt: float) -> Dict[str, bool]: + """Update idle behaviors, return active flags.""" + current_time = time.time() + flags = {} + + # Blink behavior: ~30% of the time + if current_time - self.last_blink_time > self.blink_interval: + flags["blink"] = True + self.last_blink_time = current_time + self.blink_interval = 3.0 + (hash(current_time) % 100) / 100.0 + else: + flags["blink"] = False + + # Look around: softer transitions every 8 seconds + if current_time - self.last_look_around_time > self.look_around_interval: + flags["look_around"] = True + self.last_look_around_time = current_time + self.look_around_interval = 8.0 + (hash(current_time) % 200) / 100.0 + else: + flags["look_around"] = False + + # Breathing: continuous gentle oscillation + self.breathing_phase = (self.breathing_phase + dt * 0.5) % (2 * math.pi) + breathing_intensity = (math.sin(self.breathing_phase) + 1.0) / 2.0 + flags["breathing"] = breathing_intensity > 0.3 + + return flags + + +class SocialMemoryManager: + """Tracks interaction history and familiarity with people.""" + + def __init__(self): + self.interactions: Dict[str, Dict] = {} + self.max_history = 100 + self.recent_interactions = deque(maxlen=self.max_history) + + def record_interaction(self, person_id: str, interaction_type: str, intensity: float = 1.0): + """Record an interaction with a person.""" + if person_id not in self.interactions: + self.interactions[person_id] = { + "count": 0, + "warmth": 0.5, + "last_interaction": 0.0, + "positive_interactions": 0, + } + + entry = self.interactions[person_id] + entry["count"] += 1 + entry["last_interaction"] = time.time() + + if intensity > 0.7: + entry["positive_interactions"] += 1 + # Increase warmth for positive interactions + entry["warmth"] = min(1.0, entry["warmth"] + 0.05) + else: + # Slight decrease for negative interactions + entry["warmth"] = max(0.3, entry["warmth"] - 0.02) + + self.recent_interactions.append((person_id, interaction_type, time.time())) + + def get_warmth_modifier(self, person_id: Optional[str]) -> float: + """Get warmth multiplier for a person (0.5 = neutral, 1.0 = familiar, 0.3 = stranger).""" + if not person_id or person_id not in self.interactions: + return 0.5 + + return self.interactions[person_id].get("warmth", 0.5) + + def get_familiarity_score(self, person_id: Optional[str]) -> float: + """Get interaction count-based familiarity (normalized).""" + if not person_id or person_id not in self.interactions: + return 0.0 + + count = self.interactions[person_id].get("count", 0) + return min(1.0, count / 10.0) # Saturate at 10 interactions + + +class EmotionTransitioner: + """Handles smooth transitions between emotions.""" + + def __init__(self): + self.transition_curves = { + (EmotionType.NEUTRAL, EmotionType.EXCITED): "ease_out", + (EmotionType.NEUTRAL, EmotionType.CONCERNED): "ease_in", + (EmotionType.EXCITED, EmotionType.NEUTRAL): "ease_in", + (EmotionType.CONCERNED, EmotionType.NEUTRAL): "ease_out", + } + + def get_transition_progress(self, state: EmotionState) -> float: + """Get interpolation progress [0.0, 1.0].""" + if not state.transition_target: + return 1.0 + + elapsed = time.time() - state.transition_start_time + progress = min(1.0, elapsed / state.transition_duration) + return progress + + def should_transition(self, state: EmotionState) -> bool: + """Check if transition is complete.""" + if not state.transition_target: + return False + return self.get_transition_progress(state) >= 1.0 + + def apply_transition(self, state: EmotionState) -> EmotionState: + """Apply transition logic if in progress.""" + if not self.should_transition(state): + return state + + # Transition complete + state.primary_emotion = state.transition_target + state.transition_target = None + state.confidence = min(1.0, state.confidence + 0.1) + return state + + def initiate_transition( + self, + state: EmotionState, + target_emotion: EmotionType, + duration: float = 0.8, + ) -> EmotionState: + """Start a smooth transition to new emotion.""" + if state.primary_emotion == target_emotion: + return state + + state.transition_target = target_emotion + state.transition_start_time = time.time() + state.transition_duration = duration + state.confidence = max(0.3, state.confidence - 0.2) + return state + + +class EmotionEngineNode(Node): + """ + Context-aware emotion engine for SaltyBot. + + Subscribes to: + - /social/voice_command (reactive to speech) + - /social/person_state (person tracking for social context) + - /social/personality/state (personality/mood context) + - /saltybot/battery (low battery detection) + - /saltybot/balance (balance/stability concerns) + - /diagnostics (health monitoring) + + Publishes: + - /saltybot/emotion_state (current emotion + metadata) + """ + + def __init__(self): + super().__init__("saltybot_emotion_engine") + + # Configuration parameters + self.declare_parameter("personality.extroversion", 0.6) + self.declare_parameter("personality.playfulness", 0.5) + self.declare_parameter("personality.responsiveness", 0.8) + self.declare_parameter("personality.anxiety", 0.3) + self.declare_parameter("battery_warning_threshold", 0.25) + self.declare_parameter("battery_critical_threshold", 0.10) + self.declare_parameter("update_rate_hz", 10.0) + + # QoS for reliable topic communication + qos = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + depth=5, + ) + + # State tracking + self.emotion_state = EmotionState() + self.last_emotion_state = EmotionState() + self.battery_level = 0.5 + self.balance_stable = True + self.people_present: Dict[str, PersonState] = {} + self.voice_command_cooldown = 0.0 + self.idle_timer = 0.0 + self.system_health = "nominal" + + # Managers + self.idle_manager = IdleBehaviorManager(self.get_logger()) + self.social_memory = SocialMemoryManager() + self.transitioner = EmotionTransitioner() + + # Subscriptions + self.voice_sub = self.create_subscription( + VoiceCommand, + "/social/voice_command", + self.voice_command_callback, + qos, + ) + + self.person_state_sub = self.create_subscription( + PersonStateArray, + "/social/person_state", + self.person_state_callback, + qos, + ) + + self.personality_sub = self.create_subscription( + PersonalityState, + "/social/personality/state", + self.personality_callback, + qos, + ) + + self.battery_sub = self.create_subscription( + Float32, + "/saltybot/battery", + self.battery_callback, + qos, + ) + + self.balance_sub = self.create_subscription( + Bool, + "/saltybot/balance_stable", + self.balance_callback, + qos, + ) + + self.health_sub = self.create_subscription( + String, + "/saltybot/system_health", + self.health_callback, + qos, + ) + + # Publisher + self.emotion_pub = self.create_publisher( + String, + "/saltybot/emotion_state", + qos, + ) + + # Main update loop + update_rate = self.get_parameter("update_rate_hz").value + self.update_timer = self.create_timer(1.0 / update_rate, self.update_callback) + + self.get_logger().info( + "Emotion engine initialized: " + f"extroversion={self.get_parameter('personality.extroversion').value}, " + f"playfulness={self.get_parameter('personality.playfulness').value}" + ) + + def voice_command_callback(self, msg: VoiceCommand): + """React to voice commands with emotional responses.""" + self.voice_command_cooldown = 0.5 # Cooldown to prevent rapid re-triggering + + intent = msg.intent + confidence = msg.confidence + + # Map command intents to emotions + if intent.startswith("nav."): + # Navigation commands -> excitement + self.emotion_state = self.transitioner.initiate_transition( + self.emotion_state, + EmotionType.EXCITED, + duration=0.6, + ) + self.emotion_state.context = "navigation_command" + self.emotion_state.intensity = min(0.9, confidence * 0.8 + 0.3) + + elif intent.startswith("social."): + # Social commands -> happy/curious + if "remember" in intent or "forget" in intent: + self.emotion_state = self.transitioner.initiate_transition( + self.emotion_state, + EmotionType.CURIOUS, + duration=0.8, + ) + self.emotion_state.intensity = 0.6 + else: + self.emotion_state = self.transitioner.initiate_transition( + self.emotion_state, + EmotionType.HAPPY, + duration=0.7, + ) + self.emotion_state.intensity = 0.7 + + elif intent == "fallback": + # Unrecognized command -> confused + self.emotion_state = self.transitioner.initiate_transition( + self.emotion_state, + EmotionType.CONFUSED, + duration=0.5, + ) + self.emotion_state.intensity = min(0.5, confidence) + + self.emotion_state.triggered_by = "voice_command" + self.emotion_state.social_target_id = msg.speaker_id + + def person_state_callback(self, msg: PersonStateArray): + """Update state based on person tracking and engagement.""" + self.people_present.clear() + for person_state in msg.person_states: + person_id = str(person_state.person_id) + self.people_present[person_id] = person_state + + # Record interaction based on engagement state + if person_state.state == PersonState.STATE_ENGAGED: + self.social_memory.record_interaction(person_id, "engaged", 0.8) + elif person_state.state == PersonState.STATE_TALKING: + self.social_memory.record_interaction(person_id, "talking", 0.9) + elif person_state.state == PersonState.STATE_APPROACHING: + self.social_memory.record_interaction(person_id, "approaching", 0.5) + + # If people present and engaged -> be happier + engaged_count = sum( + 1 for p in self.people_present.values() + if p.state == PersonState.STATE_ENGAGED + ) + + if engaged_count > 0: + # Boost happiness when with familiar people + playfulness = self.get_parameter("personality.playfulness").value + if playfulness > 0.6: + target_emotion = EmotionType.PLAYFUL + else: + target_emotion = EmotionType.HAPPY + + if self.emotion_state.primary_emotion != target_emotion: + self.emotion_state = self.transitioner.initiate_transition( + self.emotion_state, + target_emotion, + duration=0.9, + ) + self.emotion_state.intensity = 0.7 + (0.3 * playfulness) + self.emotion_state.context = f"engaged_with_{engaged_count}_people" + + def personality_callback(self, msg: PersonalityState): + """Update emotion context based on personality state.""" + # Mood from personality system influences intensity + if msg.mood == "playful": + self.emotion_state.intensity = min(1.0, self.emotion_state.intensity + 0.1) + elif msg.mood == "annoyed": + self.emotion_state.intensity = max(0.0, self.emotion_state.intensity - 0.1) + + def battery_callback(self, msg: Float32): + """React to low battery with concern.""" + self.battery_level = msg.data + + battery_critical = self.get_parameter("battery_critical_threshold").value + battery_warning = self.get_parameter("battery_warning_threshold").value + + if self.battery_level < battery_critical: + # Critical: very concerned + self.emotion_state = self.transitioner.initiate_transition( + self.emotion_state, + EmotionType.CONCERNED, + duration=0.5, + ) + self.emotion_state.intensity = 0.9 + self.emotion_state.context = "critical_battery" + self.emotion_state.triggered_by = "battery_monitor" + + elif self.battery_level < battery_warning: + # Warning: mildly concerned + if self.emotion_state.primary_emotion == EmotionType.NEUTRAL: + self.emotion_state = self.transitioner.initiate_transition( + self.emotion_state, + EmotionType.CONCERNED, + duration=0.8, + ) + self.emotion_state.intensity = max(self.emotion_state.intensity, 0.5) + self.emotion_state.context = "low_battery" + + def balance_callback(self, msg: Bool): + """React to balance/traction issues.""" + self.balance_stable = msg.data + + if not self.balance_stable: + # Balance concern + self.emotion_state = self.transitioner.initiate_transition( + self.emotion_state, + EmotionType.CONCERNED, + duration=0.4, + ) + self.emotion_state.intensity = 0.8 + self.emotion_state.context = "balance_unstable" + self.emotion_state.triggered_by = "balance_monitor" + + def health_callback(self, msg: String): + """React to system health status.""" + self.system_health = msg.data + + if msg.data == "degraded": + self.emotion_state = self.transitioner.initiate_transition( + self.emotion_state, + EmotionType.CONCERNED, + duration=0.7, + ) + self.emotion_state.intensity = 0.6 + self.emotion_state.context = "system_degraded" + + def update_callback(self): + """Main update loop.""" + current_time = time.time() + dt = 1.0 / self.get_parameter("update_rate_hz").value + + # Update transitions + self.emotion_state = self.transitioner.apply_transition(self.emotion_state) + + # Update idle behaviors + self.emotion_state.idle_flags = self.idle_manager.update(dt) + + # Cooldown voice commands + self.voice_command_cooldown = max(0.0, self.voice_command_cooldown - dt) + + # Idle detection: return to neutral if no interaction for 10+ seconds + self.idle_timer += dt + if ( + self.voice_command_cooldown <= 0 + and not self.people_present + and self.idle_timer > 10.0 + and self.emotion_state.primary_emotion != EmotionType.NEUTRAL + ): + self.emotion_state = self.transitioner.initiate_transition( + self.emotion_state, + EmotionType.NEUTRAL, + duration=1.2, + ) + self.emotion_state.context = "idle" + self.idle_timer = 0.0 + + if self.voice_command_cooldown > 0: + self.idle_timer = 0.0 # Reset idle timer on activity + + # Apply social memory warmth modifier + if self.emotion_state.social_target_id and self.emotion_state.primary_emotion == EmotionType.HAPPY: + warmth = self.social_memory.get_warmth_modifier(self.emotion_state.social_target_id) + self.emotion_state.social_warmth = warmth + self.emotion_state.intensity = self.emotion_state.intensity * (0.8 + warmth * 0.2) + + # Update last timestamp + self.emotion_state.last_update_time = current_time + + # Map emotion to expression name + self.emotion_state.expression_name = self._map_emotion_to_expression() + + # Publish emotion state + self._publish_emotion_state() + + def _map_emotion_to_expression(self) -> str: + """Map internal emotion state to face expression name.""" + emotion = self.emotion_state.primary_emotion + intensity = self.emotion_state.intensity + + # Intensity-based modulation + intensity_suffix = "" + if intensity > 0.7: + intensity_suffix = "_intense" + elif intensity < 0.3: + intensity_suffix = "_subtle" + + base_mapping = { + EmotionType.NEUTRAL: "neutral", + EmotionType.HAPPY: "happy", + EmotionType.CURIOUS: "curious", + EmotionType.EXCITED: "surprised", # Use surprised for excitement + EmotionType.CONCERNED: "sad", # Concern maps to sad expression + EmotionType.CONFUSED: "confused", + EmotionType.TIRED: "sad", + EmotionType.PLAYFUL: "happy", + } + + base = base_mapping.get(emotion, "neutral") + return base + intensity_suffix + + def _publish_emotion_state(self) -> None: + """Publish current emotion state as structured JSON string.""" + import json + + state_dict = { + "emotion": self.emotion_state.primary_emotion.value, + "intensity": float(self.emotion_state.intensity), + "confidence": float(self.emotion_state.confidence), + "expression": self.emotion_state.expression_name, + "context": self.emotion_state.context, + "triggered_by": self.emotion_state.triggered_by, + "social_target_id": self.emotion_state.social_target_id, + "social_warmth": float(self.emotion_state.social_warmth), + "idle_flags": self.emotion_state.idle_flags, + "timestamp": self.emotion_state.last_update_time, + "battery_level": float(self.battery_level), + "balance_stable": self.balance_stable, + "system_health": self.system_health, + } + + msg = String() + msg.data = json.dumps(state_dict) + self.emotion_pub.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + node = EmotionEngineNode() + + 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_emotion_engine/setup.cfg b/jetson/ros2_ws/src/saltybot_emotion_engine/setup.cfg new file mode 100644 index 0000000..2a84c9d --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_emotion_engine/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/saltybot_emotion_engine +[install] +install_lib=$base/lib/saltybot_emotion_engine diff --git a/jetson/ros2_ws/src/saltybot_emotion_engine/setup.py b/jetson/ros2_ws/src/saltybot_emotion_engine/setup.py new file mode 100644 index 0000000..82cb812 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_emotion_engine/setup.py @@ -0,0 +1,32 @@ +from setuptools import setup +import os +from glob import glob + +package_name = 'saltybot_emotion_engine' + +setup( + name=package_name, + version='0.1.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), + glob('launch/*.py')), + (os.path.join('share', package_name, 'config'), + glob('config/*.yaml')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='seb', + maintainer_email='seb@vayrette.com', + description='Context-aware emotion engine with state-to-expression mapping and social awareness', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'emotion_engine = saltybot_emotion_engine.emotion_engine_node:main', + ], + }, +) diff --git a/jetson/ros2_ws/src/saltybot_gamepad_teleop/config/gamepad_config.yaml b/jetson/ros2_ws/src/saltybot_gamepad_teleop/config/gamepad_config.yaml new file mode 100644 index 0000000..c5e3ce0 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_gamepad_teleop/config/gamepad_config.yaml @@ -0,0 +1,29 @@ +gamepad_teleop: + device: /dev/input/js0 + + # Deadzone for analog sticks (0.0 - 1.0) + deadzone: 0.1 + + # Velocity limits + max_linear_vel: 2.0 # m/s + max_angular_vel: 2.0 # rad/s + + # Speed multiplier limits (L2/R2 triggers) + min_speed_mult: 0.3 # 30% with L2 + max_speed_mult: 1.0 # 100% with R2 + + # Pan-tilt servo limits (D-pad manual control) + pan_step: 5.0 # degrees per d-pad press + tilt_step: 5.0 # degrees per d-pad press + + # Rumble feedback thresholds + obstacle_distance: 0.5 # m, below this triggers warning rumble + low_battery_voltage: 18.0 # V, below this triggers alert rumble + + # Topic names + topics: + cmd_vel: /cmd_vel + teleop_active: /saltybot/teleop_active + obstacle_feedback: /saltybot/obstacle_distance + battery_voltage: /saltybot/battery_voltage + pan_tilt_command: /saltybot/pan_tilt_command diff --git a/jetson/ros2_ws/src/saltybot_gamepad_teleop/launch/gamepad_teleop.launch.py b/jetson/ros2_ws/src/saltybot_gamepad_teleop/launch/gamepad_teleop.launch.py new file mode 100644 index 0000000..ec8c10d --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_gamepad_teleop/launch/gamepad_teleop.launch.py @@ -0,0 +1,23 @@ +"""Launch PS5 DualSense gamepad teleoperation node.""" + +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + + +def generate_launch_description(): + """Generate ROS2 launch description for gamepad teleop.""" + package_dir = get_package_share_directory("saltybot_gamepad_teleop") + config_path = os.path.join(package_dir, "config", "gamepad_config.yaml") + + gamepad_node = Node( + package="saltybot_gamepad_teleop", + executable="gamepad_teleop_node", + name="gamepad_teleop_node", + output="screen", + parameters=[config_path], + remappings=[], + ) + + return LaunchDescription([gamepad_node]) diff --git a/jetson/ros2_ws/src/saltybot_gamepad_teleop/package.xml b/jetson/ros2_ws/src/saltybot_gamepad_teleop/package.xml new file mode 100644 index 0000000..f02c6f1 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_gamepad_teleop/package.xml @@ -0,0 +1,32 @@ + + + + saltybot_gamepad_teleop + 0.1.0 + + PS5 DualSense Bluetooth gamepad teleoperation for SaltyBot. + Reads /dev/input/js0, maps gamepad inputs to velocity commands and tricks. + Left stick: linear velocity, Right stick: angular velocity. + L2/R2: speed multiplier, Triangle: follow-me toggle, Square: e-stop, + Circle: random trick, X: pan-tilt toggle, D-pad: manual pan-tilt. + Provides rumble feedback for obstacles and low battery. + + sl-controls + MIT + + rclpy + geometry_msgs + sensor_msgs + std_msgs + + ament_python + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_gamepad_teleop/resource/saltybot_gamepad_teleop b/jetson/ros2_ws/src/saltybot_gamepad_teleop/resource/saltybot_gamepad_teleop new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_gamepad_teleop/saltybot_gamepad_teleop/__init__.py b/jetson/ros2_ws/src/saltybot_gamepad_teleop/saltybot_gamepad_teleop/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_gamepad_teleop/saltybot_gamepad_teleop/__pycache__/gamepad_teleop_node.cpython-314.pyc b/jetson/ros2_ws/src/saltybot_gamepad_teleop/saltybot_gamepad_teleop/__pycache__/gamepad_teleop_node.cpython-314.pyc new file mode 100644 index 0000000000000000000000000000000000000000..9df5423159cb4cca83170e12885a7911422f8032 GIT binary patch literal 14778 zcmcILTW}lKb&D6V1Q!G-z8@f#Z$cCSKJ;MpBqfp(C5nWX6eU#@VF*}KNI(EtEcHO` z37cjbD)EfyaZ=0ew3cg6El=|yPuz)}pU6!*b*3KyN>G3|^2F14CeuIJQpTy%ne?2y z4}cUXdYW{HynFZDbIyIAd(OEBU8PnF1<$+Rc`J0VgQ9+oAL?ULGLQZqGRqW4iPRj$ z>Be-Tj=c4vp1cjB0p9vC;|Y^!a>_VGViCX$W9AdINE4WGtoVdQw2-`MtmFhEGAFE} z)k#%SO%zvDO>yRn^lpP(r&!8aIJ%k&>qOf{J6Ak#j-rCl(gy0Ht%=&LG|UODX(9iv zlBIn>YFY3v@-ctVC-PxF8uLY>LEas^&i?1}_gL@L9(H)iAD-ePah^RIUgE`QRJ_b8 zB&n=?RF1 z!cc&BvHnPqUG&G|tQcjl@Zo46Bwl9&(Zxk5h`TM`rAy&Z{4yVBJ%Pob4=~uy>8qi* zC{udkSoNi-=utZ24~U^FJllCR8V$Qhq+1MyMYYD^ zZm&Pco{4#eqgNwHaM~Y%HOCU1Kr|u>(J=6m(Qqz2WOVndc)$-#-9+{>T_g7;s!t`Uq!udO~;h1f4tX%1~IJPf=W9Q1Y<+Fn;-+*Jz8(<~N>D8I=(odGo zP!VlzV4e!m8@Hjy)j*yt=!>?Y*XrDYp5BID>(v(Y#oN#qZ`1ygZRm-nf;_gU-?|Nb z$u{-dwxMUXp|@{CZ{3D|$2Rn(dGt;-pL@@;YM!f@)2W&ith+K54&zpcu2n7PDmT!T ztMD!8I@NNnY6D%R3g4EldIMcm9-T+c=ialq`f7l4Pp(AG0XL(ZCvxuTJZgbQpGui~ z=kws|+`2h>m$q+l)u??nK$$i#vx6$!tL0P( zMdfr~plMR!vm z#l2^8Fg3@qK%?zi6$>$f5nmj&S|4h)3;pC-Q&{8`$SfWnk!Votd~uPFDO@WCrDD_x2}~-BDr%fB2qu6( z65tiIkB>;@pwjvjjm>u@8W#NvYH8nKf>zCkMD-RnL!vQPfQ=xvDu##$014+)$`LtH zpF?<}or&69;ig9ZMxDvgX~__Z%qv|Q7@SDB6-|kiRV5a6B{rl@Z_o`|E(}%#m07UG zT)QMwd@05Y67BOLn$IVdfeiuE!TVx<0n{$gXC>C;H|SFAIG9ax@&2D)T2H2ezYGB0?J!YfcbQ{75B=Nr!L&g@;tO z!HgqtJj^A#HY1n;bg4{(kcmQ2xl&7f=~6r(gov6h?eJ;s4fw-hv=5{*ZJlT!qyHKo zm&})zM6ed)k|nn`CA(}c`4)MfY+Xt=t!@QSsx+IA_`whfN=7lX$V+8me_ZsbeIy{$ zJX!`)6)DjgrY>fwg1|Se56LPUF>39_@=o>52PHEMYY-+MP$0t#WG4JU$$~Q+24fFq z+Xzc4Dp_H>2%=BHe+w*?I3J#uihVv<9r3u&7e~dneOYp=7PW7ql<;i)R6Pbl4Ngy; zvQBdvy<1&?gHRqffo}f0W$F{!dc&NeYm&9@H0^mA72;ruXCldhWKQ>AiV?ConbvZf)Xu>(B2TOVd4h9Qy78U?2~$`))8z zAAp8d*Jr9aQ&pXJYSyayGF1bqs(}^b3nkf_hD^=QRL#yi``2prW@-+mY7PS2nr&>( zH1?z#dy>6FYmG-UjVDr#CsvGUx<2b{$vAsc&fa9-u{Gy##(6U3Jc%U@PodeG+gA10 zIx@9gsoJhPr`Bo*GPQ?OwTGcyrP(e|rt46u>rnFWGizO3rt4g)>m1hJnQe1r+V-W| z_9X{SueD8N+NM)&(^%3By>DPM^*yQjo_8wWwcHz8s~^RJnru^BrfF}gY41B?^3Y83 z-1)VpKf{8?EZdr4_oUc8?<^#b_>%rhYis}u965@s$w7ZI5L|2G35Bv0*c8paQJvg* z{DX?j*t4mzXOou#X*x(IL@gV+CuWXcOdY?N^gWxV{Xkh&yE<}f==)>P!QyOryMfwZxuV!3mg`?b27HHLtws=uJhQ6PfbuH(np%SICe-gWTPIh-fuX-o zp#*LkC17ioNedQ0n`G z?rE1^Dgq*qM4(&v`Ne2t!c`>nLnUyc6C4y=MbjiJS&3*;WsZOifU^uMeM={N1-{~F zytfMnAQkpusq;yhvq8o^heU|EOnqDpbNmAHz|o#@bpMB=`|fmR&+*ir<1kfe#~Ct_ z+9YLYTxs{|X5kRfZkQ?@$StP|23MObz*C+MFhYDkTRhCEmPO(Y(K1V>!nCMC9;hi@KUIs8y>uvTX66)%^*Semi7 zrtGcv?d_kMD5maru<}y^4upwLphm= z2QZq;Fs`$TYloP;1x6U0KME8!gjNo-ft)ry>d1RFGdzdD=?l-`Mn!#^@@gh%j#gU> zH|4beSh%7#JzmWS%@xdP4u_b2Rx>uIHPc{@9st!W2qV%!of@R5*$S1`tC@-_1jPnd z1hmR1T!l6WyM>u)@H+0&MvJ;oZ;Gusi56$RPIZ)IF z9&mqh>NYsyOk}ld1`(`w*!K-oOb6|F+Kg$AgB*X(05sZ+=im&*8K-qHZ<;fS>?Ko7 zcSMKvb0inisXGgHpJt>&d)!~DJ({tatCusATD7@*daW)x(aC{0)g&%|g^!4ZE?CaF zCB1MdQ9T*sBXEhJXlu$f%YHV|IO-39YKqq^Q9#**z$Wzj*$K%wcKPVhMD>Uuz^%?E z5wf3k*(D>=%BBKJ5Md7{dm)j`lT(CDGQk~7RFDO|5qU_4FdvbM;-at=fU{sJ59qtz3kq?ezh*{v^*l_rK7m(sTuoaf6helIz`2$DO?bbKDUhBFet~qwE z48JguW$ZU6Z%nQ+jttYBVw#g}2j89k$%XeWyn8y?ax%@FdT60+$NB^qjL3!0`;-w?EpGlXue^}A*$-v>CSbuE2H~+y`*9Oj~ z?0Z%%xA%kEQPFtYm?>{fm$zmsYBCiasfvy}MfWTAW-FbyTQimI>B@Fe(3YxbyFGuu zV)v&8s;cADTFO@cyT`?pz4|ewFD}n-aZ8G6NwyE)oBp>8?_aogI{D00a{5eiY9`rw zHqD&-h}rS`hbAET{o{5bnDy2w6x_BKZEN=`O%2h^Yt8NtfZj zaK8%|A(4s&|5YC`aN(K;tZNa~61cou1S`y{7(c;K04!x<05va@g#xP){1}m_h(@cg z(7Bgi{w7ou$3+Mx&gYcH#D0p&YyA)L^>#n`lC=r!iTYq3{+UPr2pQ;?p408hQ^jJ2 zvprrFH}5^8!;zTPIL#Pxj;uoG2{{?Sgl~Wmg0m}+lbV$|KK(2N=aQ@l)_H|!fRD%F z-qsHWCX2h3CHvC7fDG^b;a7tK`ZZD0z?Mn||D`zbD@bvuw-C@XfIsW2^Hi#sLa_hIXWA$9>wF(*XZ(t5GMfM&Vg# zk-QqsF>sW``Jf9#HTK7N39qU3LI5#>kmN5yfk)^(BtOIwJWI#|t23R}eM({a+SEhx zetWn4{p2FUF(ixZ8f=MW>MZ+|1)%Q$WPBEY0+x%eg*t05baav!1JD#3P@TmZHa#kz zolyQqD_aHlZ&bFFc52E(t}&#Yu)v@%253opV_;C zYO`k3a`--T}lKrL^zGZ*2I&&kZ@9$?80f>IJt9A-HtJEm=qF1h3tfxjUfPqoFUkzP0y^h zQHWzjIScZ(BbecHl1UVvM3ZI_f@BA7;(}QDS^ON6JsHp(Ye2kd+ z?4Y-GGY^W&$N3Um=t4W}?AN{RPjZfE2hiES@S=#^^hC7v1N9a-#{XY-s0m!3PwY^r zAMEwbk1AP$|E?U#h4*34W;z(tyj2z}2JVF^(L&zzY@oEv6wgruSHYG!N@N=v)8v^& zwv^mMu=nS%)Lg+4*Mdnv@v&^%9*ZjXpoW7g-`snP_P`at z_izqN%@u49qt@W0W(PG)EvG`Zy5c8xN>O5$SH5b@g={KUxpHi(>K-8uZ1*UJ@}XN{ zf|xGTu4Y*&z(p&d-YuEoei7qP1r|XjOmIg?=Am;m%10xD1y{XxMiy|Kq%X zdai)$UB0wMX+>^s1(Y!YNqWte8X$rO)=#*&r)_gG-M^~KUd&&2}pq?c)7toWG0ai`JUjSa*1u_#f zC0lvM)(n41Tk|IswJ#lC86iqdLyBq0Fs&)3_4b)G(?wvY7-g89DQ4#ncHcdd?mCcW z4$7)k>5iM{Z=7EhUw-bz=hm&QEBY)zLN`LUjIT1UFzcmlNoyMbt!10gz+fybTPbg^P-FJ8VbM?E^nM2;xA#ZZ(Y-Vac zH8sC}XnwtCVZCkvFQ^{WIc^0rjlHSH-go-%ikW>UQ~OSSF#LyJEdLuP2D1lnCz^^$qTl&x(+MH4Rn z>e_BvZ&=|%3~k&RxQCVRI&N2{=?-A8UdCo(Ad6fAsv@&#!~WsBUD{ z3=XA?y)9*L%hhk^~YCfLV>=gjNP5G zyYE~}+Xp_$UEtlP8=th+VfCPXNj+jAd7AIY14hbQdSa);7K|^zl@o5F?G5uk09G82 zL1LKeO`cCFNM2hYFS$Y32=!4K#5}^y4hpZXkirX!fpUOh%qmz<`ount5AYb)u0{;7 zYQz8upNGI}yy;g$m)X!f8-a)$KIoo+9$bN{1P>(8LE>!r9^~cAe#LZY1%T9AU`wjC z`P3*0(p}VOVN|8vvx10)wmud#GbfdZ>(XYsZ@>UW2%S`*c%o zjDZn(aAgzgX&5r{>h@|2M@GR^wUgj9)s`X&i3TS_u1r~ImB+v!kFZNIj3$a-UJ`>4 zoey0z2q%G+Pz{O11bJ}fN3W9z5CL@(xude@RQNl@DR5DkPi4kgL>&sY4RN^Df@cnI zP>d7ff?0Iqy%kfEQkC6*Z*{UZ4jwc#f<^l!}%F#=B1j*4$sM`x@Fvv$D zDyu>hCe4`OFvz!}y_mzzDWCJiGk~^~s}m!tA`m|X-e8N*2S1xwf~yN3`ibGE6Je#q z=JU;mgt&;uW+V#5W}hz@4fuS**O6x&69JPLBvKJN7sh+#H`gnblJ7p>@YwOO>7jA? z8&n1LyBN!-PJqioD#7fecY17cVoKrDukcac-YLZaGI{3M2~ZM*Ygq4fOcGeRthjLJ zh363TJSKmM$+s{;Q;l4*%hP@gKhI)(+w5IY|l55jr0 zqkQ>fma#66WXpCyuDoJ-JZmjo9+i{Is^y6+N$h3IV_85xm9^TiR7=V$m&bvR&Axm* zTWZ^oL}BxEa-zLA>_=o)#9kivd8KH3YrV>#~!424{<82Z8K zR7(f;HmaoLQTKFA2e;xXuwa-7@W}Uv=$;-3lE#CyFCiHhp+e!uP-sP| z7KRK2x*3bVee#M3}4}(hNjIn5@57)U%Da>vYW*aoiuTR}Hb$`)tXO+wgvueyEpvzgvfS zw*m9L9Y<>nD8I0!KA%fB;VKhe2QpzAlOasdDu8gE>-cLp0nZ8H9fVlOc)79D0<2Al z&s^Z65JW0m_ko%ecS9maazdO4g!rl;VjmGRgluCR9N|?+-lF8S5109rL?4u;=wab~ z_=K^e;^UA&e|5S~^_4o~BR8ch{Uv4pB~|hfRsU;>f%GG)