57 changed files with 155 additions and 3347 deletions
--- a/esp32/social_expression/social_expression.ino
+++ b/esp32/social_expression/social_expression.ino
@ -1,206 +0,0 @@
 /*
 * 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();
 }
--- a/jetson/ros2_ws/src/saltybot_social/config/attention_params.yaml
+++ b/jetson/ros2_ws/src/saltybot_social/config/attention_params.yaml
@ -1,30 +0,0 @@
 # 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
--- a/jetson/ros2_ws/src/saltybot_social/config/expression_params.yaml
+++ b/jetson/ros2_ws/src/saltybot_social/config/expression_params.yaml
@ -1,22 +0,0 @@
 # 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
--- a/jetson/ros2_ws/src/saltybot_social/config/social_params.yaml
+++ b/jetson/ros2_ws/src/saltybot_social/config/social_params.yaml
@ -1,8 +0,0 @@
 person_state_tracker:
  ros__parameters:
    engagement_distance: 2.0
    absent_timeout: 5.0
    prune_timeout: 30.0
    update_rate: 10.0
    n_cameras: 4
    uwb_enabled: false
--- a/jetson/ros2_ws/src/saltybot_social/launch/social.launch.py
+++ b/jetson/ros2_ws/src/saltybot_social/launch/social.launch.py
@ -1,100 +0,0 @@
 """
 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
 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',
            description='Distance in metres below which a person is considered engaged'
        ),
        DeclareLaunchArgument(
            'absent_timeout',
            default_value='5.0',
            description='Seconds without detection before marking person as ABSENT'
        ),
        DeclareLaunchArgument(
            'uwb_enabled',
            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',
            name='person_state_tracker',
            output='screen',
            parameters=[{
                'engagement_distance': LaunchConfiguration('engagement_distance'),
                'absent_timeout': LaunchConfiguration('absent_timeout'),
                '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"),
                },
            ],
        ),
    ])
--- a/jetson/ros2_ws/src/saltybot_social/package.xml
+++ b/jetson/ros2_ws/src/saltybot_social/package.xml
@ -1,30 +0,0 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
  <name>saltybot_social</name>
  <version>0.1.0</version>
  <description>
    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).
  </description>
  <maintainer email="seb@vayrette.com">seb</maintainer>
  <license>MIT</license>
  <depend>rclpy</depend>
  <depend>std_msgs</depend>
  <depend>geometry_msgs</depend>
  <depend>sensor_msgs</depend>
  <depend>vision_msgs</depend>
  <depend>saltybot_social_msgs</depend>
  <depend>tf2_ros</depend>
  <depend>tf2_geometry_msgs</depend>
  <depend>cv_bridge</depend>
  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
  <test_depend>ament_pep257</test_depend>
  <test_depend>python3-pytest</test_depend>
  <export>
    <build_type>ament_python</build_type>
  </export>
 </package>
--- a/jetson/ros2_ws/src/saltybot_social/resource/saltybot_social
+++ b/jetson/ros2_ws/src/saltybot_social/resource/saltybot_social
--- a/jetson/ros2_ws/src/saltybot_social/saltybot_social/init.py
+++ b/jetson/ros2_ws/src/saltybot_social/saltybot_social/init.py
--- a/jetson/ros2_ws/src/saltybot_social/saltybot_social/attention_node.py
+++ b/jetson/ros2_ws/src/saltybot_social/saltybot_social/attention_node.py
@ -1,205 +0,0 @@
 """
 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()
--- a/jetson/ros2_ws/src/saltybot_social/saltybot_social/expression_node.py
+++ b/jetson/ros2_ws/src/saltybot_social/saltybot_social/expression_node.py
@ -1,157 +0,0 @@
 """
 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()
--- a/jetson/ros2_ws/src/saltybot_social/saltybot_social/identity_fuser.py
+++ b/jetson/ros2_ws/src/saltybot_social/saltybot_social/identity_fuser.py
@ -1,201 +0,0 @@
 import math
 import time
 from typing import List, Optional, Tuple
 from saltybot_social.person_state import PersonTrack, State
 class IdentityFuser:
    """Multi-modal identity fusion: face_id + speaker_id + UWB anchor -> unified person_id."""
    def __init__(self, position_window: float = 3.0):
        self._tracks: dict[int, PersonTrack] = {}
        self._next_id: int = 1
        self._position_window = position_window
        self._max_history = 20
    def _distance_3d(self, a: Optional[tuple], b: Optional[tuple]) -> float:
        if a is None or b is None:
            return float('inf')
        return math.sqrt(sum((ai - bi) ** 2 for ai, bi in zip(a, b)))
    def _compute_distance_and_bearing(self, position: tuple) -> Tuple[float, float]:
        x, y, z = position
        distance = math.sqrt(x * x + y * y + z * z)
        bearing = math.degrees(math.atan2(y, x))
        return distance, bearing
    def _allocate_id(self) -> int:
        pid = self._next_id
        self._next_id += 1
        return pid
    def update_face(self, face_id: int, name: str,
                    position_3d: Optional[tuple], camera_id: int,
                    now: float) -> int:
        """Match by face_id first. If face_id >= 0, find existing track or create new."""
        if face_id >= 0:
            for track in self._tracks.values():
                if track.face_id == face_id:
                    track.name = name if name and name != 'unknown' else track.name
                    if position_3d is not None:
                        track.position = position_3d
                        dist, bearing = self._compute_distance_and_bearing(position_3d)
                        track.distance = dist
                        track.bearing_deg = bearing
                        track.history_distances.append(dist)
                        if len(track.history_distances) > self._max_history:
                            track.history_distances = track.history_distances[-self._max_history:]
                    track.camera_id = camera_id
                    track.last_seen = now
                    track.last_face_seen = now
                    return track.person_id
        # No existing track with this face_id; try proximity match for face_id < 0
        if face_id < 0 and position_3d is not None:
            best_track = None
            best_dist = 0.5  # 0.5m proximity threshold
            for track in self._tracks.values():
                d = self._distance_3d(track.position, position_3d)
                if d < best_dist and (now - track.last_seen) < self._position_window:
                    best_dist = d
                    best_track = track
            if best_track is not None:
                best_track.position = position_3d
                dist, bearing = self._compute_distance_and_bearing(position_3d)
                best_track.distance = dist
                best_track.bearing_deg = bearing
                best_track.history_distances.append(dist)
                if len(best_track.history_distances) > self._max_history:
                    best_track.history_distances = best_track.history_distances[-self._max_history:]
                best_track.camera_id = camera_id
                best_track.last_seen = now
                return best_track.person_id
        # Create new track
        pid = self._allocate_id()
        track = PersonTrack(person_id=pid, face_id=face_id, name=name,
                            camera_id=camera_id, last_seen=now, last_face_seen=now)
        if position_3d is not None:
            track.position = position_3d
            dist, bearing = self._compute_distance_and_bearing(position_3d)
            track.distance = dist
            track.bearing_deg = bearing
            track.history_distances.append(dist)
        self._tracks[pid] = track
        return pid
    def update_speaker(self, speaker_id: str, now: float) -> int:
        """Find track with nearest recently-seen position, assign speaker_id."""
        # First check if any track already has this speaker_id
        for track in self._tracks.values():
            if track.speaker_id == speaker_id:
                track.last_seen = now
                return track.person_id
        # Assign to nearest recently-seen track
        best_track = None
        best_dist = float('inf')
        for track in self._tracks.values():
            if (now - track.last_seen) < self._position_window and track.distance > 0:
                if track.distance < best_dist:
                    best_dist = track.distance
                    best_track = track
        if best_track is not None:
            best_track.speaker_id = speaker_id
            best_track.last_seen = now
            return best_track.person_id
        # No suitable track found; create a new one
        pid = self._allocate_id()
        track = PersonTrack(person_id=pid, speaker_id=speaker_id,
                            last_seen=now)
        self._tracks[pid] = track
        return pid
    def update_uwb(self, uwb_anchor_id: str, position_3d: tuple,
                    now: float) -> int:
        """Match by proximity (nearest track within 0.5m)."""
        # Check existing UWB assignment
        for track in self._tracks.values():
            if track.uwb_anchor_id == uwb_anchor_id:
                track.position = position_3d
                dist, bearing = self._compute_distance_and_bearing(position_3d)
                track.distance = dist
                track.bearing_deg = bearing
                track.history_distances.append(dist)
                if len(track.history_distances) > self._max_history:
                    track.history_distances = track.history_distances[-self._max_history:]
                track.last_seen = now
                return track.person_id
        # Proximity match
        best_track = None
        best_dist = 0.5
        for track in self._tracks.values():
            d = self._distance_3d(track.position, position_3d)
            if d < best_dist and (now - track.last_seen) < self._position_window:
                best_dist = d
                best_track = track
        if best_track is not None:
            best_track.uwb_anchor_id = uwb_anchor_id
            best_track.position = position_3d
            dist, bearing = self._compute_distance_and_bearing(position_3d)
            best_track.distance = dist
            best_track.bearing_deg = bearing
            best_track.history_distances.append(dist)
            if len(best_track.history_distances) > self._max_history:
                best_track.history_distances = best_track.history_distances[-self._max_history:]
            best_track.last_seen = now
            return best_track.person_id
        # Create new track
        pid = self._allocate_id()
        track = PersonTrack(person_id=pid, uwb_anchor_id=uwb_anchor_id,
                            position=position_3d, last_seen=now)
        dist, bearing = self._compute_distance_and_bearing(position_3d)
        track.distance = dist
        track.bearing_deg = bearing
        track.history_distances.append(dist)
        self._tracks[pid] = track
        return pid
    def get_all_tracks(self) -> List[PersonTrack]:
        return list(self._tracks.values())
    @staticmethod
    def compute_attention(tracks: List[PersonTrack]) -> int:
        """Focus on nearest engaged/talking person, or -1 if none."""
        candidates = [t for t in tracks
                      if t.state in (State.ENGAGED, State.TALKING) and t.distance > 0]
        if not candidates:
            return -1
        # Prefer talking, then nearest engaged
        talking = [t for t in candidates if t.state == State.TALKING]
        if talking:
            return min(talking, key=lambda t: t.distance).person_id
        return min(candidates, key=lambda t: t.distance).person_id
    def prune_absent(self, absent_timeout: float = 30.0):
        """Remove tracks absent longer than timeout."""
        now = time.monotonic()
        to_remove = [pid for pid, t in self._tracks.items()
                     if t.state == State.ABSENT and (now - t.last_seen) > absent_timeout]
        for pid in to_remove:
            del self._tracks[pid]
    @staticmethod
    def detect_group(tracks: List[PersonTrack]) -> bool:
        """Returns True if >= 2 persons within 1.5m of each other."""
        active = [t for t in tracks
                  if t.position is not None and t.state != State.ABSENT]
        for i, a in enumerate(active):
            for b in active[i + 1:]:
                dx = a.position[0] - b.position[0]
                dy = a.position[1] - b.position[1]
                dz = a.position[2] - b.position[2]
                if math.sqrt(dx * dx + dy * dy + dz * dz) < 1.5:
                    return True
        return False
--- a/jetson/ros2_ws/src/saltybot_social/saltybot_social/person_state.py
+++ b/jetson/ros2_ws/src/saltybot_social/saltybot_social/person_state.py
@ -1,54 +0,0 @@
 import time
 from dataclasses import dataclass, field
 from enum import IntEnum
 from typing import Optional
 class State(IntEnum):
    UNKNOWN = 0
    APPROACHING = 1
    ENGAGED = 2
    TALKING = 3
    LEAVING = 4
    ABSENT = 5
@dataclass
 class PersonTrack:
    person_id: int
    face_id: int = -1
    speaker_id: str = ''
    uwb_anchor_id: str = ''
    name: str = 'unknown'
    state: State = State.UNKNOWN
    position: Optional[tuple] = None   # (x, y, z) in base_link
    distance: float = 0.0
    bearing_deg: float = 0.0
    engagement_score: float = 0.0
    camera_id: int = -1
    last_seen: float = field(default_factory=time.monotonic)
    last_face_seen: float = 0.0
    history_distances: list = field(default_factory=list)  # last N distances for trend
    def update_state(self, now: float, engagement_distance: float = 2.0,
                     absent_timeout: float = 5.0):
        """Transition state machine based on distance trend and time."""
        age = now - self.last_seen
        if age > absent_timeout:
            self.state = State.ABSENT
            return
        if self.speaker_id:
            self.state = State.TALKING
            return
        if len(self.history_distances) >= 3:
            trend = self.history_distances[-1] - self.history_distances[-3]
            if self.distance < engagement_distance:
                self.state = State.ENGAGED
            elif trend < -0.2:  # moving closer
                self.state = State.APPROACHING
            elif trend > 0.3:   # moving away
                self.state = State.LEAVING
            else:
                self.state = State.ENGAGED if self.distance < engagement_distance else State.UNKNOWN
        elif self.distance > 0:
            self.state = State.ENGAGED if self.distance < engagement_distance else State.APPROACHING
--- a/jetson/ros2_ws/src/saltybot_social/saltybot_social/person_state_tracker_node.py
+++ b/jetson/ros2_ws/src/saltybot_social/saltybot_social/person_state_tracker_node.py
@ -1,273 +0,0 @@
 import time
 import rclpy
 from rclpy.node import Node
 from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
 from std_msgs.msg import Int32, String
 from geometry_msgs.msg import PoseArray, PoseStamped, Point
 from sensor_msgs.msg import Image
 from builtin_interfaces.msg import Time as TimeMsg
 from saltybot_social_msgs.msg import (
    FaceDetection,
    FaceDetectionArray,
    PersonState,
    PersonStateArray,
 )
 from saltybot_social.identity_fuser import IdentityFuser
 from saltybot_social.person_state import State
 class PersonStateTrackerNode(Node):
    """Main ROS2 node for multi-modal person tracking."""
    def __init__(self):
        super().__init__('person_state_tracker')
        # Parameters
        self.declare_parameter('engagement_distance', 2.0)
        self.declare_parameter('absent_timeout', 5.0)
        self.declare_parameter('prune_timeout', 30.0)
        self.declare_parameter('update_rate', 10.0)
        self.declare_parameter('n_cameras', 4)
        self.declare_parameter('uwb_enabled', False)
        self._engagement_distance = self.get_parameter('engagement_distance').value
        self._absent_timeout = self.get_parameter('absent_timeout').value
        self._prune_timeout = self.get_parameter('prune_timeout').value
        update_rate = self.get_parameter('update_rate').value
        n_cameras = self.get_parameter('n_cameras').value
        uwb_enabled = self.get_parameter('uwb_enabled').value
        self._fuser = IdentityFuser()
        # QoS profiles
        best_effort_qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            history=HistoryPolicy.KEEP_LAST,
            depth=5
        )
        reliable_qos = QoSProfile(
            reliability=ReliabilityPolicy.RELIABLE,
            history=HistoryPolicy.KEEP_LAST,
            depth=1
        )
        # Subscriptions
        self.create_subscription(
            FaceDetectionArray,
            '/social/faces/detections',
            self._on_face_detections,
            best_effort_qos
        )
        self.create_subscription(
            String,
            '/social/speech/speaker_id',
            self._on_speaker_id,
            10
        )
        if uwb_enabled:
            self.create_subscription(
                PoseArray,
                '/uwb/positions',
                self._on_uwb,
                10
            )
        # Camera subscriptions (monitor topic existence)
        self.create_subscription(
            Image, '/camera/color/image_raw',
            self._on_camera_image, best_effort_qos)
        for i in range(n_cameras):
            self.create_subscription(
                Image, f'/surround/cam{i}/image_raw',
                self._on_camera_image, best_effort_qos)
        # Existing person tracker position
        self.create_subscription(
            PoseStamped,
            '/person/target',
            self._on_person_target,
            10
        )
        # Publishers
        self._persons_pub = self.create_publisher(
            PersonStateArray, '/social/persons', best_effort_qos)
        self._attention_pub = self.create_publisher(
            Int32, '/social/attention/target_id', reliable_qos)
        # Timer
        timer_period = 1.0 / update_rate
        self.create_timer(timer_period, self._on_timer)
        self.get_logger().info(
            f'PersonStateTracker started: engagement={self._engagement_distance}m, '
            f'absent_timeout={self._absent_timeout}s, rate={update_rate}Hz, '
            f'cameras={n_cameras}, uwb={uwb_enabled}')
    def _on_face_detections(self, msg: FaceDetectionArray):
        now = time.monotonic()
        for face in msg.faces:
            position_3d = None
            # Use bbox center as rough position estimate if depth is available
            # Real 3D position would come from depth camera projection
            if face.bbox_x > 0 or face.bbox_y > 0:
                # Approximate: use bbox center as bearing proxy, distance from bbox size
                # This is a placeholder; real impl would use depth image lookup
                position_3d = (
                    max(0.5, 1.0 / max(face.bbox_w, 0.01)),  # rough depth from bbox width
                    face.bbox_x + face.bbox_w / 2.0 - 0.5,   # x offset from center
                    face.bbox_y + face.bbox_h / 2.0 - 0.5    # y offset from center
                )
            camera_id = -1
            if hasattr(face.header, 'frame_id') and face.header.frame_id:
                frame = face.header.frame_id
                if 'cam' in frame:
                    try:
                        camera_id = int(frame.split('cam')[-1].split('_')[0])
                    except ValueError:
                        pass
            self._fuser.update_face(
                face_id=face.face_id,
                name=face.person_name,
                position_3d=position_3d,
                camera_id=camera_id,
                now=now
            )
    def _on_speaker_id(self, msg: String):
        now = time.monotonic()
        speaker_id = msg.data
        # Parse "speaker_<id>" format or raw id
        if speaker_id.startswith('speaker_'):
            speaker_id = speaker_id[len('speaker_'):]
        self._fuser.update_speaker(speaker_id, now)
    def _on_uwb(self, msg: PoseArray):
        now = time.monotonic()
        for i, pose in enumerate(msg.poses):
            position = (pose.position.x, pose.position.y, pose.position.z)
            anchor_id = f'uwb_{i}'
            if hasattr(msg, 'header') and msg.header.frame_id:
                anchor_id = f'{msg.header.frame_id}_{i}'
            self._fuser.update_uwb(anchor_id, position, now)
    def _on_camera_image(self, msg: Image):
        # Monitor only -- no processing here
        pass
    def _on_person_target(self, msg: PoseStamped):
        now = time.monotonic()
        position_3d = (
            msg.pose.position.x,
            msg.pose.position.y,
            msg.pose.position.z
        )
        # Update position for unidentified person (from YOLOv8 tracker)
        self._fuser.update_face(
            face_id=-1,
            name='unknown',
            position_3d=position_3d,
            camera_id=-1,
            now=now
        )
    def _on_timer(self):
        now = time.monotonic()
        tracks = self._fuser.get_all_tracks()
        # Update state machine for all tracks
        for track in tracks:
            track.update_state(
                now,
                engagement_distance=self._engagement_distance,
                absent_timeout=self._absent_timeout
            )
        # Compute engagement scores
        for track in tracks:
            if track.state == State.ABSENT:
                track.engagement_score = 0.0
            elif track.state == State.TALKING:
                track.engagement_score = 1.0
            elif track.state == State.ENGAGED:
                track.engagement_score = max(0.0, 1.0 - track.distance / self._engagement_distance)
            elif track.state == State.APPROACHING:
                track.engagement_score = 0.3
            elif track.state == State.LEAVING:
                track.engagement_score = 0.1
            else:
                track.engagement_score = 0.0
        # Prune long-absent tracks
        self._fuser.prune_absent(self._prune_timeout)
        # Compute attention target
        attention_id = IdentityFuser.compute_attention(tracks)
        # Build and publish PersonStateArray
        msg = PersonStateArray()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'base_link'
        msg.primary_attention_id = attention_id
        for track in tracks:
            ps = PersonState()
            ps.header.stamp = msg.header.stamp
            ps.header.frame_id = 'base_link'
            ps.person_id = track.person_id
            ps.person_name = track.name
            ps.face_id = track.face_id
            ps.speaker_id = track.speaker_id
            ps.uwb_anchor_id = track.uwb_anchor_id
            if track.position is not None:
                ps.position = Point(
                    x=float(track.position[0]),
                    y=float(track.position[1]),
                    z=float(track.position[2]))
            ps.distance = float(track.distance)
            ps.bearing_deg = float(track.bearing_deg)
            ps.state = int(track.state)
            ps.engagement_score = float(track.engagement_score)
            ps.last_seen = self._mono_to_ros_time(track.last_seen)
            ps.camera_id = track.camera_id
            msg.persons.append(ps)
        self._persons_pub.publish(msg)
        # Publish attention target
        att_msg = Int32()
        att_msg.data = attention_id
        self._attention_pub.publish(att_msg)
    def _mono_to_ros_time(self, mono: float) -> TimeMsg:
        """Convert monotonic timestamp to approximate ROS time."""
        # Offset from monotonic to wall clock
        offset = time.time() - time.monotonic()
        wall = mono + offset
        sec = int(wall)
        nsec = int((wall - sec) * 1e9)
        t = TimeMsg()
        t.sec = sec
        t.nanosec = nsec
        return t
 def main(args=None):
    rclpy.init(args=args)
    node = PersonStateTrackerNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()
 if __name__ == '__main__':
    main()
--- a/jetson/ros2_ws/src/saltybot_social/setup.cfg
+++ b/jetson/ros2_ws/src/saltybot_social/setup.cfg
@ -1,4 +0,0 @@
 [develop]
 script_dir=$base/lib/saltybot_social
 [install]
 install_scripts=$base/lib/saltybot_social
--- a/jetson/ros2_ws/src/saltybot_social/setup.py
+++ b/jetson/ros2_ws/src/saltybot_social/setup.py
@ -1,34 +0,0 @@
 from setuptools import find_packages, setup
 import os
 from glob import glob
 package_name = 'saltybot_social'
 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='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',
        ],
    },
 )
--- a/jetson/ros2_ws/src/saltybot_social/test/init.py
+++ b/jetson/ros2_ws/src/saltybot_social/test/init.py
--- a/jetson/ros2_ws/src/saltybot_social/test/test_attention.py
+++ b/jetson/ros2_ws/src/saltybot_social/test/test_attention.py
@ -1,140 +0,0 @@
 """
 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
--- a/jetson/ros2_ws/src/saltybot_social_msgs/CMakeLists.txt
+++ b/jetson/ros2_ws/src/saltybot_social_msgs/CMakeLists.txt
@ -1,30 +0,0 @@
 cmake_minimum_required(VERSION 3.8)
 project(saltybot_social_msgs)
 find_package(ament_cmake REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(std_msgs REQUIRED)
 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"
  "msg/FaceEmbeddingArray.msg"
  "msg/PersonState.msg"
  "msg/PersonStateArray.msg"
  "srv/EnrollPerson.srv"
  "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()
--- a/jetson/ros2_ws/src/saltybot_social_msgs/msg/FaceDetection.msg
+++ b/jetson/ros2_ws/src/saltybot_social_msgs/msg/FaceDetection.msg
@ -1,10 +0,0 @@
 std_msgs/Header header
 int32 face_id
 string person_name
 float32 confidence
 float32 recognition_score
 float32 bbox_x
 float32 bbox_y
 float32 bbox_w
 float32 bbox_h
 float32[10] landmarks
--- a/jetson/ros2_ws/src/saltybot_social_msgs/msg/FaceDetectionArray.msg
+++ b/jetson/ros2_ws/src/saltybot_social_msgs/msg/FaceDetectionArray.msg
@ -1,2 +0,0 @@
 std_msgs/Header header
 saltybot_social_msgs/FaceDetection[] faces
--- a/jetson/ros2_ws/src/saltybot_social_msgs/msg/FaceEmbedding.msg
+++ b/jetson/ros2_ws/src/saltybot_social_msgs/msg/FaceEmbedding.msg
@ -1,5 +0,0 @@
 int32 person_id
 string person_name
 float32[] embedding
 builtin_interfaces/Time enrolled_at
 int32 sample_count
--- a/jetson/ros2_ws/src/saltybot_social_msgs/msg/FaceEmbeddingArray.msg
+++ b/jetson/ros2_ws/src/saltybot_social_msgs/msg/FaceEmbeddingArray.msg
@ -1,2 +0,0 @@
 std_msgs/Header header
 saltybot_social_msgs/FaceEmbedding[] embeddings
--- a/jetson/ros2_ws/src/saltybot_social_msgs/msg/Mood.msg
+++ b/jetson/ros2_ws/src/saltybot_social_msgs/msg/Mood.msg
@ -1,7 +0,0 @@
 # 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
--- a/jetson/ros2_ws/src/saltybot_social_msgs/msg/Person.msg
+++ b/jetson/ros2_ws/src/saltybot_social_msgs/msg/Person.msg
@ -1,17 +0,0 @@
 # 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
--- a/jetson/ros2_ws/src/saltybot_social_msgs/msg/PersonArray.msg
+++ b/jetson/ros2_ws/src/saltybot_social_msgs/msg/PersonArray.msg
@ -1,9 +0,0 @@
 # 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
--- a/jetson/ros2_ws/src/saltybot_social_msgs/msg/PersonState.msg
+++ b/jetson/ros2_ws/src/saltybot_social_msgs/msg/PersonState.msg
@ -1,19 +0,0 @@
 std_msgs/Header header
 int32 person_id
 string person_name
 int32 face_id
 string speaker_id
 string uwb_anchor_id
 geometry_msgs/Point position
 float32 distance
 float32 bearing_deg
 uint8 state
 uint8 STATE_UNKNOWN=0
 uint8 STATE_APPROACHING=1
 uint8 STATE_ENGAGED=2
 uint8 STATE_TALKING=3
 uint8 STATE_LEAVING=4
 uint8 STATE_ABSENT=5
 float32 engagement_score
 builtin_interfaces/Time last_seen
 int32 camera_id
--- a/jetson/ros2_ws/src/saltybot_social_msgs/msg/PersonStateArray.msg
+++ b/jetson/ros2_ws/src/saltybot_social_msgs/msg/PersonStateArray.msg
@ -1,3 +0,0 @@
 std_msgs/Header header
 saltybot_social_msgs/PersonState[] persons
 int32 primary_attention_id
--- a/jetson/ros2_ws/src/saltybot_social_msgs/package.xml
+++ b/jetson/ros2_ws/src/saltybot_social_msgs/package.xml
@ -1,19 +0,0 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
  <name>saltybot_social_msgs</name>
  <version>0.1.0</version>
  <description>Custom ROS2 messages and services for saltybot social capabilities</description>
  <maintainer email="seb@vayrette.com">seb</maintainer>
  <license>MIT</license>
  <buildtool_depend>ament_cmake</buildtool_depend>
  <depend>std_msgs</depend>
  <depend>geometry_msgs</depend>
  <depend>builtin_interfaces</depend>
  <build_depend>rosidl_default_generators</build_depend>
  <exec_depend>rosidl_default_runtime</exec_depend>
  <member_of_group>rosidl_interface_packages</member_of_group>
  <export>
    <build_type>ament_cmake</build_type>
  </export>
 </package>
--- a/jetson/ros2_ws/src/saltybot_social_msgs/srv/DeletePerson.srv
+++ b/jetson/ros2_ws/src/saltybot_social_msgs/srv/DeletePerson.srv
@ -1,4 +0,0 @@
 int32 person_id
 ---
 bool success
 string message
--- a/jetson/ros2_ws/src/saltybot_social_msgs/srv/EnrollPerson.srv
+++ b/jetson/ros2_ws/src/saltybot_social_msgs/srv/EnrollPerson.srv
@ -1,7 +0,0 @@
 string name
 string mode
 int32 n_samples
 ---
 bool success
 string message
 int32 person_id
--- a/jetson/ros2_ws/src/saltybot_social_msgs/srv/ListPersons.srv
+++ b/jetson/ros2_ws/src/saltybot_social_msgs/srv/ListPersons.srv
@ -1,2 +0,0 @@
 ---
 saltybot_social_msgs/FaceEmbedding[] persons
--- a/jetson/ros2_ws/src/saltybot_social_msgs/srv/UpdatePerson.srv
+++ b/jetson/ros2_ws/src/saltybot_social_msgs/srv/UpdatePerson.srv
@ -1,5 +0,0 @@
 int32 person_id
 string new_name
 ---
 bool success
 string message
--- a/jetson/ros2_ws/src/saltybot_social_nav/config/social_nav_params.yaml
+++ b/jetson/ros2_ws/src/saltybot_social_nav/config/social_nav_params.yaml
@ -1,22 +0,0 @@
 social_nav:
  ros__parameters:
    follow_mode: 'shadow'
    follow_distance: 1.2
    lead_distance: 2.0
    orbit_radius: 1.5
    max_linear_speed: 1.0
    max_linear_speed_fast: 5.5
    max_angular_speed: 1.0
    goal_tolerance: 0.3
    routes_dir: '/mnt/nvme/saltybot/routes'
    home_x: 0.0
    home_y: 0.0
    map_resolution: 0.05
    obstacle_inflation_cells: 3
 midas_depth:
  ros__parameters:
    onnx_path: '/mnt/nvme/saltybot/models/midas_small.onnx'
    engine_path: '/mnt/nvme/saltybot/models/midas_small.engine'
    process_rate: 5.0
    output_scale: 1.0
--- a/jetson/ros2_ws/src/saltybot_social_nav/launch/social_nav.launch.py
+++ b/jetson/ros2_ws/src/saltybot_social_nav/launch/social_nav.launch.py
@ -1,57 +0,0 @@
 """Launch file for saltybot social navigation."""
 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([
        DeclareLaunchArgument('follow_mode', default_value='shadow',
                              description='Follow mode: shadow/lead/side/orbit/loose/tight'),
        DeclareLaunchArgument('follow_distance', default_value='1.2',
                              description='Follow distance in meters'),
        DeclareLaunchArgument('max_linear_speed', default_value='1.0',
                              description='Max linear speed (m/s)'),
        DeclareLaunchArgument('routes_dir',
                              default_value='/mnt/nvme/saltybot/routes',
                              description='Directory for saved routes'),
        Node(
            package='saltybot_social_nav',
            executable='social_nav',
            name='social_nav',
            output='screen',
            parameters=[{
                'follow_mode': LaunchConfiguration('follow_mode'),
                'follow_distance': LaunchConfiguration('follow_distance'),
                'max_linear_speed': LaunchConfiguration('max_linear_speed'),
                'routes_dir': LaunchConfiguration('routes_dir'),
            }],
        ),
        Node(
            package='saltybot_social_nav',
            executable='midas_depth',
            name='midas_depth',
            output='screen',
            parameters=[{
                'onnx_path': '/mnt/nvme/saltybot/models/midas_small.onnx',
                'engine_path': '/mnt/nvme/saltybot/models/midas_small.engine',
                'process_rate': 5.0,
                'output_scale': 1.0,
            }],
        ),
        Node(
            package='saltybot_social_nav',
            executable='waypoint_teacher',
            name='waypoint_teacher',
            output='screen',
            parameters=[{
                'routes_dir': LaunchConfiguration('routes_dir'),
                'recording_interval': 0.5,
            }],
        ),
    ])
--- a/jetson/ros2_ws/src/saltybot_social_nav/package.xml
+++ b/jetson/ros2_ws/src/saltybot_social_nav/package.xml
@ -1,28 +0,0 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
  <name>saltybot_social_nav</name>
  <version>0.1.0</version>
  <description>Social navigation for saltybot: follow modes, waypoint teaching, A* avoidance, MiDaS depth</description>
  <maintainer email="seb@vayrette.com">seb</maintainer>
  <license>MIT</license>
  <depend>rclpy</depend>
  <depend>std_msgs</depend>
  <depend>geometry_msgs</depend>
  <depend>nav_msgs</depend>
  <depend>sensor_msgs</depend>
  <depend>cv_bridge</depend>
  <depend>tf2_ros</depend>
  <depend>tf2_geometry_msgs</depend>
  <depend>saltybot_social_msgs</depend>
  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
  <test_depend>ament_pep257</test_depend>
  <test_depend>python3-pytest</test_depend>
  <export>
    <build_type>ament_python</build_type>
  </export>
 </package>
--- a/jetson/ros2_ws/src/saltybot_social_nav/resource/saltybot_social_nav
+++ b/jetson/ros2_ws/src/saltybot_social_nav/resource/saltybot_social_nav
--- a/jetson/ros2_ws/src/saltybot_social_nav/saltybot_social_nav/init.py
+++ b/jetson/ros2_ws/src/saltybot_social_nav/saltybot_social_nav/init.py
--- a/jetson/ros2_ws/src/saltybot_social_nav/saltybot_social_nav/astar.py
+++ b/jetson/ros2_ws/src/saltybot_social_nav/saltybot_social_nav/astar.py
@ -1,82 +0,0 @@
 """astar.py -- A* path planner for saltybot social navigation."""
 import heapq
 import numpy as np
 def astar(grid: np.ndarray, start: tuple, goal: tuple,
          obstacle_val: int = 100) -> list | None:
    """
    A* on a 2D occupancy grid (row, col indexing).
    Args:
        grid: 2D numpy array, values 0=free, >=obstacle_val=obstacle
        start: (row, col) start cell
        goal: (row, col) goal cell
        obstacle_val: cells with value >= obstacle_val are blocked
    Returns:
        List of (row, col) tuples from start to goal, or None if no path.
    """
    rows, cols = grid.shape
    def h(a, b):
        return abs(a[0] - b[0]) + abs(a[1] - b[1])  # Manhattan heuristic
    open_set = []
    heapq.heappush(open_set, (h(start, goal), 0, start))
    came_from = {}
    g_score = {start: 0}
    # 8-directional movement
    neighbors_delta = [
        (-1, -1), (-1, 0), (-1, 1),
        (0, -1),           (0, 1),
        (1, -1),  (1, 0),  (1, 1),
    ]
    while open_set:
        _, cost, current = heapq.heappop(open_set)
        if current == goal:
            path = []
            while current in came_from:
                path.append(current)
                current = came_from[current]
            path.append(start)
            return list(reversed(path))
        if cost > g_score.get(current, float('inf')):
            continue
        for dr, dc in neighbors_delta:
            nr, nc = current[0] + dr, current[1] + dc
            if not (0 <= nr < rows and 0 <= nc < cols):
                continue
            if grid[nr, nc] >= obstacle_val:
                continue
            move_cost = 1.414 if (dr != 0 and dc != 0) else 1.0
            new_g = g_score[current] + move_cost
            neighbor = (nr, nc)
            if new_g < g_score.get(neighbor, float('inf')):
                g_score[neighbor] = new_g
                f = new_g + h(neighbor, goal)
                came_from[neighbor] = current
                heapq.heappush(open_set, (f, new_g, neighbor))
    return None  # No path found
 def inflate_obstacles(grid: np.ndarray, inflation_radius_cells: int) -> np.ndarray:
    """Inflate obstacles for robot footprint safety."""
    from scipy.ndimage import binary_dilation
    obstacle_mask = grid >= 50
    kernel = np.ones(
        (2 * inflation_radius_cells + 1, 2 * inflation_radius_cells + 1),
        dtype=bool,
    )
    inflated = binary_dilation(obstacle_mask, structure=kernel)
    result = grid.copy()
    result[inflated] = 100
    return result
--- a/jetson/ros2_ws/src/saltybot_social_nav/saltybot_social_nav/follow_modes.py
+++ b/jetson/ros2_ws/src/saltybot_social_nav/saltybot_social_nav/follow_modes.py
@ -1,82 +0,0 @@
 """follow_modes.py -- Follow mode geometry for saltybot social navigation."""
 import math
 from enum import Enum
 import numpy as np
 class FollowMode(Enum):
    SHADOW = 'shadow'    # stay directly behind at follow_distance
    LEAD   = 'lead'      # move ahead of person by lead_distance
    SIDE   = 'side'      # stay to the right (or left) at side_offset
    ORBIT  = 'orbit'     # circle around person at orbit_radius
    LOOSE  = 'loose'     # general follow, larger tolerance
    TIGHT  = 'tight'     # close follow, small tolerance
 def compute_shadow_target(person_pos, person_bearing_deg, follow_dist=1.2):
    """Target position: behind person along their movement direction."""
    bearing_rad = math.radians(person_bearing_deg + 180.0)
    tx = person_pos[0] + follow_dist * math.sin(bearing_rad)
    ty = person_pos[1] + follow_dist * math.cos(bearing_rad)
    return (tx, ty, person_pos[2])
 def compute_lead_target(person_pos, person_bearing_deg, lead_dist=2.0):
    """Target position: ahead of person."""
    bearing_rad = math.radians(person_bearing_deg)
    tx = person_pos[0] + lead_dist * math.sin(bearing_rad)
    ty = person_pos[1] + lead_dist * math.cos(bearing_rad)
    return (tx, ty, person_pos[2])
 def compute_side_target(person_pos, person_bearing_deg, side_dist=1.0, right=True):
    """Target position: to the right (or left) of person."""
    sign = 1.0 if right else -1.0
    bearing_rad = math.radians(person_bearing_deg + sign * 90.0)
    tx = person_pos[0] + side_dist * math.sin(bearing_rad)
    ty = person_pos[1] + side_dist * math.cos(bearing_rad)
    return (tx, ty, person_pos[2])
 def compute_orbit_target(person_pos, orbit_angle_deg, orbit_radius=1.5):
    """Target on circle of radius orbit_radius around person."""
    angle_rad = math.radians(orbit_angle_deg)
    tx = person_pos[0] + orbit_radius * math.sin(angle_rad)
    ty = person_pos[1] + orbit_radius * math.cos(angle_rad)
    return (tx, ty, person_pos[2])
 def compute_loose_target(person_pos, robot_pos, follow_dist=2.0, tolerance=0.8):
    """Only move if farther than follow_dist + tolerance."""
    dx = person_pos[0] - robot_pos[0]
    dy = person_pos[1] - robot_pos[1]
    dist = math.hypot(dx, dy)
    if dist <= follow_dist + tolerance:
        return robot_pos
    # Target at follow_dist behind person (toward robot)
    scale = (dist - follow_dist) / dist
    return (robot_pos[0] + dx * scale, robot_pos[1] + dy * scale, person_pos[2])
 def compute_tight_target(person_pos, follow_dist=0.6):
    """Close follow: stay very near person."""
    return (person_pos[0], person_pos[1] - follow_dist, person_pos[2])
 MODE_VOICE_COMMANDS = {
    'shadow': FollowMode.SHADOW,
    'follow me': FollowMode.SHADOW,
    'behind me': FollowMode.SHADOW,
    'lead': FollowMode.LEAD,
    'go ahead': FollowMode.LEAD,
    'lead me': FollowMode.LEAD,
    'side': FollowMode.SIDE,
    'stay beside': FollowMode.SIDE,
    'orbit': FollowMode.ORBIT,
    'circle me': FollowMode.ORBIT,
    'loose': FollowMode.LOOSE,
    'give me space': FollowMode.LOOSE,
    'tight': FollowMode.TIGHT,
    'stay close': FollowMode.TIGHT,
 }
--- a/jetson/ros2_ws/src/saltybot_social_nav/saltybot_social_nav/midas_depth_node.py
+++ b/jetson/ros2_ws/src/saltybot_social_nav/saltybot_social_nav/midas_depth_node.py
@ -1,231 +0,0 @@
 """
 midas_depth_node.py -- MiDaS monocular depth estimation for saltybot.
 Uses MiDaS_small via ONNX Runtime or TensorRT FP16.
 Provides relative depth estimates for cameras without active depth (CSI cameras).
 Publishes /social/depth/cam{i}/image  (sensor_msgs/Image, float32, relative depth)
 """
 import os
 import numpy as np
 import rclpy
 from rclpy.node import Node
 from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
 from sensor_msgs.msg import Image
 from cv_bridge import CvBridge
 # MiDaS_small input size
 _MIDAS_H = 256
 _MIDAS_W = 256
 # ImageNet normalization
 _MEAN = np.array([0.485, 0.456, 0.406], dtype=np.float32)
 _STD = np.array([0.229, 0.224, 0.225], dtype=np.float32)
 class _TRTBackend:
    """TensorRT inference backend for MiDaS."""
    def __init__(self, engine_path: str, logger):
        self._logger = logger
        try:
            import tensorrt as trt
            import pycuda.driver as cuda
            import pycuda.autoinit  # noqa: F401
            self._cuda = cuda
            rt_logger = trt.Logger(trt.Logger.WARNING)
            with open(engine_path, 'rb') as f:
                engine = trt.Runtime(rt_logger).deserialize_cuda_engine(f.read())
            self._context = engine.create_execution_context()
            # Allocate buffers
            self._d_input = cuda.mem_alloc(1 * 3 * _MIDAS_H * _MIDAS_W * 4)
            self._d_output = cuda.mem_alloc(1 * _MIDAS_H * _MIDAS_W * 4)
            self._h_output = np.empty((_MIDAS_H, _MIDAS_W), dtype=np.float32)
            self._stream = cuda.Stream()
            self._logger.info(f'TRT engine loaded: {engine_path}')
        except Exception as e:
            raise RuntimeError(f'TRT init failed: {e}')
    def infer(self, input_tensor: np.ndarray) -> np.ndarray:
        self._cuda.memcpy_htod_async(
            self._d_input, input_tensor.ravel(), self._stream)
        self._context.execute_async_v2(
            bindings=[int(self._d_input), int(self._d_output)],
            stream_handle=self._stream.handle)
        self._cuda.memcpy_dtoh_async(
            self._h_output, self._d_output, self._stream)
        self._stream.synchronize()
        return self._h_output.copy()
 class _ONNXBackend:
    """ONNX Runtime inference backend for MiDaS."""
    def __init__(self, onnx_path: str, logger):
        self._logger = logger
        try:
            import onnxruntime as ort
            providers = ['CUDAExecutionProvider', 'CPUExecutionProvider']
            self._session = ort.InferenceSession(onnx_path, providers=providers)
            self._input_name = self._session.get_inputs()[0].name
            self._logger.info(f'ONNX model loaded: {onnx_path}')
        except Exception as e:
            raise RuntimeError(f'ONNX init failed: {e}')
    def infer(self, input_tensor: np.ndarray) -> np.ndarray:
        result = self._session.run(None, {self._input_name: input_tensor})
        return result[0].squeeze()
 class MiDaSDepthNode(Node):
    """MiDaS monocular depth estimation node."""
    def __init__(self):
        super().__init__('midas_depth')
        # Parameters
        self.declare_parameter('onnx_path',
                               '/mnt/nvme/saltybot/models/midas_small.onnx')
        self.declare_parameter('engine_path',
                               '/mnt/nvme/saltybot/models/midas_small.engine')
        self.declare_parameter('camera_topics', [
            '/surround/cam0/image_raw',
            '/surround/cam1/image_raw',
            '/surround/cam2/image_raw',
            '/surround/cam3/image_raw',
        ])
        self.declare_parameter('output_scale', 1.0)
        self.declare_parameter('process_rate', 5.0)
        onnx_path = self.get_parameter('onnx_path').value
        engine_path = self.get_parameter('engine_path').value
        self._camera_topics = self.get_parameter('camera_topics').value
        self._output_scale = self.get_parameter('output_scale').value
        process_rate = self.get_parameter('process_rate').value
        # Initialize inference backend (TRT preferred, ONNX fallback)
        self._backend = None
        if os.path.exists(engine_path):
            try:
                self._backend = _TRTBackend(engine_path, self.get_logger())
            except RuntimeError:
                self.get_logger().warn('TRT failed, trying ONNX fallback')
        if self._backend is None and os.path.exists(onnx_path):
            try:
                self._backend = _ONNXBackend(onnx_path, self.get_logger())
            except RuntimeError:
                self.get_logger().error('Both TRT and ONNX backends failed')
        if self._backend is None:
            self.get_logger().error(
                'No MiDaS model found. Depth estimation disabled.')
        self._bridge = CvBridge()
        # Latest frames per camera (round-robin processing)
        self._latest_frames = [None] * len(self._camera_topics)
        self._current_cam_idx = 0
        # QoS for camera subscriptions
        cam_qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            history=HistoryPolicy.KEEP_LAST,
            depth=1,
        )
        # Subscribe to each camera topic
        self._cam_subs = []
        for i, topic in enumerate(self._camera_topics):
            sub = self.create_subscription(
                Image, topic,
                lambda msg, idx=i: self._on_image(msg, idx),
                cam_qos)
            self._cam_subs.append(sub)
        # Publishers: one per camera
        self._depth_pubs = []
        for i in range(len(self._camera_topics)):
            pub = self.create_publisher(
                Image, f'/social/depth/cam{i}/image', 10)
            self._depth_pubs.append(pub)
        # Timer: round-robin across cameras
        timer_period = 1.0 / process_rate
        self._timer = self.create_timer(timer_period, self._timer_callback)
        self.get_logger().info(
            f'MiDaS depth node started: {len(self._camera_topics)} cameras '
            f'@ {process_rate} Hz')
    def _on_image(self, msg: Image, cam_idx: int):
        """Cache latest frame for each camera."""
        self._latest_frames[cam_idx] = msg
    def _preprocess(self, bgr: np.ndarray) -> np.ndarray:
        """Preprocess BGR image to MiDaS input tensor [1,3,256,256]."""
        import cv2
        rgb = cv2.cvtColor(bgr, cv2.COLOR_BGR2RGB)
        resized = cv2.resize(rgb, (_MIDAS_W, _MIDAS_H),
                             interpolation=cv2.INTER_LINEAR)
        normalized = (resized.astype(np.float32) / 255.0 - _MEAN) / _STD
        # HWC -> CHW -> NCHW
        tensor = normalized.transpose(2, 0, 1)[np.newaxis, ...]
        return tensor.astype(np.float32)
    def _infer(self, tensor: np.ndarray) -> np.ndarray:
        """Run inference, returns [256,256] float32 relative inverse depth."""
        if self._backend is None:
            return np.zeros((_MIDAS_H, _MIDAS_W), dtype=np.float32)
        return self._backend.infer(tensor)
    def _postprocess(self, raw: np.ndarray, orig_shape: tuple) -> np.ndarray:
        """Resize depth back to original image shape, apply output_scale."""
        import cv2
        h, w = orig_shape[:2]
        depth = cv2.resize(raw, (w, h), interpolation=cv2.INTER_LINEAR)
        depth = depth * self._output_scale
        return depth
    def _timer_callback(self):
        """Process one camera per tick (round-robin)."""
        if not self._camera_topics:
            return
        idx = self._current_cam_idx
        self._current_cam_idx = (idx + 1) % len(self._camera_topics)
        msg = self._latest_frames[idx]
        if msg is None:
            return
        try:
            bgr = self._bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
        except Exception as e:
            self.get_logger().warn(f'cv_bridge error cam{idx}: {e}')
            return
        tensor = self._preprocess(bgr)
        raw_depth = self._infer(tensor)
        depth_map = self._postprocess(raw_depth, bgr.shape)
        # Publish as float32 Image
        depth_msg = self._bridge.cv2_to_imgmsg(depth_map, encoding='32FC1')
        depth_msg.header = msg.header
        self._depth_pubs[idx].publish(depth_msg)
 def main(args=None):
    rclpy.init(args=args)
    node = MiDaSDepthNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()
 if __name__ == '__main__':
    main()
--- a/jetson/ros2_ws/src/saltybot_social_nav/saltybot_social_nav/social_nav_node.py
+++ b/jetson/ros2_ws/src/saltybot_social_nav/saltybot_social_nav/social_nav_node.py
@ -1,584 +0,0 @@
 """
 social_nav_node.py -- Social navigation node for saltybot.
 Orchestrates person following with multiple modes, voice commands,
 waypoint teaching/replay, and A* obstacle avoidance.
 Follow modes:
  shadow  -- stay directly behind at follow_distance
  lead    -- move ahead of person
  side    -- stay beside (default right)
  orbit   -- circle around person
  loose   -- relaxed follow with deadband
  tight   -- close follow
 Waypoint teaching:
  Voice command "teach route <name>" -> record mode ON
  Voice command "stop teaching" -> save route
  Voice command "replay route <name>" -> playback
 Voice commands:
  "follow me" / "shadow" -> SHADOW mode
  "lead me" / "go ahead" -> LEAD mode
  "stay beside" -> SIDE mode
  "orbit" -> ORBIT mode
  "give me space" -> LOOSE mode
  "stay close" -> TIGHT mode
  "stop" / "halt" -> STOP
  "go home" -> navigate to home waypoint
  "teach route <name>" -> start recording
  "stop teaching" -> finish recording
  "replay route <name>" -> playback recorded route
 """
 import math
 import time
 import re
 from collections import deque
 import numpy as np
 import rclpy
 from rclpy.node import Node
 from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
 from geometry_msgs.msg import Twist, PoseStamped
 from nav_msgs.msg import OccupancyGrid, Odometry
 from std_msgs.msg import String, Int32
 from .follow_modes import (
    FollowMode, MODE_VOICE_COMMANDS,
    compute_shadow_target, compute_lead_target, compute_side_target,
    compute_orbit_target, compute_loose_target, compute_tight_target,
 )
 from .astar import astar, inflate_obstacles
 from .waypoint_teacher import WaypointRoute, WaypointReplayer
 # Try importing social msgs; fallback gracefully
 try:
    from saltybot_social_msgs.msg import PersonStateArray
    _HAS_SOCIAL_MSGS = True
 except ImportError:
    _HAS_SOCIAL_MSGS = False
 # Proportional controller gains
 _K_ANG = 2.0   # angular gain
 _K_LIN = 0.8   # linear gain
 _HIGH_SPEED_THRESHOLD = 3.0  # m/s person velocity triggers fast mode
 _PREDICT_AHEAD_S = 0.3       # seconds to extrapolate position
 _TEACH_MIN_DIST = 0.5        # meters between recorded waypoints
 class SocialNavNode(Node):
    """Main social navigation orchestrator."""
    def __init__(self):
        super().__init__('social_nav')
        # -- Parameters --
        self.declare_parameter('follow_mode', 'shadow')
        self.declare_parameter('follow_distance', 1.2)
        self.declare_parameter('lead_distance', 2.0)
        self.declare_parameter('orbit_radius', 1.5)
        self.declare_parameter('max_linear_speed', 1.0)
        self.declare_parameter('max_linear_speed_fast', 5.5)
        self.declare_parameter('max_angular_speed', 1.0)
        self.declare_parameter('goal_tolerance', 0.3)
        self.declare_parameter('routes_dir', '/mnt/nvme/saltybot/routes')
        self.declare_parameter('home_x', 0.0)
        self.declare_parameter('home_y', 0.0)
        self.declare_parameter('map_resolution', 0.05)
        self.declare_parameter('obstacle_inflation_cells', 3)
        self._follow_mode = FollowMode(
            self.get_parameter('follow_mode').value)
        self._follow_distance = self.get_parameter('follow_distance').value
        self._lead_distance = self.get_parameter('lead_distance').value
        self._orbit_radius = self.get_parameter('orbit_radius').value
        self._max_lin = self.get_parameter('max_linear_speed').value
        self._max_lin_fast = self.get_parameter('max_linear_speed_fast').value
        self._max_ang = self.get_parameter('max_angular_speed').value
        self._goal_tol = self.get_parameter('goal_tolerance').value
        self._routes_dir = self.get_parameter('routes_dir').value
        self._home_x = self.get_parameter('home_x').value
        self._home_y = self.get_parameter('home_y').value
        self._map_resolution = self.get_parameter('map_resolution').value
        self._inflation_cells = self.get_parameter(
            'obstacle_inflation_cells').value
        # -- State --
        self._robot_x = 0.0
        self._robot_y = 0.0
        self._robot_yaw = 0.0
        self._target_person_pos = None   # (x, y, z)
        self._target_person_bearing = 0.0
        self._target_person_id = -1
        self._person_history = deque(maxlen=5)  # for velocity estimation
        self._stopped = False
        self._go_home = False
        # Occupancy grid for A*
        self._occ_grid = None
        self._occ_origin = (0.0, 0.0)
        self._occ_resolution = 0.05
        self._astar_path = None
        # Orbit state
        self._orbit_angle = 0.0
        # Waypoint teaching / replay
        self._teaching = False
        self._current_route = None
        self._last_teach_x = None
        self._last_teach_y = None
        self._replayer = None
        # -- QoS profiles --
        best_effort_qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            history=HistoryPolicy.KEEP_LAST, depth=1)
        reliable_qos = QoSProfile(
            reliability=ReliabilityPolicy.RELIABLE,
            history=HistoryPolicy.KEEP_LAST, depth=1)
        # -- Subscriptions --
        if _HAS_SOCIAL_MSGS:
            self.create_subscription(
                PersonStateArray, '/social/persons',
                self._on_persons, best_effort_qos)
        else:
            self.get_logger().warn(
                'saltybot_social_msgs not found; '
                'using /person/target fallback')
        self.create_subscription(
            PoseStamped, '/person/target',
            self._on_person_target, best_effort_qos)
        self.create_subscription(
            String, '/social/speech/command',
            self._on_voice_command, 10)
        self.create_subscription(
            String, '/social/speech/transcript',
            self._on_transcript, 10)
        self.create_subscription(
            OccupancyGrid, '/map',
            self._on_map, reliable_qos)
        self.create_subscription(
            Odometry, '/odom',
            self._on_odom, best_effort_qos)
        self.create_subscription(
            Int32, '/social/attention/target_id',
            self._on_target_id, 10)
        # -- Publishers --
        self._cmd_vel_pub = self.create_publisher(
            Twist, '/cmd_vel', best_effort_qos)
        self._mode_pub = self.create_publisher(
            String, '/social/nav/mode', reliable_qos)
        self._target_pub = self.create_publisher(
            PoseStamped, '/social/nav/target_pos', 10)
        self._status_pub = self.create_publisher(
            String, '/social/nav/status', best_effort_qos)
        # -- Main loop timer (20 Hz) --
        self._timer = self.create_timer(0.05, self._control_loop)
        self.get_logger().info(
            f'Social nav started: mode={self._follow_mode.value}, '
            f'dist={self._follow_distance}m')
    # ----------------------------------------------------------------
    # Subscriptions
    # ----------------------------------------------------------------
    def _on_persons(self, msg):
        """Handle PersonStateArray from social perception."""
        target_id = msg.primary_attention_id
        if self._target_person_id >= 0:
            target_id = self._target_person_id
        for p in msg.persons:
            if p.person_id == target_id or (
                    target_id < 0 and len(msg.persons) > 0):
                pos = (p.position.x, p.position.y, p.position.z)
                self._update_person_position(pos, p.bearing_deg)
                break
    def _on_person_target(self, msg: PoseStamped):
        """Fallback: single person target pose."""
        pos = (msg.pose.position.x, msg.pose.position.y,
               msg.pose.position.z)
        # Estimate bearing from quaternion yaw
        q = msg.pose.orientation
        yaw = math.atan2(2.0 * (q.w * q.z + q.x * q.y),
                         1.0 - 2.0 * (q.y * q.y + q.z * q.z))
        self._update_person_position(pos, math.degrees(yaw))
    def _update_person_position(self, pos, bearing_deg):
        """Update person tracking state and record history."""
        now = time.time()
        self._target_person_pos = pos
        self._target_person_bearing = bearing_deg
        self._person_history.append((now, pos[0], pos[1]))
    def _on_odom(self, msg: Odometry):
        """Update robot pose from odometry."""
        self._robot_x = msg.pose.pose.position.x
        self._robot_y = msg.pose.pose.position.y
        q = msg.pose.pose.orientation
        self._robot_yaw = math.atan2(
            2.0 * (q.w * q.z + q.x * q.y),
            1.0 - 2.0 * (q.y * q.y + q.z * q.z))
    def _on_map(self, msg: OccupancyGrid):
        """Cache occupancy grid for A* planning."""
        w, h = msg.info.width, msg.info.height
        data = np.array(msg.data, dtype=np.int8).reshape((h, w))
        # Convert -1 (unknown) to free (0) for planning
        data[data < 0] = 0
        self._occ_grid = data.astype(np.int32)
        self._occ_origin = (msg.info.origin.position.x,
                            msg.info.origin.position.y)
        self._occ_resolution = msg.info.resolution
    def _on_target_id(self, msg: Int32):
        """Switch target person."""
        self._target_person_id = msg.data
        self.get_logger().info(f'Target person ID set to {msg.data}')
    def _on_voice_command(self, msg: String):
        """Handle discrete voice commands for mode switching."""
        cmd = msg.data.strip().lower()
        if cmd in ('stop', 'halt'):
            self._stopped = True
            self._replayer = None
            self._publish_status('STOPPED')
            return
        if cmd in ('resume', 'go', 'start'):
            self._stopped = False
            self._publish_status('RESUMED')
            return
        matched = MODE_VOICE_COMMANDS.get(cmd)
        if matched:
            self._follow_mode = matched
            self._stopped = False
            mode_msg = String()
            mode_msg.data = self._follow_mode.value
            self._mode_pub.publish(mode_msg)
            self._publish_status(f'MODE: {self._follow_mode.value}')
    def _on_transcript(self, msg: String):
        """Handle free-form voice transcripts for route teaching."""
        text = msg.data.strip().lower()
        # "teach route <name>"
        m = re.match(r'teach\s+route\s+(\w+)', text)
        if m:
            name = m.group(1)
            self._teaching = True
            self._current_route = WaypointRoute(name)
            self._last_teach_x = self._robot_x
            self._last_teach_y = self._robot_y
            self._publish_status(f'TEACHING: {name}')
            self.get_logger().info(f'Recording route: {name}')
            return
        # "stop teaching"
        if 'stop teaching' in text:
            if self._teaching and self._current_route:
                self._current_route.save(self._routes_dir)
                self._publish_status(
                    f'SAVED: {self._current_route.name} '
                    f'({len(self._current_route.waypoints)} pts)')
                self.get_logger().info(
                    f'Route saved: {self._current_route.name}')
            self._teaching = False
            self._current_route = None
            return
        # "replay route <name>"
        m = re.match(r'replay\s+route\s+(\w+)', text)
        if m:
            name = m.group(1)
            try:
                route = WaypointRoute.load(self._routes_dir, name)
                self._replayer = WaypointReplayer(route)
                self._stopped = False
                self._publish_status(f'REPLAY: {name}')
                self.get_logger().info(f'Replaying route: {name}')
            except FileNotFoundError:
                self._publish_status(f'ROUTE NOT FOUND: {name}')
            return
        # "go home"
        if 'go home' in text:
            self._go_home = True
            self._stopped = False
            self._publish_status('GO HOME')
            return
        # Also try mode commands from transcript
        for phrase, mode in MODE_VOICE_COMMANDS.items():
            if phrase in text:
                self._follow_mode = mode
                self._stopped = False
                mode_msg = String()
                mode_msg.data = self._follow_mode.value
                self._mode_pub.publish(mode_msg)
                self._publish_status(f'MODE: {self._follow_mode.value}')
                return
    # ----------------------------------------------------------------
    # Control loop
    # ----------------------------------------------------------------
    def _control_loop(self):
        """Main 20Hz control loop."""
        # Record waypoint if teaching
        if self._teaching and self._current_route:
            self._maybe_record_waypoint()
        # Publish zero velocity if stopped
        if self._stopped:
            self._publish_cmd_vel(0.0, 0.0)
            return
        # Determine navigation target
        target = self._get_nav_target()
        if target is None:
            self._publish_cmd_vel(0.0, 0.0)
            return
        tx, ty, tz = target
        # Publish debug target
        self._publish_target_pose(tx, ty, tz)
        # Check if arrived
        dist_to_target = math.hypot(tx - self._robot_x, ty - self._robot_y)
        if dist_to_target < self._goal_tol:
            self._publish_cmd_vel(0.0, 0.0)
            if self._go_home:
                self._go_home = False
                self._publish_status('HOME REACHED')
            return
        # Try A* path if map available
        if self._occ_grid is not None:
            path_target = self._plan_astar(tx, ty)
            if path_target:
                tx, ty = path_target
        # Determine speed limit
        max_lin = self._max_lin
        person_vel = self._estimate_person_velocity()
        if person_vel > _HIGH_SPEED_THRESHOLD:
            max_lin = self._max_lin_fast
        # Compute and publish cmd_vel
        lin, ang = self._compute_cmd_vel(
            self._robot_x, self._robot_y, self._robot_yaw,
            tx, ty, max_lin)
        self._publish_cmd_vel(lin, ang)
    def _get_nav_target(self):
        """Determine current navigation target based on mode/state."""
        # Route replay takes priority
        if self._replayer and not self._replayer.is_done:
            self._replayer.check_arrived(self._robot_x, self._robot_y)
            wp = self._replayer.current_waypoint()
            if wp:
                return (wp.x, wp.y, wp.z)
            else:
                self._replayer = None
                self._publish_status('REPLAY DONE')
                return None
        # Go home
        if self._go_home:
            return (self._home_x, self._home_y, 0.0)
        # Person following
        if self._target_person_pos is None:
            return None
        # Predict person position ahead for high-speed tracking
        px, py, pz = self._predict_person_position()
        bearing = self._target_person_bearing
        robot_pos = (self._robot_x, self._robot_y, 0.0)
        if self._follow_mode == FollowMode.SHADOW:
            return compute_shadow_target(
                (px, py, pz), bearing, self._follow_distance)
        elif self._follow_mode == FollowMode.LEAD:
            return compute_lead_target(
                (px, py, pz), bearing, self._lead_distance)
        elif self._follow_mode == FollowMode.SIDE:
            return compute_side_target(
                (px, py, pz), bearing, self._follow_distance)
        elif self._follow_mode == FollowMode.ORBIT:
            self._orbit_angle = (self._orbit_angle + 1.0) % 360.0
            return compute_orbit_target(
                (px, py, pz), self._orbit_angle, self._orbit_radius)
        elif self._follow_mode == FollowMode.LOOSE:
            return compute_loose_target(
                (px, py, pz), robot_pos, self._follow_distance)
        elif self._follow_mode == FollowMode.TIGHT:
            return compute_tight_target(
                (px, py, pz), self._follow_distance)
        return (px, py, pz)
    def _predict_person_position(self):
        """Extrapolate person position using velocity from recent samples."""
        if self._target_person_pos is None:
            return (0.0, 0.0, 0.0)
        px, py, pz = self._target_person_pos
        if len(self._person_history) >= 3:
            # Use last 3 samples for velocity estimation
            t0, x0, y0 = self._person_history[-3]
            t1, x1, y1 = self._person_history[-1]
            dt = t1 - t0
            if dt > 0.01:
                vx = (x1 - x0) / dt
                vy = (y1 - y0) / dt
                speed = math.hypot(vx, vy)
                if speed > _HIGH_SPEED_THRESHOLD:
                    px += vx * _PREDICT_AHEAD_S
                    py += vy * _PREDICT_AHEAD_S
        return (px, py, pz)
    def _estimate_person_velocity(self) -> float:
        """Estimate person speed from recent position history."""
        if len(self._person_history) < 2:
            return 0.0
        t0, x0, y0 = self._person_history[-2]
        t1, x1, y1 = self._person_history[-1]
        dt = t1 - t0
        if dt < 0.01:
            return 0.0
        return math.hypot(x1 - x0, y1 - y0) / dt
    def _plan_astar(self, target_x, target_y):
        """Run A* on occupancy grid, return next waypoint in world coords."""
        grid = self._occ_grid
        res = self._occ_resolution
        ox, oy = self._occ_origin
        # World to grid
        def w2g(wx, wy):
            return (int((wy - oy) / res), int((wx - ox) / res))
        start = w2g(self._robot_x, self._robot_y)
        goal = w2g(target_x, target_y)
        rows, cols = grid.shape
        if not (0 <= start[0] < rows and 0 <= start[1] < cols):
            return None
        if not (0 <= goal[0] < rows and 0 <= goal[1] < cols):
            return None
        inflated = inflate_obstacles(grid, self._inflation_cells)
        path = astar(inflated, start, goal)
        if path and len(path) > 1:
            # Follow a lookahead point (3 steps ahead or end)
            lookahead_idx = min(3, len(path) - 1)
            r, c = path[lookahead_idx]
            wx = ox + c * res + res / 2.0
            wy = oy + r * res + res / 2.0
            self._astar_path = path
            return (wx, wy)
        return None
    def _compute_cmd_vel(self, rx, ry, ryaw, tx, ty, max_lin):
        """Proportional controller: compute linear and angular velocity."""
        dx = tx - rx
        dy = ty - ry
        dist = math.hypot(dx, dy)
        angle_to_target = math.atan2(dy, dx)
        angle_error = angle_to_target - ryaw
        # Normalize angle error to [-pi, pi]
        while angle_error > math.pi:
            angle_error -= 2.0 * math.pi
        while angle_error < -math.pi:
            angle_error += 2.0 * math.pi
        angular_vel = _K_ANG * angle_error
        angular_vel = max(-self._max_ang,
                          min(self._max_ang, angular_vel))
        # Reduce linear speed when turning hard
        angle_factor = max(0.0, 1.0 - abs(angle_error) / (math.pi / 2.0))
        linear_vel = _K_LIN * dist * angle_factor
        linear_vel = max(0.0, min(max_lin, linear_vel))
        return (linear_vel, angular_vel)
    # ----------------------------------------------------------------
    # Waypoint teaching
    # ----------------------------------------------------------------
    def _maybe_record_waypoint(self):
        """Record waypoint if robot moved > _TEACH_MIN_DIST."""
        if self._last_teach_x is None:
            self._last_teach_x = self._robot_x
            self._last_teach_y = self._robot_y
        dist = math.hypot(
            self._robot_x - self._last_teach_x,
            self._robot_y - self._last_teach_y)
        if dist >= _TEACH_MIN_DIST:
            yaw_deg = math.degrees(self._robot_yaw)
            self._current_route.add(
                self._robot_x, self._robot_y, 0.0, yaw_deg)
            self._last_teach_x = self._robot_x
            self._last_teach_y = self._robot_y
    # ----------------------------------------------------------------
    # Publishers
    # ----------------------------------------------------------------
    def _publish_cmd_vel(self, linear: float, angular: float):
        twist = Twist()
        twist.linear.x = linear
        twist.angular.z = angular
        self._cmd_vel_pub.publish(twist)
    def _publish_target_pose(self, x, y, z):
        msg = PoseStamped()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'map'
        msg.pose.position.x = x
        msg.pose.position.y = y
        msg.pose.position.z = z
        self._target_pub.publish(msg)
    def _publish_status(self, status: str):
        msg = String()
        msg.data = status
        self._status_pub.publish(msg)
        self.get_logger().info(f'Nav status: {status}')
 def main(args=None):
    rclpy.init(args=args)
    node = SocialNavNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()
 if __name__ == '__main__':
    main()
--- a/jetson/ros2_ws/src/saltybot_social_nav/saltybot_social_nav/waypoint_teacher.py
+++ b/jetson/ros2_ws/src/saltybot_social_nav/saltybot_social_nav/waypoint_teacher.py
@ -1,91 +0,0 @@
 """waypoint_teacher.py -- Record and replay waypoint routes."""
 import json
 import time
 import math
 from pathlib import Path
 from dataclasses import dataclass, asdict
@dataclass
 class Waypoint:
    x: float
    y: float
    z: float
    yaw_deg: float
    timestamp: float
    label: str = ''
 class WaypointRoute:
    """A named sequence of waypoints."""
    def __init__(self, name: str):
        self.name = name
        self.waypoints: list[Waypoint] = []
        self.created_at = time.time()
    def add(self, x, y, z, yaw_deg, label=''):
        self.waypoints.append(Waypoint(x, y, z, yaw_deg, time.time(), label))
    def to_dict(self):
        return {
            'name': self.name,
            'created_at': self.created_at,
            'waypoints': [asdict(w) for w in self.waypoints],
        }
    @classmethod
    def from_dict(cls, d):
        route = cls(d['name'])
        route.created_at = d.get('created_at', 0)
        route.waypoints = [Waypoint(**w) for w in d['waypoints']]
        return route
    def save(self, routes_dir: str):
        Path(routes_dir).mkdir(parents=True, exist_ok=True)
        path = Path(routes_dir) / f'{self.name}.json'
        path.write_text(json.dumps(self.to_dict(), indent=2))
    @classmethod
    def load(cls, routes_dir: str, name: str):
        path = Path(routes_dir) / f'{name}.json'
        return cls.from_dict(json.loads(path.read_text()))
    @staticmethod
    def list_routes(routes_dir: str) -> list[str]:
        d = Path(routes_dir)
        if not d.exists():
            return []
        return [p.stem for p in d.glob('*.json')]
 class WaypointReplayer:
    """Iterates through waypoints, returning next target."""
    def __init__(self, route: WaypointRoute, arrival_radius: float = 0.3):
        self._route = route
        self._idx = 0
        self._arrival_radius = arrival_radius
    def current_waypoint(self) -> Waypoint | None:
        if self._idx < len(self._route.waypoints):
            return self._route.waypoints[self._idx]
        return None
    def check_arrived(self, robot_x, robot_y) -> bool:
        wp = self.current_waypoint()
        if wp is None:
            return False
        dist = math.hypot(robot_x - wp.x, robot_y - wp.y)
        if dist < self._arrival_radius:
            self._idx += 1
            return True
        return False
    @property
    def is_done(self) -> bool:
        return self._idx >= len(self._route.waypoints)
    def reset(self):
        self._idx = 0
--- a/jetson/ros2_ws/src/saltybot_social_nav/saltybot_social_nav/waypoint_teacher_node.py
+++ b/jetson/ros2_ws/src/saltybot_social_nav/saltybot_social_nav/waypoint_teacher_node.py
@ -1,135 +0,0 @@
 """
 waypoint_teacher_node.py -- Standalone waypoint teacher ROS2 node.
 Listens to /social/speech/transcript for "teach route <name>" and "stop teaching".
 Records robot pose at configurable intervals. Saves/loads routes via WaypointRoute.
 """
 import math
 import rclpy
 from rclpy.node import Node
 from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
 from nav_msgs.msg import Odometry
 from std_msgs.msg import String
 from .waypoint_teacher import WaypointRoute
 class WaypointTeacherNode(Node):
    """Standalone waypoint teaching node."""
    def __init__(self):
        super().__init__('waypoint_teacher')
        self.declare_parameter('routes_dir', '/mnt/nvme/saltybot/routes')
        self.declare_parameter('recording_interval', 0.5)  # meters
        self._routes_dir = self.get_parameter('routes_dir').value
        self._interval = self.get_parameter('recording_interval').value
        self._teaching = False
        self._route = None
        self._last_x = None
        self._last_y = None
        self._robot_x = 0.0
        self._robot_y = 0.0
        self._robot_yaw = 0.0
        best_effort_qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            history=HistoryPolicy.KEEP_LAST, depth=1)
        self.create_subscription(
            Odometry, '/odom', self._on_odom, best_effort_qos)
        self.create_subscription(
            String, '/social/speech/transcript',
            self._on_transcript, 10)
        self._status_pub = self.create_publisher(
            String, '/social/waypoint/status', 10)
        # Record timer at 10Hz (check distance)
        self._timer = self.create_timer(0.1, self._record_tick)
        self.get_logger().info(
            f'Waypoint teacher ready (interval={self._interval}m, '
            f'dir={self._routes_dir})')
    def _on_odom(self, msg: Odometry):
        self._robot_x = msg.pose.pose.position.x
        self._robot_y = msg.pose.pose.position.y
        q = msg.pose.pose.orientation
        self._robot_yaw = math.atan2(
            2.0 * (q.w * q.z + q.x * q.y),
            1.0 - 2.0 * (q.y * q.y + q.z * q.z))
    def _on_transcript(self, msg: String):
        text = msg.data.strip().lower()
        import re
        m = re.match(r'teach\s+route\s+(\w+)', text)
        if m:
            name = m.group(1)
            self._route = WaypointRoute(name)
            self._teaching = True
            self._last_x = self._robot_x
            self._last_y = self._robot_y
            self._pub_status(f'RECORDING: {name}')
            self.get_logger().info(f'Recording route: {name}')
            return
        if 'stop teaching' in text:
            if self._teaching and self._route:
                self._route.save(self._routes_dir)
                n = len(self._route.waypoints)
                self._pub_status(
                    f'SAVED: {self._route.name} ({n} waypoints)')
                self.get_logger().info(
                    f'Route saved: {self._route.name} ({n} pts)')
            self._teaching = False
            self._route = None
            return
        if 'list routes' in text:
            routes = WaypointRoute.list_routes(self._routes_dir)
            self._pub_status(f'ROUTES: {", ".join(routes) or "(none)"}')
    def _record_tick(self):
        if not self._teaching or self._route is None:
            return
        if self._last_x is None:
            self._last_x = self._robot_x
            self._last_y = self._robot_y
        dist = math.hypot(
            self._robot_x - self._last_x,
            self._robot_y - self._last_y)
        if dist >= self._interval:
            yaw_deg = math.degrees(self._robot_yaw)
            self._route.add(self._robot_x, self._robot_y, 0.0, yaw_deg)
            self._last_x = self._robot_x
            self._last_y = self._robot_y
    def _pub_status(self, text: str):
        msg = String()
        msg.data = text
        self._status_pub.publish(msg)
 def main(args=None):
    rclpy.init(args=args)
    node = WaypointTeacherNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()
 if __name__ == '__main__':
    main()
--- a/jetson/ros2_ws/src/saltybot_social_nav/scripts/build_midas_trt_engine.py
+++ b/jetson/ros2_ws/src/saltybot_social_nav/scripts/build_midas_trt_engine.py
@ -1,80 +0,0 @@
 #!/usr/bin/env python3
 """
 build_midas_trt_engine.py -- Build TensorRT FP16 engine for MiDaS_small from ONNX.
 Usage:
    python3 build_midas_trt_engine.py \
        --onnx /mnt/nvme/saltybot/models/midas_small.onnx \
        --engine /mnt/nvme/saltybot/models/midas_small.engine \
        --fp16
 Requires: tensorrt, pycuda
 """
 import argparse
 import os
 import sys
 def build_engine(onnx_path: str, engine_path: str, fp16: bool = True):
    try:
        import tensorrt as trt
    except ImportError:
        print('ERROR: tensorrt not found. Install TensorRT first.')
        sys.exit(1)
    logger = trt.Logger(trt.Logger.INFO)
    builder = trt.Builder(logger)
    network = builder.create_network(
        1 << int(trt.NetworkDefinitionCreationFlag.EXPLICIT_BATCH))
    parser = trt.OnnxParser(network, logger)
    print(f'Parsing ONNX model: {onnx_path}')
    with open(onnx_path, 'rb') as f:
        if not parser.parse(f.read()):
            for i in range(parser.num_errors):
                print(f'  ONNX parse error: {parser.get_error(i)}')
            sys.exit(1)
    config = builder.create_builder_config()
    config.set_memory_pool_limit(trt.MemoryPoolType.WORKSPACE, 1 << 30)  # 1GB
    if fp16 and builder.platform_has_fast_fp16:
        config.set_flag(trt.BuilderFlag.FP16)
        print('FP16 mode enabled')
    elif fp16:
        print('WARNING: FP16 not supported on this platform, using FP32')
    print('Building TensorRT engine (this may take several minutes)...')
    engine_bytes = builder.build_serialized_network(network, config)
    if engine_bytes is None:
        print('ERROR: Failed to build engine')
        sys.exit(1)
    os.makedirs(os.path.dirname(engine_path) or '.', exist_ok=True)
    with open(engine_path, 'wb') as f:
        f.write(engine_bytes)
    size_mb = len(engine_bytes) / (1024 * 1024)
    print(f'Engine saved: {engine_path} ({size_mb:.1f} MB)')
 def main():
    parser = argparse.ArgumentParser(
        description='Build TensorRT FP16 engine for MiDaS_small')
    parser.add_argument('--onnx', required=True,
                        help='Path to MiDaS ONNX model')
    parser.add_argument('--engine', required=True,
                        help='Output TRT engine path')
    parser.add_argument('--fp16', action='store_true', default=True,
                        help='Enable FP16 (default: True)')
    parser.add_argument('--fp32', action='store_true',
                        help='Force FP32 (disable FP16)')
    args = parser.parse_args()
    fp16 = not args.fp32
    build_engine(args.onnx, args.engine, fp16=fp16)
 if __name__ == '__main__':
    main()
--- a/jetson/ros2_ws/src/saltybot_social_nav/setup.cfg
+++ b/jetson/ros2_ws/src/saltybot_social_nav/setup.cfg
@ -1,4 +0,0 @@
 [develop]
 script_dir=$base/lib/saltybot_social_nav
 [install]
 install_scripts=$base/lib/saltybot_social_nav
--- a/jetson/ros2_ws/src/saltybot_social_nav/setup.py
+++ b/jetson/ros2_ws/src/saltybot_social_nav/setup.py
@ -1,31 +0,0 @@
 from setuptools import setup
 import os
 from glob import glob
 package_name = 'saltybot_social_nav'
 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='Social navigation for saltybot: follow modes, waypoint teaching, A* avoidance, MiDaS depth',
    license='MIT',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'social_nav = saltybot_social_nav.social_nav_node:main',
            'midas_depth = saltybot_social_nav.midas_depth_node:main',
            'waypoint_teacher = saltybot_social_nav.waypoint_teacher_node:main',
        ],
    },
 )
--- a/jetson/ros2_ws/src/saltybot_social_nav/test/test_copyright.py
+++ b/jetson/ros2_ws/src/saltybot_social_nav/test/test_copyright.py
@ -1,12 +0,0 @@
 # Copyright 2026 SaltyLab
 # Licensed under MIT
 from ament_copyright.main import main
 import pytest
@pytest.mark.copyright
@pytest.mark.linter
 def test_copyright():
    rc = main(argv=['.', 'test'])
    assert rc == 0, 'Found errors'
--- a/jetson/ros2_ws/src/saltybot_social_nav/test/test_flake8.py
+++ b/jetson/ros2_ws/src/saltybot_social_nav/test/test_flake8.py
@ -1,14 +0,0 @@
 # Copyright 2026 SaltyLab
 # Licensed under MIT
 from ament_flake8.main import main_with_errors
 import pytest
@pytest.mark.flake8
@pytest.mark.linter
 def test_flake8():
    rc, errors = main_with_errors(argv=[])
    assert rc == 0, \
        'Found %d code style errors / warnings:\n' % len(errors) + \
        '\n'.join(errors)
--- a/jetson/ros2_ws/src/saltybot_social_nav/test/test_pep257.py
+++ b/jetson/ros2_ws/src/saltybot_social_nav/test/test_pep257.py
@ -1,12 +0,0 @@
 # Copyright 2026 SaltyLab
 # Licensed under MIT
 from ament_pep257.main import main
 import pytest
@pytest.mark.pep257
@pytest.mark.linter
 def test_pep257():
    rc = main(argv=['.', 'test'])
    assert rc == 0, 'Found code style errors / warnings'
--- a/jetson/ros2_ws/src/saltybot_uwb/config/uwb_config.yaml
+++ b/jetson/ros2_ws/src/saltybot_uwb/config/uwb_config.yaml
@ -1,24 +1,57 @@
-# uwb_config.yaml — MaUWB ESP32-S3 DW3000 UWB integration (Issue #90)
+# uwb_config.yaml — MaUWB ESP32-S3 DW3000 UWB follow-me system
 #
 # Hardware layout:
 #   Anchor-0 (port side)      → USB port_a, y = +anchor_separation/2
 #   Anchor-1 (starboard side) → USB port_b, y = -anchor_separation/2
 #   Tag on person             → belt clip, ~0.9m above ground
 #
 # Run with:
 #   ros2 launch saltybot_uwb uwb.launch.py
 # Override at launch:
 #   ros2 launch saltybot_uwb uwb.launch.py port_a:=/dev/ttyUSB2
-port_a:    /dev/uwb-anchor0
+# ── Serial ports ──────────────────────────────────────────────────────────────
-port_b:    /dev/uwb-anchor1
+# Set udev rules to get stable symlinks:
-baudrate:  115200
+#   /dev/uwb-anchor0  → port_a
 #   /dev/uwb-anchor1  → port_b
 # (See jetson/docs/pinout.md for udev setup)
 port_a:    /dev/uwb-anchor0      # Anchor-0 (port)
 port_b:    /dev/uwb-anchor1      # Anchor-1 (starboard)
 baudrate:  115200                 # MaUWB default — do not change
-anchor_separation: 0.25
+# ── Anchor geometry ────────────────────────────────────────────────────────────
-anchor_height:     0.80
+# anchor_separation: centre-to-centre distance between anchors (metres)
-tag_height:        0.90
+#   Must match physical mounting. Larger = more accurate lateral resolution.
 anchor_separation: 0.25          # metres (25cm)
-range_timeout_s: 1.0
+# anchor_height: height of anchors above ground (metres)
-max_range_m:     8.0
+#   Orin stem mount ≈ 0.80m on the saltybot platform
-min_range_m:     0.05
+anchor_height: 0.80              # metres
 # tag_height: height of person's belt-clip tag above ground (metres)
 tag_height: 0.90                 # metres (adjust per user)
 # ── Range validity ─────────────────────────────────────────────────────────────
 # range_timeout_s: stale anchor — excluded from triangulation after this gap
 range_timeout_s: 1.0             # seconds
 # max_range_m: discard ranges beyond this (DW3000 indoor practical limit ≈8m)
 max_range_m: 8.0                 # metres
 # min_range_m: discard ranges below this (likely multipath artefacts)
 min_range_m: 0.05                # metres
 # ── Kalman filter ──────────────────────────────────────────────────────────────
 # kf_process_noise: Q scalar — how dynamic the person's motion is
 #   Higher → faster response, more jitter
 kf_process_noise: 0.1
 # kf_meas_noise: R scalar — how noisy the UWB ranging is
 #   DW3000 indoor accuracy ≈ 10cm RMS → 0.1m → R ≈ 0.01
 #   Use 0.3 to be conservative on first deployment
 kf_meas_noise: 0.3
-range_rate:   100.0
+# ── Publish rate ──────────────────────────────────────────────────────────────
-bearing_rate: 10.0
+# Should match or exceed the AT+RANGE? poll rate from both anchors.
-
+# 20Hz means 50ms per cycle; each anchor query takes ~10ms → headroom ok.
-enrolled_tag_ids: [""]
+publish_rate: 20.0               # Hz
--- a/jetson/ros2_ws/src/saltybot_uwb/launch/uwb.launch.py
+++ b/jetson/ros2_ws/src/saltybot_uwb/launch/uwb.launch.py
@ -1,14 +1,10 @@
 """
-uwb.launch.py — Launch UWB driver node for MaUWB ESP32-S3 DW3000 (Issue #90).
+uwb.launch.py — Launch UWB driver node for MaUWB ESP32-S3 follow-me.
 Topics:
  /uwb/ranges   100 Hz raw TWR ranges
  /uwb/bearing   10 Hz Kalman-fused bearing
  /uwb/target    10 Hz triangulated PoseStamped (backwards compat)
 Usage:
  ros2 launch saltybot_uwb uwb.launch.py
-  ros2 launch saltybot_uwb uwb.launch.py enrolled_tag_ids:="['0xDEADBEEF']"
+  ros2 launch saltybot_uwb uwb.launch.py port_a:=/dev/ttyUSB2 port_b:=/dev/ttyUSB3
  ros2 launch saltybot_uwb uwb.launch.py anchor_separation:=0.30 publish_rate:=10.0
 """
 import os
@ -35,9 +31,7 @@ def generate_launch_description():
        DeclareLaunchArgument("min_range_m",        default_value="0.05"),
        DeclareLaunchArgument("kf_process_noise",   default_value="0.1"),
        DeclareLaunchArgument("kf_meas_noise",      default_value="0.3"),
-        DeclareLaunchArgument("range_rate",         default_value="100.0"),
+        DeclareLaunchArgument("publish_rate",       default_value="20.0"),
        DeclareLaunchArgument("bearing_rate",       default_value="10.0"),
        DeclareLaunchArgument("enrolled_tag_ids",   default_value="['']"),
        Node(
            package="saltybot_uwb",
@ -58,9 +52,7 @@ def generate_launch_description():
                    "min_range_m":       LaunchConfiguration("min_range_m"),
                    "kf_process_noise":  LaunchConfiguration("kf_process_noise"),
                    "kf_meas_noise":     LaunchConfiguration("kf_meas_noise"),
-                    "range_rate":        LaunchConfiguration("range_rate"),
+                    "publish_rate":      LaunchConfiguration("publish_rate"),
                    "bearing_rate":      LaunchConfiguration("bearing_rate"),
                    "enrolled_tag_ids":  LaunchConfiguration("enrolled_tag_ids"),
                },
            ],
        ),
--- a/jetson/ros2_ws/src/saltybot_uwb/saltybot_uwb/ranging_math.py
+++ b/jetson/ros2_ws/src/saltybot_uwb/saltybot_uwb/ranging_math.py
@ -29,26 +29,6 @@ Returns (x_t, y_t); caller should treat negative x_t as 0.
 import math
 # ── Bearing helper ────────────────────────────────────────────────────────────
 def bearing_from_pos(x: float, y: float) -> float:
    """
    Compute horizontal bearing to a point (x, y) in base_link.
    Parameters
    ----------
    x : forward distance (metres)
    y : lateral offset (metres, positive = left of robot / CCW)
    Returns
    -------
    bearing_rad : bearing in radians, range -π .. +π
                  positive = target to the left (CCW)
                  0 = directly ahead
    """
    return math.atan2(y, x)
 # ── Triangulation ─────────────────────────────────────────────────────────────
 def triangulate_2anchor(
--- a/jetson/ros2_ws/src/saltybot_uwb/saltybot_uwb/uwb_driver_node.py
+++ b/jetson/ros2_ws/src/saltybot_uwb/saltybot_uwb/uwb_driver_node.py
@ -1,52 +1,32 @@
 """
-uwb_driver_node.py — ROS2 node for MaUWB ESP32-S3 DW3000 UWB integration.
+uwb_driver_node.py — ROS2 node for MaUWB ESP32-S3 DW3000 follow-me system.
 Hardware
 ────────
-  • 2× MaUWB ESP32-S3 DW3000 anchors on robot stem (USB → Orin)
+  • 2× MaUWB ESP32-S3 DW3000 anchors on robot stem (USB → Orin Nano)
    - Anchor-0: port side  (y = +sep/2)
    - Anchor-1: starboard  (y = -sep/2)
-  • 1× MaUWB tag per enrolled person (belt clip)
+  • 1× MaUWB tag on person (belt clip)
 AT command interface (115200 8N1)
 ──────────────────────────────────
-  Query:
+  Query:   AT+RANGE?\r\n
-    AT+RANGE?\r\n
+  Response (from anchors):
    +RANGE:<anchor_id>,<range_mm>[,<rssi>]\r\n
-  Response (from anchors, TWR protocol):
+  Config:
-    +RANGE:<anchor_id>,<range_mm>[,<rssi>[,<tag_addr>]]\r\n
+    AT+anchor_tag=ANCHOR\r\n  — set module as anchor
-
+    AT+anchor_tag=TAG\r\n     — set module as tag
  Tag pairing (optional — targets a specific enrolled tag):
    AT+RANGE_ADDR=<tag_addr>\r\n   → anchor only ranges with that tag
 Publishes
 ─────────
-  /uwb/ranges   (saltybot_uwb_msgs/UwbRangeArray)  100 Hz — raw anchor ranges
+  /uwb/target  (geometry_msgs/PoseStamped) — triangulated person position in base_link
-  /uwb/bearing  (saltybot_uwb_msgs/UwbBearing)      10 Hz — Kalman-fused bearing
+  /uwb/ranges  (saltybot_uwb_msgs/UwbRangeArray) — raw ranges from both anchors
  /uwb/target   (geometry_msgs/PoseStamped)          10 Hz — triangulated position
                                                              (kept for backwards compat)
-Tag pairing
+Safety
-───────────
+──────
-  Set enrolled_tag_ids to a list of tag address strings (e.g. ["0x1234ABCD"]).
+  If a range is stale (> range_timeout_s), that anchor is excluded from
-  When non-empty, ranges from unrecognised tags are silently discarded.
+  triangulation. If both anchors are stale, /uwb/target is not published.
  The matched tag address is stamped in UwbRange.tag_id and UwbBearing.tag_id.
  When enrolled_tag_ids is empty, all ranges are accepted (tag_id = "").
 Parameters
 ──────────
  port_a, port_b          serial ports for anchor-0 / anchor-1
  baudrate                115200 (default)
  anchor_separation       centre-to-centre anchor spacing (m)
  anchor_height           anchor mounting height (m)
  tag_height              person tag height (m)
  range_timeout_s         stale-anchor threshold (s)
  max_range_m / min_range_m  validity window (m)
  kf_process_noise        Kalman Q scalar
  kf_meas_noise           Kalman R scalar
  range_rate              Hz — /uwb/ranges publish rate  (default 100)
  bearing_rate            Hz — /uwb/bearing publish rate (default 10)
  enrolled_tag_ids        list[str] — accepted tag addresses; [] = accept all
 Usage
 ─────
@ -64,8 +44,8 @@ from rclpy.node import Node
 from geometry_msgs.msg import PoseStamped
 from std_msgs.msg import Header
-from saltybot_uwb_msgs.msg import UwbRange, UwbRangeArray, UwbBearing
+from saltybot_uwb_msgs.msg import UwbRange, UwbRangeArray
-from saltybot_uwb.ranging_math import triangulate_2anchor, KalmanFilter2D, bearing_from_pos
+from saltybot_uwb.ranging_math import triangulate_2anchor, KalmanFilter2D
 try:
    import serial
@ -74,31 +54,26 @@ except ImportError:
    _SERIAL_AVAILABLE = False
-# +RANGE:<id>,<mm>  or  +RANGE:<id>,<mm>,<rssi>  or  +RANGE:<id>,<mm>,<rssi>,<tag>
+# Regex: +RANGE:<id>,<mm>  or  +RANGE:<id>,<mm>,<rssi>
 _RANGE_RE = re.compile(
-    r"\+RANGE\s*:\s*(\d+)\s*,\s*(\d+)"
+    r"\+RANGE\s*:\s*(\d+)\s*,\s*(\d+)(?:\s*,\s*(-?[\d.]+))?",
    r"(?:\s*,\s*(-?[\d.]+)"
    r"(?:\s*,\s*([\w:x]+))?"
    r")?",
    re.IGNORECASE,
 )
 class SerialReader(threading.Thread):
    """
-    Background thread: polls one anchor's UART at maximum TWR rate,
+    Background thread: polls an anchor's UART, fires callback on every
-    fires callback on every valid +RANGE response.
+    valid +RANGE response.
    Supports optional tag pairing via AT+RANGE_ADDR command.
    """
-    def __init__(self, port, baudrate, anchor_id, callback, logger, tag_addr=None):
+    def __init__(self, port, baudrate, anchor_id, callback, logger):
        super().__init__(daemon=True)
        self._port      = port
        self._baudrate  = baudrate
        self._anchor_id = anchor_id
        self._callback  = callback
        self._logger    = logger
        self._tag_addr  = tag_addr
        self._running   = False
        self._ser       = None
@ -111,10 +86,7 @@ class SerialReader(threading.Thread):
                )
                self._logger.info(
                    f"Anchor-{self._anchor_id}: opened {self._port}"
                    + (f" paired with tag {self._tag_addr}" if self._tag_addr else "")
                )
                if self._tag_addr:
                    self._send_pairing_cmd()
                self._read_loop()
            except Exception as exc:
                self._logger.warn(
@ -124,24 +96,12 @@ class SerialReader(threading.Thread):
                    self._ser.close()
                time.sleep(2.0)
    def _send_pairing_cmd(self):
        """Configure the anchor to range only with the paired tag."""
        try:
            cmd = f"AT+RANGE_ADDR={self._tag_addr}\r\n".encode("ascii")
            self._ser.write(cmd)
            time.sleep(0.1)
            self._logger.info(
                f"Anchor-{self._anchor_id}: sent tag pairing {self._tag_addr}"
            )
        except Exception as exc:
            self._logger.warn(
                f"Anchor-{self._anchor_id}: pairing cmd failed: {exc}"
            )
    def _read_loop(self):
        while self._running:
            try:
                # Query the anchor
                self._ser.write(b"AT+RANGE?\r\n")
                # Read up to 10 lines waiting for a +RANGE response
                for _ in range(10):
                    raw = self._ser.readline()
                    if not raw:
@ -151,14 +111,13 @@ class SerialReader(threading.Thread):
                    if m:
                        range_mm = int(m.group(2))
                        rssi     = float(m.group(3)) if m.group(3) else 0.0
-                        tag_addr = m.group(4) if m.group(4) else ""
+                        self._callback(self._anchor_id, range_mm, rssi)
                        self._callback(self._anchor_id, range_mm, rssi, tag_addr)
                        break
            except Exception as exc:
                self._logger.warn(
                    f"Anchor-{self._anchor_id} read error: {exc}"
                )
-                break
+                break  # trigger reconnect
    def stop(self):
        self._running = False
@ -171,8 +130,9 @@ class UwbDriverNode(Node):
    def __init__(self):
        super().__init__("uwb_driver")
-        self.declare_parameter("port_a",             "/dev/uwb-anchor0")
+        # ── Parameters ────────────────────────────────────────────────────────
-        self.declare_parameter("port_b",             "/dev/uwb-anchor1")
+        self.declare_parameter("port_a",           "/dev/ttyUSB0")
        self.declare_parameter("port_b",           "/dev/ttyUSB1")
        self.declare_parameter("baudrate",         115200)
        self.declare_parameter("anchor_separation", 0.25)
        self.declare_parameter("anchor_height",    0.80)
@ -182,40 +142,36 @@ class UwbDriverNode(Node):
        self.declare_parameter("min_range_m",      0.05)
        self.declare_parameter("kf_process_noise", 0.1)
        self.declare_parameter("kf_meas_noise",    0.3)
-        self.declare_parameter("range_rate",         100.0)
+        self.declare_parameter("publish_rate",     20.0)
        self.declare_parameter("bearing_rate",       10.0)
        self.declare_parameter("enrolled_tag_ids",   [""])
        self._p = self._load_params()
-        raw_ids = self.get_parameter("enrolled_tag_ids").value
+        # ── State (protected by lock) ──────────────────────────────────────
        self._enrolled_tags = [t.strip() for t in raw_ids if t.strip()]
        paired_tag = self._enrolled_tags[0] if self._enrolled_tags else None
        self._lock         = threading.Lock()
-        self._ranges: dict = {}
+        self._ranges       = {}    # anchor_id → (range_m, rssi, timestamp)
        self._kf           = KalmanFilter2D(
            process_noise=self._p["kf_process_noise"],
            measurement_noise=self._p["kf_meas_noise"],
-            dt=1.0 / self._p["bearing_rate"],
+            dt=1.0 / self._p["publish_rate"],
        )
-        self._ranges_pub  = self.create_publisher(UwbRangeArray, "/uwb/ranges",  10)
+        # ── Publishers ────────────────────────────────────────────────────
-        self._bearing_pub = self.create_publisher(UwbBearing,    "/uwb/bearing", 10)
+        self._target_pub = self.create_publisher(
-        self._target_pub  = self.create_publisher(PoseStamped,   "/uwb/target",  10)
+            PoseStamped, "/uwb/target", 10)
        self._ranges_pub = self.create_publisher(
            UwbRangeArray, "/uwb/ranges", 10)
        # ── Serial readers ────────────────────────────────────────────────
        if _SERIAL_AVAILABLE:
            self._reader_a = SerialReader(
                self._p["port_a"], self._p["baudrate"],
                anchor_id=0, callback=self._range_cb,
                logger=self.get_logger(),
                tag_addr=paired_tag,
            )
            self._reader_b = SerialReader(
                self._p["port_b"], self._p["baudrate"],
                anchor_id=1, callback=self._range_cb,
                logger=self.get_logger(),
                tag_addr=paired_tag,
            )
            self._reader_a.start()
            self._reader_b.start()
@ -224,21 +180,19 @@ class UwbDriverNode(Node):
                "pyserial not installed — running in simulation mode (no serial I/O)"
            )
-        self._range_timer = self.create_timer(
+        # ── Publish timer ─────────────────────────────────────────────────
-            1.0 / self._p["range_rate"], self._range_publish_cb
+        self._timer = self.create_timer(
-        )
+            1.0 / self._p["publish_rate"], self._publish_cb
        self._bearing_timer = self.create_timer(
            1.0 / self._p["bearing_rate"], self._bearing_publish_cb
        )
        self.get_logger().info(
            f"UWB driver ready  sep={self._p['anchor_separation']}m "
            f"ports={self._p['port_a']},{self._p['port_b']} "
-            f"range={self._p['range_rate']}Hz "
+            f"rate={self._p['publish_rate']}Hz"
            f"bearing={self._p['bearing_rate']}Hz "
            f"enrolled_tags={self._enrolled_tags or ['<any>']}"
        )
    # ── Helpers ───────────────────────────────────────────────────────────────
    def _load_params(self):
        return {
            "port_a":            self.get_parameter("port_a").value,
@ -252,106 +206,90 @@ class UwbDriverNode(Node):
            "min_range_m":       self.get_parameter("min_range_m").value,
            "kf_process_noise":  self.get_parameter("kf_process_noise").value,
            "kf_meas_noise":     self.get_parameter("kf_meas_noise").value,
-            "range_rate":        self.get_parameter("range_rate").value,
+            "publish_rate":      self.get_parameter("publish_rate").value,
            "bearing_rate":      self.get_parameter("bearing_rate").value,
        }
-    def _is_enrolled(self, tag_addr: str) -> bool:
+    # ── Callbacks ─────────────────────────────────────────────────────────────
        if not self._enrolled_tags:
            return True
        return tag_addr in self._enrolled_tags
-    def _range_cb(self, anchor_id: int, range_mm: int, rssi: float, tag_addr: str):
+    def _range_cb(self, anchor_id: int, range_mm: int, rssi: float):
-        if not self._is_enrolled(tag_addr):
+        """Called from serial reader threads — thread-safe update."""
            return
        range_m = range_mm / 1000.0
        p = self._p
        if range_m < p["min_range_m"] or range_m > p["max_range_m"]:
            return
        with self._lock:
-            self._ranges[anchor_id] = (range_m, rssi, tag_addr, time.monotonic())
+            self._ranges[anchor_id] = (range_m, rssi, time.monotonic())
-    def _range_publish_cb(self):
+    def _publish_cb(self):
        """100 Hz: publish current raw ranges as UwbRangeArray."""
        now = time.monotonic()
        timeout = self._p["range_timeout_s"]
        sep     = self._p["anchor_separation"]
        with self._lock:
-            valid = {
+            # Collect valid (non-stale) ranges
-                aid: entry
+            valid = {}
-                for aid, entry in self._ranges.items()
+            for aid, (r, rssi, t) in self._ranges.items():
-                if (now - entry[3]) <= timeout
+                if now - t <= timeout:
-            }
+                    valid[aid] = (r, rssi, t)
        # Build and publish UwbRangeArray regardless (even if partial)
        hdr = Header()
        hdr.stamp    = self.get_clock().now().to_msg()
        hdr.frame_id = "base_link"
        arr = UwbRangeArray()
        arr.header = hdr
-        for aid, (r, rssi, tag_id, _) in valid.items():
+        for aid, (r, rssi, _) in valid.items():
            entry            = UwbRange()
            entry.header     = hdr
            entry.anchor_id  = aid
            entry.range_m    = float(r)
            entry.raw_mm     = int(round(r * 1000.0))
            entry.rssi       = float(rssi)
            entry.tag_id     = tag_id
            arr.ranges.append(entry)
        self._ranges_pub.publish(arr)
-    def _bearing_publish_cb(self):
+        # Need both anchors to triangulate
-        """10 Hz: Kalman predict+update, publish fused bearing."""
+        if 0 not in valid or 1 not in valid:
        now     = time.monotonic()
        timeout = self._p["range_timeout_s"]
        sep     = self._p["anchor_separation"]
        with self._lock:
            valid = {
                aid: entry
                for aid, entry in self._ranges.items()
                if (now - entry[3]) <= timeout
            }
        if not valid:
            return
-        both_fresh  = 0 in valid and 1 in valid
+
        confidence  = 1.0 if both_fresh else 0.5
        active_tag  = valid[next(iter(valid))][2]
        dt = 1.0 / self._p["bearing_rate"]
        self._kf.predict(dt=dt)
        if both_fresh:
        r0 = valid[0][0]
        r1 = valid[1][0]
        try:
            x_t, y_t = triangulate_2anchor(
-                    r0=r0, r1=r1, sep=sep,
+                r0=r0,
                r1=r1,
                sep=sep,
                anchor_z=self._p["anchor_height"],
                tag_z=self._p["tag_height"],
            )
        except (ValueError, ZeroDivisionError) as exc:
            self.get_logger().warn(f"Triangulation error: {exc}")
            return
        # Kalman filter update
        dt = 1.0 / self._p["publish_rate"]
        self._kf.predict(dt=dt)
        self._kf.update(x_t, y_t)
        kx, ky = self._kf.position()
-        bearing = bearing_from_pos(kx, ky)
+
-        range_m = math.sqrt(kx * kx + ky * ky)
+        # Publish PoseStamped in base_link
        hdr = Header()
        hdr.stamp    = self.get_clock().now().to_msg()
        hdr.frame_id = "base_link"
        brg_msg             = UwbBearing()
        brg_msg.header      = hdr
        brg_msg.bearing_rad = float(bearing)
        brg_msg.range_m     = float(range_m)
        brg_msg.confidence  = float(confidence)
        brg_msg.tag_id      = active_tag
        self._bearing_pub.publish(brg_msg)
        pose = PoseStamped()
        pose.header = hdr
        pose.pose.position.x = kx
        pose.pose.position.y = ky
        pose.pose.position.z = 0.0
-        yaw = bearing
+        # Orientation: face the person (yaw = atan2(y, x))
        yaw = math.atan2(ky, kx)
        pose.pose.orientation.z = math.sin(yaw / 2.0)
        pose.pose.orientation.w = math.cos(yaw / 2.0)
        self._target_pub.publish(pose)
 # ── Entry point ───────────────────────────────────────────────────────────────
 def main(args=None):
    rclpy.init(args=args)
    node = UwbDriverNode()
--- a/jetson/ros2_ws/src/saltybot_uwb/test/test_ranging_math.py
+++ b/jetson/ros2_ws/src/saltybot_uwb/test/test_ranging_math.py
@ -7,7 +7,7 @@ No ROS2 / serial / GPU dependencies — runs with plain pytest.
 import math
 import pytest
-from saltybot_uwb.ranging_math import triangulate_2anchor, KalmanFilter2D, bearing_from_pos
+from saltybot_uwb.ranging_math import triangulate_2anchor, KalmanFilter2D
 # ── triangulate_2anchor ───────────────────────────────────────────────────────
@ -172,47 +172,3 @@ class TestKalmanFilter2D:
        x, y = kf.position()
        assert not math.isnan(x)
        assert not math.isnan(y)
 # ── bearing_from_pos ──────────────────────────────────────────────────────────
 class TestBearingFromPos:
    def test_directly_ahead_zero_bearing(self):
        """Person directly ahead: x=2, y=0 → bearing ≈ 0."""
        b = bearing_from_pos(2.0, 0.0)
        assert abs(b) < 0.001
    def test_left_gives_positive_bearing(self):
        """Person to the left (y>0): bearing should be positive."""
        b = bearing_from_pos(1.0, 1.0)
        assert b > 0.0
        assert abs(b - math.pi / 4.0) < 0.001
    def test_right_gives_negative_bearing(self):
        """Person to the right (y<0): bearing should be negative."""
        b = bearing_from_pos(1.0, -1.0)
        assert b < 0.0
        assert abs(b + math.pi / 4.0) < 0.001
    def test_directly_left_ninety_degrees(self):
        """Person directly to the left: x=0, y=1 → bearing = π/2."""
        b = bearing_from_pos(0.0, 1.0)
        assert abs(b - math.pi / 2.0) < 0.001
    def test_directly_right_minus_ninety_degrees(self):
        """Person directly to the right: x=0, y=-1 → bearing = -π/2."""
        b = bearing_from_pos(0.0, -1.0)
        assert abs(b + math.pi / 2.0) < 0.001
    def test_range_pi_to_minus_pi(self):
        """Bearing is always in -π..+π."""
        for x in [-2.0, -0.1, 0.1, 2.0]:
            for y in [-2.0, -0.1, 0.1, 2.0]:
                b = bearing_from_pos(x, y)
                assert -math.pi <= b <= math.pi
    def test_no_nan_for_tiny_distance(self):
        """Very close target should not produce NaN."""
        b = bearing_from_pos(0.001, 0.001)
        assert not math.isnan(b)
--- a/jetson/ros2_ws/src/saltybot_uwb_msgs/CMakeLists.txt
+++ b/jetson/ros2_ws/src/saltybot_uwb_msgs/CMakeLists.txt
@ -8,7 +8,6 @@ find_package(rosidl_default_generators REQUIRED)
 rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/UwbRange.msg"
  "msg/UwbRangeArray.msg"
  "msg/UwbBearing.msg"
  DEPENDENCIES std_msgs
 )
--- a/jetson/ros2_ws/src/saltybot_uwb_msgs/msg/UwbBearing.msg
+++ b/jetson/ros2_ws/src/saltybot_uwb_msgs/msg/UwbBearing.msg
@ -1,18 +0,0 @@
 # UwbBearing.msg — 10 Hz Kalman-fused bearing estimate from dual-anchor UWB.
 #
 # bearing_rad : horizontal bearing to tag in base_link frame (radians)
 #               positive = tag to the left (CCW), negative = right (CW)
 #               range: -π .. +π
 # range_m     : Kalman-filtered horizontal range to tag (metres)
 # confidence  : quality indicator
 #                 1.0 = both anchors fresh
 #                 0.5 = single anchor only (bearing less reliable)
 #                 0.0 = stale / no data (message not published in this state)
 # tag_id      : enrolled tag identifier that produced this estimate
 std_msgs/Header header
 float32 bearing_rad
 float32 range_m
 float32 confidence
 string  tag_id
--- a/jetson/ros2_ws/src/saltybot_uwb_msgs/msg/UwbRange.msg
+++ b/jetson/ros2_ws/src/saltybot_uwb_msgs/msg/UwbRange.msg
@ -4,7 +4,6 @@
 # range_m   : measured horizontal range in metres (after height correction)
 # raw_mm    : raw TWR range from AT+RANGE? response, millimetres
 # rssi      : received signal strength (dBm), 0 if not reported by module
 # tag_id    : enrolled tag identifier; empty string if tag pairing is disabled
 std_msgs/Header header
@ -12,4 +11,3 @@ uint8   anchor_id
 float32 range_m
 uint32  raw_mm
 float32 rssi
 string  tag_id
		`@ -1,2 +0,0 @@`
			`std_msgs/Header header`
			`saltybot_social_msgs/FaceDetection[] faces`
		`@ -1,2 +0,0 @@`
			`std_msgs/Header header`
			`saltybot_social_msgs/FaceEmbedding[] embeddings`
		`@ -1,2 +0,0 @@`
			`---`
			`saltybot_social_msgs/FaceEmbedding[] persons`