sl-controls
fa0162fadc
New packages:
saltybot_social_msgs — FusedTarget.msg custom message
saltybot_social_tracking — 4-state Kalman fusion node
saltybot_social_tracking/tracking_fusion_node.py
Subscribes to /uwb/target (PoseStamped, ~10 Hz) and /person/target
(PoseStamped, ~30 Hz) and publishes /social/tracking/fused_target
(FusedTarget) at 20 Hz.
Source arbitration:
• "fused" — both UWB and camera are fresh; confidence-weighted blend
• "uwb" — UWB fresh, camera stale
• "camera" — camera fresh, UWB stale
• "predicted" — all sources stale; KF coasts for up to predict_timeout (3 s)
Kalman filter (kalman_tracker.py):
State [x, y, vx, vy] with discrete Wiener acceleration noise model
(process_noise=3.0 m/s²) sized for EUC speeds (20-30 km/h, ≈5.5-8.3 m/s).
Separate UWB (0.20 m) and camera (0.12 m) measurement noise.
Velocity estimate converges after ~3 s of 10 Hz UWB measurements.
Confidence model (source_arbiter.py):
Per-source confidence = quality × max(0, 1 - age/timeout).
Composite confidence accounts for KF positional uncertainty and
is capped at 0.4 during dead-reckoning ("predicted") mode.
Tests: 58/58 pass (no ROS2 runtime required).
Note: saltybot_social_msgs here adds FusedTarget.msg; PR #98
(Issue #84) adds PersonalityState.msg + QueryMood.srv to the same
package. The maintainer should squash-merge #98 first and rebase
this branch on top of it before merging to avoid the package.xml
conflict.
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>