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>
207 lines
8.0 KiB
C++
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();
|
|
}
|