feat: Issue #410 - Auto-dock charging with 20% SoC trigger and charging animation

- Update battery_low_pct from 15% to 20% for auto-dock trigger (Issue #410)
- 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
This commit is contained in:
sl-mechanical 2026-03-04 22:44:04 -05:00
parent 9257f4c7de
commit 4e6ecacd37

View File

@ -5,7 +5,7 @@ Overview
Orchestrates dock detection, Nav2 corridor approach, visual-servo final Orchestrates dock detection, Nav2 corridor approach, visual-servo final
alignment, and charge monitoring. Interrupts the active Nav2 mission when 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) Pipeline (20 Hz)
@ -49,7 +49,7 @@ Parameters
aruco_marker_id 42 aruco_marker_id 42
aruco_marker_size_m 0.10 aruco_marker_size_m 0.10
ir_threshold 0.50 ir_threshold 0.50
battery_low_pct 15.0 battery_low_pct 20.0
battery_high_pct 80.0 battery_high_pct 80.0
servo_range_m 1.00 m (switch to IBVS within this distance) servo_range_m 1.00 m (switch to IBVS within this distance)
k_linear 0.30 k_linear 0.30
@ -100,6 +100,12 @@ try:
except ImportError: except ImportError:
_NAV2_OK = False _NAV2_OK = False
try:
from saltybot_social_msgs.msg import Mood
_SOCIAL_OK = True
except ImportError:
_SOCIAL_OK = False
try: try:
import cv2 import cv2
import numpy as np import numpy as np
@ -208,6 +214,11 @@ class DockingNode(Node):
self._status_pub = self.create_publisher( self._status_pub = self.create_publisher(
DockingStatus, "/saltybot/docking_status", reliable DockingStatus, "/saltybot/docking_status", reliable
) )
self._mood_pub = None
if _SOCIAL_OK:
self._mood_pub = self.create_publisher(
Mood, "/saltybot/mood", reliable
)
# ── Services ───────────────────────────────────────────────────────── # ── Services ─────────────────────────────────────────────────────────
if _MSGS_OK: if _MSGS_OK:
@ -240,7 +251,7 @@ class DockingNode(Node):
self.declare_parameter("aruco_marker_id", 42) self.declare_parameter("aruco_marker_id", 42)
self.declare_parameter("aruco_marker_size_m", 0.10) self.declare_parameter("aruco_marker_size_m", 0.10)
self.declare_parameter("ir_threshold", 0.50) 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("battery_high_pct", 80.0)
self.declare_parameter("servo_range_m", 1.00) self.declare_parameter("servo_range_m", 1.00)
self.declare_parameter("k_linear", 0.30) self.declare_parameter("k_linear", 0.30)
@ -343,6 +354,12 @@ class DockingNode(Node):
# ── State-entry side effects ────────────────────────────────────────── # ── State-entry side effects ──────────────────────────────────────────
if out.state_changed: if out.state_changed:
self.get_logger().info(f"Docking FSM → {out.state.value}") 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: if out.request_nav2:
self._send_nav2_goal() self._send_nav2_goal()