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, 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)