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>
32 lines
1.1 KiB
XML
32 lines
1.1 KiB
XML
<?xml version="1.0"?>
|
|
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
|
<package format="3">
|
|
<name>saltybot_social_tracking</name>
|
|
<version>0.1.0</version>
|
|
<description>
|
|
Multi-modal tracking fusion for saltybot.
|
|
Fuses UWB triangulated position (/uwb/target) and camera-detected position
|
|
(/person/target) using a 4-state Kalman filter to produce a smooth, low-latency
|
|
fused estimate at /social/tracking/fused_target.
|
|
Handles EUC rider speeds (20-30 km/h), signal handoff, and predictive coasting.
|
|
</description>
|
|
<maintainer email="sl-controls@saltylab.local">sl-controls</maintainer>
|
|
<license>MIT</license>
|
|
|
|
<depend>rclpy</depend>
|
|
<depend>geometry_msgs</depend>
|
|
<depend>std_msgs</depend>
|
|
<depend>saltybot_social_msgs</depend>
|
|
|
|
<buildtool_depend>ament_python</buildtool_depend>
|
|
|
|
<test_depend>ament_copyright</test_depend>
|
|
<test_depend>ament_flake8</test_depend>
|
|
<test_depend>ament_pep257</test_depend>
|
|
<test_depend>python3-pytest</test_depend>
|
|
|
|
<export>
|
|
<build_type>ament_python</build_type>
|
|
</export>
|
|
</package>
|