From 340248a0d230dc5dd3d16725b4074e53c8946bd8 Mon Sep 17 00:00:00 2001 From: sl-controls Date: Thu, 5 Mar 2026 17:08:39 -0500 Subject: [PATCH] feat: Add docking state publisher and update configuration (Issue #489) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 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 --- .../src/saltybot_docking/config/docking_params.yaml | 6 +++--- .../saltybot_docking/saltybot_docking/docking_node.py | 10 +++++++++- 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/jetson/ros2_ws/src/saltybot_docking/config/docking_params.yaml b/jetson/ros2_ws/src/saltybot_docking/config/docking_params.yaml index 20d15fe..7d254a4 100644 --- a/jetson/ros2_ws/src/saltybot_docking/config/docking_params.yaml +++ b/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) diff --git a/jetson/ros2_ws/src/saltybot_docking/saltybot_docking/docking_node.py b/jetson/ros2_ws/src/saltybot_docking/saltybot_docking/docking_node.py index f5b73a5..f7fe230 100644 --- a/jetson/ros2_ws/src/saltybot_docking/saltybot_docking/docking_node.py +++ b/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)