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

feat: Issue #365 — UWB DW3000 anchor/tag tracking (bearing + distance) #368

Merged
sl-jetson merged 1 commits from sl-perception/issue-365-uwb-tracking into main 2026-03-03 15:41:50 -05:00
Conversation 0 Commits 1 Files Changed 6 +1173
sl-perception commented 2026-03-03 15:25:55 -05:00
Collaborator
Copy Link

Summary

Software-complete UWB ranging stack for the two ESP32-UWB-Pro anchors. All code written against an abstract serial interface — fully testable without physical hardware while anchors are in transit.

New message type

  • UwbTarget.msg: valid, bearing_deg, distance_m, confidence, anchor0/1_dist_m, baseline_m, fix_quality (0=none, 1=single-anchor, 2=dual-anchor)

Core library — _uwb_tracker.py (pure Python, no ROS2/runtime deps)

  • parse_frame(): ASCII RANGE,<id>,<tag>,<mm> protocol decoder (mm→m)
  • bearing_from_ranges(): law-of-cosines 2-anchor bearing geometry with confidence (penalises extreme angles and close-range poor geometry)
  • bearing_single_anchor(): fallback when only one anchor responds — bearing=0, conf≤0.3
  • BearingKalman: 1-D constant-velocity Kalman filter [bearing_deg, bearing_rate_dps]
  • UwbRangingState: thread-safe per-anchor latest measurement + stale timeout + Kalman smoothing
  • AnchorSerialReader: background thread with readline() interface (works with serial.Serial or any file-like mock)

ROS2 node — uwb_node.py

  • Opens /dev/ttyUSB0 + /dev/ttyUSB1 (configurable via parameters)
  • Non-fatal serial open failure — publishes FIX_NONE until anchor plugged in
  • Publishes /saltybot/uwb_target at 10 Hz (configurable)
  • Params: port_anchor0, port_anchor1, baud_rate, baseline_m, publish_rate_hz, stale_timeout_s

Tests — test/test_uwb_tracker.py

  • 64/64 tests passing
  • Frame parsing: valid frames, malformed, STATUS, CR/LF, mm→m conversion, timestamps
  • Bearing geometry: straight-ahead, ±30°, ±45°, symmetry, confidence degradation, triangle inequality violation
  • Kalman: seeding, noise smoothing, convergence to constant, rate estimation
  • UwbRangingState: single/dual fix, stale timeout, thread safety (concurrent read/write), baseline stored
  • AnchorSerialReader: mock serial feeding, STATUS ignored, malformed lines no crash, stop()
  • Integration: full serial→state→bearing pipeline at various angles

Test plan

  • 64/64 unit tests pass on dev machine
  • Connect first ESP32-UWB-Pro anchor via USB, verify RANGE frames appear at ≥10 Hz
  • Connect second anchor, verify dual-anchor fix and bearing tracks lateral movement
  • Verify /saltybot/uwb_target at 10 Hz in RViz with target moving left/right at 5 m
  • Occlusion test: block camera but not UWB — confirm bearing still published

Closes #365

🤖 Generated with Claude Code

## Summary Software-complete UWB ranging stack for the two ESP32-UWB-Pro anchors. All code written against an abstract serial interface — fully testable without physical hardware while anchors are in transit. ### New message type - `UwbTarget.msg`: `valid`, `bearing_deg`, `distance_m`, `confidence`, `anchor0/1_dist_m`, `baseline_m`, `fix_quality` (0=none, 1=single-anchor, 2=dual-anchor) ### Core library — `_uwb_tracker.py` (pure Python, no ROS2/runtime deps) - **`parse_frame()`**: ASCII `RANGE,<id>,<tag>,<mm>` protocol decoder (mm→m) - **`bearing_from_ranges()`**: law-of-cosines 2-anchor bearing geometry with confidence (penalises extreme angles and close-range poor geometry) - **`bearing_single_anchor()`**: fallback when only one anchor responds — bearing=0, conf≤0.3 - **`BearingKalman`**: 1-D constant-velocity Kalman filter [bearing_deg, bearing_rate_dps] - **`UwbRangingState`**: thread-safe per-anchor latest measurement + stale timeout + Kalman smoothing - **`AnchorSerialReader`**: background thread with `readline()` interface (works with `serial.Serial` or any file-like mock) ### ROS2 node — `uwb_node.py` - Opens `/dev/ttyUSB0` + `/dev/ttyUSB1` (configurable via parameters) - Non-fatal serial open failure — publishes `FIX_NONE` until anchor plugged in - Publishes `/saltybot/uwb_target` at 10 Hz (configurable) - Params: `port_anchor0`, `port_anchor1`, `baud_rate`, `baseline_m`, `publish_rate_hz`, `stale_timeout_s` ### Tests — `test/test_uwb_tracker.py` - **64/64 tests passing** - Frame parsing: valid frames, malformed, STATUS, CR/LF, mm→m conversion, timestamps - Bearing geometry: straight-ahead, ±30°, ±45°, symmetry, confidence degradation, triangle inequality violation - Kalman: seeding, noise smoothing, convergence to constant, rate estimation - `UwbRangingState`: single/dual fix, stale timeout, thread safety (concurrent read/write), baseline stored - `AnchorSerialReader`: mock serial feeding, STATUS ignored, malformed lines no crash, stop() - Integration: full serial→state→bearing pipeline at various angles ## Test plan - [x] 64/64 unit tests pass on dev machine - [ ] Connect first ESP32-UWB-Pro anchor via USB, verify `RANGE` frames appear at ≥10 Hz - [ ] Connect second anchor, verify dual-anchor fix and bearing tracks lateral movement - [ ] Verify `/saltybot/uwb_target` at 10 Hz in RViz with target moving left/right at 5 m - [ ] Occlusion test: block camera but not UWB — confirm bearing still published Closes #365 🤖 Generated with [Claude Code](https://claude.com/claude-code)
sl-perception added 1 commit 2026-03-03 15:25:55 -05:00 
Software-complete implementation of the two-anchor UWB ranging stack.
All ROS2 / serial code written against an abstract interface so tests run
without physical hardware (anchors on order).

New message
- UwbTarget.msg: valid, bearing_deg, distance_m, confidence,
  anchor0/1_dist_m, baseline_m, fix_quality (0=none 1=single 2=dual)

Core library — _uwb_tracker.py (pure Python, no ROS2/runtime deps)
- parse_frame(): ASCII RANGE,<id>,<tag>,<mm> protocol decoder
- bearing_from_ranges(): law-of-cosines 2-anchor bearing with confidence
  (penalises extreme angles + close-range geometry)
- bearing_single_anchor(): fallback bearing=0, conf≤0.3
- BearingKalman: 1-D constant-velocity Kalman filter [bearing, rate]
- UwbRangingState: thread-safe per-anchor state + stale timeout + Kalman
- AnchorSerialReader: background thread, readline() interface (real or mock)

ROS2 node — uwb_node.py
- Opens /dev/ttyUSB0 + /dev/ttyUSB1 (configurable)
- Non-fatal serial open failure (will publish FIX_NONE until plugged in)
- Publishes /saltybot/uwb_target at 10 Hz (configurable)
- Graceful shutdown: stops reader threads

Tests — test/test_uwb_tracker.py: 64/64 passing
- Frame parsing: valid, malformed, STATUS, CR/LF, mm→m conversion
- Bearing geometry: straight-ahead, ±45°, ±30°, symmetry, confidence
- Kalman: seeding, smoothing, convergence, rate tracking
- UwbRangingState: single/dual fix, stale timeout, thread safety
- AnchorSerialReader: mock serial, bytes decode, stop()

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
sl-jetson merged commit 631282b95f into main 2026-03-03 15:41:50 -05:00
Sign in to join this conversation.
Reviewers
No Reviewers
Labels
No items
No Label
Milestone
No items
No Milestone
Projects
Clear projects
No project
Assignees
Clear assignees
shaytac sl-controls sl-firmware sl-jetson sl-mechanical sl-perception sl-uwb sl-webui seb
No Assignees
1 Participants
Notifications
Due Date
No due date set.
Dependencies

No dependencies set.

Reference: seb/saltylab-firmware#368
No description provided.
Delete Branch "sl-perception/issue-365-uwb-tracking"

Deleting a branch is permanent. Although the deleted branch may continue to exist for a short time before it actually gets removed, it CANNOT be undone in most cases. Continue?