feat: Docking station behavior for auto-charging (Issue #489)

- Integrate saltybot_docking package into full_stack.launch.py
- Auto-trigger docking when battery drops to 20% (configurable via battery_low_pct)
- Launch docking at t=7s (after sensors, before Nav2)
- Add /saltybot/docking_state publisher (std_msgs/String) for state monitoring
- Update docking_params.yaml:
  - battery_low_pct: 15% → 20% per Issue #489
  - Add references to Issue #475 for conservative FC+hoverboard speeds
- Docking behavior includes:
  - ArUco marker or IR beacon detection for dock location
  - Nav2-based approach to pre-dock pose (~1m away)
  - Visual servoing final alignment with contact detection
  - Auto-undocking on full charge (80%) or command
  - Integration with power management for mission interruption/resumption

Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
This commit is contained in:
sl-controls 2026-03-05 17:08:05 -05:00 committed by sl-firmware
parent 285178b2f9
commit b986702aed

View File

@ -194,6 +194,12 @@ def generate_launch_description():
description="Launch rosbridge WebSocket server (port 9090)", description="Launch rosbridge WebSocket server (port 9090)",
) )
enable_docking_arg = DeclareLaunchArgument(
"enable_docking",
default_value="true",
description="Launch autonomous docking behavior (auto-dock at 20% battery, Issue #489)",
)
follow_distance_arg = DeclareLaunchArgument( follow_distance_arg = DeclareLaunchArgument(
"follow_distance", "follow_distance",
default_value="1.5", default_value="1.5",
@ -418,6 +424,25 @@ def generate_launch_description():
], ],
) )
# ── t=7s Docking (auto-dock at 20% battery, Issue #489) ──────────────────
docking = TimerAction(
period=7.0,
actions=[
GroupAction(
condition=IfCondition(LaunchConfiguration("enable_docking")),
actions=[
LogInfo(msg="[full_stack] Starting autonomous docking (auto-trigger at 20% battery)"),
IncludeLaunchDescription(
_launch("saltybot_docking", "launch", "docking.launch.py"),
launch_arguments={
"battery_low_pct": "20.0",
}.items(),
),
],
),
],
)
# ── t=14s Nav2 (indoor only; SLAM needs ~8s to build initial map) ──────── # ── t=14s Nav2 (indoor only; SLAM needs ~8s to build initial map) ────────
nav2 = TimerAction( nav2 = TimerAction(
period=14.0, period=14.0,
@ -468,6 +493,7 @@ def generate_launch_description():
enable_follower_arg, enable_follower_arg,
enable_bridge_arg, enable_bridge_arg,
enable_rosbridge_arg, enable_rosbridge_arg,
enable_docking_arg,
follow_distance_arg, follow_distance_arg,
max_linear_vel_arg, max_linear_vel_arg,
uwb_port_a_arg, uwb_port_a_arg,
@ -498,6 +524,9 @@ def generate_launch_description():
perception, perception,
object_detection, object_detection,
# t=7s
docking,
# t=9s # t=9s
follower, follower,