fix: resolve merge conflicts for voice router PR #499 (keep both docking + mission logging)

2026-03-05 19:25:23 -05:00 · 2026-03-05 19:25:23 -05:00 · 868b453777
commit 868b453777
parent 6d6909d9d9 83e3033abe
25 changed files with 1089 additions and 237 deletions
--- a/jetson/ros2_ws/src/saltybot_bag_recorder/config/bag_recorder.yaml
+++ b/jetson/ros2_ws/src/saltybot_bag_recorder/config/bag_recorder.yaml
@ -1,24 +1,26 @@
 bag_recorder:
  ros__parameters:
-    # Path where bags are stored
-    bag_dir: '/home/seb/rosbags'
+    # Path where bags are stored (Issue #488: mission logging)
+    bag_dir: '~/saltybot-data/bags'

-    # Topics to record (empty list = record all)
-    topics: []
-    # topics:
-    #   - '/camera/image_raw'
-    #   - '/lidar/scan'
-    #   - '/odom'
+    # Topics to record for mission logging (Issue #488)
+    topics:
+      - '/scan'
+      - '/cmd_vel'
+      - '/odom'
+      - '/tf'
+      - '/camera/color/image_raw/compressed'
+      - '/saltybot/diagnostics'

-    # Circular buffer duration (minutes)
+    # Rotation interval: save new bag every N minutes (Issue #488: 30 min)
    buffer_duration_minutes: 30

-    # Storage management
+    # Storage management (Issue #488: FIFO 20GB limit)
    storage_ttl_days: 7        # Remove bags older than 7 days
-    max_storage_gb: 50         # Enforce 50GB quota
+    max_storage_gb: 20         # Enforce 20GB FIFO quota

-    # Compression
-    compression: 'zstd'        # Options: zstd, zstandard
+    # Storage format (Issue #488: prefer MCAP)
+    storage_format: 'mcap'     # Options: mcap, sqlite3

    # NAS sync (optional)
    enable_rsync: false
--- a/jetson/ros2_ws/src/saltybot_bag_recorder/package.xml
+++ b/jetson/ros2_ws/src/saltybot_bag_recorder/package.xml
@ -4,8 +4,9 @@
  <name>saltybot_bag_recorder</name>
  <version>0.1.0</version>
  <description>
-    ROS2 bag recording service with circular buffer, auto-save on crash, and storage management.
-    Configurable topics, 7-day TTL, 50GB cap, zstd compression, and optional NAS rsync.
+    ROS2 bag recording service for mission logging with circular buffer and storage management.
+    Records mission-critical topics to MCAP format with 30min rotation, 20GB FIFO cap, auto-start,
+    start/stop services, and optional NAS rsync for archival (Issue #488).
  </description>
  <maintainer email="seb@vayrette.com">seb</maintainer>
  <license>MIT</license>
--- a/jetson/ros2_ws/src/saltybot_bag_recorder/saltybot_bag_recorder/bag_recorder_node.py
+++ b/jetson/ros2_ws/src/saltybot_bag_recorder/saltybot_bag_recorder/bag_recorder_node.py
@ -17,20 +17,28 @@ from std_msgs.msg import String


 class BagRecorderNode(Node):
-    """ROS2 bag recording service with circular buffer and storage management."""
+    """ROS2 bag recording service for mission logging (Issue #488)."""

    def __init__(self):
        super().__init__('saltybot_bag_recorder')

-        # Configuration
-        self.declare_parameter('bag_dir', '/home/seb/rosbags')
-        self.declare_parameter('topics', [''])
+        # Configuration (Issue #488: mission logging)
+        default_bag_dir = str(Path.home() / 'saltybot-data' / 'bags')
+        self.declare_parameter('bag_dir', default_bag_dir)
+        self.declare_parameter('topics', [
+            '/scan',
+            '/cmd_vel',
+            '/odom',
+            '/tf',
+            '/camera/color/image_raw/compressed',
+            '/saltybot/diagnostics'
+        ])
        self.declare_parameter('buffer_duration_minutes', 30)
        self.declare_parameter('storage_ttl_days', 7)
-        self.declare_parameter('max_storage_gb', 50)
+        self.declare_parameter('max_storage_gb', 20)
        self.declare_parameter('enable_rsync', False)
        self.declare_parameter('rsync_destination', '')
-        self.declare_parameter('compression', 'zstd')
+        self.declare_parameter('storage_format', 'mcap')

        self.bag_dir = Path(self.get_parameter('bag_dir').value)
        self.topics = self.get_parameter('topics').value
@ -39,7 +47,7 @@ class BagRecorderNode(Node):
        self.max_storage_gb = self.get_parameter('max_storage_gb').value
        self.enable_rsync = self.get_parameter('enable_rsync').value
        self.rsync_destination = self.get_parameter('rsync_destination').value
-        self.compression = self.get_parameter('compression').value
+        self.storage_format = self.get_parameter('storage_format').value

        self.bag_dir.mkdir(parents=True, exist_ok=True)

@ -51,11 +59,9 @@ class BagRecorderNode(Node):
        self.recording_lock = threading.Lock()

        # Services
-        self.save_service = self.create_service(
-            Trigger,
-            '/saltybot/save_bag',
-            self.save_bag_callback
-        )
+        self.save_service = self.create_service(Trigger, '/saltybot/save_bag', self.save_bag_callback)
+        self.start_service = self.create_service(Trigger, '/saltybot/start_recording', self.start_recording_callback)
+        self.stop_service = self.create_service(Trigger, '/saltybot/stop_recording', self.stop_recording_callback)

        # Watchdog to handle crash recovery
        self.setup_signal_handlers()
@ -67,9 +73,8 @@ class BagRecorderNode(Node):
        self.maintenance_timer = self.create_timer(300.0, self.maintenance_callback)

        self.get_logger().info(
-            f'Bag recorder initialized: {self.bag_dir}, '
-            f'buffer={self.buffer_duration}s, ttl={self.storage_ttl_days}d, '
-            f'max={self.max_storage_gb}GB'
+            f'Bag recorder initialized: {self.bag_dir}, format={self.storage_format}, '
+            f'buffer={self.buffer_duration}s, max={self.max_storage_gb}GB, topics={len(self.topics)}'
        )

    def setup_signal_handlers(self):
@ -95,27 +100,21 @@ class BagRecorderNode(Node):

            try:
                # Build rosbag2 record command
-                cmd = [
-                    'ros2', 'bag', 'record',
-                    f'--output', str(bag_path),
-                    f'--compression-format', self.compression,
-                    f'--compression-mode', 'file',
-                ]
+                cmd = ['ros2', 'bag', 'record', '--output', str(bag_path), '--storage', self.storage_format]

-                # Add topics or record all if empty
+                # Add compression for sqlite3 format
+                if self.storage_format == 'sqlite3':
+                    cmd.extend(['--compression-format', 'zstd', '--compression-mode', 'file'])
+
+                # Add topics (required for mission logging)
                if self.topics and self.topics[0]:
                    cmd.extend(self.topics)
                else:
-                    cmd.append('--all')
+                    cmd.extend(['/scan', '/cmd_vel', '/odom', '/tf', '/camera/color/image_raw/compressed', '/saltybot/diagnostics'])

-                self.current_bag_process = subprocess.Popen(
-                    cmd,
-                    stdout=subprocess.PIPE,
-                    stderr=subprocess.PIPE
-                )
+                self.current_bag_process = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE)
                self.is_recording = True
                self.buffer_bags.append(self.current_bag_name)
-
                self.get_logger().info(f'Started recording: {self.current_bag_name}')

            except Exception as e:
@ -133,7 +132,40 @@ class BagRecorderNode(Node):
            response.success = False
            response.message = f'Failed to save bag: {e}'
            self.get_logger().error(response.message)
+        return response

+    def start_recording_callback(self, request, response):
+        """Service callback to start recording."""
+        if self.is_recording:
+            response.success = False
+            response.message = 'Recording already in progress'
+            return response
+        try:
+            self.start_recording()
+            response.success = True
+            response.message = f'Recording started: {self.current_bag_name}'
+            self.get_logger().info(response.message)
+        except Exception as e:
+            response.success = False
+            response.message = f'Failed to start recording: {e}'
+            self.get_logger().error(response.message)
+        return response
+
+    def stop_recording_callback(self, request, response):
+        """Service callback to stop recording."""
+        if not self.is_recording:
+            response.success = False
+            response.message = 'No recording in progress'
+            return response
+        try:
+            self.stop_recording(save=True)
+            response.success = True
+            response.message = f'Recording stopped and saved: {self.current_bag_name}'
+            self.get_logger().info(response.message)
+        except Exception as e:
+            response.success = False
+            response.message = f'Failed to stop recording: {e}'
+            self.get_logger().error(response.message)
        return response

    def stop_recording(self, save: bool = False):
@ -141,9 +173,7 @@ class BagRecorderNode(Node):
        with self.recording_lock:
            if not self.is_recording or not self.current_bag_process:
                return
-
            try:
-                # Send SIGINT to gracefully close rosbag2
                self.current_bag_process.send_signal(signal.SIGINT)
                self.current_bag_process.wait(timeout=5.0)
            except subprocess.TimeoutExpired:
@ -152,11 +182,8 @@ class BagRecorderNode(Node):
                self.current_bag_process.wait()
            except Exception as e:
                self.get_logger().error(f'Error stopping recording: {e}')
-
            self.is_recording = False
            self.get_logger().info(f'Stopped recording: {self.current_bag_name}')
-
-            # Apply compression if needed (rosbag2 does this by default with -compression-mode file)
            if save:
                self.apply_compression()

@ -164,13 +191,9 @@ class BagRecorderNode(Node):
        """Compress the current bag using zstd."""
        if not self.current_bag_name:
            return
-
        bag_path = self.bag_dir / self.current_bag_name
        try:
-            # rosbag2 with compression-mode file already compresses the sqlite db
-            # This is a secondary option to compress the entire directory
            tar_path = f'{bag_path}.tar.zst'
-
            if bag_path.exists():
                cmd = ['tar', '-I', 'zstd', '-cf', tar_path, '-C', str(self.bag_dir), self.current_bag_name]
                subprocess.run(cmd, check=True, capture_output=True, timeout=60)
@ -189,14 +212,11 @@ class BagRecorderNode(Node):
        """Remove bags older than TTL."""
        try:
            cutoff_time = datetime.now() - timedelta(days=self.storage_ttl_days)
-
            for item in self.bag_dir.iterdir():
                if item.is_dir() and item.name.startswith('saltybot_'):
                    try:
-                        # Parse timestamp from directory name
                        timestamp_str = item.name.replace('saltybot_', '')
                        item_time = datetime.strptime(timestamp_str, '%Y%m%d_%H%M%S')
-
                        if item_time < cutoff_time:
                            shutil.rmtree(item, ignore_errors=True)
                            self.get_logger().info(f'Removed expired bag: {item.name}')
@ -206,51 +226,26 @@ class BagRecorderNode(Node):
            self.get_logger().error(f'Cleanup failed: {e}')

    def enforce_storage_quota(self):
-        """Remove oldest bags if total size exceeds quota."""
+        """Remove oldest bags if total size exceeds quota (FIFO)."""
        try:
-            total_size = sum(
-                f.stat().st_size
-                for f in self.bag_dir.rglob('*')
-                if f.is_file()
-            ) / (1024 ** 3)  # Convert to GB
-
+            total_size = sum(f.stat().st_size for f in self.bag_dir.rglob('*') if f.is_file()) / (1024 ** 3)
            if total_size > self.max_storage_gb:
-                self.get_logger().warn(
-                    f'Storage quota exceeded: {total_size:.2f}GB > {self.max_storage_gb}GB'
-                )
-
-                # Get bags sorted by modification time
-                bags = sorted(
-                    [d for d in self.bag_dir.iterdir() if d.is_dir() and d.name.startswith('saltybot_')],
-                    key=lambda x: x.stat().st_mtime
-                )
-
-                # Remove oldest bags until under quota
+                self.get_logger().warn(f'Storage quota exceeded: {total_size:.2f}GB > {self.max_storage_gb}GB')
+                bags = sorted([d for d in self.bag_dir.iterdir() if d.is_dir() and d.name.startswith('saltybot_')], key=lambda x: x.stat().st_mtime)
                for bag in bags:
                    if total_size <= self.max_storage_gb:
                        break
-
-                    bag_size = sum(
-                        f.stat().st_size
-                        for f in bag.rglob('*')
-                        if f.is_file()
-                    ) / (1024 ** 3)
-
+                    bag_size = sum(f.stat().st_size for f in bag.rglob('*') if f.is_file()) / (1024 ** 3)
                    shutil.rmtree(bag, ignore_errors=True)
                    total_size -= bag_size
                    self.get_logger().info(f'Removed bag to enforce quota: {bag.name}')
-
        except Exception as e:
            self.get_logger().error(f'Storage quota enforcement failed: {e}')

    def rsync_bags(self):
        """Optional: rsync bags to NAS."""
        try:
-            cmd = [
-                'rsync', '-avz', '--delete',
-                f'{self.bag_dir}/',
-                self.rsync_destination
-            ]
+            cmd = ['rsync', '-avz', '--delete', f'{self.bag_dir}/', self.rsync_destination]
            subprocess.run(cmd, check=False, timeout=300)
            self.get_logger().info(f'Synced bags to NAS: {self.rsync_destination}')
        except subprocess.TimeoutExpired:
@ -267,7 +262,6 @@ class BagRecorderNode(Node):
 def main(args=None):
    rclpy.init(args=args)
    node = BagRecorderNode()
-
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
--- a/jetson/ros2_ws/src/saltybot_bringup/config/nav2_params.yaml
+++ b/jetson/ros2_ws/src/saltybot_bringup/config/nav2_params.yaml
@ -1,10 +1,10 @@
 # Nav2 parameters — SaltyBot (Jetson Orin Nano Super / ROS2 Humble)
 #
 # Robot: differential-drive self-balancing two-wheeler
-#   robot_radius:   0.15 m  (~0.2m with margin)
-#   footprint:      0.4 x 0.4 m (x 2m for buffer)
-#   max_vel_x:      1.0  m/s
-#   max_vel_theta:  1.5  rad/s
+#   robot_radius:   0.22 m (0.4m x 0.4m footprint)
+#   footprint:      0.4 x 0.4 m
+#   max_vel_x:      0.3  m/s (conservative for FC + hoverboard ESC, Issue #475)
+#   max_vel_theta:  0.5  rad/s (conservative for FC + hoverboard ESC, Issue #475)
 #
 # Localization: RTAB-Map (publishes /map + map→odom TF + /rtabmap/odom)
 #   → No AMCL, no map_server needed.
@ -120,14 +120,14 @@ controller_server:
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: false
-      # Velocity limits
-      min_vel_x: -0.25          # allow limited reverse
+      # Velocity limits (conservative for FC + hoverboard ESC, Issue #475)
+      min_vel_x: -0.15          # allow limited reverse (half of max forward)
      min_vel_y: 0.0
-      max_vel_x: 1.0
+      max_vel_x: 0.3            # conservative: 0.3 m/s linear
      max_vel_y: 0.0
-      max_vel_theta: 1.5
+      max_vel_theta: 0.5         # conservative: 0.5 rad/s angular
      min_speed_xy: 0.0
-      max_speed_xy: 1.0
+      max_speed_xy: 0.3          # match max_vel_x
      min_speed_theta: 0.0
      # Acceleration limits (differential drive)
      acc_lim_x: 2.5
@ -243,14 +243,15 @@ waypoint_follower:
      waypoint_pause_duration: 200

 # ── Velocity Smoother ────────────────────────────────────────────────────────
+# Conservative speeds for FC + hoverboard ESC (Issue #475)
 velocity_smoother:
  ros__parameters:
    use_sim_time: false
    smoothing_frequency: 20.0
    scale_velocities: false
    feedback: "OPEN_LOOP"
-    max_velocity: [1.0, 0.0, 1.5]
-    min_velocity: [-0.25, 0.0, -1.5]
+    max_velocity: [0.3, 0.0, 0.5]           # conservative: 0.3 m/s linear, 0.5 rad/s angular
+    min_velocity: [-0.15, 0.0, -0.5]
    max_accel: [2.5, 0.0, 3.2]
    max_decel: [-2.5, 0.0, -3.2]
    odom_topic: /rtabmap/odom
@ -271,7 +272,7 @@ local_costmap:
      width: 3
      height: 3
      resolution: 0.05
-      robot_radius: 0.15
+      robot_radius: 0.22
      # Footprint: [x, y] in base_link frame, in counterclockwise order
      # Robot footprint ~0.4m x 0.4m, with 2m lookahead buffer for controller stability
      footprint: "[[-0.2, -0.2], [-0.2, 0.2], [0.2, 0.2], [0.2, -0.2]]"
@ -342,7 +343,7 @@ global_costmap:
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
-      robot_radius: 0.15
+      robot_radius: 0.22
      # Footprint: [x, y] in base_link frame, in counterclockwise order
      footprint: "[[-0.2, -0.2], [-0.2, 0.2], [0.2, 0.2], [0.2, -0.2]]"
      resolution: 0.05
--- a/jetson/ros2_ws/src/saltybot_bringup/launch/full_stack.launch.py
+++ b/jetson/ros2_ws/src/saltybot_bringup/launch/full_stack.launch.py
@ -194,12 +194,18 @@ def generate_launch_description():
        description="Launch rosbridge WebSocket server (port 9090)",
    )

-    enable_mission_logging_arg = DeclareLaunchArgument(
+enable_mission_logging_arg = DeclareLaunchArgument(
        "enable_mission_logging",
        default_value="true",
        description="Launch ROS2 bag recorder for mission logging (Issue #488)",
    )

+    enable_docking_arg = DeclareLaunchArgument(
+        "enable_docking",
+        default_value="true",
+        description="Launch autonomous docking behavior (auto-dock at 20% battery, Issue #489)",
+    )
+
    follow_distance_arg = DeclareLaunchArgument(
        "follow_distance",
        default_value="1.5",
@ -440,6 +446,25 @@ def generate_launch_description():
        ],
    )

+    # ── t=7s  Docking (auto-dock at 20% battery, Issue #489) ──────────────────
+    docking = TimerAction(
+        period=7.0,
+        actions=[
+            GroupAction(
+                condition=IfCondition(LaunchConfiguration("enable_docking")),
+                actions=[
+                    LogInfo(msg="[full_stack] Starting autonomous docking (auto-trigger at 20% battery)"),
+                    IncludeLaunchDescription(
+                        _launch("saltybot_docking", "launch", "docking.launch.py"),
+                        launch_arguments={
+                            "battery_low_pct": "20.0",
+                        }.items(),
+                    ),
+                ],
+            ),
+        ],
+    )
+
    # ── t=14s  Nav2 (indoor only; SLAM needs ~8s to build initial map) ────────
    nav2 = TimerAction(
        period=14.0,
@ -490,7 +515,8 @@ def generate_launch_description():
        enable_follower_arg,
        enable_bridge_arg,
        enable_rosbridge_arg,
-        enable_mission_logging_arg,
+enable_mission_logging_arg,
+        enable_docking_arg,
        follow_distance_arg,
        max_linear_vel_arg,
        uwb_port_a_arg,
@ -524,6 +550,9 @@ def generate_launch_description():
        perception,
        object_detection,

+        # t=7s
+        docking,
+
        # t=9s
        follower,

--- a/jetson/ros2_ws/src/saltybot_docking/config/docking_params.yaml
+++ b/jetson/ros2_ws/src/saltybot_docking/config/docking_params.yaml
@ -17,7 +17,7 @@
    ir_threshold: 0.50           # amplitude threshold for beacon detection

    # ── Battery thresholds ────────────────────────────────────────────────────
-    battery_low_pct:  15.0       # SOC triggering auto-dock (%)
+    battery_low_pct:  20.0       # SOC triggering auto-dock (%) — Issue #489
    battery_high_pct: 80.0       # SOC declaring charge complete (%)

    # ── Visual servo ──────────────────────────────────────────────────────────
@ -26,8 +26,8 @@
    k_angular:          0.80     # yaw P-gain
    lateral_tol_m:      0.005    # ±5 mm alignment tolerance
    contact_distance_m: 0.05     # distance below which contact is assumed (m)
-    max_linear_ms:      0.10     # forward speed ceiling during servo (m/s)
-    max_angular_rads:   0.30     # yaw rate ceiling (rad/s)
+    max_linear_ms:      0.10     # forward speed ceiling during servo (m/s) — conservative for FC+hoverboard (Issue #475)
+    max_angular_rads:   0.30     # yaw rate ceiling (rad/s) — conservative for FC+hoverboard (Issue #475)

    # ── Undocking maneuver ────────────────────────────────────────────────────
    undock_speed_ms:   -0.20     # reverse speed (m/s; must be negative)
--- a/jetson/ros2_ws/src/saltybot_ota_updater/config/ota_updater.yaml
+++ b/jetson/ros2_ws/src/saltybot_ota_updater/config/ota_updater.yaml
@ -0,0 +1,28 @@
+# OTA Firmware Updater Configuration
+
+ota_updater_node:
+  ros__parameters:
+    # Gitea repository configuration
+    gitea_api_base: https://gitea.vayrette.com/api/v1/repos/seb/saltylab-firmware
+    repo_owner: seb
+    repo_name: saltylab-firmware
+
+    # Directories
+    data_dir: ~/.saltybot-data
+    staging_dir: ~/saltybot-ota-staging
+    install_dir: ~/saltybot-ros2-install
+    versions_file: ~/.saltybot-data/versions.json
+
+    # Safety thresholds
+    max_velocity_threshold: 0.05  # m/s - block update if robot moving faster
+    build_timeout: 3600           # seconds (1 hour)
+
+    # Update behavior
+    auto_restart_services: true
+    backup_before_update: true
+    keep_backup_days: 7
+
+    # MQTT topics
+    ota_command_topic: /saltybot/ota_command
+    ota_status_topic: /saltybot/ota_status
+    odometry_topic: /odom
--- a/jetson/ros2_ws/src/saltybot_ota_updater/launch/ota_updater.launch.py
+++ b/jetson/ros2_ws/src/saltybot_ota_updater/launch/ota_updater.launch.py
@ -0,0 +1,21 @@
+"""
+Launch file for OTA Firmware Updater.
+
+Launches the OTA update manager that handles firmware downloads, builds,
+and deployments with automatic rollback.
+"""
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='saltybot_ota_updater',
+            executable='ota_updater_node',
+            name='ota_updater_node',
+            output='screen',
+            emulate_tty=True,
+        ),
+    ])
--- a/jetson/ros2_ws/src/saltybot_ota_updater/package.xml
+++ b/jetson/ros2_ws/src/saltybot_ota_updater/package.xml
@ -0,0 +1,23 @@
+<?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_ota_updater</name>
+  <version>0.1.0</version>
+  <description>OTA firmware update mechanism with Gitea release download, colcon build, rollback, and safety checks.</description>
+  <maintainer email="seb@vayrette.com">seb</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <depend>rclpy</depend>
+  <depend>std_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_ota_updater/resource/saltybot_ota_updater
+++ b/jetson/ros2_ws/src/saltybot_ota_updater/resource/saltybot_ota_updater
@ -0,0 +1 @@
+# Marker file for ament resource index
--- a/jetson/ros2_ws/src/saltybot_ota_updater/saltybot_ota_updater/init.py
+++ b/jetson/ros2_ws/src/saltybot_ota_updater/saltybot_ota_updater/init.py
@ -0,0 +1 @@
+"""SaltyBot OTA Firmware Update - Download, build, deploy, and rollback."""
--- a/jetson/ros2_ws/src/saltybot_ota_updater/saltybot_ota_updater/ota_updater_node.py
+++ b/jetson/ros2_ws/src/saltybot_ota_updater/saltybot_ota_updater/ota_updater_node.py
@ -0,0 +1,404 @@
+#!/usr/bin/env python3
+"""
+OTA Firmware Update Node - Downloads, builds, deploys, and rolls back firmware.
+
+Features:
+- Downloads release archives from Gitea (seb/saltylab-firmware)
+- Runs colcon build in staging directory
+- Swaps symlink to activate new version
+- Restarts ROS2 services (systemd)
+- Rolls back on build failure
+- Tracks versions in ~/saltybot-data/versions.json
+- Safety: blocks update if robot is moving (velocity > threshold)
+- Triggers via MQTT /saltybot/ota_command or dashboard button
+"""
+
+import json
+import os
+import subprocess
+import shutil
+import requests
+import threading
+from datetime import datetime
+from pathlib import Path
+from typing import Dict, Any, Optional, Tuple
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+
+
+class OTAUpdater(Node):
+    """OTA firmware update manager for SaltyBot."""
+
+    def __init__(self):
+        super().__init__('ota_updater_node')
+
+        # Configuration
+        self.data_dir = Path(os.path.expanduser('~/.saltybot-data'))
+        self.data_dir.mkdir(parents=True, exist_ok=True)
+        self.versions_file = self.data_dir / 'versions.json'
+
+        self.staging_dir = Path(os.path.expanduser('~/saltybot-ota-staging'))
+        self.install_dir = Path(os.path.expanduser('~/saltybot-ros2-install'))
+
+        self.gitea_api = 'https://gitea.vayrette.com/api/v1/repos/seb/saltylab-firmware'
+        self.max_velocity_threshold = 0.05  # m/s - block update if moving faster
+
+        # Runtime state
+        self.updating = False
+        self.current_velocity = 0.0
+
+        self.get_logger().info(f'OTA Updater initialized')
+
+        # Subscriptions
+        self.create_subscription(String, '/saltybot/ota_command', self._on_ota_command, 10)
+        self.create_subscription(String, '/odom', self._on_odometry, 10)
+
+        # Publisher for status
+        self.status_pub = self.create_publisher(String, '/saltybot/ota_status', 10)
+
+    def _on_ota_command(self, msg: String):
+        """Handle OTA update request from MQTT or dashboard."""
+        try:
+            cmd = msg.data.strip()
+            if cmd == 'check':
+                self._check_for_updates()
+            elif cmd.startswith('update:'):
+                version = cmd.split(':', 1)[1].strip()
+                self._start_update_thread(version)
+            elif cmd == 'rollback':
+                self._rollback_update()
+        except Exception as e:
+            self.get_logger().error(f'Error handling OTA command: {e}')
+
+    def _on_odometry(self, msg: String):
+        """Track current velocity from odometry for safety checks."""
+        try:
+            # Parse velocity from odom message (simplified)
+            data = json.loads(msg.data)
+            vx = data.get('vx', 0.0)
+            vy = data.get('vy', 0.0)
+            self.current_velocity = (vx**2 + vy**2) ** 0.5
+        except:
+            pass
+
+    def _check_for_updates(self):
+        """Check Gitea for new releases."""
+        try:
+            response = requests.get(f'{self.gitea_api}/releases', timeout=5)
+            response.raise_for_status()
+            releases = response.json()
+
+            if releases:
+                latest = releases[0]
+                version = latest.get('tag_name', 'unknown')
+                current = self._get_current_version()
+
+                status = {
+                    'timestamp': datetime.now().isoformat(),
+                    'status': 'update_available' if version != current else 'up_to_date',
+                    'current': current,
+                    'latest': version,
+                }
+            else:
+                status = {'status': 'no_releases'}
+
+            self._publish_status(status)
+        except Exception as e:
+            self.get_logger().error(f'Error checking for updates: {e}')
+            self._publish_status({'status': 'check_failed', 'error': str(e)})
+
+    def _start_update_thread(self, version: str):
+        """Start update in background thread."""
+        if self.updating:
+            self._publish_status({'status': 'already_updating'})
+            return
+
+        thread = threading.Thread(target=self._update_firmware, args=(version,), daemon=True)
+        thread.start()
+
+    def _update_firmware(self, version: str):
+        """Execute firmware update: download, build, deploy, with rollback."""
+        self.updating = True
+        try:
+            # Safety check
+            if self.current_velocity > self.max_velocity_threshold:
+                self._publish_status({'status': 'blocked_robot_moving', 'velocity': self.current_velocity})
+                self.updating = False
+                return
+
+            self._publish_status({'status': 'starting_update', 'version': version})
+
+            # Step 1: Backup current installation
+            backup_dir = self._backup_current_install()
+
+            # Step 2: Download release
+            archive_path = self._download_release(version)
+
+            # Step 3: Extract to staging
+            self._extract_to_staging(archive_path, version)
+
+            # Step 4: Build with colcon
+            if not self._colcon_build():
+                self.get_logger().error('Build failed, rolling back')
+                self._restore_from_backup(backup_dir)
+                self._publish_status({'status': 'build_failed_rolled_back', 'version': version})
+                self.updating = False
+                return
+
+            # Step 5: Swap symlink
+            self._swap_install_symlink(version)
+
+            # Step 6: Restart services
+            self._restart_ros_services()
+
+            # Step 7: Update version tracking
+            self._update_version_file(version)
+
+            # Cleanup
+            shutil.rmtree(backup_dir, ignore_errors=True)
+
+            self._publish_status({'status': 'update_complete', 'version': version, 'timestamp': datetime.now().isoformat()})
+
+        except Exception as e:
+            self.get_logger().error(f'Update failed: {e}')
+            self._publish_status({'status': 'update_failed', 'error': str(e)})
+        finally:
+            self.updating = False
+
+    def _backup_current_install(self) -> Path:
+        """Backup current installation for rollback."""
+        try:
+            timestamp = datetime.now().strftime('%Y%m%d_%H%M%S')
+            backup_dir = self.data_dir / f'backup_{timestamp}'
+
+            if self.install_dir.exists():
+                shutil.copytree(self.install_dir, backup_dir, dirs_exist_ok=True)
+
+            self.get_logger().info(f'Backup created: {backup_dir}')
+            return backup_dir
+        except Exception as e:
+            self.get_logger().warning(f'Backup failed: {e}')
+            return None
+
+    def _download_release(self, version: str) -> Path:
+        """Download release archive from Gitea."""
+        try:
+            self._publish_status({'status': 'downloading', 'version': version})
+
+            # Get release info
+            response = requests.get(f'{self.gitea_api}/releases/tags/{version}', timeout=10)
+            response.raise_for_status()
+            release = response.json()
+
+            # Download source code archive
+            archive_url = release.get('tarball_url')
+            if not archive_url:
+                raise Exception(f'No tarball URL for release {version}')
+
+            archive_path = self.data_dir / f'saltylab-firmware-{version}.tar.gz'
+
+            response = requests.get(archive_url, timeout=60, stream=True)
+            response.raise_for_status()
+
+            with open(archive_path, 'wb') as f:
+                for chunk in response.iter_content(chunk_size=8192):
+                    if chunk:
+                        f.write(chunk)
+
+            self.get_logger().info(f'Downloaded {archive_path}')
+            return archive_path
+
+        except Exception as e:
+            self.get_logger().error(f'Download failed: {e}')
+            raise
+
+    def _extract_to_staging(self, archive_path: Path, version: str):
+        """Extract release archive to staging directory."""
+        try:
+            self._publish_status({'status': 'extracting', 'version': version})
+
+            # Clean staging
+            if self.staging_dir.exists():
+                shutil.rmtree(self.staging_dir)
+            self.staging_dir.mkdir(parents=True)
+
+            # Extract
+            subprocess.run(['tar', 'xzf', str(archive_path), '-C', str(self.staging_dir)],
+                         check=True, capture_output=True)
+
+            # Move extracted content to correct location
+            extracted = list(self.staging_dir.glob('*'))
+            if len(extracted) == 1 and extracted[0].is_dir():
+                # Rename extracted directory
+                src_dir = extracted[0]
+                final_dir = self.staging_dir / 'firmware'
+                src_dir.rename(final_dir)
+
+            self.get_logger().info(f'Extracted to {self.staging_dir}')
+
+        except Exception as e:
+            self.get_logger().error(f'Extract failed: {e}')
+            raise
+
+    def _colcon_build(self) -> bool:
+        """Build firmware with colcon."""
+        try:
+            self._publish_status({'status': 'building'})
+
+            build_dir = self.staging_dir / 'firmware' / 'jetson' / 'ros2_ws'
+
+            result = subprocess.run(
+                ['colcon', 'build', '--symlink-install'],
+                cwd=str(build_dir),
+                capture_output=True,
+                timeout=3600,  # 1 hour timeout
+                text=True
+            )
+
+            if result.returncode != 0:
+                self.get_logger().error(f'Build failed: {result.stderr}')
+                return False
+
+            self.get_logger().info('Build succeeded')
+            return True
+
+        except subprocess.TimeoutExpired:
+            self.get_logger().error('Build timed out')
+            return False
+        except Exception as e:
+            self.get_logger().error(f'Build error: {e}')
+            return False
+
+    def _swap_install_symlink(self, version: str):
+        """Swap symlink to activate new installation."""
+        try:
+            self._publish_status({'status': 'deploying', 'version': version})
+
+            new_install = self.staging_dir / 'firmware' / 'jetson' / 'ros2_ws' / 'install'
+            symlink = self.install_dir
+
+            # Remove old symlink
+            if symlink.is_symlink():
+                symlink.unlink()
+            elif symlink.exists():
+                shutil.rmtree(symlink)
+
+            # Create new symlink
+            symlink.parent.mkdir(parents=True, exist_ok=True)
+            symlink.symlink_to(new_install)
+
+            self.get_logger().info(f'Symlink swapped to {new_install}')
+
+        except Exception as e:
+            self.get_logger().error(f'Deploy failed: {e}')
+            raise
+
+    def _restart_ros_services(self):
+        """Restart ROS2 systemd services."""
+        try:
+            self._publish_status({'status': 'restarting_services'})
+
+            # Restart main ROS2 service
+            subprocess.run(['sudo', 'systemctl', 'restart', 'saltybot-ros2'],
+                         check=False, capture_output=True)
+
+            self.get_logger().info('ROS2 services restarted')
+
+        except Exception as e:
+            self.get_logger().warning(f'Service restart failed: {e}')
+
+    def _update_version_file(self, version: str):
+        """Update version tracking file."""
+        try:
+            versions = {}
+            if self.versions_file.exists():
+                with open(self.versions_file, 'r') as f:
+                    versions = json.load(f)
+
+            versions['current'] = version
+            versions['updated'] = datetime.now().isoformat()
+            versions['history'] = versions.get('history', [])
+            versions['history'].append({
+                'version': version,
+                'timestamp': datetime.now().isoformat(),
+                'status': 'success'
+            })
+
+            with open(self.versions_file, 'w') as f:
+                json.dump(versions, f, indent=2)
+
+            self.get_logger().info(f'Version file updated: {version}')
+
+        except Exception as e:
+            self.get_logger().warning(f'Version file update failed: {e}')
+
+    def _restore_from_backup(self, backup_dir: Path):
+        """Restore installation from backup."""
+        try:
+            if backup_dir and backup_dir.exists():
+                if self.install_dir.exists():
+                    shutil.rmtree(self.install_dir)
+
+                shutil.copytree(backup_dir, self.install_dir)
+                self.get_logger().info(f'Restored from backup')
+        except Exception as e:
+            self.get_logger().error(f'Restore failed: {e}')
+
+    def _rollback_update(self):
+        """Rollback to previous version."""
+        try:
+            self._publish_status({'status': 'rolling_back'})
+
+            versions = {}
+            if self.versions_file.exists():
+                with open(self.versions_file, 'r') as f:
+                    versions = json.load(f)
+
+            history = versions.get('history', [])
+            if len(history) > 1:
+                previous = history[-2]['version']
+                self._start_update_thread(previous)
+            else:
+                self._publish_status({'status': 'rollback_unavailable'})
+
+        except Exception as e:
+            self.get_logger().error(f'Rollback failed: {e}')
+            self._publish_status({'status': 'rollback_failed', 'error': str(e)})
+
+    def _get_current_version(self) -> str:
+        """Get currently installed version."""
+        try:
+            if self.versions_file.exists():
+                with open(self.versions_file, 'r') as f:
+                    data = json.load(f)
+                    return data.get('current', 'unknown')
+            return 'unknown'
+        except:
+            return 'unknown'
+
+    def _publish_status(self, status: Dict[str, Any]):
+        """Publish OTA status update."""
+        try:
+            msg = String()
+            msg.data = json.dumps(status)
+            self.status_pub.publish(msg)
+        except Exception as e:
+            self.get_logger().error(f'Status publish failed: {e}')
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = OTAUpdater()
+
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
--- a/jetson/ros2_ws/src/saltybot_ota_updater/setup.cfg
+++ b/jetson/ros2_ws/src/saltybot_ota_updater/setup.cfg
@ -0,0 +1,4 @@
+[develop]
+script-dir=$base/lib/saltybot_ota_updater
+[egg_info]
+tag_date = 0
--- a/jetson/ros2_ws/src/saltybot_ota_updater/setup.py
+++ b/jetson/ros2_ws/src/saltybot_ota_updater/setup.py
@ -0,0 +1,22 @@
+from setuptools import setup, find_packages
+
+setup(
+    name='saltybot_ota_updater',
+    version='0.1.0',
+    packages=find_packages(),
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/saltybot_ota_updater']),
+        ('share/saltybot_ota_updater', ['package.xml']),
+    ],
+    install_requires=['setuptools', 'requests'],
+    zip_safe=True,
+    author='seb',
+    author_email='seb@vayrette.com',
+    description='OTA firmware update with Gitea release download and rollback',
+    license='Apache-2.0',
+    entry_points={
+        'console_scripts': [
+            'ota_updater_node = saltybot_ota_updater.ota_updater_node:main',
+        ],
+    },
+)
--- a/jetson/ros2_ws/src/saltybot_sensor_fusion/launch/sensor_fusion.launch.py
+++ b/jetson/ros2_ws/src/saltybot_sensor_fusion/launch/sensor_fusion.launch.py
@ -0,0 +1,42 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from pathlib import Path
+
+
+def generate_launch_description():
+    pkg_share = FindPackageShare("saltybot_sensor_fusion")
+    config_dir = Path(str(pkg_share)) / "config"
+    config_file = str(config_dir / "sensor_fusion_params.yaml")
+
+    lidar_topic_arg = DeclareLaunchArgument(
+        "lidar_topic",
+        default_value="/scan",
+        description="RPLIDAR topic"
+    )
+
+    depth_topic_arg = DeclareLaunchArgument(
+        "depth_topic",
+        default_value="/depth_scan",
+        description="RealSense depth_to_laserscan topic"
+    )
+
+    sensor_fusion_node = Node(
+        package="saltybot_sensor_fusion",
+        executable="sensor_fusion",
+        name="sensor_fusion",
+        parameters=[
+            config_file,
+            {"lidar_topic": LaunchConfiguration("lidar_topic")},
+            {"depth_topic": LaunchConfiguration("depth_topic")},
+        ],
+        output="screen",
+    )
+
+    return LaunchDescription([
+        lidar_topic_arg,
+        depth_topic_arg,
+        sensor_fusion_node,
+    ])
--- a/jetson/ros2_ws/src/saltybot_sensor_fusion/resource/saltybot_sensor_fusion
+++ b/jetson/ros2_ws/src/saltybot_sensor_fusion/resource/saltybot_sensor_fusion
--- a/jetson/ros2_ws/src/saltybot_tts_personality/package.xml
+++ b/jetson/ros2_ws/src/saltybot_tts_personality/package.xml
@ -0,0 +1,24 @@
+<?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_tts_personality</name>
+  <version>0.1.0</version>
+  <description>TTS personality engine with context-aware greetings, emotion-based rate/pitch modulation, and priority queue management (Issue #494).</description>
+  <maintainer email="seb@vayrette.com">seb</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_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_tts_personality/saltybot_tts_personality/init.py
+++ b/jetson/ros2_ws/src/saltybot_tts_personality/saltybot_tts_personality/init.py
@ -0,0 +1,5 @@
+"""
+saltybot_tts_personality — TTS personality engine for SaltyBot
+
+Context-aware text-to-speech with emotion-based rate/pitch modulation.
+"""
--- a/jetson/ros2_ws/src/saltybot_tts_personality/setup.cfg
+++ b/jetson/ros2_ws/src/saltybot_tts_personality/setup.cfg
@ -0,0 +1,5 @@
+[develop]
+script_dir=$base/lib/saltybot_tts_personality
+
+[install]
+install_lib=$base/lib/python3/dist-packages
--- a/jetson/ros2_ws/src/saltybot_tts_personality/setup.py
+++ b/jetson/ros2_ws/src/saltybot_tts_personality/setup.py
@ -0,0 +1,25 @@
+from setuptools import setup
+
+package_name = 'saltybot_tts_personality'
+
+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']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='seb',
+    maintainer_email='seb@vayrette.com',
+    description='TTS personality engine with context-aware greetings and emotion expression',
+    license='Apache-2.0',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'tts_personality_node = saltybot_tts_personality.tts_personality_node:main',
+        ],
+    },
+)
--- a/jetson/ros2_ws/src/saltybot_voice_router/VOICE_ROUTER_README.md
+++ b/jetson/ros2_ws/src/saltybot_voice_router/VOICE_ROUTER_README.md
@ -1,54 +1,145 @@
-# Voice Command Router (Issue #491)
+# SaltyBot Voice Command Router (Issue #491)

-Natural language voice command routing with fuzzy matching.
+## Overview

-## Supported Commands
- Follow me / come with me
- Stop / halt / freeze
- Go home / return to dock / charge
- Patrol / autonomous mode
- Come here / approach
- Sit / sit down
- Spin / rotate
- Dance / groove
- Take photo / picture / smile
- What's that / identify / recognize
- Battery status / battery level
+Natural language voice command routing with fuzzy matching for handling speech variations.
+
+**Supported Commands**:
+- Follow me
+- Stop / Halt
+- Go home / Return to dock
+- Patrol
+- Come here
+- Sit
+- Spin
+- Dance
+- Take photo
+- What's that (object identification)
+- Battery status

 ## Features
- **Fuzzy Matching**: Tolerates speech variations using rapidfuzz
- **Multiple Patterns**: Each command has multiple recognition patterns
- **Three Action Types**:
-  - Velocity commands (stop → /cmd_vel)
-  - Action commands (patrol → /saltybot/action_command)
-  - Service calls (photo → /photo/capture)
+
+### Fuzzy Matching
+Uses `rapidfuzz` library with `token_set_ratio` for robust pattern matching:
+- Tolerates speech variations ("stop", "halt", "hold", "freeze")
+- Configurable threshold (default 80%)
+- Matches against multiple patterns per command
+
+### Command Routing
+Three routing types:
+1. **Publish**: Direct topic publishing (most commands)
+2. **Service**: Service calls (photo capture)
+3. **Velocity**: Direct `/cmd_vel` publishing (stop command)
+
+### Monitoring
+- Publishes all recognized commands to `/saltybot/voice_command`
+- JSON format with timestamp
+- Enables logging and UI feedback

 ## Architecture
+
 ```
-/saltybot/speech/transcribed_text
+/speech_recognition/transcribed_text
       ↓
 [VoiceCommandRouter]
-    ↓ Fuzzy Match (threshold: 75%)
       ↓
-/cmd_vel | /saltybot/action_command | Services
+   Fuzzy Match
+       ↓
+   Execute
+   /cmd_vel │ /saltybot/action_command │ Services
+       ↓
+   [Robot Actions]
 ```

+## Command Mapping
+
+| Voice Command | Type | Topic/Service | Args |
+|---|---|---|---|
+| Follow me | publish | /saltybot/action_command | action: follow_person |
+| Stop | publish | /cmd_vel | velocity: 0,0,0 |
+| Go home | publish | /saltybot/action_command | action: return_home |
+| Patrol | publish | /saltybot/action_command | action: start_patrol |
+| Come here | publish | /saltybot/action_command | action: approach_person |
+| Sit | publish | /saltybot/action_command | action: sit |
+| Spin | publish | /saltybot/action_command | action: spin, count: 1 |
+| Dance | publish | /saltybot/action_command | action: dance |
+| Take photo | service | /photo/capture | save: true |
+| What's that | publish | /saltybot/action_command | action: identify_object |
+| Battery status | publish | /saltybot/action_command | action: report_battery |
+
 ## Launch
+
 ```bash
 ros2 launch saltybot_voice_router voice_router.launch.py
 ```

-## Test
+## Integration
+
+Subscribe to `/speech_recognition/transcribed_text`:
+- Typically from wake word engine (e.g., Porcupine)
+- Or manual speech recognition node
+- Format: `std_msgs/String` with lowercase text
+
+## Parameters
+
+- `fuzzy_match_threshold`: Fuzzy matching score (0-100, default 80)
+- `enable_debug_logging`: Detailed command matching logs
+
+## Adding New Commands
+
+Edit `_init_commands()` in `voice_router_node.py`:
+
+```python
+'new_command': VoiceCommand(
+    'new_command',
+    ['pattern 1', 'pattern 2', 'pattern 3'],
+    'publish',  # or 'service'
+    topic='/saltybot/action_command',
+    args={'action': 'new_action'}
+),
+```
+
+## Dependencies
+
+- `rclpy`: ROS2 Python client
+- `rapidfuzz`: Fuzzy string matching
+
+Install: `pip install rapidfuzz`
+
+## Example Usage
+
+### With Speech Recognition
 ```bash
+# Terminal 1: Start voice router
+ros2 launch saltybot_voice_router voice_router.launch.py
+
+# Terminal 2: Test with manual transcription
 ros2 topic pub /saltybot/speech/transcribed_text std_msgs/String '{data: "follow me"}'
 ```

-## Topics
- **Subscribe**: `/saltybot/speech/transcribed_text` (std_msgs/String)
- **Publish**: `/saltybot/action_command` (std_msgs/String)
- **Publish**: `/saltybot/voice_command` (std_msgs/String)
- **Publish**: `/cmd_vel` (geometry_msgs/Twist)
+### Monitor Commands
+```bash
+ros2 topic echo /saltybot/voice_command
+```

-## Dependencies
- `rapidfuzz`: Fuzzy string matching
- `rclpy`: ROS2 Python client
+## Performance
+
+- Fuzzy matching: <10ms per command
+- Multiple pattern matching: <50ms worst case
+- No blocking operations
+
+## Safety
+
+- Stop command has highest priority
+- Can be integrated with emergency stop system
+- All commands validated before execution
+- Graceful handling of unknown commands
+
+## Future Enhancements
+
+- [ ] Confidence scoring display
+- [ ] Command feedback (audio confirmation)
+- [ ] Learning user preferences
+- [ ] Multi-language support
+- [ ] Voice emotion detection
+- [ ] Command context awareness
--- a/jetson/ros2_ws/src/saltybot_voice_router/package.xml
+++ b/jetson/ros2_ws/src/saltybot_voice_router/package.xml
@ -3,17 +3,21 @@
 <package format="3">
  <name>saltybot_voice_router</name>
  <version>0.1.0</version>
-  <description>Voice command router with fuzzy matching (Issue #491)</description>
+  <description>Voice command router with fuzzy matching for natural speech (Issue #491)</description>
  <maintainer email="seb@vayrette.com">seb</maintainer>
  <license>MIT</license>
+
  <depend>rclpy</depend>
  <depend>std_msgs</depend>
  <depend>geometry_msgs</depend>
  <depend>rapidfuzz</depend>
+
  <exec_depend>python3-launch-ros</exec_depend>
+
  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
  <test_depend>python3-pytest</test_depend>
+
  <export>
    <build_type>ament_python</build_type>
  </export>
--- a/jetson/ros2_ws/src/saltybot_voice_router/saltybot_voice_router/voice_router_node.py
+++ b/jetson/ros2_ws/src/saltybot_voice_router/saltybot_voice_router/voice_router_node.py
@ -1,118 +1,237 @@
 #!/usr/bin/env python3
-"""Voice Command Router (Issue #491) - Fuzzy matching for natural voice commands"""
+"""
+Voice Command Router (Issue #491)
+Receives transcribed text and routes to appropriate action commands.
+Uses fuzzy matching to handle natural speech variations.
+"""

 import rclpy
 from rclpy.node import Node
 from std_msgs.msg import String
 from geometry_msgs.msg import Twist
-from rapidfuzz import fuzz
+from rapidfuzz import process, fuzz
 import json


+class VoiceCommand:
+    """Voice command definition with fuzzy matching patterns."""
+    def __init__(self, name, patterns, action_type, topic=None, service=None, args=None):
+        self.name = name
+        self.patterns = patterns  # List of recognized variations
+        self.action_type = action_type  # 'publish' or 'service'
+        self.topic = topic
+        self.service = service
+        self.args = args or {}
+
+
 class VoiceCommandRouter(Node):
-    """Routes voice commands with fuzzy matching to handle natural speech variations."""
+    """Routes voice commands to appropriate ROS2 actions."""

    def __init__(self):
        super().__init__('voice_command_router')

-        # Define voice commands with patterns for fuzzy matching
-        self.commands = {
-            'follow': {
-                'patterns': ['follow me', 'follow me please', 'come with me', 'follow'],
-                'type': 'action', 'action': 'follow_person'
-            },
-            'stop': {
-                'patterns': ['stop', 'halt', 'hold', 'pause', 'freeze'],
-                'type': 'velocity', 'linear': 0.0, 'angular': 0.0
-            },
-            'go_home': {
-                'patterns': ['go home', 'return home', 'back home', 'go to dock', 'charge'],
-                'type': 'action', 'action': 'return_home'
-            },
-            'patrol': {
-                'patterns': ['patrol', 'start patrol', 'patrol mode', 'autonomous'],
-                'type': 'action', 'action': 'start_patrol'
-            },
-            'come_here': {
-                'patterns': ['come here', 'come to me', 'approach', 'get closer'],
-                'type': 'action', 'action': 'approach_person'
-            },
-            'sit': {
-                'patterns': ['sit', 'sit down', 'sit please'],
-                'type': 'action', 'action': 'sit'
-            },
-            'spin': {
-                'patterns': ['spin', 'spin around', 'rotate', 'turn around'],
-                'type': 'action', 'action': 'spin'
-            },
-            'dance': {
-                'patterns': ['dance', 'dance with me', 'do a dance'],
-                'type': 'action', 'action': 'dance'
-            },
-            'photo': {
-                'patterns': ['take a photo', 'take picture', 'photo', 'take photo', 'smile'],
-                'type': 'service', 'service': '/photo/capture'
-            },
-            'identify': {
-                'patterns': ['whats that', 'what is that', 'identify', 'recognize'],
-                'type': 'action', 'action': 'identify_object'
-            },
-            'battery': {
-                'patterns': ['battery status', 'battery level', 'check battery', 'whats my battery'],
-                'type': 'action', 'action': 'report_battery'
-            },
-        }
+        # Define all voice commands with fuzzy matching patterns
+        self.commands = self._init_commands()

-        # Subscriptions and publishers
-        self.create_subscription(String, '/saltybot/speech/transcribed_text', self.transcription_cb, 10)
+        # Create subscription to transcribed speech
+        self.transcription_sub = self.create_subscription(
+            String,
+            '/speech_recognition/transcribed_text',
+            self.transcription_callback,
+            10
+        )
+
+        # Create publisher for parsed voice commands
+        self.command_pub = self.create_publisher(String, '/saltybot/voice_command', 10)
+
+        # Create publishers for direct command topics
        self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)
        self.action_pub = self.create_publisher(String, '/saltybot/action_command', 10)
-        self.voice_cmd_pub = self.create_publisher(String, '/saltybot/voice_command', 10)

-        self.get_logger().info('Voice command router ready')
+        # Create service clients (lazily initialized on first use)
+        self.service_clients = {}

-    def transcription_cb(self, msg):
-        """Process transcribed text and route command."""
-        text = msg.data.lower().strip()
-        self.get_logger().debug(f'Transcribed: "{text}"')
+        self.get_logger().info('Voice command router initialized')

-        # Fuzzy match against known commands
-        best_cmd = None
-        best_score = 0
+    def _init_commands(self):
+        """Initialize voice command definitions."""
+        return {
+            'follow_me': VoiceCommand(
+                'follow_me',
+                ['follow me', 'follow me please', 'start following', 'come with me', 'follow'],
+                'publish',
+                topic='/saltybot/action_command',
+                args={'action': 'follow_person'}
+            ),
+            'stop': VoiceCommand(
+                'stop',
+                ['stop', 'halt', 'hold', 'pause', 'freeze', 'dont move'],
+                'publish',
+                topic='/cmd_vel',
+                args={'linear': {'x': 0.0, 'y': 0.0}, 'angular': {'z': 0.0}}
+            ),
+            'go_home': VoiceCommand(
+                'go_home',
+                ['go home', 'return home', 'back home', 'go to dock', 'charge', 'return to dock'],
+                'publish',
+                topic='/saltybot/action_command',
+                args={'action': 'return_home'}
+            ),
+            'patrol': VoiceCommand(
+                'patrol',
+                ['patrol', 'start patrol', 'begin patrol', 'patrol mode', 'autonomous mode'],
+                'publish',
+                topic='/saltybot/action_command',
+                args={'action': 'start_patrol'}
+            ),
+            'come_here': VoiceCommand(
+                'come_here',
+                ['come here', 'come to me', 'approach', 'move closer', 'get closer'],
+                'publish',
+                topic='/saltybot/action_command',
+                args={'action': 'approach_person'}
+            ),
+            'sit': VoiceCommand(
+                'sit',
+                ['sit', 'sit down', 'sit please', 'lower yourself'],
+                'publish',
+                topic='/saltybot/action_command',
+                args={'action': 'sit'}
+            ),
+            'spin': VoiceCommand(
+                'spin',
+                ['spin', 'spin around', 'rotate', 'turn around', 'twirl'],
+                'publish',
+                topic='/saltybot/action_command',
+                args={'action': 'spin', 'count': 1}
+            ),
+            'dance': VoiceCommand(
+                'dance',
+                ['dance', 'dance with me', 'do a dance', 'move to music', 'groove'],
+                'publish',
+                topic='/saltybot/action_command',
+                args={'action': 'dance'}
+            ),
+            'take_photo': VoiceCommand(
+                'take_photo',
+                ['take a photo', 'take a picture', 'photo', 'picture', 'take photo', 'smile'],
+                'service',
+                service='/photo/capture',
+                args={'save': True}
+            ),
+            'whats_that': VoiceCommand(
+                'whats_that',
+                ['whats that', 'what is that', 'identify', 'recognize', 'what do you see'],
+                'publish',
+                topic='/saltybot/action_command',
+                args={'action': 'identify_object'}
+            ),
+            'battery_status': VoiceCommand(
+                'battery_status',
+                ['battery status', 'battery level', 'how much battery', 'check battery', 'whats my battery'],
+                'publish',
+                topic='/saltybot/action_command',
+                args={'action': 'report_battery'}
+            ),
+        }

-        for cmd_name, cmd_info in self.commands.items():
-            for pattern in cmd_info['patterns']:
-                score = fuzz.token_set_ratio(text, pattern)
-                if score > best_score and score >= 75:
-                    best_score = score
-                    best_cmd = (cmd_name, cmd_info)
+    def transcription_callback(self, msg):
+        """Handle incoming transcribed text from speech recognition."""
+        transcribed_text = msg.data.lower().strip()
+        self.get_logger().info(f'Received transcription: "{transcribed_text}"')

-        if best_cmd:
-            self.get_logger().info(f'Matched: {best_cmd[0]} (score: {best_score})')
-            self.execute_command(best_cmd[0], best_cmd[1])
+        # Try to match against known commands using fuzzy matching
+        command = self.match_command(transcribed_text)
+
+        if command:
+            self.get_logger().info(f'Matched command: {command.name}')
+            self.execute_command(command)
        else:
-            self.get_logger().warning(f'No match for: "{text}"')
+            self.get_logger().warning(f'No matching command found for: "{transcribed_text}"')

-    def execute_command(self, cmd_name, cmd_info):
+    def match_command(self, text, threshold=80):
+        """
+        Use fuzzy matching to find best matching command.
+        Returns the best matching VoiceCommand or None.
+        """
+        best_score = 0
+        best_command = None
+
+        for cmd_name, cmd in self.commands.items():
+            # Try matching against each pattern for this command
+            for pattern in cmd.patterns:
+                score = fuzz.token_set_ratio(text, pattern)
+
+                if score > best_score and score >= threshold:
+                    best_score = score
+                    best_command = cmd
+
+        return best_command
+
+    def execute_command(self, command: VoiceCommand):
        """Execute the matched voice command."""
-        if cmd_info['type'] == 'velocity':
-            # Stop command - publish Twist
+        try:
+            if command.action_type == 'publish':
+                self.publish_command(command)
+            elif command.action_type == 'service':
+                self.call_service(command)
+
+            # Always publish to /saltybot/voice_command for monitoring
+            cmd_msg = String(data=json.dumps({
+                'command': command.name,
+                'action_type': command.action_type,
+                'timestamp': self.get_clock().now().to_msg(),
+            }))
+            self.command_pub.publish(cmd_msg)
+
+        except Exception as e:
+            self.get_logger().error(f'Error executing command {command.name}: {e}')
+
+    def publish_command(self, command: VoiceCommand):
+        """Publish command to appropriate topic."""
+        if not command.topic:
+            return
+
+        if command.topic == '/cmd_vel':
+            # Publish Twist message for velocity commands
            msg = Twist()
-            msg.linear.x = cmd_info.get('linear', 0.0)
-            msg.angular.z = cmd_info.get('angular', 0.0)
+            msg.linear.x = command.args['linear'].get('x', 0.0)
+            msg.linear.y = command.args['linear'].get('y', 0.0)
+            msg.angular.z = command.args['angular'].get('z', 0.0)
            self.cmd_vel_pub.publish(msg)

-        elif cmd_info['type'] == 'action':
-            # Action command
-            msg = String(data=json.dumps({'action': cmd_info['action']}))
+        else:
+            # Publish String message with command data
+            msg = String(data=json.dumps(command.args))
            self.action_pub.publish(msg)

-        elif cmd_info['type'] == 'service':
-            # Service call (simplified - would need proper async handling)
-            pass
+    def call_service(self, command: VoiceCommand):
+        """Call a service for the command."""
+        if not command.service:
+            return

-        # Publish to voice_command topic for monitoring
-        self.voice_cmd_pub.publish(String(data=cmd_name))
+        try:
+            # Lazy initialize service client
+            if command.service not in self.service_clients:
+                from std_srvs.srv import Empty
+                client = self.create_client(Empty, command.service)
+                self.service_clients[command.service] = client
+
+            client = self.service_clients[command.service]
+
+            # Wait for service to be available
+            if not client.wait_for_service(timeout_sec=2.0):
+                self.get_logger().warning(f'Service {command.service} not available')
+                return
+
+            # Call service
+            from std_srvs.srv import Empty
+            request = Empty.Request()
+            future = client.call_async(request)
+            rclpy.spin_until_future_complete(self, future)
+
+        except Exception as e:
+            self.get_logger().error(f'Error calling service {command.service}: {e}')


 def main(args=None):
--- a/jetson/ros2_ws/src/saltybot_voice_router/setup.cfg
+++ b/jetson/ros2_ws/src/saltybot_voice_router/setup.cfg
@ -1,4 +1,5 @@
 [develop]
 script_dir=$base/lib/saltybot_voice_router
+
 [install]
 install_scripts=$base/lib/saltybot_voice_router
--- a/jetson/ros2_ws/src/saltybot_voice_router/setup.py
+++ b/jetson/ros2_ws/src/saltybot_voice_router/setup.py
@ -1,12 +1,16 @@
 from setuptools import setup
+import os
+from glob import glob
+
+package_name = 'saltybot_voice_router'

 setup(
-    name='saltybot_voice_router',
+    name=package_name,
    version='0.1.0',
-    packages=['saltybot_voice_router'],
+    packages=[package_name],
    data_files=[
-        ('share/ament_index/resource_index/packages', ['resource/saltybot_voice_router']),
-        ('share/saltybot_voice_router', ['package.xml']),
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools', 'rapidfuzz'],
    zip_safe=True,
@ -14,6 +18,7 @@ setup(
    maintainer_email='seb@vayrette.com',
    description='Voice command router with fuzzy matching (Issue #491)',
    license='MIT',
+    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'voice_command_router = saltybot_voice_router.voice_router_node:main',
				`@ -0,0 +1 @@`
				`"""SaltyBot OTA Firmware Update - Download, build, deploy, and rollback."""`