sl-jetson
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>
58 lines
3.1 KiB
YAML
58 lines
3.1 KiB
YAML
# uwb_config.yaml — MaUWB ESP32-S3 DW3000 UWB follow-me system
|
|
#
|
|
# Hardware layout:
|
|
# Anchor-0 (port side) → USB port_a, y = +anchor_separation/2
|
|
# Anchor-1 (starboard side) → USB port_b, y = -anchor_separation/2
|
|
# Tag on person → belt clip, ~0.9m above ground
|
|
#
|
|
# Run with:
|
|
# ros2 launch saltybot_uwb uwb.launch.py
|
|
# Override at launch:
|
|
# ros2 launch saltybot_uwb uwb.launch.py port_a:=/dev/ttyUSB2
|
|
|
|
# ── Serial ports ──────────────────────────────────────────────────────────────
|
|
# Set udev rules to get stable symlinks:
|
|
# /dev/uwb-anchor0 → port_a
|
|
# /dev/uwb-anchor1 → port_b
|
|
# (See jetson/docs/pinout.md for udev setup)
|
|
port_a: /dev/uwb-anchor0 # Anchor-0 (port)
|
|
port_b: /dev/uwb-anchor1 # Anchor-1 (starboard)
|
|
baudrate: 115200 # MaUWB default — do not change
|
|
|
|
# ── Anchor geometry ────────────────────────────────────────────────────────────
|
|
# anchor_separation: centre-to-centre distance between anchors (metres)
|
|
# Must match physical mounting. Larger = more accurate lateral resolution.
|
|
anchor_separation: 0.25 # metres (25cm)
|
|
|
|
# anchor_height: height of anchors above ground (metres)
|
|
# Orin stem mount ≈ 0.80m on the saltybot platform
|
|
anchor_height: 0.80 # metres
|
|
|
|
# tag_height: height of person's belt-clip tag above ground (metres)
|
|
tag_height: 0.90 # metres (adjust per user)
|
|
|
|
# ── Range validity ─────────────────────────────────────────────────────────────
|
|
# range_timeout_s: stale anchor — excluded from triangulation after this gap
|
|
range_timeout_s: 1.0 # seconds
|
|
|
|
# max_range_m: discard ranges beyond this (DW3000 indoor practical limit ≈8m)
|
|
max_range_m: 8.0 # metres
|
|
|
|
# min_range_m: discard ranges below this (likely multipath artefacts)
|
|
min_range_m: 0.05 # metres
|
|
|
|
# ── Kalman filter ──────────────────────────────────────────────────────────────
|
|
# kf_process_noise: Q scalar — how dynamic the person's motion is
|
|
# Higher → faster response, more jitter
|
|
kf_process_noise: 0.1
|
|
|
|
# kf_meas_noise: R scalar — how noisy the UWB ranging is
|
|
# DW3000 indoor accuracy ≈ 10cm RMS → 0.1m → R ≈ 0.01
|
|
# Use 0.3 to be conservative on first deployment
|
|
kf_meas_noise: 0.3
|
|
|
|
# ── Publish rate ──────────────────────────────────────────────────────────────
|
|
# Should match or exceed the AT+RANGE? poll rate from both anchors.
|
|
# 20Hz means 50ms per cycle; each anchor query takes ~10ms → headroom ok.
|
|
publish_rate: 20.0 # Hz
|