Merge pull request 'feat: auto-dock (Issue #410)' (#417) from sl-mechanical/issue-410-auto-dock into main

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()