sl-controls
0821845210
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>
20 lines
862 B
Plaintext
20 lines
862 B
Plaintext
# FusedTarget.msg — output of the multi-modal tracking fusion node.
|
|
#
|
|
# Position and velocity are in the base_link frame (robot-centred,
|
|
# +X forward, +Y left). z components are always 0.0 for ground-plane tracking.
|
|
#
|
|
# Confidence: 0.0 = no data / fully predicted; 1.0 = strong fused measurement.
|
|
# active_source: "fused" | "uwb" | "camera" | "predicted"
|
|
|
|
std_msgs/Header header
|
|
|
|
geometry_msgs/Point position # filtered 2-D position (m), z=0
|
|
geometry_msgs/Vector3 velocity # filtered 2-D velocity (m/s), z=0
|
|
|
|
float32 range_m # Euclidean distance from robot to fused position
|
|
float32 bearing_rad # bearing in base_link (+ve = person to the left)
|
|
float32 confidence # composite confidence [0.0, 1.0]
|
|
|
|
string active_source # "fused" | "uwb" | "camera" | "predicted"
|
|
string tag_id # UWB tag address (empty when UWB not contributing)
|