feat: UWB follow-me system (#57) #66

sl-jetson · 2026-03-01T00:48:41-05:00

sl-jetson commented

2026-03-01 00:48:41 -05:00

Summary

Implements issue #57 — UWB follow-me system using 3× MaUWB ESP32-S3 DW3000 modules (2 anchors on robot stem, 1 tag on person).

New packages

saltybot_uwb_msgs (ament_cmake)

UwbRange.msg — per-anchor TWR range measurement (anchor_id, range_m, raw_mm, rssi)
UwbRangeArray.msg — published on /uwb/ranges

saltybot_uwb (ament_python)

ranging_math.py — pure triangulate_2anchor() (height-compensated 2-anchor geometry) + KalmanFilter2D (constant-velocity, numpy-free)
uwb_driver_node.py — SerialReader background threads poll both anchors via AT+RANGE? (115200 8N1), triangulate, Kalman-smooth, publish /uwb/target (PoseStamped in base_link) + /uwb/ranges
config/uwb_config.yaml — anchor geometry, serial ports, Kalman tuning
launch/uwb.launch.py — all params overridable at launch time
test/test_ranging_math.py — 16 tests (triangulation geometry + Kalman convergence)

Updated `saltybot_follower`

person_follower_node.py — adds fuse_targets() pure helper + /uwb/target subscriber (primary, default weight=0.7); /person/target secondary (weight=0.3); weighted blend when both fresh, graceful fallback to single source; new params uwb_weight + uwb_timeout
person_follower_params.yaml — uwb_weight: 0.7, uwb_timeout: 1.0s
test_person_follower.py — 7 new TestFuseTargets cases; 60 tests total, all pass

Test plan

pytest jetson/ros2_ws/src/saltybot_uwb/test/ — 16 pass
pytest jetson/ros2_ws/src/saltybot_follower/test/ — 60 pass
ros2 launch saltybot_uwb uwb.launch.py on Orin with anchors on USB
Verify /uwb/ranges publishes at 20Hz with both anchors active
Verify /uwb/target x-position tracks person walking away/toward robot
ros2 launch saltybot_follower person_follower.launch.py — confirm fusion log: "Fusing UWB+camera" when both active
Kill camera node → log shows "UWB only", robot still follows
Kill UWB node → log shows "Camera only", robot still follows
Kill both → STOPPING then SEARCHING state

🤖 Generated with Claude Code

## Summary Implements issue #57 — UWB follow-me system using 3× MaUWB ESP32-S3 DW3000 modules (2 anchors on robot stem, 1 tag on person). ### New packages **`saltybot_uwb_msgs`** (ament_cmake) - `UwbRange.msg` — per-anchor TWR range measurement (anchor_id, range_m, raw_mm, rssi) - `UwbRangeArray.msg` — published on `/uwb/ranges` **`saltybot_uwb`** (ament_python) - `ranging_math.py` — pure `triangulate_2anchor()` (height-compensated 2-anchor geometry) + `KalmanFilter2D` (constant-velocity, numpy-free) - `uwb_driver_node.py` — SerialReader background threads poll both anchors via `AT+RANGE?` (115200 8N1), triangulate, Kalman-smooth, publish `/uwb/target` (PoseStamped in base_link) + `/uwb/ranges` - `config/uwb_config.yaml` — anchor geometry, serial ports, Kalman tuning - `launch/uwb.launch.py` — all params overridable at launch time - `test/test_ranging_math.py` — **16 tests** (triangulation geometry + Kalman convergence) ### Updated `saltybot_follower` - `person_follower_node.py` — adds `fuse_targets()` pure helper + `/uwb/target` subscriber (primary, default weight=0.7); `/person/target` secondary (weight=0.3); weighted blend when both fresh, graceful fallback to single source; new params `uwb_weight` + `uwb_timeout` - `person_follower_params.yaml` — `uwb_weight: 0.7`, `uwb_timeout: 1.0s` - `test_person_follower.py` — 7 new `TestFuseTargets` cases; **60 tests total, all pass** ## Test plan - [ ] `pytest jetson/ros2_ws/src/saltybot_uwb/test/` — 16 pass - [ ] `pytest jetson/ros2_ws/src/saltybot_follower/test/` — 60 pass - [ ] `ros2 launch saltybot_uwb uwb.launch.py` on Orin with anchors on USB - [ ] Verify `/uwb/ranges` publishes at 20Hz with both anchors active - [ ] Verify `/uwb/target` x-position tracks person walking away/toward robot - [ ] `ros2 launch saltybot_follower person_follower.launch.py` — confirm fusion log: "Fusing UWB+camera" when both active - [ ] Kill camera node → log shows "UWB only", robot still follows - [ ] Kill UWB node → log shows "Camera only", robot still follows - [ ] Kill both → STOPPING then SEARCHING state 🤖 Generated with [Claude Code](https://claude.com/claude-code)

sl-jetson added 1 commit 2026-03-01 00:48:42 -05:00

feat: UWB follow-me system (#57 ) — saltybot_uwb package + sensor fusion a00dbe6429

New packages
------------
saltybot_uwb_msgs (ament_cmake)
  • UwbRange.msg     — per-anchor range reading (anchor_id, range_m, raw_mm, rssi)
  • UwbRangeArray.msg — array of UwbRange published on /uwb/ranges

saltybot_uwb (ament_python)
  • ranging_math.py    — pure triangulate_2anchor() (height-compensated TWR
                         geometry, 2-anchor intersection) + KalmanFilter2D
                         (constant-velocity, numpy-free, 16 tests pass)
  • uwb_driver_node.py — SerialReader threads poll MaUWB ESP32-S3 DW3000
                         anchors via AT+RANGE?, triangulate, Kalman-smooth,
                         publish /uwb/target (PoseStamped/base_link) + /uwb/ranges
  • config/uwb_config.yaml, launch/uwb.launch.py
  • test/test_ranging_math.py — 16 unit tests (triangulation + Kalman), all pass

Updated saltybot_follower
-------------------------
  • person_follower_node.py — adds fuse_targets() pure helper + /uwb/target
    subscriber (primary, weight=0.7); /person/target secondary (weight=0.3);
    weighted blend when both fresh, graceful fallback to single source; new
    params uwb_weight + uwb_timeout
  • person_follower_params.yaml — uwb_weight: 0.7, uwb_timeout: 1.0s
  • test_person_follower.py — 7 new TestFuseTargets cases; total 60 pass

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

seb merged commit 64d411b48a into main

2026-03-01 00:51:21 -05:00

seb deleted branch sl-jetson/uwb-follow-me

2026-03-01 00:51:21 -05:00

seb referenced this issue from a commit

2026-03-01 00:51:22 -05:00

Merge pull request 'feat: UWB follow-me system (#57)' (#66) from sl-jetson/uwb-follow-me into main

sl-perception referenced this issue from a commit

2026-03-01 01:07:17 -05:00

feat: route recording + autonomous replay (#71)

sl-perception referenced this pull request

2026-03-01 01:07:38 -05:00