diff --git a/jetson/ros2_ws/src/saltybot_bringup/launch/full_stack.launch.py b/jetson/ros2_ws/src/saltybot_bringup/launch/full_stack.launch.py index 4bef9a8..7e37c82 100644 --- a/jetson/ros2_ws/src/saltybot_bringup/launch/full_stack.launch.py +++ b/jetson/ros2_ws/src/saltybot_bringup/launch/full_stack.launch.py @@ -194,6 +194,12 @@ def generate_launch_description(): 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", 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) ──────── nav2 = TimerAction( period=14.0, @@ -468,6 +493,7 @@ def generate_launch_description(): enable_follower_arg, enable_bridge_arg, enable_rosbridge_arg, + enable_docking_arg, follow_distance_arg, max_linear_vel_arg, uwb_port_a_arg, @@ -498,6 +524,9 @@ def generate_launch_description(): perception, object_detection, + # t=7s + docking, + # t=9s follower,