sl-controls
783dedf4d4
Two new ROS2 packages implementing Issue #158: saltybot_docking_msgs (ament_cmake) - DockingStatus.msg: stamp, state, dock_detected, distance_m, lateral_error_m, battery_pct, charging, aligned - Dock.srv / Undock.srv: force + resume_mission flags saltybot_docking (ament_python, 20 Hz) - dock_detector.py: ArucoDetector (cv2.aruco PnP → DockPose) + IRBeaconDetector (EMA envelope with amplitude threshold); both gracefully degrade if unavailable - visual_servo.py: IBVS proportional controller — v = k_lin×(d−target), ω = −k_ang×yaw; aligned when |lateral| < 5mm AND d < contact_distance - charge_monitor.py: edge-triggered events (CHARGING_STARTED/STOPPED, THRESHOLD_LOW at 15%, THRESHOLD_HIGH at 80%) - docking_state_machine.py: 7-state FSM (IDLE→DETECTING→NAV2_APPROACH→ VISUAL_SERVO→CONTACT→CHARGING→UNDOCKING); mission_resume signal on auto-dock cycle; contact retry on timeout; lost-detection timeout - docking_node.py: 20Hz ROS2 node; Nav2 NavigateToPose action client (optional); /saltybot/dock + /saltybot/undock services; /saltybot/docking_cmd_vel; /saltybot/resume_mission; /saltybot/docking_status - config/docking_params.yaml, launch/docking.launch.py Tests: 64/64 passing (IRBeaconDetector, VisualServo, ChargeMonitor, DockingStateMachine — all state transitions and guard conditions covered) Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
44 lines
2.8 KiB
YAML
44 lines
2.8 KiB
YAML
/**:
|
|
ros__parameters:
|
|
# Control loop rate (Hz)
|
|
control_rate: 20.0
|
|
|
|
# Topic names
|
|
image_topic: "/camera/image_raw"
|
|
camera_info_topic: "/camera/camera_info"
|
|
ir_topic: "/saltybot/ir_beacon"
|
|
battery_topic: "/saltybot/battery_state"
|
|
|
|
# ── ArUco detection ────────────────────────────────────────────────────────
|
|
aruco_marker_id: 42 # marker ID on the dock face
|
|
aruco_marker_size_m: 0.10 # printed marker side length (m)
|
|
|
|
# ── IR beacon ─────────────────────────────────────────────────────────────
|
|
ir_threshold: 0.50 # amplitude threshold for beacon detection
|
|
|
|
# ── Battery thresholds ────────────────────────────────────────────────────
|
|
battery_low_pct: 15.0 # SOC triggering auto-dock (%)
|
|
battery_high_pct: 80.0 # SOC declaring charge complete (%)
|
|
|
|
# ── Visual servo ──────────────────────────────────────────────────────────
|
|
servo_range_m: 1.00 # switch from Nav2 to IBVS at this distance (m)
|
|
k_linear: 0.30 # distance P-gain
|
|
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)
|
|
|
|
# ── Undocking maneuver ────────────────────────────────────────────────────
|
|
undock_speed_ms: -0.20 # reverse speed (m/s; must be negative)
|
|
undock_ticks_max: 50 # ticks at undock speed (2.5 s at 20 Hz)
|
|
|
|
# ── FSM timeouts ──────────────────────────────────────────────────────────
|
|
lost_ticks_max: 40 # ticks of lost detection before retry (2 s)
|
|
contact_timeout_ticks: 20 # ticks at CONTACT without charging before retry
|
|
|
|
# ── Nav2 waypoints [x, y, z, qx, qy, qz, qw] in map frame ───────────────
|
|
# Set these to the actual dock and pre-dock poses in your environment.
|
|
dock_pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
|
|
pre_dock_pose: [0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 1.0] # 1 m in front of dock
|