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

Merge pull request 'feat(uwb): MaUWB ESP32-S3 DW3000 dual-anchor bearing driver (Issue #90)' (#99) from sl-firmware/uwb-integration into main

Browse Source
This commit is contained in:
sl-jetson 2026-03-01 23:30:12 -05:00
commit 54e9274405
8 changed files with 277 additions and 155 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
jetson/ros2_ws/src/saltybot_uwb/config/uwb_config.yaml
View File

@ -1,57 +1,24 @@
# uwb_config.yaml — MaUWB ESP32-S3 DW3000 UWB follow-me system # uwb_config.yaml — MaUWB ESP32-S3 DW3000 UWB integration (Issue #90)
#
# 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: # Run with:
# ros2 launch saltybot_uwb uwb.launch.py # ros2 launch saltybot_uwb uwb.launch.py
# Override at launch:
# ros2 launch saltybot_uwb uwb.launch.py port_a:=/dev/ttyUSB2
# ── Serial ports ────────────────────────────────────────────────────────────── port_a: /dev/uwb-anchor0
# Set udev rules to get stable symlinks: port_b: /dev/uwb-anchor1
# /dev/uwb-anchor0 → port_a baudrate: 115200
# /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: 0.25
# anchor_separation: centre-to-centre distance between anchors (metres) anchor_height: 0.80
# Must match physical mounting. Larger = more accurate lateral resolution. tag_height: 0.90
anchor_separation: 0.25 # metres (25cm)
# anchor_height: height of anchors above ground (metres) range_timeout_s: 1.0
# Orin stem mount ≈ 0.80m on the saltybot platform max_range_m: 8.0
anchor_height: 0.80 # metres min_range_m: 0.05
# 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_process_noise: 0.1
kf_meas_noise: 0.3
# kf_meas_noise: R scalar — how noisy the UWB ranging is range_rate: 100.0
# DW3000 indoor accuracy ≈ 10cm RMS → 0.1m → R ≈ 0.01 bearing_rate: 10.0
# Use 0.3 to be conservative on first deployment
kf_meas_noise: 0.3
# ── Publish rate ────────────────────────────────────────────────────────────── enrolled_tag_ids: [""]
# 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

18
jetson/ros2_ws/src/saltybot_uwb/launch/uwb.launch.py
View File

@ -1,10 +1,14 @@
""" """
uwb.launch.py — Launch UWB driver node for MaUWB ESP32-S3 follow-me. uwb.launch.py — Launch UWB driver node for MaUWB ESP32-S3 DW3000 (Issue #90).
Topics:
/uwb/ranges 100 Hz raw TWR ranges
/uwb/bearing 10 Hz Kalman-fused bearing
/uwb/target 10 Hz triangulated PoseStamped (backwards compat)
Usage: Usage:
ros2 launch saltybot_uwb uwb.launch.py ros2 launch saltybot_uwb uwb.launch.py
ros2 launch saltybot_uwb uwb.launch.py port_a:=/dev/ttyUSB2 port_b:=/dev/ttyUSB3 ros2 launch saltybot_uwb uwb.launch.py enrolled_tag_ids:="['0xDEADBEEF']"
ros2 launch saltybot_uwb uwb.launch.py anchor_separation:=0.30 publish_rate:=10.0
""" """
import os import os
@ -31,7 +35,9 @@ def generate_launch_description():
DeclareLaunchArgument("min_range_m", default_value="0.05"), DeclareLaunchArgument("min_range_m", default_value="0.05"),
DeclareLaunchArgument("kf_process_noise", default_value="0.1"), DeclareLaunchArgument("kf_process_noise", default_value="0.1"),
DeclareLaunchArgument("kf_meas_noise", default_value="0.3"), DeclareLaunchArgument("kf_meas_noise", default_value="0.3"),
DeclareLaunchArgument("publish_rate", default_value="20.0"), DeclareLaunchArgument("range_rate", default_value="100.0"),
DeclareLaunchArgument("bearing_rate", default_value="10.0"),
DeclareLaunchArgument("enrolled_tag_ids", default_value="['']"),
Node( Node(
package="saltybot_uwb", package="saltybot_uwb",
@ -52,7 +58,9 @@ def generate_launch_description():
"min_range_m": LaunchConfiguration("min_range_m"), "min_range_m": LaunchConfiguration("min_range_m"),
"kf_process_noise": LaunchConfiguration("kf_process_noise"), "kf_process_noise": LaunchConfiguration("kf_process_noise"),
"kf_meas_noise": LaunchConfiguration("kf_meas_noise"), "kf_meas_noise": LaunchConfiguration("kf_meas_noise"),
"publish_rate": LaunchConfiguration("publish_rate"), "range_rate": LaunchConfiguration("range_rate"),
"bearing_rate": LaunchConfiguration("bearing_rate"),
"enrolled_tag_ids": LaunchConfiguration("enrolled_tag_ids"),
}, },
], ],
), ),

20
jetson/ros2_ws/src/saltybot_uwb/saltybot_uwb/ranging_math.py
View File

@ -29,6 +29,26 @@ Returns (x_t, y_t); caller should treat negative x_t as 0.
import math import math
# ── Bearing helper ────────────────────────────────────────────────────────────
def bearing_from_pos(x: float, y: float) -> float:
"""
Compute horizontal bearing to a point (x, y) in base_link.
Parameters
----------
x : forward distance (metres)
y : lateral offset (metres, positive = left of robot / CCW)
Returns
-------
bearing_rad : bearing in radians, range -π .. +π
positive = target to the left (CCW)
0 = directly ahead
"""
return math.atan2(y, x)
# ── Triangulation ───────────────────────────────────────────────────────────── # ── Triangulation ─────────────────────────────────────────────────────────────
def triangulate_2anchor( def triangulate_2anchor(

266
jetson/ros2_ws/src/saltybot_uwb/saltybot_uwb/uwb_driver_node.py
View File

@ -1,32 +1,52 @@
""" """
uwb_driver_node.py — ROS2 node for MaUWB ESP32-S3 DW3000 follow-me system. uwb_driver_node.py — ROS2 node for MaUWB ESP32-S3 DW3000 UWB integration.
Hardware Hardware
──────── ────────
• 2× MaUWB ESP32-S3 DW3000 anchors on robot stem (USB → Orin Nano) • 2× MaUWB ESP32-S3 DW3000 anchors on robot stem (USB → Orin)
- Anchor-0: port side (y = +sep/2) - Anchor-0: port side (y = +sep/2)
- Anchor-1: starboard (y = -sep/2) - Anchor-1: starboard (y = -sep/2)
• 1× MaUWB tag on person (belt clip) • 1× MaUWB tag per enrolled person (belt clip)
AT command interface (115200 8N1) AT command interface (115200 8N1)
────────────────────────────────── ──────────────────────────────────
Query: AT+RANGE?\r\n Query:
Response (from anchors): AT+RANGE?\r\n
+RANGE:<anchor_id>,<range_mm>[,<rssi>]\r\n
Config: Response (from anchors, TWR protocol):
AT+anchor_tag=ANCHOR\r\n — set module as anchor +RANGE:<anchor_id>,<range_mm>[,<rssi>[,<tag_addr>]]\r\n
AT+anchor_tag=TAG\r\n — set module as tag
Tag pairing (optional — targets a specific enrolled tag):
AT+RANGE_ADDR=<tag_addr>\r\n → anchor only ranges with that tag
Publishes Publishes
───────── ─────────
/uwb/target (geometry_msgs/PoseStamped) — triangulated person position in base_link /uwb/ranges (saltybot_uwb_msgs/UwbRangeArray) 100 Hz — raw anchor ranges
/uwb/ranges (saltybot_uwb_msgs/UwbRangeArray) — raw ranges from both anchors /uwb/bearing (saltybot_uwb_msgs/UwbBearing) 10 Hz — Kalman-fused bearing
/uwb/target (geometry_msgs/PoseStamped) 10 Hz — triangulated position
(kept for backwards compat)
Safety Tag pairing
────── ───────────
If a range is stale (> range_timeout_s), that anchor is excluded from Set enrolled_tag_ids to a list of tag address strings (e.g. ["0x1234ABCD"]).
triangulation. If both anchors are stale, /uwb/target is not published. When non-empty, ranges from unrecognised tags are silently discarded.
The matched tag address is stamped in UwbRange.tag_id and UwbBearing.tag_id.
When enrolled_tag_ids is empty, all ranges are accepted (tag_id = "").
Parameters
──────────
port_a, port_b serial ports for anchor-0 / anchor-1
baudrate 115200 (default)
anchor_separation centre-to-centre anchor spacing (m)
anchor_height anchor mounting height (m)
tag_height person tag height (m)
range_timeout_s stale-anchor threshold (s)
max_range_m / min_range_m validity window (m)
kf_process_noise Kalman Q scalar
kf_meas_noise Kalman R scalar
range_rate Hz — /uwb/ranges publish rate (default 100)
bearing_rate Hz — /uwb/bearing publish rate (default 10)
enrolled_tag_ids list[str] — accepted tag addresses; [] = accept all
Usage Usage
───── ─────
@ -44,8 +64,8 @@ from rclpy.node import Node
from geometry_msgs.msg import PoseStamped from geometry_msgs.msg import PoseStamped
from std_msgs.msg import Header from std_msgs.msg import Header
from saltybot_uwb_msgs.msg import UwbRange, UwbRangeArray from saltybot_uwb_msgs.msg import UwbRange, UwbRangeArray, UwbBearing
from saltybot_uwb.ranging_math import triangulate_2anchor, KalmanFilter2D from saltybot_uwb.ranging_math import triangulate_2anchor, KalmanFilter2D, bearing_from_pos
try: try:
import serial import serial
@ -54,26 +74,31 @@ except ImportError:
_SERIAL_AVAILABLE = False _SERIAL_AVAILABLE = False
# Regex: +RANGE:<id>,<mm> or +RANGE:<id>,<mm>,<rssi> # +RANGE:<id>,<mm> or +RANGE:<id>,<mm>,<rssi> or +RANGE:<id>,<mm>,<rssi>,<tag>
_RANGE_RE = re.compile( _RANGE_RE = re.compile(
r"\+RANGE\s*:\s*(\d+)\s*,\s*(\d+)(?:\s*,\s*(-?[\d.]+))?", r"\+RANGE\s*:\s*(\d+)\s*,\s*(\d+)"
r"(?:\s*,\s*(-?[\d.]+)"
r"(?:\s*,\s*([\w:x]+))?"
r")?",
re.IGNORECASE, re.IGNORECASE,
) )
class SerialReader(threading.Thread): class SerialReader(threading.Thread):
""" """
Background thread: polls an anchor's UART, fires callback on every Background thread: polls one anchor's UART at maximum TWR rate,
valid +RANGE response. fires callback on every valid +RANGE response.
Supports optional tag pairing via AT+RANGE_ADDR command.
""" """
def __init__(self, port, baudrate, anchor_id, callback, logger): def __init__(self, port, baudrate, anchor_id, callback, logger, tag_addr=None):
super().__init__(daemon=True) super().__init__(daemon=True)
self._port = port self._port = port
self._baudrate = baudrate self._baudrate = baudrate
self._anchor_id = anchor_id self._anchor_id = anchor_id
self._callback = callback self._callback = callback
self._logger = logger self._logger = logger
self._tag_addr = tag_addr
self._running = False self._running = False
self._ser = None self._ser = None
@ -86,7 +111,10 @@ class SerialReader(threading.Thread):
) )
self._logger.info( self._logger.info(
f"Anchor-{self._anchor_id}: opened {self._port}" f"Anchor-{self._anchor_id}: opened {self._port}"
+ (f" paired with tag {self._tag_addr}" if self._tag_addr else "")
) )
if self._tag_addr:
self._send_pairing_cmd()
self._read_loop() self._read_loop()
except Exception as exc: except Exception as exc:
self._logger.warn( self._logger.warn(
@ -96,12 +124,24 @@ class SerialReader(threading.Thread):
self._ser.close() self._ser.close()
time.sleep(2.0) time.sleep(2.0)
def _send_pairing_cmd(self):
"""Configure the anchor to range only with the paired tag."""
try:
cmd = f"AT+RANGE_ADDR={self._tag_addr}\r\n".encode("ascii")
self._ser.write(cmd)
time.sleep(0.1)
self._logger.info(
f"Anchor-{self._anchor_id}: sent tag pairing {self._tag_addr}"
)
except Exception as exc:
self._logger.warn(
f"Anchor-{self._anchor_id}: pairing cmd failed: {exc}"
)
def _read_loop(self): def _read_loop(self):
while self._running: while self._running:
try: try:
# Query the anchor
self._ser.write(b"AT+RANGE?\r\n") self._ser.write(b"AT+RANGE?\r\n")
# Read up to 10 lines waiting for a +RANGE response
for _ in range(10): for _ in range(10):
raw = self._ser.readline() raw = self._ser.readline()
if not raw: if not raw:
@ -111,13 +151,14 @@ class SerialReader(threading.Thread):
if m: if m:
range_mm = int(m.group(2)) range_mm = int(m.group(2))
rssi = float(m.group(3)) if m.group(3) else 0.0 rssi = float(m.group(3)) if m.group(3) else 0.0
self._callback(self._anchor_id, range_mm, rssi) tag_addr = m.group(4) if m.group(4) else ""
self._callback(self._anchor_id, range_mm, rssi, tag_addr)
break break
except Exception as exc: except Exception as exc:
self._logger.warn( self._logger.warn(
f"Anchor-{self._anchor_id} read error: {exc}" f"Anchor-{self._anchor_id} read error: {exc}"
) )
break # trigger reconnect break
def stop(self): def stop(self):
self._running = False self._running = False
@ -130,48 +171,51 @@ class UwbDriverNode(Node):
def __init__(self): def __init__(self):
super().__init__("uwb_driver") super().__init__("uwb_driver")
# ── Parameters ──────────────────────────────────────────────────────── self.declare_parameter("port_a", "/dev/uwb-anchor0")
self.declare_parameter("port_a", "/dev/ttyUSB0") self.declare_parameter("port_b", "/dev/uwb-anchor1")
self.declare_parameter("port_b", "/dev/ttyUSB1") self.declare_parameter("baudrate", 115200)
self.declare_parameter("baudrate", 115200) self.declare_parameter("anchor_separation", 0.25)
self.declare_parameter("anchor_separation", 0.25) self.declare_parameter("anchor_height", 0.80)
self.declare_parameter("anchor_height", 0.80) self.declare_parameter("tag_height", 0.90)
self.declare_parameter("tag_height", 0.90) self.declare_parameter("range_timeout_s", 1.0)
self.declare_parameter("range_timeout_s", 1.0) self.declare_parameter("max_range_m", 8.0)
self.declare_parameter("max_range_m", 8.0) self.declare_parameter("min_range_m", 0.05)
self.declare_parameter("min_range_m", 0.05) self.declare_parameter("kf_process_noise", 0.1)
self.declare_parameter("kf_process_noise", 0.1) self.declare_parameter("kf_meas_noise", 0.3)
self.declare_parameter("kf_meas_noise", 0.3) self.declare_parameter("range_rate", 100.0)
self.declare_parameter("publish_rate", 20.0) self.declare_parameter("bearing_rate", 10.0)
self.declare_parameter("enrolled_tag_ids", [""])
self._p = self._load_params() self._p = self._load_params()
# ── State (protected by lock) ────────────────────────────────────── raw_ids = self.get_parameter("enrolled_tag_ids").value
self._lock = threading.Lock() self._enrolled_tags = [t.strip() for t in raw_ids if t.strip()]
self._ranges = {} # anchor_id → (range_m, rssi, timestamp) paired_tag = self._enrolled_tags[0] if self._enrolled_tags else None
self._kf = KalmanFilter2D(
self._lock = threading.Lock()
self._ranges: dict = {}
self._kf = KalmanFilter2D(
process_noise=self._p["kf_process_noise"], process_noise=self._p["kf_process_noise"],
measurement_noise=self._p["kf_meas_noise"], measurement_noise=self._p["kf_meas_noise"],
dt=1.0 / self._p["publish_rate"], dt=1.0 / self._p["bearing_rate"],
) )
# ── Publishers ──────────────────────────────────────────────────── self._ranges_pub = self.create_publisher(UwbRangeArray, "/uwb/ranges", 10)
self._target_pub = self.create_publisher( self._bearing_pub = self.create_publisher(UwbBearing, "/uwb/bearing", 10)
PoseStamped, "/uwb/target", 10) self._target_pub = self.create_publisher(PoseStamped, "/uwb/target", 10)
self._ranges_pub = self.create_publisher(
UwbRangeArray, "/uwb/ranges", 10)
# ── Serial readers ────────────────────────────────────────────────
if _SERIAL_AVAILABLE: if _SERIAL_AVAILABLE:
self._reader_a = SerialReader( self._reader_a = SerialReader(
self._p["port_a"], self._p["baudrate"], self._p["port_a"], self._p["baudrate"],
anchor_id=0, callback=self._range_cb, anchor_id=0, callback=self._range_cb,
logger=self.get_logger(), logger=self.get_logger(),
tag_addr=paired_tag,
) )
self._reader_b = SerialReader( self._reader_b = SerialReader(
self._p["port_b"], self._p["baudrate"], self._p["port_b"], self._p["baudrate"],
anchor_id=1, callback=self._range_cb, anchor_id=1, callback=self._range_cb,
logger=self.get_logger(), logger=self.get_logger(),
tag_addr=paired_tag,
) )
self._reader_a.start() self._reader_a.start()
self._reader_b.start() self._reader_b.start()
@ -180,19 +224,21 @@ class UwbDriverNode(Node):
"pyserial not installed — running in simulation mode (no serial I/O)" "pyserial not installed — running in simulation mode (no serial I/O)"
) )
# ── Publish timer ───────────────────────────────────────────────── self._range_timer = self.create_timer(
self._timer = self.create_timer( 1.0 / self._p["range_rate"], self._range_publish_cb
1.0 / self._p["publish_rate"], self._publish_cb )
self._bearing_timer = self.create_timer(
1.0 / self._p["bearing_rate"], self._bearing_publish_cb
) )
self.get_logger().info( self.get_logger().info(
f"UWB driver ready sep={self._p['anchor_separation']}m " f"UWB driver ready sep={self._p['anchor_separation']}m "
f"ports={self._p['port_a']},{self._p['port_b']} " f"ports={self._p['port_a']},{self._p['port_b']} "
f"rate={self._p['publish_rate']}Hz" f"range={self._p['range_rate']}Hz "
f"bearing={self._p['bearing_rate']}Hz "
f"enrolled_tags={self._enrolled_tags or ['<any>']}"
) )
# ── Helpers ───────────────────────────────────────────────────────────────
def _load_params(self): def _load_params(self):
return { return {
"port_a": self.get_parameter("port_a").value, "port_a": self.get_parameter("port_a").value,
@ -206,90 +252,106 @@ class UwbDriverNode(Node):
"min_range_m": self.get_parameter("min_range_m").value, "min_range_m": self.get_parameter("min_range_m").value,
"kf_process_noise": self.get_parameter("kf_process_noise").value, "kf_process_noise": self.get_parameter("kf_process_noise").value,
"kf_meas_noise": self.get_parameter("kf_meas_noise").value, "kf_meas_noise": self.get_parameter("kf_meas_noise").value,
"publish_rate": self.get_parameter("publish_rate").value, "range_rate": self.get_parameter("range_rate").value,
"bearing_rate": self.get_parameter("bearing_rate").value,
} }
# ── Callbacks ───────────────────────────────────────────────────────────── def _is_enrolled(self, tag_addr: str) -> bool:
if not self._enrolled_tags:
return True
return tag_addr in self._enrolled_tags
def _range_cb(self, anchor_id: int, range_mm: int, rssi: float): def _range_cb(self, anchor_id: int, range_mm: int, rssi: float, tag_addr: str):
"""Called from serial reader threads — thread-safe update.""" if not self._is_enrolled(tag_addr):
return
range_m = range_mm / 1000.0 range_m = range_mm / 1000.0
p = self._p p = self._p
if range_m < p["min_range_m"] or range_m > p["max_range_m"]: if range_m < p["min_range_m"] or range_m > p["max_range_m"]:
return return
with self._lock: with self._lock:
self._ranges[anchor_id] = (range_m, rssi, time.monotonic()) self._ranges[anchor_id] = (range_m, rssi, tag_addr, time.monotonic())
def _publish_cb(self): def _range_publish_cb(self):
now = time.monotonic() """100 Hz: publish current raw ranges as UwbRangeArray."""
now = time.monotonic()
timeout = self._p["range_timeout_s"] timeout = self._p["range_timeout_s"]
sep = self._p["anchor_separation"]
with self._lock: with self._lock:
# Collect valid (non-stale) ranges valid = {
valid = {} aid: entry
for aid, (r, rssi, t) in self._ranges.items(): for aid, entry in self._ranges.items()
if now - t <= timeout: if (now - entry[3]) <= timeout
valid[aid] = (r, rssi, t) }
# Build and publish UwbRangeArray regardless (even if partial)
hdr = Header() hdr = Header()
hdr.stamp = self.get_clock().now().to_msg() hdr.stamp = self.get_clock().now().to_msg()
hdr.frame_id = "base_link" hdr.frame_id = "base_link"
arr = UwbRangeArray() arr = UwbRangeArray()
arr.header = hdr arr.header = hdr
for aid, (r, rssi, _) in valid.items(): for aid, (r, rssi, tag_id, _) in valid.items():
entry = UwbRange() entry = UwbRange()
entry.header = hdr entry.header = hdr
entry.anchor_id = aid entry.anchor_id = aid
entry.range_m = float(r) entry.range_m = float(r)
entry.raw_mm = int(round(r * 1000.0)) entry.raw_mm = int(round(r * 1000.0))
entry.rssi = float(rssi) entry.rssi = float(rssi)
entry.tag_id = tag_id
arr.ranges.append(entry) arr.ranges.append(entry)
self._ranges_pub.publish(arr) self._ranges_pub.publish(arr)
# Need both anchors to triangulate def _bearing_publish_cb(self):
if 0 not in valid or 1 not in valid: """10 Hz: Kalman predict+update, publish fused bearing."""
now = time.monotonic()
timeout = self._p["range_timeout_s"]
sep = self._p["anchor_separation"]
with self._lock:
valid = {
aid: entry
for aid, entry in self._ranges.items()
if (now - entry[3]) <= timeout
}
if not valid:
return return
both_fresh = 0 in valid and 1 in valid
r0 = valid[0][0] confidence = 1.0 if both_fresh else 0.5
r1 = valid[1][0] active_tag = valid[next(iter(valid))][2]
dt = 1.0 / self._p["bearing_rate"]
try:
x_t, y_t = triangulate_2anchor(
r0=r0,
r1=r1,
sep=sep,
anchor_z=self._p["anchor_height"],
tag_z=self._p["tag_height"],
)
except (ValueError, ZeroDivisionError) as exc:
self.get_logger().warn(f"Triangulation error: {exc}")
return
# Kalman filter update
dt = 1.0 / self._p["publish_rate"]
self._kf.predict(dt=dt) self._kf.predict(dt=dt)
self._kf.update(x_t, y_t) if both_fresh:
r0 = valid[0][0]
r1 = valid[1][0]
try:
x_t, y_t = triangulate_2anchor(
r0=r0, r1=r1, sep=sep,
anchor_z=self._p["anchor_height"],
tag_z=self._p["tag_height"],
)
except (ValueError, ZeroDivisionError) as exc:
self.get_logger().warn(f"Triangulation error: {exc}")
return
self._kf.update(x_t, y_t)
kx, ky = self._kf.position() kx, ky = self._kf.position()
bearing = bearing_from_pos(kx, ky)
# Publish PoseStamped in base_link range_m = math.sqrt(kx * kx + ky * ky)
hdr = Header()
hdr.stamp = self.get_clock().now().to_msg()
hdr.frame_id = "base_link"
brg_msg = UwbBearing()
brg_msg.header = hdr
brg_msg.bearing_rad = float(bearing)
brg_msg.range_m = float(range_m)
brg_msg.confidence = float(confidence)
brg_msg.tag_id = active_tag
self._bearing_pub.publish(brg_msg)
pose = PoseStamped() pose = PoseStamped()
pose.header = hdr pose.header = hdr
pose.pose.position.x = kx pose.pose.position.x = kx
pose.pose.position.y = ky pose.pose.position.y = ky
pose.pose.position.z = 0.0 pose.pose.position.z = 0.0
# Orientation: face the person (yaw = atan2(y, x)) yaw = bearing
yaw = math.atan2(ky, kx)
pose.pose.orientation.z = math.sin(yaw / 2.0) pose.pose.orientation.z = math.sin(yaw / 2.0)
pose.pose.orientation.w = math.cos(yaw / 2.0) pose.pose.orientation.w = math.cos(yaw / 2.0)
self._target_pub.publish(pose) self._target_pub.publish(pose)
# ── Entry point ───────────────────────────────────────────────────────────────
def main(args=None): def main(args=None):
rclpy.init(args=args) rclpy.init(args=args)
node = UwbDriverNode() node = UwbDriverNode()

46
jetson/ros2_ws/src/saltybot_uwb/test/test_ranging_math.py
View File

@ -7,7 +7,7 @@ No ROS2 / serial / GPU dependencies — runs with plain pytest.
import math import math
import pytest import pytest
from saltybot_uwb.ranging_math import triangulate_2anchor, KalmanFilter2D from saltybot_uwb.ranging_math import triangulate_2anchor, KalmanFilter2D, bearing_from_pos
# ── triangulate_2anchor ─────────────────────────────────────────────────────── # ── triangulate_2anchor ───────────────────────────────────────────────────────
@ -172,3 +172,47 @@ class TestKalmanFilter2D:
x, y = kf.position() x, y = kf.position()
assert not math.isnan(x) assert not math.isnan(x)
assert not math.isnan(y) assert not math.isnan(y)
# ── bearing_from_pos ──────────────────────────────────────────────────────────
class TestBearingFromPos:
def test_directly_ahead_zero_bearing(self):
"""Person directly ahead: x=2, y=0 → bearing ≈ 0."""
b = bearing_from_pos(2.0, 0.0)
assert abs(b) < 0.001
def test_left_gives_positive_bearing(self):
"""Person to the left (y>0): bearing should be positive."""
b = bearing_from_pos(1.0, 1.0)
assert b > 0.0
assert abs(b - math.pi / 4.0) < 0.001
def test_right_gives_negative_bearing(self):
"""Person to the right (y<0): bearing should be negative."""
b = bearing_from_pos(1.0, -1.0)
assert b < 0.0
assert abs(b + math.pi / 4.0) < 0.001
def test_directly_left_ninety_degrees(self):
"""Person directly to the left: x=0, y=1 → bearing = π/2."""
b = bearing_from_pos(0.0, 1.0)
assert abs(b - math.pi / 2.0) < 0.001
def test_directly_right_minus_ninety_degrees(self):
"""Person directly to the right: x=0, y=-1 → bearing = -π/2."""
b = bearing_from_pos(0.0, -1.0)
assert abs(b + math.pi / 2.0) < 0.001
def test_range_pi_to_minus_pi(self):
"""Bearing is always in -π..+π."""
for x in [-2.0, -0.1, 0.1, 2.0]:
for y in [-2.0, -0.1, 0.1, 2.0]:
b = bearing_from_pos(x, y)
assert -math.pi <= b <= math.pi
def test_no_nan_for_tiny_distance(self):
"""Very close target should not produce NaN."""
b = bearing_from_pos(0.001, 0.001)
assert not math.isnan(b)

1
jetson/ros2_ws/src/saltybot_uwb_msgs/CMakeLists.txt
View File

@ -8,6 +8,7 @@ find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME} rosidl_generate_interfaces(${PROJECT_NAME}
"msg/UwbRange.msg" "msg/UwbRange.msg"
"msg/UwbRangeArray.msg" "msg/UwbRangeArray.msg"
"msg/UwbBearing.msg"
DEPENDENCIES std_msgs DEPENDENCIES std_msgs
) )

18
jetson/ros2_ws/src/saltybot_uwb_msgs/msg/UwbBearing.msg Normal file
View File

@ -0,0 +1,18 @@
# UwbBearing.msg — 10 Hz Kalman-fused bearing estimate from dual-anchor UWB.
#
# bearing_rad : horizontal bearing to tag in base_link frame (radians)
# positive = tag to the left (CCW), negative = right (CW)
# range: -π .. +π
# range_m : Kalman-filtered horizontal range to tag (metres)
# confidence : quality indicator
# 1.0 = both anchors fresh
# 0.5 = single anchor only (bearing less reliable)
# 0.0 = stale / no data (message not published in this state)
# tag_id : enrolled tag identifier that produced this estimate
std_msgs/Header header
float32 bearing_rad
float32 range_m
float32 confidence
string tag_id

2
jetson/ros2_ws/src/saltybot_uwb_msgs/msg/UwbRange.msg
View File

@ -4,6 +4,7 @@
# range_m : measured horizontal range in metres (after height correction) # range_m : measured horizontal range in metres (after height correction)
# raw_mm : raw TWR range from AT+RANGE? response, millimetres # raw_mm : raw TWR range from AT+RANGE? response, millimetres
# rssi : received signal strength (dBm), 0 if not reported by module # rssi : received signal strength (dBm), 0 if not reported by module
# tag_id : enrolled tag identifier; empty string if tag pairing is disabled
std_msgs/Header header std_msgs/Header header
@ -11,3 +12,4 @@ uint8 anchor_id
float32 range_m float32 range_m
uint32 raw_mm uint32 raw_mm
float32 rssi float32 rssi
string tag_id