seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 5 Actions Packages Projects Releases Wiki Activity

feat: Docking station behavior (Issue #489) #497

Merged
sl-jetson merged 2 commits from sl-controls/issue-489-docking into main 2026-03-05 17:16:38 -05:00
Conversation 0 Commits 2 Files Changed 3 +12 -4
2 changed files with 12 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 340248a0d2 - Show all commits

6
jetson/ros2_ws/src/saltybot_docking/config/docking_params.yaml
View File

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

10
jetson/ros2_ws/src/saltybot_docking/saltybot_docking/docking_node.py
View File

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