seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 5 Actions Packages Projects Releases Wiki Activity

feat: auto-dock (Issue #410) #417

Merged
sl-jetson merged 1 commits from sl-mechanical/issue-410-auto-dock into main 2026-03-04 22:46:50 -05:00
Conversation 0 Commits 1 Files Changed 1 +20 -3
1 changed files with 20 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
jetson/ros2_ws/src/saltybot_docking/saltybot_docking/docking_node.py
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()