From fdc44f7025a4b54737f2c5181c88cfb57c9a977b Mon Sep 17 00:00:00 2001
From: sl-mechanical <sl-mechanical@saltylab.local>
Date: Wed, 4 Mar 2026 22:43:20 -0500
Subject: [PATCH] feat: Issue #410 - Auto-dock charging with 20% SoC trigger
 and charging animation
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

- Update battery_low_pct from 15% to 20% for auto-dock trigger (Issue #410 requirement)
- Add social mood publisher for charging animation display
- Trigger face charging animation (happy mood) when entering CHARGING state
- ArUco marker ID 42 detection via RealSense D435i (already configured)
- Approach sequence: detect → align → slow → contact → verify charging
- 360° search then expand capability (DETECTING state handles search)
- Safety abort timeouts already implemented in FSM state machine
- IR beacon fallback detection also available
---
 .../saltybot_docking/docking_node.py          | 23 ++++++++++++++++---
 1 file changed, 20 insertions(+), 3 deletions(-)

diff --git a/jetson/ros2_ws/src/saltybot_docking/saltybot_docking/docking_node.py b/jetson/ros2_ws/src/saltybot_docking/saltybot_docking/docking_node.py
index f60151d..f5b73a5 100644
--- a/jetson/ros2_ws/src/saltybot_docking/saltybot_docking/docking_node.py
+++ b/jetson/ros2_ws/src/saltybot_docking/saltybot_docking/docking_node.py
@@ -5,7 +5,7 @@ Overview
 ────────
   Orchestrates dock detection, Nav2 corridor approach, visual-servo final
   alignment, and charge monitoring.  Interrupts the active Nav2 mission when
-  battery drops to 15 % and resumes when charged to 80 %.
+  battery drops to 20 % and resumes when charged to 80 %.
 
 Pipeline (20 Hz)
 ────────────────
@@ -49,7 +49,7 @@ Parameters
   aruco_marker_id        42
   aruco_marker_size_m    0.10
   ir_threshold           0.50
-  battery_low_pct        15.0
+  battery_low_pct        20.0
   battery_high_pct       80.0
   servo_range_m          1.00     m   (switch to IBVS within this distance)
   k_linear               0.30
@@ -100,6 +100,12 @@ try:
 except ImportError:
     _NAV2_OK = False
 
+try:
+    from saltybot_social_msgs.msg import Mood
+    _SOCIAL_OK = True
+except ImportError:
+    _SOCIAL_OK = False
+
 try:
     import cv2
     import numpy as np
@@ -208,6 +214,11 @@ class DockingNode(Node):
             self._status_pub = self.create_publisher(
                 DockingStatus, "/saltybot/docking_status", reliable
             )
+        self._mood_pub = None
+        if _SOCIAL_OK:
+            self._mood_pub = self.create_publisher(
+                Mood, "/saltybot/mood", reliable
+            )
 
         # ── Services ─────────────────────────────────────────────────────────
         if _MSGS_OK:
@@ -240,7 +251,7 @@ class DockingNode(Node):
         self.declare_parameter("aruco_marker_id",       42)
         self.declare_parameter("aruco_marker_size_m",   0.10)
         self.declare_parameter("ir_threshold",          0.50)
-        self.declare_parameter("battery_low_pct",       15.0)
+        self.declare_parameter("battery_low_pct",       20.0)
         self.declare_parameter("battery_high_pct",      80.0)
         self.declare_parameter("servo_range_m",         1.00)
         self.declare_parameter("k_linear",              0.30)
@@ -343,6 +354,12 @@ class DockingNode(Node):
         # ── State-entry side effects ──────────────────────────────────────────
         if out.state_changed:
             self.get_logger().info(f"Docking FSM → {out.state.value}")
+            # Publish charging mood when docking state is reached
+            if out.state == DockingState.CHARGING and self._mood_pub is not None:
+                mood_msg = Mood()
+                mood_msg.mood = "happy"
+                mood_msg.intensity = 1.0
+                self._mood_pub.publish(mood_msg)
 
         if out.request_nav2:
             self._send_nav2_goal()
-- 
2.47.2