feat: Add docking state publisher and update configuration (Issue #489)

- Add /saltybot/docking_state publisher (std_msgs/String) for monitoring
- Update docking_params.yaml battery_low_pct: 15% → 20%
- Add Issue #475 references for conservative servo speeds

Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>

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,
 

jetson/ros2_ws/src/saltybot_docking/config/docking_params.yaml

@@ -17,7 +17,7 @@
     ir_threshold: 0.50           # amplitude threshold for beacon detection
 
     # ── Battery thresholds ────────────────────────────────────────────────────
-    battery_low_pct:  15.0       # SOC triggering auto-dock (%)
+    battery_low_pct:  20.0       # SOC triggering auto-dock (%) — Issue #489
     battery_high_pct: 80.0       # SOC declaring charge complete (%)
 
     # ── Visual servo ──────────────────────────────────────────────────────────
@@ -26,8 +26,8 @@
     k_angular:          0.80     # yaw P-gain
     lateral_tol_m:      0.005    # ±5 mm alignment tolerance
     contact_distance_m: 0.05     # distance below which contact is assumed (m)
-    max_linear_ms:      0.10     # forward speed ceiling during servo (m/s)
-    max_angular_rads:   0.30     # yaw rate ceiling (rad/s)
+    max_linear_ms:      0.10     # forward speed ceiling during servo (m/s) — conservative for FC+hoverboard (Issue #475)
+    max_angular_rads:   0.30     # yaw rate ceiling (rad/s) — conservative for FC+hoverboard (Issue #475)
 
     # ── Undocking maneuver ────────────────────────────────────────────────────
     undock_speed_ms:   -0.20     # reverse speed (m/s; must be negative)

jetson/ros2_ws/src/saltybot_docking/saltybot_docking/docking_node.py

@@ -76,7 +76,7 @@ from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
 
 from geometry_msgs.msg import PoseStamped, Twist
 from sensor_msgs.msg import BatteryState, CameraInfo, Image
-from std_msgs.msg import Bool, Float32
+from std_msgs.msg import Bool, Float32, String
 
 from saltybot_docking.charge_monitor import ChargeEvent, ChargeMonitor
 from saltybot_docking.dock_detector import ArucoDetector, DockPose, IRBeaconDetector
@@ -209,6 +209,9 @@ class DockingNode(Node):
         self._resume_pub = self.create_publisher(
             Bool, "/saltybot/resume_mission", reliable
         )
+        self._docking_state_pub = self.create_publisher(
+            String, "/saltybot/docking_state", reliable
+        )
         self._status_pub = None
         if _MSGS_OK:
             self._status_pub = self.create_publisher(
@@ -383,6 +386,11 @@ class DockingNode(Node):
             self._cmd_vel_pub.publish(twist)
 
         # ── Status ────────────────────────────────────────────────────────────
+        # Publish docking state (Issue #489)
+        state_msg = String()
+        state_msg.data = out.state.value
+        self._docking_state_pub.publish(state_msg)
+
         if self._status_pub is not None:
             self._publish_status(inp, out)