From 5143e5bfac60e4aa14cd214d4cd399234012cb85 Mon Sep 17 00:00:00 2001 From: sl-firmware Date: Sun, 1 Mar 2026 23:09:13 -0500 Subject: [PATCH] =?UTF-8?q?feat(social):=20Issue=20#86=20=E2=80=94=20physi?= =?UTF-8?q?cal=20expression=20+=20motor=20attention?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit ESP32-C3 NeoPixel sketch (esp32/social_expression/social_expression.ino): - Adafruit NeoPixel + ArduinoJson, serial JSON protocol 115200 8N1 - Mood→colour: happy=green, curious=blue, annoyed=red, playful=rainbow - Idle breathing animation (sine-modulated warm white) - Auto-falls to idle after IDLE_TIMEOUT_MS (3 s) with no command ROS2 saltybot_social_msgs (new package): - Mood.msg — {mood, intensity} - Person.msg — {track_id, bearing_rad, distance_m, confidence, is_speaking, source} - PersonArray.msg — {persons[], active_id} ROS2 saltybot_social (new package): - expression_node: subscribes /social/mood → JSON serial to ESP32-C3 reconnects on port error; sends idle frame after idle_timeout_s - attention_node: subscribes /social/persons → /cmd_vel rotation-only proportional control with dead zone; prefers active speaker, falls back to highest-confidence person; lost-target idle after 2 s - launch/social.launch.py — combined launch - config YAML for both nodes with documented parameters - test/test_attention.py — 15 pytest-only unit tests Co-Authored-By: Claude Sonnet 4.6 --- esp32/social_expression/social_expression.ino | 206 ++++++++++++++++++ .../config/attention_params.yaml | 30 +++ .../config/expression_params.yaml | 22 ++ .../saltybot_social/launch/social.launch.py | 65 ++++++ .../ros2_ws/src/saltybot_social/package.xml | 7 +- .../saltybot_social/attention_node.py | 205 +++++++++++++++++ .../saltybot_social/expression_node.py | 157 +++++++++++++ jetson/ros2_ws/src/saltybot_social/setup.py | 4 +- .../saltybot_social/test/test_attention.py | 140 ++++++++++++ .../src/saltybot_social_msgs/CMakeLists.txt | 6 + .../src/saltybot_social_msgs/msg/Mood.msg | 7 + .../src/saltybot_social_msgs/msg/Person.msg | 17 ++ .../saltybot_social_msgs/msg/PersonArray.msg | 9 + 13 files changed, 873 insertions(+), 2 deletions(-) create mode 100644 esp32/social_expression/social_expression.ino create mode 100644 jetson/ros2_ws/src/saltybot_social/config/attention_params.yaml create mode 100644 jetson/ros2_ws/src/saltybot_social/config/expression_params.yaml create mode 100644 jetson/ros2_ws/src/saltybot_social/saltybot_social/attention_node.py create mode 100644 jetson/ros2_ws/src/saltybot_social/saltybot_social/expression_node.py create mode 100644 jetson/ros2_ws/src/saltybot_social/test/test_attention.py create mode 100644 jetson/ros2_ws/src/saltybot_social_msgs/msg/Mood.msg create mode 100644 jetson/ros2_ws/src/saltybot_social_msgs/msg/Person.msg create mode 100644 jetson/ros2_ws/src/saltybot_social_msgs/msg/PersonArray.msg diff --git a/esp32/social_expression/social_expression.ino b/esp32/social_expression/social_expression.ino new file mode 100644 index 0000000..8cf7f91 --- /dev/null +++ b/esp32/social_expression/social_expression.ino @@ -0,0 +1,206 @@ +/* + * social_expression.ino — LED mood display for saltybot social layer. + * + * Hardware + * ──────── + * MCU : ESP32-C3 (e.g. Seeed XIAO ESP32-C3 or equivalent) + * LEDs : NeoPixel ring or strip, DATA_PIN → GPIO5, power via 5V rail + * + * Serial protocol (from Orin Nano, 115200 8N1) + * ───────────────────────────────────────────── + * One JSON line per command, newline-terminated: + * {"mood":"happy","intensity":0.8}\n + * + * mood values : "happy" | "curious" | "annoyed" | "playful" | "idle" + * intensity : 0.0 (off) .. 1.0 (full brightness) + * + * If no command arrives for IDLE_TIMEOUT_MS, the node enters the + * breathing idle animation automatically. + * + * Mood → colour mapping + * ────────────────────── + * happy → green #00FF00 + * curious → blue #0040FF + * annoyed → red #FF1000 + * playful → rainbow (cycling hue across all pixels) + * idle → soft white breathing (sine modulated) + * + * Dependencies (Arduino Library Manager) + * ─────────────────────────────────────── + * Adafruit NeoPixel ≥ 1.11.0 + * ArduinoJson ≥ 7.0.0 + * + * Build: Arduino IDE 2 / arduino-cli with board "ESP32C3 Dev Module" + * Board manager URL: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_index.json + */ + +#include +#include + +// ── Hardware config ────────────────────────────────────────────────────────── +#define LED_PIN 5 // GPIO connected to NeoPixel data line +#define LED_COUNT 16 // pixels in ring / strip — adjust as needed +#define LED_BRIGHTNESS 120 // global cap 0-255 (≈47%), protects PSU + +// ── Timing constants ───────────────────────────────────────────────────────── +#define IDLE_TIMEOUT_MS 3000 // fall back to breathing after 3 s silence +#define LOOP_INTERVAL_MS 20 // animation tick ≈ 50 Hz + +// ── Colours (R, G, B) ──────────────────────────────────────────────────────── +static const uint8_t COL_HAPPY[3] = { 0, 220, 0 }; // green +static const uint8_t COL_CURIOUS[3] = { 0, 64, 255 }; // blue +static const uint8_t COL_ANNOYED[3] = {255, 16, 0 }; // red + +// ── State ──────────────────────────────────────────────────────────────────── +enum Mood { MOOD_IDLE, MOOD_HAPPY, MOOD_CURIOUS, MOOD_ANNOYED, MOOD_PLAYFUL }; + +static Mood g_mood = MOOD_IDLE; +static float g_intensity = 1.0f; +static uint32_t g_last_cmd_ms = 0; // millis() of last received command + +// Animation counters +static uint16_t g_rainbow_hue = 0; // 0..65535, cycles for playful +static uint32_t g_last_tick = 0; + +// Serial receive buffer +static char g_serial_buf[128]; +static uint8_t g_buf_pos = 0; + +// ── NeoPixel object ────────────────────────────────────────────────────────── +Adafruit_NeoPixel strip(LED_COUNT, LED_PIN, NEO_GRB + NEO_KHZ800); + +// ── Helpers ────────────────────────────────────────────────────────────────── + +static uint8_t scale(uint8_t v, float intensity) { + return (uint8_t)(v * intensity); +} + +/* + * Breathing: sine envelope over time. + * Returns brightness factor 0.0..1.0, period ~4 s. + */ +static float breath_factor(uint32_t now_ms) { + float phase = (float)(now_ms % 4000) / 4000.0f; // 0..1 per period + return 0.08f + 0.35f * (0.5f + 0.5f * sinf(2.0f * M_PI * phase)); +} + +static void set_all(uint8_t r, uint8_t g, uint8_t b) { + for (int i = 0; i < LED_COUNT; i++) { + strip.setPixelColor(i, strip.Color(r, g, b)); + } +} + +// ── Animation drivers ──────────────────────────────────────────────────────── + +static void animate_solid(const uint8_t col[3], float intensity) { + set_all(scale(col[0], intensity), + scale(col[1], intensity), + scale(col[2], intensity)); +} + +static void animate_breathing(uint32_t now_ms, float intensity) { + float bf = breath_factor(now_ms) * intensity; + // Warm white: R=255 G=200 B=120 + set_all(scale(255, bf), scale(200, bf), scale(120, bf)); +} + +static void animate_rainbow(float intensity) { + // Spread full wheel across all pixels + for (int i = 0; i < LED_COUNT; i++) { + uint16_t hue = g_rainbow_hue + (uint16_t)((float)i / LED_COUNT * 65536.0f); + uint32_t rgb = strip.ColorHSV(hue, 255, + (uint8_t)(255.0f * intensity)); + strip.setPixelColor(i, rgb); + } + // Advance hue each tick (full cycle in ~6 s at 50 Hz) + g_rainbow_hue += 218; +} + +// ── Serial parser ──────────────────────────────────────────────────────────── + +static void parse_command(const char *line) { + StaticJsonDocument<128> doc; + DeserializationError err = deserializeJson(doc, line); + if (err) return; + + const char *mood_str = doc["mood"] | ""; + float intensity = doc["intensity"] | 1.0f; + if (intensity < 0.0f) intensity = 0.0f; + if (intensity > 1.0f) intensity = 1.0f; + + if (strcmp(mood_str, "happy") == 0) g_mood = MOOD_HAPPY; + else if (strcmp(mood_str, "curious") == 0) g_mood = MOOD_CURIOUS; + else if (strcmp(mood_str, "annoyed") == 0) g_mood = MOOD_ANNOYED; + else if (strcmp(mood_str, "playful") == 0) g_mood = MOOD_PLAYFUL; + else g_mood = MOOD_IDLE; + + g_intensity = intensity; + g_last_cmd_ms = millis(); +} + +static void read_serial(void) { + while (Serial.available()) { + char c = (char)Serial.read(); + if (c == '\n' || c == '\r') { + if (g_buf_pos > 0) { + g_serial_buf[g_buf_pos] = '\0'; + parse_command(g_serial_buf); + g_buf_pos = 0; + } + } else if (g_buf_pos < (sizeof(g_serial_buf) - 1)) { + g_serial_buf[g_buf_pos++] = c; + } else { + // Buffer overflow — discard line + g_buf_pos = 0; + } + } +} + +// ── Arduino entry points ───────────────────────────────────────────────────── + +void setup(void) { + Serial.begin(115200); + + strip.begin(); + strip.setBrightness(LED_BRIGHTNESS); + strip.clear(); + strip.show(); + + g_last_tick = millis(); +} + +void loop(void) { + uint32_t now = millis(); + + read_serial(); + + // Fall back to idle if no command for IDLE_TIMEOUT_MS + if ((now - g_last_cmd_ms) > IDLE_TIMEOUT_MS) { + g_mood = MOOD_IDLE; + } + + // Throttle animation ticks + if ((now - g_last_tick) < LOOP_INTERVAL_MS) return; + g_last_tick = now; + + switch (g_mood) { + case MOOD_HAPPY: + animate_solid(COL_HAPPY, g_intensity); + break; + case MOOD_CURIOUS: + animate_solid(COL_CURIOUS, g_intensity); + break; + case MOOD_ANNOYED: + animate_solid(COL_ANNOYED, g_intensity); + break; + case MOOD_PLAYFUL: + animate_rainbow(g_intensity); + break; + case MOOD_IDLE: + default: + animate_breathing(now, g_intensity > 0.0f ? g_intensity : 1.0f); + break; + } + + strip.show(); +} diff --git a/jetson/ros2_ws/src/saltybot_social/config/attention_params.yaml b/jetson/ros2_ws/src/saltybot_social/config/attention_params.yaml new file mode 100644 index 0000000..bcc413a --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_social/config/attention_params.yaml @@ -0,0 +1,30 @@ +# attention_params.yaml — Social attention controller configuration. +# +# ── Proportional gain ───────────────────────────────────────────────────────── +# angular_z = clamp(kp_angular * bearing_rad, ±max_angular_vel) +# +# kp_angular : 1.0 → 1.0 rad/s per 1 rad error (57° → full speed). +# Increase for snappier tracking; decrease if robot oscillates. +kp_angular: 1.0 # rad/s per rad + +# ── Velocity limits ────────────────────────────────────────────────────────── +# Hard cap on rotation output. In social mode the balance loop must not be +# disturbed by large sudden angular commands; keep this ≤ 1.0 rad/s. +max_angular_vel: 0.8 # rad/s + +# ── Dead zone ──────────────────────────────────────────────────────────────── +# If |bearing| ≤ dead_zone_rad, rotation is suppressed. +# 0.15 rad ≈ 8.6° — prevents chattering when person is roughly centred. +dead_zone_rad: 0.15 # radians + +# ── Control loop ───────────────────────────────────────────────────────────── +control_rate: 20.0 # Hz + +# ── Lost-target timeout ─────────────────────────────────────────────────────── +# If no /social/persons message arrives for this many seconds, publish zero +# and enter IDLE state. The cmd_vel bridge deadman will also kick in. +lost_timeout_s: 2.0 # seconds + +# ── Master enable ───────────────────────────────────────────────────────────── +# Toggle at runtime: ros2 param set /attention_node attention_enabled false +attention_enabled: true diff --git a/jetson/ros2_ws/src/saltybot_social/config/expression_params.yaml b/jetson/ros2_ws/src/saltybot_social/config/expression_params.yaml new file mode 100644 index 0000000..9134a74 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_social/config/expression_params.yaml @@ -0,0 +1,22 @@ +# expression_params.yaml — LED mood display bridge configuration. +# +# serial_port : udev symlink or device path for the ESP32-C3 USB-CDC port. +# Recommended: create a udev rule: +# SUBSYSTEM=="tty", ATTRS{idVendor}=="303a", ATTRS{idProduct}=="1001", +# SYMLINK+="esp32-social" +# +# baud_rate : must match social_expression.ino (default 115200) +# +# idle_timeout_s : if no /social/mood message arrives for this many seconds, +# the node sends {"mood":"idle","intensity":1.0} so the ESP32 breathing +# animation is synchronised with node awareness. +# +# control_rate : how often to check the idle timeout (Hz). +# Does NOT gate the mood messages — those are forwarded immediately. + +expression_node: + ros__parameters: + serial_port: /dev/esp32-social + baud_rate: 115200 + idle_timeout_s: 3.0 + control_rate: 10.0 diff --git a/jetson/ros2_ws/src/saltybot_social/launch/social.launch.py b/jetson/ros2_ws/src/saltybot_social/launch/social.launch.py index 3046d30..ea957c5 100644 --- a/jetson/ros2_ws/src/saltybot_social/launch/social.launch.py +++ b/jetson/ros2_ws/src/saltybot_social/launch/social.launch.py @@ -1,3 +1,18 @@ +""" +social.launch.py — Launch the full saltybot social stack. + +Includes: + person_state_tracker — multi-modal person identity fusion (Issue #82) + expression_node — /social/mood → ESP32-C3 NeoPixel serial (Issue #86) + attention_node — /social/persons → /cmd_vel rotation (Issue #86) + +Usage: + ros2 launch saltybot_social social.launch.py + ros2 launch saltybot_social social.launch.py serial_port:=/dev/ttyUSB1 +""" + +import os +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration @@ -5,7 +20,12 @@ from launch_ros.actions import Node def generate_launch_description(): + pkg = get_package_share_directory("saltybot_social") + exp_cfg = os.path.join(pkg, "config", "expression_params.yaml") + att_cfg = os.path.join(pkg, "config", "attention_params.yaml") + return LaunchDescription([ + # person_state_tracker args (Issue #82) DeclareLaunchArgument( 'engagement_distance', default_value='2.0', @@ -21,6 +41,19 @@ def generate_launch_description(): default_value='false', description='Whether UWB anchor data is available' ), + + # expression_node args (Issue #86) + DeclareLaunchArgument("serial_port", default_value="/dev/esp32-social"), + DeclareLaunchArgument("baud_rate", default_value="115200"), + DeclareLaunchArgument("idle_timeout_s", default_value="3.0"), + + # attention_node args (Issue #86) + DeclareLaunchArgument("kp_angular", default_value="1.0"), + DeclareLaunchArgument("max_angular_vel", default_value="0.8"), + DeclareLaunchArgument("dead_zone_rad", default_value="0.15"), + DeclareLaunchArgument("lost_timeout_s", default_value="2.0"), + DeclareLaunchArgument("attention_enabled", default_value="true"), + Node( package='saltybot_social', executable='person_state_tracker', @@ -32,4 +65,36 @@ def generate_launch_description(): 'uwb_enabled': LaunchConfiguration('uwb_enabled'), }], ), + + Node( + package="saltybot_social", + executable="expression_node", + name="expression_node", + output="screen", + parameters=[ + exp_cfg, + { + "serial_port": LaunchConfiguration("serial_port"), + "baud_rate": LaunchConfiguration("baud_rate"), + "idle_timeout_s": LaunchConfiguration("idle_timeout_s"), + }, + ], + ), + + Node( + package="saltybot_social", + executable="attention_node", + name="attention_node", + output="screen", + parameters=[ + att_cfg, + { + "kp_angular": LaunchConfiguration("kp_angular"), + "max_angular_vel": LaunchConfiguration("max_angular_vel"), + "dead_zone_rad": LaunchConfiguration("dead_zone_rad"), + "lost_timeout_s": LaunchConfiguration("lost_timeout_s"), + "attention_enabled": LaunchConfiguration("attention_enabled"), + }, + ], + ), ]) diff --git a/jetson/ros2_ws/src/saltybot_social/package.xml b/jetson/ros2_ws/src/saltybot_social/package.xml index 7319aaa..ca4b63c 100644 --- a/jetson/ros2_ws/src/saltybot_social/package.xml +++ b/jetson/ros2_ws/src/saltybot_social/package.xml @@ -3,7 +3,12 @@ saltybot_social 0.1.0 - Multi-modal person identity fusion and state tracking for saltybot + + Social interaction layer for saltybot. + person_state_tracker: multi-modal person identity fusion (Issue #82). + expression_node: bridges /social/mood to ESP32-C3 NeoPixel ring over serial (Issue #86). + attention_node: rotates robot toward active speaker via /social/persons bearing (Issue #86). + seb MIT rclpy diff --git a/jetson/ros2_ws/src/saltybot_social/saltybot_social/attention_node.py b/jetson/ros2_ws/src/saltybot_social/saltybot_social/attention_node.py new file mode 100644 index 0000000..7ac2a52 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_social/saltybot_social/attention_node.py @@ -0,0 +1,205 @@ +""" +attention_node.py — Social attention controller for saltybot. + +Rotates the robot to face the active speaker or most confident detected person. +Publishes rotation-only cmd_vel (linear.x = 0) so balance is not disturbed. + +Subscribes +────────── + /social/persons (saltybot_social_msgs/PersonArray) + Contains all tracked persons; active_id marks the current speaker. + bearing_rad: signed bearing in base_link (+ve = left/CCW). + +Publishes +───────── + /cmd_vel (geometry_msgs/Twist) + angular.z = clamp(kp_angular * bearing, ±max_angular_vel) + linear.x = 0 always (social mode, not follow mode) + +Control law +─────────── + 1. Pick target: person with active_id; fall back to highest-confidence person. + 2. Dead zone: if |bearing| ≤ dead_zone_rad → angular_z = 0. + 3. Proportional: angular_z = kp_angular * bearing. + 4. Clamp to ±max_angular_vel. + 5. If no fresh person data for lost_timeout_s → publish zero and stay idle. + +State machine +───────────── + ATTENDING — fresh target; publishing proportional rotation + IDLE — no target; publishing zero + +Parameters +────────── + kp_angular (float) 1.0 proportional gain (rad/s per rad error) + max_angular_vel (float) 0.8 hard cap (rad/s) + dead_zone_rad (float) 0.15 ~8.6° dead zone around robot heading + control_rate (float) 20.0 Hz + lost_timeout_s (float) 2.0 seconds before going idle + attention_enabled (bool) True runtime kill switch +""" + +import math +import time + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Twist + +from saltybot_social_msgs.msg import PersonArray + + +# ── Pure helpers (used by tests) ────────────────────────────────────────────── + +def clamp(v: float, lo: float, hi: float) -> float: + return max(lo, min(hi, v)) + + +def select_target(persons, active_id: int): + """ + Pick the attention target from a PersonArray's persons list. + + Priority: + 1. Person whose track_id == active_id (speaking / explicitly active). + 2. Person with highest confidence (fallback when no speaker declared). + + Returns the selected Person message, or None if persons is empty. + """ + if not persons: + return None + + # Prefer the declared active speaker + for p in persons: + if p.track_id == active_id: + return p + + # Fall back to most confident detection + return max(persons, key=lambda p: p.confidence) + + +def compute_attention_cmd( + bearing_rad: float, + dead_zone_rad: float, + kp_angular: float, + max_angular_vel: float, +) -> float: + """ + Proportional rotation command toward bearing. + + Returns angular_z (rad/s). + Zero inside dead zone; proportional + clamped outside. + """ + if abs(bearing_rad) <= dead_zone_rad: + return 0.0 + return clamp(kp_angular * bearing_rad, -max_angular_vel, max_angular_vel) + + +# ── ROS2 node ───────────────────────────────────────────────────────────────── + +class AttentionNode(Node): + + def __init__(self): + super().__init__("attention_node") + + self.declare_parameter("kp_angular", 1.0) + self.declare_parameter("max_angular_vel", 0.8) + self.declare_parameter("dead_zone_rad", 0.15) + self.declare_parameter("control_rate", 20.0) + self.declare_parameter("lost_timeout_s", 2.0) + self.declare_parameter("attention_enabled", True) + + self._reload_params() + + # State + self._last_persons_t = 0.0 # monotonic; 0 = never received + self._persons = [] + self._active_id = -1 + self._state = "idle" + + self.create_subscription( + PersonArray, "/social/persons", self._persons_cb, 10 + ) + + self._cmd_pub = self.create_publisher(Twist, "/cmd_vel", 10) + + rate = self._p["control_rate"] + self._timer = self.create_timer(1.0 / rate, self._control_cb) + + self.get_logger().info( + f"AttentionNode ready kp={self._p['kp_angular']} " + f"dead_zone={math.degrees(self._p['dead_zone_rad']):.1f}° " + f"max_ω={self._p['max_angular_vel']} rad/s" + ) + + def _reload_params(self): + self._p = { + "kp_angular": self.get_parameter("kp_angular").value, + "max_angular_vel": self.get_parameter("max_angular_vel").value, + "dead_zone_rad": self.get_parameter("dead_zone_rad").value, + "control_rate": self.get_parameter("control_rate").value, + "lost_timeout_s": self.get_parameter("lost_timeout_s").value, + "attention_enabled": self.get_parameter("attention_enabled").value, + } + + # ── Callbacks ───────────────────────────────────────────────────────────── + + def _persons_cb(self, msg: PersonArray): + self._persons = msg.persons + self._active_id = msg.active_id + self._last_persons_t = time.monotonic() + if self._state == "idle" and msg.persons: + self._state = "attending" + self.get_logger().info("Attention: person detected — attending") + + def _control_cb(self): + self._reload_params() + twist = Twist() + + if not self._p["attention_enabled"]: + self._cmd_pub.publish(twist) + return + + now = time.monotonic() + fresh = ( + self._last_persons_t > 0.0 + and (now - self._last_persons_t) < self._p["lost_timeout_s"] + ) + + if not fresh: + if self._state == "attending": + self._state = "idle" + self.get_logger().info("Attention: no person — idle") + self._cmd_pub.publish(twist) + return + + target = select_target(self._persons, self._active_id) + if target is None: + self._cmd_pub.publish(twist) + return + + self._state = "attending" + twist.angular.z = compute_attention_cmd( + bearing_rad=target.bearing_rad, + dead_zone_rad=self._p["dead_zone_rad"], + kp_angular=self._p["kp_angular"], + max_angular_vel=self._p["max_angular_vel"], + ) + self._cmd_pub.publish(twist) + + +# ── Entry point ─────────────────────────────────────────────────────────────── + +def main(args=None): + rclpy.init(args=args) + node = AttentionNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == "__main__": + main() diff --git a/jetson/ros2_ws/src/saltybot_social/saltybot_social/expression_node.py b/jetson/ros2_ws/src/saltybot_social/saltybot_social/expression_node.py new file mode 100644 index 0000000..9b324e6 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_social/saltybot_social/expression_node.py @@ -0,0 +1,157 @@ +""" +expression_node.py — LED mood display bridge for saltybot social layer. + +Subscribes +────────── + /social/mood (saltybot_social_msgs/Mood) + mood : "happy" | "curious" | "annoyed" | "playful" | "idle" + intensity : 0.0 .. 1.0 + +Behaviour +───────── + * On every /social/mood message, serialises the command as a single-line + JSON frame and writes it over the ESP32-C3 serial link: + + {"mood":"happy","intensity":0.80}\n + + * If no mood message arrives for idle_timeout_s, sends an explicit + idle command so the ESP32 breathing animation is synchronised with the + node's own awareness of whether the robot is active. + + * Re-opens the serial port automatically if the device disconnects. + +Parameters +────────── + serial_port (str) /dev/ttyUSB0 — ESP32-C3 USB-CDC device + baud_rate (int) 115200 + idle_timeout_s (float) 3.0 — seconds before sending idle + control_rate (float) 10.0 — Hz; how often to check idle +""" + +import json +import threading +import time + +import rclpy +from rclpy.node import Node + +from saltybot_social_msgs.msg import Mood + +try: + import serial + _SERIAL_AVAILABLE = True +except ImportError: + _SERIAL_AVAILABLE = False + + +class ExpressionNode(Node): + + def __init__(self): + super().__init__("expression_node") + + self.declare_parameter("serial_port", "/dev/ttyUSB0") + self.declare_parameter("baud_rate", 115200) + self.declare_parameter("idle_timeout_s", 3.0) + self.declare_parameter("control_rate", 10.0) + + self._port = self.get_parameter("serial_port").value + self._baud = self.get_parameter("baud_rate").value + self._idle_to = self.get_parameter("idle_timeout_s").value + rate = self.get_parameter("control_rate").value + + self._last_cmd_t = 0.0 # monotonic, 0 = never received + self._lock = threading.Lock() + self._ser = None + + self.create_subscription(Mood, "/social/mood", self._mood_cb, 10) + + self._timer = self.create_timer(1.0 / rate, self._tick_cb) + + if _SERIAL_AVAILABLE: + threading.Thread(target=self._open_serial, daemon=True).start() + else: + self.get_logger().warn( + "pyserial not available — serial output disabled (dry-run mode)" + ) + + self.get_logger().info( + f"ExpressionNode ready port={self._port} baud={self._baud} " + f"idle_timeout={self._idle_to}s" + ) + + # ── Serial management ───────────────────────────────────────────────────── + + def _open_serial(self): + """Background thread: keep serial port open, reconnect on error.""" + while rclpy.ok(): + try: + with self._lock: + self._ser = serial.Serial( + self._port, self._baud, timeout=1.0 + ) + self.get_logger().info(f"Opened serial port {self._port}") + # Hold serial open until error + while rclpy.ok(): + with self._lock: + if self._ser is None or not self._ser.is_open: + break + time.sleep(0.5) + except Exception as exc: + self.get_logger().warn( + f"Serial {self._port} error: {exc} — retry in 3 s" + ) + with self._lock: + if self._ser and self._ser.is_open: + self._ser.close() + self._ser = None + time.sleep(3.0) + + def _send(self, mood: str, intensity: float): + """Serialise and write one JSON line to ESP32-C3.""" + line = json.dumps({"mood": mood, "intensity": round(intensity, 3)}) + "\n" + data = line.encode("ascii") + with self._lock: + if self._ser and self._ser.is_open: + try: + self._ser.write(data) + except Exception as exc: + self.get_logger().warn(f"Serial write error: {exc}") + self._ser.close() + self._ser = None + else: + # Dry-run: log the frame + self.get_logger().debug(f"[dry-run] → {line.strip()}") + + # ── Callbacks ───────────────────────────────────────────────────────────── + + def _mood_cb(self, msg: Mood): + intensity = max(0.0, min(1.0, float(msg.intensity))) + self._send(msg.mood, intensity) + self._last_cmd_t = time.monotonic() + + def _tick_cb(self): + """Periodic check — send idle if we've been quiet too long.""" + if self._last_cmd_t == 0.0: + return + if (time.monotonic() - self._last_cmd_t) >= self._idle_to: + self._send("idle", 1.0) + # Reset so we don't flood the ESP32 with idle frames + self._last_cmd_t = time.monotonic() + + +# ── Entry point ─────────────────────────────────────────────────────────────── + +def main(args=None): + rclpy.init(args=args) + node = ExpressionNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == "__main__": + main() diff --git a/jetson/ros2_ws/src/saltybot_social/setup.py b/jetson/ros2_ws/src/saltybot_social/setup.py index 2f7c6c1..4f55aeb 100644 --- a/jetson/ros2_ws/src/saltybot_social/setup.py +++ b/jetson/ros2_ws/src/saltybot_social/setup.py @@ -21,12 +21,14 @@ setup( zip_safe=True, maintainer='seb', maintainer_email='seb@vayrette.com', - description='Multi-modal person identity fusion and state tracking for saltybot', + description='Social interaction layer — person state tracking, LED expression + attention', license='MIT', tests_require=['pytest'], entry_points={ 'console_scripts': [ 'person_state_tracker = saltybot_social.person_state_tracker_node:main', + 'expression_node = saltybot_social.expression_node:main', + 'attention_node = saltybot_social.attention_node:main', ], }, ) diff --git a/jetson/ros2_ws/src/saltybot_social/test/test_attention.py b/jetson/ros2_ws/src/saltybot_social/test/test_attention.py new file mode 100644 index 0000000..06a6cc1 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_social/test/test_attention.py @@ -0,0 +1,140 @@ +""" +test_attention.py — Unit tests for attention_node helpers. + +No ROS2 / serial / GPU dependencies — runs with plain pytest. +""" + +import math +import pytest + +from saltybot_social.attention_node import ( + clamp, + compute_attention_cmd, + select_target, +) + + +# ── Helpers ─────────────────────────────────────────────────────────────────── + +class FakePerson: + def __init__(self, track_id, bearing_rad, confidence=0.9, is_speaking=False): + self.track_id = track_id + self.bearing_rad = bearing_rad + self.confidence = confidence + self.is_speaking = is_speaking + + +# ── clamp ───────────────────────────────────────────────────────────────────── + +class TestClamp: + def test_within_range(self): + assert clamp(0.5, 0.0, 1.0) == 0.5 + + def test_below_lo(self): + assert clamp(-0.5, 0.0, 1.0) == 0.0 + + def test_above_hi(self): + assert clamp(1.5, 0.0, 1.0) == 1.0 + + def test_at_boundary(self): + assert clamp(0.0, 0.0, 1.0) == 0.0 + assert clamp(1.0, 0.0, 1.0) == 1.0 + + +# ── select_target ───────────────────────────────────────────────────────────── + +class TestSelectTarget: + + def test_empty_returns_none(self): + assert select_target([], active_id=-1) is None + + def test_picks_active_id(self): + persons = [FakePerson(1, 0.1, confidence=0.6), + FakePerson(2, 0.5, confidence=0.9)] + result = select_target(persons, active_id=1) + assert result.track_id == 1 + + def test_falls_back_to_highest_confidence(self): + persons = [FakePerson(1, 0.1, confidence=0.6), + FakePerson(2, 0.5, confidence=0.9), + FakePerson(3, 0.2, confidence=0.4)] + result = select_target(persons, active_id=-1) + assert result.track_id == 2 + + def test_single_person_no_active(self): + persons = [FakePerson(5, 0.3)] + result = select_target(persons, active_id=-1) + assert result.track_id == 5 + + def test_active_id_not_in_list_falls_back(self): + persons = [FakePerson(1, 0.0, confidence=0.5), + FakePerson(2, 0.2, confidence=0.95)] + result = select_target(persons, active_id=99) + assert result.track_id == 2 + + +# ── compute_attention_cmd ───────────────────────────────────────────────────── + +class TestComputeAttentionCmd: + + def test_inside_dead_zone_zero(self): + cmd = compute_attention_cmd( + bearing_rad=0.05, + dead_zone_rad=0.15, + kp_angular=1.0, + max_angular_vel=0.8, + ) + assert cmd == 0.0 + + def test_exactly_at_dead_zone_boundary_zero(self): + cmd = compute_attention_cmd( + bearing_rad=0.15, + dead_zone_rad=0.15, + kp_angular=1.0, + max_angular_vel=0.8, + ) + assert cmd == 0.0 + + def test_positive_bearing_gives_positive_angular_z(self): + cmd = compute_attention_cmd( + bearing_rad=0.4, + dead_zone_rad=0.15, + kp_angular=1.0, + max_angular_vel=0.8, + ) + assert cmd > 0.0 + + def test_negative_bearing_gives_negative_angular_z(self): + cmd = compute_attention_cmd( + bearing_rad=-0.4, + dead_zone_rad=0.15, + kp_angular=1.0, + max_angular_vel=0.8, + ) + assert cmd < 0.0 + + def test_clamps_to_max_angular_vel(self): + cmd = compute_attention_cmd( + bearing_rad=math.pi, # 180° off + dead_zone_rad=0.15, + kp_angular=2.0, + max_angular_vel=0.8, + ) + assert abs(cmd) <= 0.8 + + def test_proportional_scaling(self): + """Double bearing → double cmd (within linear region).""" + kp = 1.0 + dz = 0.05 + cmd1 = compute_attention_cmd(0.3, dz, kp, 10.0) + cmd2 = compute_attention_cmd(0.6, dz, kp, 10.0) + assert abs(cmd2 - 2.0 * cmd1) < 0.01 + + def test_zero_bearing_zero_cmd(self): + cmd = compute_attention_cmd( + bearing_rad=0.0, + dead_zone_rad=0.1, + kp_angular=1.0, + max_angular_vel=0.8, + ) + assert cmd == 0.0 diff --git a/jetson/ros2_ws/src/saltybot_social_msgs/CMakeLists.txt b/jetson/ros2_ws/src/saltybot_social_msgs/CMakeLists.txt index b6e70ab..9ac3850 100644 --- a/jetson/ros2_ws/src/saltybot_social_msgs/CMakeLists.txt +++ b/jetson/ros2_ws/src/saltybot_social_msgs/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(geometry_msgs REQUIRED) find_package(builtin_interfaces REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} + # Issue #80 — face detection + recognition "msg/FaceDetection.msg" "msg/FaceDetectionArray.msg" "msg/FaceEmbedding.msg" @@ -18,7 +19,12 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/ListPersons.srv" "srv/DeletePerson.srv" "srv/UpdatePerson.srv" + # Issue #86 — LED expression + motor attention + "msg/Mood.msg" + "msg/Person.msg" + "msg/PersonArray.msg" DEPENDENCIES std_msgs geometry_msgs builtin_interfaces ) +ament_export_dependencies(rosidl_default_runtime) ament_package() diff --git a/jetson/ros2_ws/src/saltybot_social_msgs/msg/Mood.msg b/jetson/ros2_ws/src/saltybot_social_msgs/msg/Mood.msg new file mode 100644 index 0000000..361f3d6 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_social_msgs/msg/Mood.msg @@ -0,0 +1,7 @@ +# Mood.msg — social expression command sent to the LED display node. +# +# mood : one of "happy", "curious", "annoyed", "playful", "idle" +# intensity : 0.0 (off) to 1.0 (full brightness) + +string mood +float32 intensity diff --git a/jetson/ros2_ws/src/saltybot_social_msgs/msg/Person.msg b/jetson/ros2_ws/src/saltybot_social_msgs/msg/Person.msg new file mode 100644 index 0000000..192570d --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_social_msgs/msg/Person.msg @@ -0,0 +1,17 @@ +# Person.msg — single tracked person for social attention. +# +# bearing_rad : signed bearing in base_link frame (rad) +# positive = person to the left (CCW), negative = right (CW) +# distance_m : estimated distance in metres; 0 if unknown +# confidence : detection confidence 0..1 +# is_speaking : true if mic DOA or VAD identifies this person as speaking +# source : "camera" | "mic_doa" + +std_msgs/Header header + +int32 track_id +float32 bearing_rad +float32 distance_m +float32 confidence +bool is_speaking +string source diff --git a/jetson/ros2_ws/src/saltybot_social_msgs/msg/PersonArray.msg b/jetson/ros2_ws/src/saltybot_social_msgs/msg/PersonArray.msg new file mode 100644 index 0000000..108685f --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_social_msgs/msg/PersonArray.msg @@ -0,0 +1,9 @@ +# PersonArray.msg — all detected persons plus the current attention target. +# +# persons : all currently tracked persons +# active_id : track_id of the active/speaking person; -1 if none + +std_msgs/Header header + +saltybot_social_msgs/Person[] persons +int32 active_id