diff --git a/jetson/ros2_ws/src/saltybot_face_bridge/config/face_bridge_params.yaml b/jetson/ros2_ws/src/saltybot_face_bridge/config/face_bridge_params.yaml new file mode 100644 index 0000000..8fc3a7b --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_face_bridge/config/face_bridge_params.yaml @@ -0,0 +1,13 @@ +face_bridge: + # HTTP server endpoint for face display + face_server_url: "http://localhost:3000/face/{id}" # {id} replaced with expression ID + + # HTTP request settings + http_timeout: 2.0 # Request timeout in seconds + update_interval: 0.1 # Update check interval in seconds + + # State to expression mapping: + # 0 = Tracking (IDLE, THROTTLED) + # 1 = Alert (LISTENING, wake word) + # 3 = Searching (THINKING) + # 4 = Social (SPEAKING) diff --git a/jetson/ros2_ws/src/saltybot_face_bridge/launch/face_bridge.launch.py b/jetson/ros2_ws/src/saltybot_face_bridge/launch/face_bridge.launch.py new file mode 100644 index 0000000..69681d1 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_face_bridge/launch/face_bridge.launch.py @@ -0,0 +1,40 @@ +"""Launch file for face display bridge node.""" + +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument + + +def generate_launch_description(): + """Generate launch description.""" + # Declare arguments + url_arg = DeclareLaunchArgument( + "face_server_url", + default_value="http://localhost:3000/face/{id}", + description="Face display server HTTP endpoint" + ) + timeout_arg = DeclareLaunchArgument( + "http_timeout", + default_value="2.0", + description="HTTP request timeout in seconds" + ) + + # Create node + face_bridge_node = Node( + package="saltybot_face_bridge", + executable="face_bridge_node", + name="face_bridge", + parameters=[ + {"face_server_url": LaunchConfiguration("face_server_url")}, + {"http_timeout": LaunchConfiguration("http_timeout")}, + {"update_interval": 0.1}, + ], + output="screen", + ) + + return LaunchDescription([ + url_arg, + timeout_arg, + face_bridge_node, + ]) diff --git a/jetson/ros2_ws/src/saltybot_face_bridge/package.xml b/jetson/ros2_ws/src/saltybot_face_bridge/package.xml new file mode 100644 index 0000000..f9504d3 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_face_bridge/package.xml @@ -0,0 +1,26 @@ + + + + saltybot_face_bridge + 0.1.0 + + Face display bridge node for orchestrator state to face expression mapping. + Maps social/orchestrator state to face display WebSocket API. + + sl-controls + MIT + + rclpy + std_msgs + + ament_python + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_face_bridge/resource/saltybot_face_bridge b/jetson/ros2_ws/src/saltybot_face_bridge/resource/saltybot_face_bridge new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_face_bridge/saltybot_face_bridge/__init__.py b/jetson/ros2_ws/src/saltybot_face_bridge/saltybot_face_bridge/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_face_bridge/saltybot_face_bridge/face_bridge_node.py b/jetson/ros2_ws/src/saltybot_face_bridge/saltybot_face_bridge/face_bridge_node.py new file mode 100644 index 0000000..abf7e61 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_face_bridge/saltybot_face_bridge/face_bridge_node.py @@ -0,0 +1,186 @@ +#!/usr/bin/env python3 +"""Face display bridge node. + +Maps orchestrator state to face expressions via HTTP WebSocket API. +Bridges /social/orchestrator/state and /saltybot/wake_word_detected to +face display server (localhost:3000/face/{id}). + +State mapping: + IDLE → 0 (Tracking) + LISTENING → 1 (Alert) + THINKING → 3 (Searching) + SPEAKING → 4 (Social) + Wake word → 1 (Alert) [immediate override] + +Subscribed topics: + /social/orchestrator/state (String) - JSON: {"state": "IDLE|LISTENING|THINKING|SPEAKING|THROTTLED"} + /saltybot/wake_word_detected (Bool) - Wake word detection trigger + +Published topics: + /face/state (String) - Current face expression ID and status +""" + +import json +import threading +import time +from typing import Optional + +import rclpy +from rclpy.node import Node +from std_msgs.msg import String, Bool + + +class FaceDisplayBridge(Node): + """Bridge orchestrator state to face display expressions.""" + + # State to face expression ID mapping + STATE_TO_FACE_ID = { + "IDLE": 0, # Tracking + "LISTENING": 1, # Alert + "THINKING": 3, # Searching + "SPEAKING": 4, # Social + "THROTTLED": 0, # Fallback to Tracking + } + + def __init__(self): + super().__init__("face_bridge") + + # Parameters + self.declare_parameter("face_server_url", "http://localhost:3000/face/1") + self.declare_parameter("http_timeout", 2.0) + self.declare_parameter("update_interval", 0.1) + + self.face_server_url = self.get_parameter("face_server_url").value + self.http_timeout = self.get_parameter("http_timeout").value + self.update_interval = self.get_parameter("update_interval").value + + # Try to import requests, fallback to urllib if unavailable + try: + import requests + self.requests = requests + self.use_requests = True + except ImportError: + import urllib.request + import urllib.error + self.urllib = urllib.request + self.urllib_error = urllib.error + self.use_requests = False + + # State + self.current_state = "IDLE" + self.current_face_id = 0 + self.wake_word_active = False + self.last_update_time = time.time() + self.state_lock = threading.Lock() + + # Subscriptions + self.create_subscription(String, "/social/orchestrator/state", self._on_state_update, 10) + self.create_subscription(Bool, "/saltybot/wake_word_detected", self._on_wake_word, 10) + + # Publishers + self.pub_state = self.create_publisher(String, "/face/state", 10) + + # Timer for update loop + self.create_timer(self.update_interval, self._update_face) + + self.get_logger().info( + f"Face bridge initialized: face_server_url={self.face_server_url}" + ) + + def _on_state_update(self, msg: String) -> None: + """Handle orchestrator state update.""" + try: + data = json.loads(msg.data) + new_state = data.get("state", "IDLE").upper() + + # Validate state + if new_state in self.STATE_TO_FACE_ID: + with self.state_lock: + self.current_state = new_state + self.get_logger().debug(f"State updated: {new_state}") + else: + self.get_logger().warn(f"Unknown state: {new_state}") + except json.JSONDecodeError: + self.get_logger().error(f"Invalid JSON in state update: {msg.data}") + + def _on_wake_word(self, msg: Bool) -> None: + """Handle wake word detection - immediate switch to Alert.""" + if msg.data: + with self.state_lock: + self.wake_word_active = True + self.get_logger().info("Wake word detected - switching to Alert") + + def _get_face_id(self) -> int: + """Get current face expression ID based on state.""" + with self.state_lock: + if self.wake_word_active: + face_id = 1 # Alert + # Clear wake word after one update + self.wake_word_active = False + else: + face_id = self.STATE_TO_FACE_ID.get(self.current_state, 0) + + return face_id + + def _send_face_command(self, face_id: int) -> bool: + """Send face expression command to display server. + + Args: + face_id: Expression ID (0-4) + + Returns: + True if successful + """ + try: + if self.use_requests: + response = self.requests.get( + self.face_server_url.format(id=face_id), + timeout=self.http_timeout + ) + return response.status_code == 200 + else: + url = self.face_server_url.format(id=face_id) + req = self.urllib.Request(url) + with self.urllib.urlopen(req, timeout=self.http_timeout) as response: + return response.status == 200 + except Exception as e: + self.get_logger().error(f"Failed to update face display: {e}") + return False + + def _update_face(self) -> None: + """Update face expression based on current state.""" + face_id = self._get_face_id() + + # Only send if changed + if face_id != self.current_face_id: + if self._send_face_command(face_id): + self.current_face_id = face_id + self.last_update_time = time.time() + + # Publish state + with self.state_lock: + state_msg = String( + data=json.dumps({ + "face_id": face_id, + "orchestrator_state": self.current_state, + "timestamp": self.last_update_time + }) + ) + self.pub_state.publish(state_msg) + self.get_logger().debug(f"Face updated: {face_id}") + + +def main(args=None): + rclpy.init(args=args) + node = FaceDisplayBridge() + 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_face_bridge/setup.cfg b/jetson/ros2_ws/src/saltybot_face_bridge/setup.cfg new file mode 100644 index 0000000..2923e00 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_face_bridge/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/saltybot_face_bridge +[egg_info] +tag_date = 0 diff --git a/jetson/ros2_ws/src/saltybot_face_bridge/setup.py b/jetson/ros2_ws/src/saltybot_face_bridge/setup.py new file mode 100644 index 0000000..e297bf9 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_face_bridge/setup.py @@ -0,0 +1,27 @@ +from setuptools import setup + +package_name = "saltybot_face_bridge" + +setup( + name=package_name, + version="0.1.0", + packages=[package_name], + data_files=[ + ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]), + (f"share/{package_name}", ["package.xml"]), + (f"share/{package_name}/launch", ["launch/face_bridge.launch.py"]), + (f"share/{package_name}/config", ["config/face_bridge_params.yaml"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="sl-controls", + maintainer_email="sl-controls@saltylab.local", + description="Face display bridge for orchestrator state mapping", + license="MIT", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "face_bridge_node = saltybot_face_bridge.face_bridge_node:main", + ], + }, +)