sl-firmware 5143e5bfac feat(social): Issue #86 — physical expression + motor attention
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 <noreply@anthropic.com>
2026-03-01 23:35:59 -05:00

207 lines
8.0 KiB
C++

/*
* 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 <Adafruit_NeoPixel.h>
#include <ArduinoJson.h>
// ── 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();
}