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:
parent
9257f4c7de
commit
4e6ecacd37
@ -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()
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user