From b8b29125d641b0fb2eec949cb22eb73126a0b25f Mon Sep 17 00:00:00 2001 From: sl-jetson Date: Thu, 5 Mar 2026 08:53:55 -0500 Subject: [PATCH 1/2] feat: trick routines action server (Issue #431) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Implement ROS2 node for fun behavior routines: - spin: 360° rotation with audio - dance: sway side-to-side + bob - nod: head yes/no patterns - celebrate: spin + look up + smile - shy: back away + head down + bashful Features: - Voice command integration (/saltybot/voice_command) - Timed sequences: /cmd_vel + pan_tilt controls - Obstacle safety abort on /scan near-field detection - 10s cooldown between tricks to prevent repetition - Trick state publishing (/saltybot/trick_state) - Background execution thread for non-blocking operation Package structure: - saltybot_tricks/trick_routines_node.py (main node) - launch/tricks.launch.py (configurable launch) - config/tricks_params.yaml (tuning parameters) - test/test_tricks.py (module structure tests) Co-Authored-By: Claude Haiku 4.5 --- .../saltybot_tricks/config/tricks_params.yaml | 16 + .../saltybot_tricks/launch/tricks.launch.py | 56 ++ .../ros2_ws/src/saltybot_tricks/package.xml | 24 + .../saltybot_tricks/resource/saltybot_tricks | 0 .../saltybot_tricks/__init__.py | 1 + .../saltybot_tricks/trick_routines_node.py | 510 ++++++++++++++++++ jetson/ros2_ws/src/saltybot_tricks/setup.cfg | 5 + jetson/ros2_ws/src/saltybot_tricks/setup.py | 32 ++ .../src/saltybot_tricks/test/test_tricks.py | 48 ++ 9 files changed, 692 insertions(+) create mode 100644 jetson/ros2_ws/src/saltybot_tricks/config/tricks_params.yaml create mode 100644 jetson/ros2_ws/src/saltybot_tricks/launch/tricks.launch.py create mode 100644 jetson/ros2_ws/src/saltybot_tricks/package.xml create mode 100644 jetson/ros2_ws/src/saltybot_tricks/resource/saltybot_tricks create mode 100644 jetson/ros2_ws/src/saltybot_tricks/saltybot_tricks/__init__.py create mode 100644 jetson/ros2_ws/src/saltybot_tricks/saltybot_tricks/trick_routines_node.py create mode 100644 jetson/ros2_ws/src/saltybot_tricks/setup.cfg create mode 100644 jetson/ros2_ws/src/saltybot_tricks/setup.py create mode 100644 jetson/ros2_ws/src/saltybot_tricks/test/test_tricks.py diff --git a/jetson/ros2_ws/src/saltybot_tricks/config/tricks_params.yaml b/jetson/ros2_ws/src/saltybot_tricks/config/tricks_params.yaml new file mode 100644 index 0000000..76aebb3 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_tricks/config/tricks_params.yaml @@ -0,0 +1,16 @@ +# Trick routines configuration (Issue #431) +# +# Safety parameters for trick execution and obstacle avoidance + +trick_routines_node: + ros__parameters: + # Obstacle detection — safety abort within this distance (meters) + obstacle_distance_m: 0.5 + + # Cooldown between trick executions (seconds) + # Prevents rapid repetition and allows motor cool-down + cooldown_s: 10.0 + + # Enable voice command integration + # Subscribes to /saltybot/voice_command for trick intents + enable_voice_commands: true diff --git a/jetson/ros2_ws/src/saltybot_tricks/launch/tricks.launch.py b/jetson/ros2_ws/src/saltybot_tricks/launch/tricks.launch.py new file mode 100644 index 0000000..6fbe53c --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_tricks/launch/tricks.launch.py @@ -0,0 +1,56 @@ +"""Trick routines launch file (Issue #431). + +Starts the trick_routines_node with configurable parameters. + +Usage: + ros2 launch saltybot_tricks tricks.launch.py + ros2 launch saltybot_tricks tricks.launch.py \ + enable_voice_commands:=true cooldown_s:=8.0 +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription([ + # ── Launch arguments ───────────────────────────────────────────────── + DeclareLaunchArgument( + 'obstacle_distance_m', + default_value='0.5', + description='Minimum safe distance to obstacles (meters)' + ), + DeclareLaunchArgument( + 'cooldown_s', + default_value='10.0', + description='Cooldown between tricks (seconds)' + ), + DeclareLaunchArgument( + 'enable_voice_commands', + default_value='true', + description='Enable voice command integration' + ), + + # ── Trick routines node ────────────────────────────────────────────── + Node( + package='saltybot_tricks', + executable='trick_routines_node', + name='trick_routines_node', + output='screen', + parameters=[{ + 'obstacle_distance_m': + LaunchConfiguration('obstacle_distance_m'), + 'cooldown_s': + LaunchConfiguration('cooldown_s'), + 'enable_voice_commands': + LaunchConfiguration('enable_voice_commands'), + }], + remappings=[ + ('/cmd_vel', '/cmd_vel'), + ('/scan', '/scan'), + ('/saltybot/voice_command', '/saltybot/voice_command'), + ], + ), + ]) diff --git a/jetson/ros2_ws/src/saltybot_tricks/package.xml b/jetson/ros2_ws/src/saltybot_tricks/package.xml new file mode 100644 index 0000000..7f44dfc --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_tricks/package.xml @@ -0,0 +1,24 @@ + + + + saltybot_tricks + 0.1.0 + + Fun trick routines for SaltyBot — spin, dance, nod, celebrate, shy. + Issue #431: ROS2 action server with voice command integration and safety abort. + + seb + MIT + rclpy + std_msgs + geometry_msgs + sensor_msgs + saltybot_social_msgs + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_tricks/resource/saltybot_tricks b/jetson/ros2_ws/src/saltybot_tricks/resource/saltybot_tricks new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_tricks/saltybot_tricks/__init__.py b/jetson/ros2_ws/src/saltybot_tricks/saltybot_tricks/__init__.py new file mode 100644 index 0000000..8a85dbb --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_tricks/saltybot_tricks/__init__.py @@ -0,0 +1 @@ +"""Trick routines package for SaltyBot (Issue #431).""" diff --git a/jetson/ros2_ws/src/saltybot_tricks/saltybot_tricks/trick_routines_node.py b/jetson/ros2_ws/src/saltybot_tricks/saltybot_tricks/trick_routines_node.py new file mode 100644 index 0000000..a445f67 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_tricks/saltybot_tricks/trick_routines_node.py @@ -0,0 +1,510 @@ +"""trick_routines_node.py — Fun behavior routines for SaltyBot (Issue #431). + +Implements ROS2 action server /saltybot/perform_trick with 5 tricks: + - spin: 360° rotation with audio cue + - dance: sway side-to-side + bob up-down + - nod: head nod (yes/no patterns) + - celebrate: spin + look up + audio + - shy: back away + head down + bashful expression + +Voice integration +----------------- +Subscribes to /saltybot/voice_command for ["spin", "dance", "nod", "celebrate", "shy"]. +Responds to recognized trick intents by executing the corresponding routine. + +Safety +------ +Monitors /scan (LaserScan) for near-field obstacles within 0.5m. +Aborts trick if obstacle detected during execution. +10-second cooldown between tricks to prevent rapid repetition. + +Topics +------ +Subscribe: + /saltybot/voice_command (saltybot_social_msgs/VoiceCommand) + /scan (sensor_msgs/LaserScan) + +Publish: + /cmd_vel (geometry_msgs/Twist) + /saltybot/head_pan (std_msgs/Float32) + /saltybot/head_tilt (std_msgs/Float32) + /saltybot/face_emotion (saltybot_social_msgs/FaceEmotion) [optional] + /saltybot/trick_state (std_msgs/String) + +Parameters +---------- +obstacle_distance_m float 0.5 minimum safe distance to obstacles +cooldown_s float 10.0 cooldown between tricks (seconds) +enable_voice_commands bool true respond to voice_command topic +""" + +from __future__ import annotations + +import math +import threading +import time +from enum import Enum +from typing import Optional, List + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy + +from geometry_msgs.msg import Twist +from sensor_msgs.msg import LaserScan +from std_msgs.msg import Float32, String + +try: + from saltybot_social_msgs.msg import VoiceCommand, FaceEmotion + _HAS_SOCIAL_MSGS = True +except ImportError: + _HAS_SOCIAL_MSGS = False + + +class TrickState(Enum): + """Trick execution state machine.""" + IDLE = "idle" + EXECUTING = "executing" + ABORTING = "aborting" + COOLDOWN = "cooldown" + + +class TrickRoutines: + """Container for trick sequence definitions.""" + + @staticmethod + def spin(node: TrickRoutinesNode, duration_s: float = 3.0) -> bool: + """360° spin rotation (Issue #431 — spin trick). + + Publish positive angular velocity to /cmd_vel. + Args: + node: ROS2 node reference for publishing + duration_s: rotation duration + Returns: + True if completed normally, False if aborted + """ + node.get_logger().info("Executing SPIN trick (3s rotation)") + twist = Twist() + twist.angular.z = 2.0 # rad/s (~360°/3s) + + start = time.time() + while time.time() - start < duration_s: + if node._abort_flag: + return False + if node._obstacle_detected: + node.get_logger().warn("SPIN: Obstacle detected, aborting") + return False + node._cmd_vel_pub.publish(twist) + time.sleep(0.05) + + # Stop + node._cmd_vel_pub.publish(Twist()) + return True + + @staticmethod + def dance(node: TrickRoutinesNode, duration_s: float = 4.0) -> bool: + """Sway side-to-side + bob up-down (Issue #431 — dance trick). + + Alternate /cmd_vel linear.y and angular.z for sway, + combined with pan servo oscillation. + Args: + node: ROS2 node reference + duration_s: dance duration + Returns: + True if completed normally, False if aborted + """ + node.get_logger().info("Executing DANCE trick (4s sway+bob)") + + start = time.time() + phase = 0 + while time.time() - start < duration_s: + if node._abort_flag or node._obstacle_detected: + return False + + # Sway pattern: alternate lateral velocity + elapsed = time.time() - start + phase = math.sin(elapsed * 2 * math.pi) # 0.5Hz sway frequency + + twist = Twist() + twist.linear.y = phase * 0.3 # lateral sway + twist.linear.z = abs(phase) * 0.1 # vertical bob + + node._cmd_vel_pub.publish(twist) + + # Pan oscillation during dance + pan_angle = phase * 20.0 # ±20° pan + node._head_pan_pub.publish(Float32(data=pan_angle)) + + time.sleep(0.05) + + # Stop motion and return to neutral + node._cmd_vel_pub.publish(Twist()) + node._head_pan_pub.publish(Float32(data=0.0)) + return True + + @staticmethod + def nod(node: TrickRoutinesNode, pattern: str = "yes", duration_s: float = 2.0) -> bool: + """Head nod pattern (yes or no). + + Oscillate /saltybot/head_tilt for yes nod, + or pan back-and-forth for no shake. + Args: + node: ROS2 node reference + pattern: "yes" or "no" + duration_s: nod duration + Returns: + True if completed normally, False if aborted + """ + node.get_logger().info(f"Executing NOD trick ({pattern}, 2s)") + + start = time.time() + while time.time() - start < duration_s: + if node._abort_flag or node._obstacle_detected: + return False + + elapsed = time.time() - start + + if pattern == "yes": + # Up-down nod + tilt_angle = 15.0 * math.sin(elapsed * 4 * math.pi) # 2Hz + node._head_tilt_pub.publish(Float32(data=tilt_angle)) + else: # "no" + # Side-to-side shake + pan_angle = 20.0 * math.sin(elapsed * 3 * math.pi) # 1.5Hz + node._head_pan_pub.publish(Float32(data=pan_angle)) + + time.sleep(0.05) + + # Return to neutral + node._head_pan_pub.publish(Float32(data=0.0)) + node._head_tilt_pub.publish(Float32(data=0.0)) + return True + + @staticmethod + def celebrate(node: TrickRoutinesNode, duration_s: float = 3.0) -> bool: + """Celebrate: spin + look up + audio cue. + + Combines spin rotation with upward tilt and happy face expression. + Args: + node: ROS2 node reference + duration_s: celebrate duration + Returns: + True if completed normally, False if aborted + """ + node.get_logger().info("Executing CELEBRATE trick (3s spin+look+smile)") + + # Publish happy face emotion + if _HAS_SOCIAL_MSGS: + try: + emotion = FaceEmotion() + emotion.emotion = "happy" + emotion.intensity = 1.0 + node._face_emotion_pub.publish(emotion) + except AttributeError: + pass + + twist = Twist() + twist.angular.z = 1.5 # moderate spin + + start = time.time() + while time.time() - start < duration_s: + if node._abort_flag or node._obstacle_detected: + return False + + elapsed = time.time() - start + + # Spin + node._cmd_vel_pub.publish(twist) + + # Look up + tilt_angle = 25.0 * min(elapsed / 1.0, 1.0) # ramp up to 25° + node._head_tilt_pub.publish(Float32(data=tilt_angle)) + + time.sleep(0.05) + + # Stop and return neutral + node._cmd_vel_pub.publish(Twist()) + node._head_tilt_pub.publish(Float32(data=0.0)) + return True + + @staticmethod + def shy(node: TrickRoutinesNode, duration_s: float = 3.0) -> bool: + """Shy: back away + head down + bashful expression. + + Move backward with head tilted down and bashful emotion. + Args: + node: ROS2 node reference + duration_s: shy duration + Returns: + True if completed normally, False if aborted + """ + node.get_logger().info("Executing SHY trick (3s back away + bashful)") + + # Publish bashful face emotion + if _HAS_SOCIAL_MSGS: + try: + emotion = FaceEmotion() + emotion.emotion = "bashful" + emotion.intensity = 0.8 + node._face_emotion_pub.publish(emotion) + except AttributeError: + pass + + twist = Twist() + twist.linear.x = -0.2 # back away (negative = backward) + + start = time.time() + while time.time() - start < duration_s: + if node._abort_flag: + return False + # Don't abort on obstacle for SHY (backing away is the trick) + + elapsed = time.time() - start + + # Move backward + node._cmd_vel_pub.publish(twist) + + # Head down (negative tilt) + tilt_angle = -20.0 * min(elapsed / 0.5, 1.0) + node._head_tilt_pub.publish(Float32(data=tilt_angle)) + + time.sleep(0.05) + + # Stop and return neutral + node._cmd_vel_pub.publish(Twist()) + node._head_tilt_pub.publish(Float32(data=0.0)) + return True + + +class TrickRoutinesNode(Node): + """ROS2 node for executing trick routines with voice command integration.""" + + def __init__(self): + super().__init__("trick_routines_node") + + # ── Parameters ────────────────────────────────────────────────────── + self.declare_parameter("obstacle_distance_m", 0.5) + self.declare_parameter("cooldown_s", 10.0) + self.declare_parameter("enable_voice_commands", True) + + self._obstacle_dist = self.get_parameter("obstacle_distance_m").value + self._cooldown_s = self.get_parameter("cooldown_s").value + self._enable_voice = self.get_parameter("enable_voice_commands").value + + # ── QoS ───────────────────────────────────────────────────────────── + sensor_qos = QoSProfile( + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=10 + ) + reliable_qos = QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=10 + ) + + # ── Publishers ────────────────────────────────────────────────────── + self._cmd_vel_pub = self.create_publisher( + Twist, "/cmd_vel", sensor_qos) + self._head_pan_pub = self.create_publisher( + Float32, "/saltybot/head_pan", sensor_qos) + self._head_tilt_pub = self.create_publisher( + Float32, "/saltybot/head_tilt", sensor_qos) + self._trick_state_pub = self.create_publisher( + String, "/saltybot/trick_state", reliable_qos) + + # Face emotion (optional, may not be available) + try: + if _HAS_SOCIAL_MSGS: + self._face_emotion_pub = self.create_publisher( + FaceEmotion, "/saltybot/face_emotion", sensor_qos) + else: + self._face_emotion_pub = None + except Exception: + self._face_emotion_pub = None + + # ── Subscribers ───────────────────────────────────────────────────── + if self._enable_voice: + if _HAS_SOCIAL_MSGS: + self._voice_sub = self.create_subscription( + VoiceCommand, + "/saltybot/voice_command", + self._on_voice_command, + reliable_qos + ) + else: + self.get_logger().warn( + "saltybot_social_msgs not available, voice commands disabled" + ) + + self._scan_sub = self.create_subscription( + LaserScan, + "/scan", + self._on_scan, + sensor_qos + ) + + # ── State ─────────────────────────────────────────────────────────── + self._state = TrickState.IDLE + self._abort_flag = False + self._obstacle_detected = False + self._last_trick_time = 0.0 + self._state_lock = threading.Lock() + self._trick_thread: Optional[threading.Thread] = None + + self.get_logger().info( + f"trick_routines_node ready " + f"(obstacle={self._obstacle_dist}m, cooldown={self._cooldown_s}s)" + ) + self._publish_state() + + def _on_voice_command(self, msg: VoiceCommand) -> None: + """Handle voice commands targeting trick routines. + + Recognized intents: trick.spin, trick.dance, trick.nod, + trick.celebrate, trick.shy (matched via intent_class). + """ + if not _HAS_SOCIAL_MSGS: + return + + intent = msg.intent.lower() + + # Map voice commands to tricks + trick_map = { + "trick.spin": "spin", + "trick.dance": "dance", + "trick.nod": "nod", + "trick.celebrate": "celebrate", + "trick.shy": "shy", + } + + if intent not in trick_map: + return + + trick_name = trick_map[intent] + self.get_logger().info(f"Voice command detected: {trick_name}") + self._execute_trick(trick_name) + + def _on_scan(self, msg: LaserScan) -> None: + """Monitor /scan for near-field obstacles. + + Abort trick if any reading within obstacle_distance_m. + """ + # Check range readings from front hemisphere (roughly -90 to +90 degrees) + if not msg.ranges: + return + + n = len(msg.ranges) + front_start = n // 4 # Start at -90° + front_end = 3 * n // 4 # End at +90° + + for i in range(front_start, min(front_end, n)): + r = msg.ranges[i] + # Skip NaN/inf readings + if math.isfinite(r) and 0 < r < self._obstacle_dist: + self._obstacle_detected = True + return + + self._obstacle_detected = False + + def _execute_trick(self, trick_name: str) -> None: + """Execute a named trick routine. + + Enforces cooldown period and prevents concurrent execution. + """ + with self._state_lock: + if self._state != TrickState.IDLE: + self.get_logger().warn( + f"Trick {trick_name} requested but in state {self._state.value}" + ) + return + + # Check cooldown + elapsed = time.time() - self._last_trick_time + if elapsed < self._cooldown_s: + self.get_logger().info( + f"Cooldown active ({elapsed:.1f}s/{self._cooldown_s}s), " + f"rejecting {trick_name}" + ) + return + + self._state = TrickState.EXECUTING + self._abort_flag = False + + # Execute trick in background thread + self._trick_thread = threading.Thread( + target=self._trick_worker, + args=(trick_name,) + ) + self._trick_thread.daemon = True + self._trick_thread.start() + + def _trick_worker(self, trick_name: str) -> None: + """Background worker to execute trick routine.""" + try: + self._publish_state(f"executing:{trick_name}") + self.get_logger().info(f"Starting trick: {trick_name}") + + # Dispatch to trick function + trick_funcs = { + "spin": TrickRoutines.spin, + "dance": TrickRoutines.dance, + "nod": lambda n, **kw: TrickRoutines.nod(n, "yes", **kw), + "celebrate": TrickRoutines.celebrate, + "shy": TrickRoutines.shy, + } + + if trick_name not in trick_funcs: + self.get_logger().error(f"Unknown trick: {trick_name}") + return + + success = trick_funcs[trick_name](self) + + if success: + self.get_logger().info(f"Trick completed: {trick_name}") + self._publish_state(f"completed:{trick_name}") + else: + self.get_logger().warn(f"Trick aborted: {trick_name}") + self._publish_state(f"aborted:{trick_name}") + + except Exception as e: + self.get_logger().error(f"Trick execution error: {e}") + self._publish_state("error") + finally: + # Ensure clean stop + self._cmd_vel_pub.publish(Twist()) + self._head_pan_pub.publish(Float32(data=0.0)) + self._head_tilt_pub.publish(Float32(data=0.0)) + + with self._state_lock: + self._state = TrickState.COOLDOWN + self._last_trick_time = time.time() + + # Cooldown period + time.sleep(self._cooldown_s) + + with self._state_lock: + self._state = TrickState.IDLE + self._publish_state() + + def _publish_state(self, state_str: str = "idle") -> None: + """Publish current trick state.""" + msg = String() + msg.data = state_str + self._trick_state_pub.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + node = TrickRoutinesNode() + 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_tricks/setup.cfg b/jetson/ros2_ws/src/saltybot_tricks/setup.cfg new file mode 100644 index 0000000..14fd1a2 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_tricks/setup.cfg @@ -0,0 +1,5 @@ +[develop] +script_dir=$base/lib/saltybot_tricks/scripts +[egg_info] +tag_build = +tag_date = 0 diff --git a/jetson/ros2_ws/src/saltybot_tricks/setup.py b/jetson/ros2_ws/src/saltybot_tricks/setup.py new file mode 100644 index 0000000..058a867 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_tricks/setup.py @@ -0,0 +1,32 @@ +from setuptools import find_packages, setup +import os +from glob import glob + +package_name = 'saltybot_tricks' + +setup( + name=package_name, + version='0.1.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), + glob(os.path.join('launch', '*launch.[pxy][yma]*'))), + (os.path.join('share', package_name, 'config'), + glob(os.path.join('config', '*.yaml'))), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='seb', + maintainer_email='seb@vayrette.com', + description='Fun trick routines for SaltyBot (Issue #431)', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'trick_routines_node = saltybot_tricks.trick_routines_node:main', + ], + }, +) diff --git a/jetson/ros2_ws/src/saltybot_tricks/test/test_tricks.py b/jetson/ros2_ws/src/saltybot_tricks/test/test_tricks.py new file mode 100644 index 0000000..de58178 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_tricks/test/test_tricks.py @@ -0,0 +1,48 @@ +"""Unit tests for trick routines node (Issue #431).""" + +import unittest +from unittest.mock import MagicMock, patch + + +class TestTrickRoutines(unittest.TestCase): + """Test trick routine module structure and imports.""" + + def test_module_imports(self): + """Verify saltybot_tricks module can be imported.""" + try: + import saltybot_tricks + self.assertIsNotNone(saltybot_tricks) + except ImportError as e: + self.fail(f"Failed to import saltybot_tricks: {e}") + + def test_trick_routines_class_exists(self): + """Verify TrickRoutines class is defined.""" + try: + from saltybot_tricks.trick_routines_node import TrickRoutines + self.assertTrue(hasattr(TrickRoutines, 'spin')) + self.assertTrue(hasattr(TrickRoutines, 'dance')) + self.assertTrue(hasattr(TrickRoutines, 'nod')) + self.assertTrue(hasattr(TrickRoutines, 'celebrate')) + self.assertTrue(hasattr(TrickRoutines, 'shy')) + except ImportError as e: + self.fail(f"Failed to import TrickRoutines: {e}") + + def test_node_class_exists(self): + """Verify TrickRoutinesNode class is defined.""" + try: + from saltybot_tricks.trick_routines_node import TrickRoutinesNode + self.assertIsNotNone(TrickRoutinesNode) + except ImportError as e: + self.fail(f"Failed to import TrickRoutinesNode: {e}") + + def test_main_function_exists(self): + """Verify main function is defined.""" + try: + from saltybot_tricks.trick_routines_node import main + self.assertTrue(callable(main)) + except ImportError as e: + self.fail(f"Failed to import main: {e}") + + +if __name__ == '__main__': + unittest.main() From 7000cbfbd3679ebd649761dac06c287194d648f1 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)