From 2180b6144012a72b4c30b6b1f0d596c3a21af3a1 Mon Sep 17 00:00:00 2001 From: sl-perception Date: Sat, 14 Mar 2026 11:43:10 -0400 Subject: [PATCH] feat: ROS2 UWB position node (Issue #546) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Add saltybot_uwb_position — ROS2 Python package that reads JSON range measurements from an ESP32 DW3000 UWB tag over USB serial, trilaterates the robot's absolute position from 3+ fixed infrastructure anchors, and publishes position + TF2 to the rest of the stack. Serial protocol (one JSON line per frame): Full frame: {"ts":…, "ranges": [{"id":0,"d_mm":1500,"rssi":-65}, …]} Per-anchor: {"id":0, "d_mm":1500, "rssi":-65.0} Accepts both "d_mm" and "range_mm" field names. Trilateration (trilateration.py, numpy, no ROS deps): Linear least-squares: linearise sphere equations around anchor 0, solve (N-1)x2 (2D) or (N-1)x3 (3D) system via np.linalg.lstsq. 2D mode (default): robot_z fixed, needs >=3 anchors. 3D mode (solve_z=true): full 3D, needs >=4 anchors. Outlier rejection: After initial solve, compute per-anchor residual |r_meas - r_pred|. Reject anchors with residual > outlier_threshold_m (0.4 m default). Re-solve with inliers if >= min_anchors remain. Track consecutive outlier strikes; flag in /status after N strikes. Kalman filter (KalmanFilter3D, constant-velocity, 6-state, numpy): Predict-only coasting when anchors drop below minimum. Q=0.05, R=0.10 (tunable). Topics: /saltybot/uwb/pose PoseStamped 10 Hz Kalman-filtered position /saltybot/uwb/range/ UwbRange on arrival, raw per-anchor ranges /saltybot/uwb/status String/JSON 10 Hz state+residuals+flags TF2: uwb_link -> map (identity rotation) Anchor config: flat float arrays in YAML. Default layout: 4-anchor 5x5m room at 2m height. Co-Authored-By: Claude Sonnet 4.6 --- .../config/uwb_position_params.yaml | 58 +++ .../launch/uwb_position.launch.py | 35 ++ .../src/saltybot_uwb_position/package.xml | 36 ++ .../resource/saltybot_uwb_position | 0 .../saltybot_uwb_position/__init__.py | 0 .../saltybot_uwb_position/trilateration.py | 254 +++++++++ .../uwb_position_node.py | 487 ++++++++++++++++++ .../src/saltybot_uwb_position/setup.cfg | 5 + .../src/saltybot_uwb_position/setup.py | 32 ++ 9 files changed, 907 insertions(+) create mode 100644 jetson/ros2_ws/src/saltybot_uwb_position/config/uwb_position_params.yaml create mode 100644 jetson/ros2_ws/src/saltybot_uwb_position/launch/uwb_position.launch.py create mode 100644 jetson/ros2_ws/src/saltybot_uwb_position/package.xml create mode 100644 jetson/ros2_ws/src/saltybot_uwb_position/resource/saltybot_uwb_position create mode 100644 jetson/ros2_ws/src/saltybot_uwb_position/saltybot_uwb_position/__init__.py create mode 100644 jetson/ros2_ws/src/saltybot_uwb_position/saltybot_uwb_position/trilateration.py create mode 100644 jetson/ros2_ws/src/saltybot_uwb_position/saltybot_uwb_position/uwb_position_node.py create mode 100644 jetson/ros2_ws/src/saltybot_uwb_position/setup.cfg create mode 100644 jetson/ros2_ws/src/saltybot_uwb_position/setup.py diff --git a/jetson/ros2_ws/src/saltybot_uwb_position/config/uwb_position_params.yaml b/jetson/ros2_ws/src/saltybot_uwb_position/config/uwb_position_params.yaml new file mode 100644 index 0000000..e728726 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_position/config/uwb_position_params.yaml @@ -0,0 +1,58 @@ +# uwb_position_params.yaml — UWB position node configuration (Issue #546) +# +# ROS2 Python node: saltybot_uwb_position +# Serial: ESP32 UWB tag → Jetson via USB-CDC (JSON per line) +# +# Usage: +# ros2 launch saltybot_uwb_position uwb_position.launch.py +# +# JSON protocol (ESP32 → Jetson): +# Full frame (preferred): +# {"ts": 123456, "ranges": [{"id": 0, "d_mm": 1500, "rssi": -65.0}, ...]} +# Per-anchor: +# {"id": 0, "d_mm": 1500, "rssi": -65.0} +# Both "d_mm" and "range_mm" accepted for the range field. + +uwb_position: + ros__parameters: + + # ── Serial (USB-CDC from ESP32 DW3000 tag) ────────────────────────────── + serial_port: /dev/ttyUSB0 # udev rule: /dev/ttyUWB_TAG recommended + baudrate: 115200 + + # ── Anchor definitions ────────────────────────────────────────────────── + # anchor_ids: integer list of anchor IDs + # anchor_positions: flat float list, 3 values per anchor [x, y, z] in + # the map frame (metres). Length must be 3 × len(anchor_ids). + # + # Example: 4-anchor rectangular room layout, anchors at 2 m height + # Corner NW (0,0), NE (5,0), SE (5,5), SW (0,5) + anchor_ids: [0, 1, 2, 3] + anchor_positions: [ + 0.0, 0.0, 2.0, # anchor 0 — NW corner + 5.0, 0.0, 2.0, # anchor 1 — NE corner + 5.0, 5.0, 2.0, # anchor 2 — SE corner + 0.0, 5.0, 2.0, # anchor 3 — SW corner + ] + + # ── Trilateration mode ────────────────────────────────────────────────── + solve_z: false # false = 2D (robot_z fixed), true = full 3D (needs 4+ anchors) + robot_z: 0.0 # m — robot floor height in map frame (used when solve_z=false) + + # ── Validity & timing ─────────────────────────────────────────────────── + publish_rate: 10.0 # Hz — /saltybot/uwb/pose + /saltybot/uwb/status + range_timeout_s: 1.5 # s — discard anchor range after this age + min_range_m: 0.05 # m — minimum valid range + max_range_m: 30.0 # m — maximum valid range (DW3000: up to 120 m, use conservative) + + # ── Outlier rejection ─────────────────────────────────────────────────── + outlier_threshold_m: 0.40 # m — residual above this → outlier + outlier_strikes_max: 5 # consecutive outlier frames before anchor is flagged + + # ── Kalman filter ─────────────────────────────────────────────────────── + kf_process_noise: 0.05 # Q — lower = smoother but slower to respond + kf_meas_noise: 0.10 # R — higher = trust KF prediction more than measurement + + # ── TF2 frames ────────────────────────────────────────────────────────── + map_frame: map # parent frame + uwb_frame: uwb_link # child frame (robot UWB tag position) diff --git a/jetson/ros2_ws/src/saltybot_uwb_position/launch/uwb_position.launch.py b/jetson/ros2_ws/src/saltybot_uwb_position/launch/uwb_position.launch.py new file mode 100644 index 0000000..348b410 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_position/launch/uwb_position.launch.py @@ -0,0 +1,35 @@ +"""Launch file for saltybot_uwb_position (Issue #546).""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description() -> LaunchDescription: + config_path = os.path.join( + get_package_share_directory("saltybot_uwb_position"), + "config", + "uwb_position_params.yaml", + ) + + port_arg = DeclareLaunchArgument( + "serial_port", + default_value="/dev/ttyUSB0", + description="USB serial port for ESP32 UWB tag (e.g. /dev/ttyUWB_TAG)", + ) + + uwb_node = Node( + package="saltybot_uwb_position", + executable="uwb_position", + name="uwb_position", + parameters=[ + config_path, + {"serial_port": LaunchConfiguration("serial_port")}, + ], + output="screen", + ) + + return LaunchDescription([port_arg, uwb_node]) diff --git a/jetson/ros2_ws/src/saltybot_uwb_position/package.xml b/jetson/ros2_ws/src/saltybot_uwb_position/package.xml new file mode 100644 index 0000000..0c1b271 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_position/package.xml @@ -0,0 +1,36 @@ + + + + saltybot_uwb_position + 0.1.0 + + ROS2 UWB position node for Jetson (Issue #546). + Reads JSON range data from an ESP32 DW3000 UWB tag over USB serial, + trilaterates from 3+ fixed infrastructure anchors, publishes + PoseStamped on /saltybot/uwb/pose, per-anchor UwbRange on + /saltybot/uwb/range/<id>, JSON diagnostics on /saltybot/uwb/status, + and broadcasts the uwb_link→map TF2 transform. + + sl-perception + MIT + + ament_python + + rclpy + geometry_msgs + std_msgs + tf2_ros + saltybot_uwb_msgs + + + python3-numpy + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_uwb_position/resource/saltybot_uwb_position b/jetson/ros2_ws/src/saltybot_uwb_position/resource/saltybot_uwb_position new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_uwb_position/saltybot_uwb_position/__init__.py b/jetson/ros2_ws/src/saltybot_uwb_position/saltybot_uwb_position/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_uwb_position/saltybot_uwb_position/trilateration.py b/jetson/ros2_ws/src/saltybot_uwb_position/saltybot_uwb_position/trilateration.py new file mode 100644 index 0000000..88ce544 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_position/saltybot_uwb_position/trilateration.py @@ -0,0 +1,254 @@ +""" +trilateration.py — Pure-math helpers for UWB trilateration (Issue #546). + +No ROS2 dependencies — importable in unit tests without a ROS2 install. + +Algorithm: linear least-squares from N ≥ 3 anchors +──────────────────────────────────────────────────── +Given anchors A_i = (x_i, y_i, z_i) with measured ranges r_i, the robot +position p = (x, y, z) satisfies: + + (x - x_i)² + (y - y_i)² + (z - z_i)² = r_i² + +Subtract the equation for anchor 0 from each subsequent equation to +linearise: + + 2(x_i - x_0)x + 2(y_i - y_0)y + 2(z_i - z_0)z + = r_0² - r_i² + ‖a_i‖² - ‖a_0‖² + +This yields A·p = b where A is (N-1)×3 and b is (N-1)×1. +Solve with numpy least-squares (lstsq). + +2D mode (solve_z=False): + z is fixed (robot_z parameter, default 0). + Reduce to 2 unknowns (x, y): subtract z terms from b, drop the z column. + Needs N ≥ 3 anchors. + +Outlier rejection: + Compute residual for each anchor: |r_meas - ‖p - a_i‖|. + Reject anchors with residual > threshold. Repeat if enough remain. +""" + +from __future__ import annotations + +import math +from typing import List, Optional, Tuple + +import numpy as np + + +AnchorPos = Tuple[float, float, float] # (x, y, z) in map frame (metres) +RangeM = float # measured range (metres) + + +# ── Core trilateration ──────────────────────────────────────────────────────── + +def trilaterate( + anchors: List[AnchorPos], + ranges: List[RangeM], + fixed_z: Optional[float] = None, +) -> Tuple[float, float, float]: + """Least-squares trilateration from N ≥ 3 anchor ranges. + + Parameters + ---------- + anchors : list of (x, y, z) anchor positions in the map frame (metres) + ranges : measured distances from robot to each anchor (metres) + fixed_z : if not None, treat robot z as this value and solve 2D only + + Returns + ------- + (x, y, z) : estimated robot position in the map frame (metres). + When fixed_z is given, z = fixed_z. + + Raises + ------ + ValueError : fewer than 3 anchors provided + RuntimeError: linear system is rank-deficient (e.g., collinear anchors) + """ + n = len(anchors) + if n < 3: + raise ValueError(f"Need ≥ 3 anchors for trilateration, got {n}") + if len(ranges) != n: + raise ValueError("len(anchors) != len(ranges)") + + a = np.array(anchors, dtype=np.float64) # (N, 3) + r = np.array(ranges, dtype=np.float64) # (N,) + + # Reference anchor (index 0) + a0 = a[0] + r0 = r[0] + norm_a0_sq = float(np.dot(a0, a0)) + + if fixed_z is not None: + # 2D mode: solve for (x, y), z is known + z = fixed_z + A_rows = [] + b_rows = [] + for i in range(1, n): + ai = a[i] + ri = r[i] + norm_ai_sq = float(np.dot(ai, ai)) + # rhs adjusted for known z + rhs = (r0**2 - ri**2 + norm_ai_sq - norm_a0_sq + + 2.0 * (ai[2] - a0[2]) * z) + A_rows.append([2.0 * (ai[0] - a0[0]), + 2.0 * (ai[1] - a0[1])]) + b_rows.append(rhs) + + A_mat = np.array(A_rows, dtype=np.float64) + b_vec = np.array(b_rows, dtype=np.float64) + result, _, rank, _ = np.linalg.lstsq(A_mat, b_vec, rcond=None) + if rank < 2: + raise RuntimeError("Rank-deficient 2D trilateration system — collinear anchors?") + return float(result[0]), float(result[1]), z + + else: + # 3D mode: solve for (x, y, z) — needs N ≥ 4 for well-conditioned solve + A_rows = [] + b_rows = [] + for i in range(1, n): + ai = a[i] + ri = r[i] + norm_ai_sq = float(np.dot(ai, ai)) + rhs = r0**2 - ri**2 + norm_ai_sq - norm_a0_sq + A_rows.append([2.0 * (ai[0] - a0[0]), + 2.0 * (ai[1] - a0[1]), + 2.0 * (ai[2] - a0[2])]) + b_rows.append(rhs) + + A_mat = np.array(A_rows, dtype=np.float64) + b_vec = np.array(b_rows, dtype=np.float64) + result, _, rank, _ = np.linalg.lstsq(A_mat, b_vec, rcond=None) + if rank < 3: + raise RuntimeError("Rank-deficient 3D trilateration system — coplanar anchors?") + return float(result[0]), float(result[1]), float(result[2]) + + +# ── Outlier rejection ───────────────────────────────────────────────────────── + +def reject_outliers( + anchors: List[AnchorPos], + ranges: List[RangeM], + position: Tuple[float, float, float], + threshold_m: float = 0.4, +) -> List[int]: + """Return indices of inlier anchors (residual ≤ threshold_m). + + Parameters + ---------- + anchors : anchor positions in map frame + ranges : measured ranges (metres) + position : current position estimate (x, y, z) + threshold_m : max allowable range residual to be an inlier + + Returns + ------- + inlier_indices : sorted list of indices whose residual is within threshold + """ + px, py, pz = position + inliers = [] + for i, (anchor, r_meas) in enumerate(zip(anchors, ranges)): + ax, ay, az = anchor + r_pred = math.sqrt((px - ax)**2 + (py - ay)**2 + (pz - az)**2) + residual = abs(r_meas - r_pred) + if residual <= threshold_m: + inliers.append(i) + return inliers + + +def residuals( + anchors: List[AnchorPos], + ranges: List[RangeM], + position: Tuple[float, float, float], +) -> List[float]: + """Compute per-anchor range residual (metres) for diagnostics.""" + px, py, pz = position + res = [] + for anchor, r_meas in zip(anchors, ranges): + ax, ay, az = anchor + r_pred = math.sqrt((px - ax)**2 + (py - ay)**2 + (pz - az)**2) + res.append(abs(r_meas - r_pred)) + return res + + +# ── 3D Kalman filter (constant-velocity, position-only observations) ────────── + +class KalmanFilter3D: + """Constant-velocity Kalman filter for 3D UWB position. + + State: [x, y, z, vx, vy, vz]ᵀ + Observation: [x, y, z] (position only) + """ + + def __init__( + self, + process_noise: float = 0.1, + measurement_noise: float = 0.15, + dt: float = 0.1, + ) -> None: + self._q = process_noise + self._r = measurement_noise + self._dt = dt + self._x = np.zeros(6) + self._P = np.diag([1.0, 1.0, 1.0, 0.5, 0.5, 0.5]) + self._init = False + + def _build_F(self, dt: float) -> np.ndarray: + F = np.eye(6) + F[0, 3] = dt + F[1, 4] = dt + F[2, 5] = dt + return F + + def _build_Q(self, dt: float) -> np.ndarray: + q = self._q + dt2 = dt * dt + dt3 = dt2 * dt + dt4 = dt3 * dt + Q = np.zeros((6, 6)) + # Position-velocity cross terms (constant white-noise model) + for i in range(3): + Q[i, i ] = q * dt4 / 4.0 + Q[i, i+3] = q * dt3 / 2.0 + Q[i+3, i ] = q * dt3 / 2.0 + Q[i+3, i+3] = q * dt2 + return Q + + def predict(self, dt: float | None = None) -> None: + if dt is not None: + self._dt = dt + F = self._build_F(self._dt) + Q = self._build_Q(self._dt) + self._x = F @ self._x + self._P = F @ self._P @ F.T + Q + + def update(self, x_m: float, y_m: float, z_m: float) -> None: + if not self._init: + self._x[:3] = [x_m, y_m, z_m] + self._init = True + return + + H = np.zeros((3, 6)) + H[0, 0] = 1.0 + H[1, 1] = 1.0 + H[2, 2] = 1.0 + R = np.eye(3) * self._r + + z_vec = np.array([x_m, y_m, z_m]) + innov = z_vec - H @ self._x + S = H @ self._P @ H.T + R + K = self._P @ H.T @ np.linalg.inv(S) + self._x = self._x + K @ innov + self._P = (np.eye(6) - K @ H) @ self._P + + def position(self) -> Tuple[float, float, float]: + return float(self._x[0]), float(self._x[1]), float(self._x[2]) + + def velocity(self) -> Tuple[float, float, float]: + return float(self._x[3]), float(self._x[4]), float(self._x[5]) + + def reset(self) -> None: + self._x = np.zeros(6) + self._P = np.diag([1.0, 1.0, 1.0, 0.5, 0.5, 0.5]) + self._init = False diff --git a/jetson/ros2_ws/src/saltybot_uwb_position/saltybot_uwb_position/uwb_position_node.py b/jetson/ros2_ws/src/saltybot_uwb_position/saltybot_uwb_position/uwb_position_node.py new file mode 100644 index 0000000..6031c3e --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_position/saltybot_uwb_position/uwb_position_node.py @@ -0,0 +1,487 @@ +#!/usr/bin/env python3 +""" +uwb_position_node.py — ROS2 UWB position node for Jetson (Issue #546). + +Reads JSON range measurements from an ESP32 UWB tag (DW3000) over USB +serial, trilaterates position from 3+ fixed infrastructure anchors, and +publishes the robot's absolute position in the map frame. + +Serial protocol (one JSON object per line, UTF-8): +──────────────────────────────────────────────────── + Tag → Jetson (full frame, all anchors at once): + {"ts": 123456, "ranges": [ + {"id": 0, "d_mm": 1500, "rssi": -65.0}, + {"id": 1, "d_mm": 2100, "rssi": -68.0}, + {"id": 2, "d_mm": 1800, "rssi": -70.0} + ]} + + Alternate (per-anchor, one line per measurement): + {"id": 0, "d_mm": 1500, "rssi": -65.0} + + Both "d_mm" and "range_mm" are accepted. + +Anchor positions: +───────────────── + Fixed anchors are configured in uwb_position_params.yaml as flat arrays: + anchor_ids: [0, 1, 2, 3] + anchor_positions: [x0, y0, z0, x1, y1, z1, ...] # metres in map frame + +Publishes: +────────── + /saltybot/uwb/pose (geometry_msgs/PoseStamped) 10 Hz + /saltybot/uwb/range/ (saltybot_uwb_msgs/UwbRange) per anchor, on arrival + /saltybot/uwb/status (std_msgs/String) 10 Hz JSON diagnostics + +TF2: +──── + Broadcasts uwb_link → map from /saltybot/uwb/pose position. + +Outlier rejection: +────────────────── + After initial trilateration, anchors with range residual > outlier_threshold_m + are dropped. If ≥ 3 inliers remain (2D mode) the position is re-solved. + Anchors rejected in N_strikes_max consecutive frames are flagged in status. +""" + +import json +import math +import threading +import time +from typing import Dict, List, Optional, Tuple + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy + +from geometry_msgs.msg import ( + PoseStamped, TransformStamped, + Quaternion, +) +from std_msgs.msg import Header, String +from tf2_ros import TransformBroadcaster + +from saltybot_uwb_msgs.msg import UwbRange + +from saltybot_uwb_position.trilateration import ( + trilaterate, + reject_outliers, + residuals, + KalmanFilter3D, +) + +try: + import serial + _SERIAL_OK = True +except ImportError: + _SERIAL_OK = False + + +# ── Serial reader thread ────────────────────────────────────────────────────── + +class SerialJsonReader(threading.Thread): + """Background thread: reads newline-delimited JSON from a serial port.""" + + def __init__(self, port: str, baudrate: int, callback, logger) -> None: + super().__init__(daemon=True) + self._port = port + self._baudrate = baudrate + self._callback = callback + self._logger = logger + self._running = False + self._ser = None + + def run(self) -> None: + self._running = True + while self._running: + try: + self._ser = serial.Serial( + self._port, self._baudrate, timeout=1.0 + ) + self._logger.info(f"UWB serial: opened {self._port} @ {self._baudrate}") + self._read_loop() + except Exception as exc: + self._logger.warning( + f"UWB serial error ({self._port}): {exc} — retry in 2 s" + ) + if self._ser and self._ser.is_open: + self._ser.close() + time.sleep(2.0) + + def _read_loop(self) -> None: + while self._running: + try: + raw = self._ser.readline() + if not raw: + continue + line = raw.decode("utf-8", errors="replace").strip() + if not line: + continue + try: + obj = json.loads(line) + self._callback(obj) + except json.JSONDecodeError: + pass # ignore malformed lines silently + except Exception as exc: + self._logger.warning(f"UWB read error: {exc}") + break + + def stop(self) -> None: + self._running = False + if self._ser and self._ser.is_open: + self._ser.close() + + +# ── Node ────────────────────────────────────────────────────────────────────── + +class UwbPositionNode(Node): + """ROS2 UWB position node — trilateration from 3+ fixed anchors.""" + + _MIN_ANCHORS_2D = 3 + _MIN_ANCHORS_3D = 4 + + def __init__(self) -> None: + super().__init__("uwb_position") + + # ── Parameters ──────────────────────────────────────────────────────── + self.declare_parameter("serial_port", "/dev/ttyUSB0") + self.declare_parameter("baudrate", 115200) + + # Anchor configuration: flat arrays, 3 floats per anchor + self.declare_parameter("anchor_ids", [0, 1, 2]) + self.declare_parameter("anchor_positions", [ + 0.0, 0.0, 2.0, # anchor 0: x, y, z (metres in map frame) + 5.0, 0.0, 2.0, # anchor 1 + 5.0, 5.0, 2.0, # anchor 2 + ]) + + self.declare_parameter("solve_z", False) # 2D mode by default + self.declare_parameter("robot_z", 0.0) # m — robot floor height + self.declare_parameter("publish_rate", 10.0) # Hz — pose + status + self.declare_parameter("range_timeout_s", 1.5) # s — stale range threshold + self.declare_parameter("max_range_m", 30.0) # m — validity max + self.declare_parameter("min_range_m", 0.05) # m — validity min + self.declare_parameter("outlier_threshold_m", 0.4) # m — RANSAC threshold + self.declare_parameter("outlier_strikes_max", 5) # frames before flagging + self.declare_parameter("kf_process_noise", 0.05) # Kalman Q + self.declare_parameter("kf_meas_noise", 0.10) # Kalman R + self.declare_parameter("map_frame", "map") + self.declare_parameter("uwb_frame", "uwb_link") + + # Load params + self._port = self.get_parameter("serial_port").value + self._baudrate = self.get_parameter("baudrate").value + self._solve_z = self.get_parameter("solve_z").value + self._robot_z = self.get_parameter("robot_z").value + self._publish_rate = self.get_parameter("publish_rate").value + self._timeout = self.get_parameter("range_timeout_s").value + self._max_r = self.get_parameter("max_range_m").value + self._min_r = self.get_parameter("min_range_m").value + self._outlier_thr = self.get_parameter("outlier_threshold_m").value + self._strikes_max = self.get_parameter("outlier_strikes_max").value + self._map_frame = self.get_parameter("map_frame").value + self._uwb_frame = self.get_parameter("uwb_frame").value + + # Parse anchor config + anchor_ids_raw = self.get_parameter("anchor_ids").value + anchor_pos_flat = self.get_parameter("anchor_positions").value + self._anchor_ids, self._anchor_positions = self._parse_anchors( + anchor_ids_raw, anchor_pos_flat + ) + self.get_logger().info( + f"UWB anchors: {[f'#{i}@({p[0]:.1f},{p[1]:.1f},{p[2]:.1f})' for i,p in zip(self._anchor_ids, self._anchor_positions)]}" + ) + + # ── State ───────────────────────────────────────────────────────────── + self._lock = threading.Lock() + # anchor_id → (range_m, rssi, timestamp_monotonic) + self._ranges: Dict[int, Tuple[float, float, float]] = {} + self._strikes: Dict[int, int] = {aid: 0 for aid in self._anchor_ids} + self._last_fix: Optional[Tuple[float, float, float]] = None + self._has_fix = False + self._fix_age = 0.0 + + # Kalman filter + self._kf = KalmanFilter3D( + process_noise=self.get_parameter("kf_process_noise").value, + measurement_noise=self.get_parameter("kf_meas_noise").value, + dt=1.0 / self._publish_rate, + ) + + # Per-anchor outlier publishers (pre-create for configured anchors) + sensor_qos = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + depth=5, + ) + self._range_pubs: Dict[int, object] = {} + for aid in self._anchor_ids: + self._range_pubs[aid] = self.create_publisher( + UwbRange, f"/saltybot/uwb/range/{aid}", sensor_qos + ) + + # Main publishers + self._pose_pub = self.create_publisher(PoseStamped, "/saltybot/uwb/pose", 10) + self._status_pub = self.create_publisher(String, "/saltybot/uwb/status", 10) + + # TF2 + self._tf_broadcaster = TransformBroadcaster(self) + + # ── Serial reader ────────────────────────────────────────────────────── + if _SERIAL_OK: + self._reader = SerialJsonReader( + self._port, self._baudrate, + callback=self._on_serial_json, + logger=self.get_logger(), + ) + self._reader.start() + else: + self.get_logger().warning( + "pyserial not installed — serial I/O disabled (simulation mode)" + ) + + # ── Publish timer ────────────────────────────────────────────────────── + self._prev_publish_t = self.get_clock().now().nanoseconds * 1e-9 + self.create_timer(1.0 / self._publish_rate, self._publish_cb) + + self.get_logger().info( + f"UwbPositionNode ready — port={self._port} " + f"anchors={len(self._anchor_ids)} " + f"mode={'3D' if self._solve_z else '2D'} " + f"rate={self._publish_rate:.0f}Hz" + ) + + # ── Anchor config parsing ───────────────────────────────────────────────── + + def _parse_anchors( + self, + ids_raw, + pos_flat, + ): + ids = list(ids_raw) + n = len(ids) + if len(pos_flat) < n * 3: + raise ValueError( + f"anchor_positions must have 3×anchor_ids entries. " + f"Got {len(pos_flat)} values for {n} anchors." + ) + positions = [] + for i in range(n): + base = i * 3 + positions.append(( + float(pos_flat[base]), + float(pos_flat[base + 1]), + float(pos_flat[base + 2]), + )) + return ids, positions + + # ── Serial JSON callback (called from reader thread) ────────────────────── + + def _on_serial_json(self, obj: dict) -> None: + """Parse incoming JSON and update range table.""" + now = time.monotonic() + + if "ranges" in obj: + # Full-frame format: {"ts": ..., "ranges": [{id, d_mm, rssi}, ...]} + for entry in obj["ranges"]: + self._ingest_range_entry(entry, now) + else: + # Per-anchor format: {"id": 0, "d_mm": 1500, "rssi": -65.0} + self._ingest_range_entry(obj, now) + + def _ingest_range_entry(self, entry: dict, timestamp: float) -> None: + """Store a single anchor range measurement after validity checks.""" + try: + anchor_id = int(entry.get("id", entry.get("anchor_id", -1))) + # Accept both "d_mm" and "range_mm" + raw_mm = int(entry.get("d_mm", entry.get("range_mm", 0))) + rssi = float(entry.get("rssi", 0.0)) + except (KeyError, TypeError, ValueError): + return + + if anchor_id not in self._anchor_ids: + return # unknown / unconfigured anchor + + range_m = raw_mm / 1000.0 + if range_m < self._min_r or range_m > self._max_r: + return # outside validity window + + with self._lock: + self._ranges[anchor_id] = (range_m, rssi, timestamp) + + # Publish per-anchor range topic immediately + self._publish_anchor_range(anchor_id, range_m, raw_mm, rssi) + + # ── Per-anchor range publisher ──────────────────────────────────────────── + + def _publish_anchor_range( + self, anchor_id: int, range_m: float, raw_mm: int, rssi: float + ) -> None: + if anchor_id not in self._range_pubs: + return + msg = UwbRange() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = self._map_frame + msg.anchor_id = anchor_id + msg.range_m = float(range_m) + msg.raw_mm = int(raw_mm) + msg.rssi = float(rssi) + msg.tag_id = "" + self._range_pubs[anchor_id].publish(msg) + + # ── Main publish callback (10 Hz) ───────────────────────────────────────── + + def _publish_cb(self) -> None: + now_ros = self.get_clock().now() + now_mono = time.monotonic() + + # Collect fresh ranges + with self._lock: + fresh = { + aid: entry + for aid, entry in self._ranges.items() + if (now_mono - entry[2]) <= self._timeout + } + + # Check minimum anchor count for chosen mode + min_anch = self._MIN_ANCHORS_3D if self._solve_z else self._MIN_ANCHORS_2D + state = "no_fix" + pos = None + used_ids: List[int] = [] + res_map: Dict[int, float] = {} + + if len(fresh) >= min_anch: + active_ids = list(fresh.keys()) + anch_pos = [self._anchor_positions[self._anchor_ids.index(i)] + for i in active_ids] + anch_r = [fresh[i][0] for i in active_ids] + + try: + fixed_z = None if self._solve_z else self._robot_z + raw_pos = trilaterate(anch_pos, anch_r, fixed_z=fixed_z) + + # Outlier rejection + inlier_idx = reject_outliers( + anch_pos, anch_r, raw_pos, + threshold_m=self._outlier_thr, + ) + res_all = residuals(anch_pos, anch_r, raw_pos) + for k, aid in enumerate(active_ids): + res_map[aid] = round(res_all[k], 3) + + # Update consecutive strike counters + for k, aid in enumerate(active_ids): + if k in inlier_idx: + self._strikes[aid] = 0 + else: + self._strikes[aid] = self._strikes.get(aid, 0) + 1 + + # Re-solve with inliers if any were rejected + if len(inlier_idx) < len(active_ids) and len(inlier_idx) >= min_anch: + inlier_anch = [anch_pos[k] for k in inlier_idx] + inlier_r = [anch_r[k] for k in inlier_idx] + raw_pos = trilaterate(inlier_anch, inlier_r, fixed_z=fixed_z) + + used_ids = [active_ids[k] for k in inlier_idx] + pos = raw_pos + + # Kalman predict + update + dt = now_ros.nanoseconds * 1e-9 - self._prev_publish_t + self._kf.predict(dt=dt) + self._kf.update(pos[0], pos[1], pos[2]) + pos = self._kf.position() + + self._last_fix = pos + self._has_fix = True + self._fix_age = 0.0 + state = "ok" if len(used_ids) >= min_anch else "degraded" + + except (ValueError, RuntimeError) as exc: + self.get_logger().warning(f"Trilateration failed: {exc}") + state = "degraded" + + elif self._has_fix: + # KF predict-only: coast on last known position + dt = now_ros.nanoseconds * 1e-9 - self._prev_publish_t + self._kf.predict(dt=dt) + pos = self._kf.position() + self._fix_age += 1.0 / self._publish_rate + state = "degraded" if self._fix_age < 5.0 else "no_fix" + + self._prev_publish_t = now_ros.nanoseconds * 1e-9 + + if pos is None: + self._publish_status(state, 0, [], {}) + return + + x, y, z = pos + + # ── Publish PoseStamped ──────────────────────────────────────────── + pose_msg = PoseStamped() + pose_msg.header.stamp = now_ros.to_msg() + pose_msg.header.frame_id = self._map_frame + pose_msg.pose.position.x = x + pose_msg.pose.position.y = y + pose_msg.pose.position.z = z + pose_msg.pose.orientation.w = 1.0 # no orientation from UWB alone + self._pose_pub.publish(pose_msg) + + # ── TF2: uwb_link → map ─────────────────────────────────────────── + tf_msg = TransformStamped() + tf_msg.header.stamp = now_ros.to_msg() + tf_msg.header.frame_id = self._map_frame + tf_msg.child_frame_id = self._uwb_frame + tf_msg.transform.translation.x = x + tf_msg.transform.translation.y = y + tf_msg.transform.translation.z = z + tf_msg.transform.rotation.w = 1.0 + self._tf_broadcaster.sendTransform(tf_msg) + + # ── Diagnostics ─────────────────────────────────────────────────── + self._publish_status(state, len(used_ids), used_ids, res_map) + + # ── Status publisher ────────────────────────────────────────────────────── + + def _publish_status( + self, + state: str, + n_anchors: int, + used_ids: List[int], + res_map: Dict[int, float], + ) -> None: + # Flag anchors with too many consecutive outlier strikes + flagged = [ + aid for aid, s in self._strikes.items() if s >= self._strikes_max + ] + pos = self._kf.position() if self._has_fix else None + status = { + "state": state, + "active_anchors": n_anchors, + "used_anchor_ids": used_ids, + "flagged_anchors": flagged, + "position": { + "x": round(pos[0], 3), + "y": round(pos[1], 3), + "z": round(pos[2], 3), + } if pos else None, + "residuals_m": res_map, + "fix_age_s": round(self._fix_age, 2), + } + self._status_pub.publish(String(data=json.dumps(status))) + + +# ── Entry point ─────────────────────────────────────────────────────────────── + +def main(args=None) -> None: + rclpy.init(args=args) + node = UwbPositionNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == "__main__": + main() diff --git a/jetson/ros2_ws/src/saltybot_uwb_position/setup.cfg b/jetson/ros2_ws/src/saltybot_uwb_position/setup.cfg new file mode 100644 index 0000000..b6b785a --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_position/setup.cfg @@ -0,0 +1,5 @@ +[develop] +script_dir=$base/lib/saltybot_uwb_position + +[install] +install_scripts=$base/lib/saltybot_uwb_position diff --git a/jetson/ros2_ws/src/saltybot_uwb_position/setup.py b/jetson/ros2_ws/src/saltybot_uwb_position/setup.py new file mode 100644 index 0000000..979af97 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_position/setup.py @@ -0,0 +1,32 @@ +import os +from glob import glob +from setuptools import setup + +package_name = "saltybot_uwb_position" + +setup( + name=package_name, + version="0.1.0", + packages=[package_name], + data_files=[ + ("share/ament_index/resource_index/packages", + ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + (os.path.join("share", package_name, "launch"), + glob("launch/*.py")), + (os.path.join("share", package_name, "config"), + glob("config/*.yaml")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="sl-perception", + maintainer_email="sl-perception@saltylab.local", + description="ROS2 UWB position node — JSON serial bridge with trilateration (Issue #546)", + license="MIT", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "uwb_position = saltybot_uwb_position.uwb_position_node:main", + ], + }, +)