From 343e53081a6daa2f9062626512117a27bbd4ea10 Mon Sep 17 00:00:00 2001 From: sl-uwb Date: Sun, 15 Mar 2026 14:42:26 -0400 Subject: [PATCH] feat: UWB position logger and accuracy analyzer (Issue #634) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit saltybot_uwb_logger_msgs (new package): - AccuracyReport.msg: n_samples, mean/bias/std (x,y,2D), CEP50, CEP95, RMSE, max_error, per-anchor range stats, test_id, duration_s - StartAccuracyTest.srv: request (truth_x/y_m, n_samples, timeout_s, test_id) → response (success, message, test_id) saltybot_uwb_logger (new package): - accuracy_stats.py: compute_stats() + RangeAccum — pure numpy, no ROS2 CEP50/CEP95 = 50th/95th percentile of 2-D error; bias, std, RMSE, max - logger_node.py: /uwb_logger ROS2 node Subscribes: /saltybot/pose/fused → fused_pose_.csv (ts, x, y, heading) /saltybot/uwb/pose → uwb_pose_.csv (ts, x, y) /uwb/ranges → uwb_ranges_.csv (ts, anchor_id, range_m, raw_mm, rssi, tag_id) Service /saltybot/uwb/start_accuracy_test: Collects N fused-pose samples at known (truth_x, truth_y) in background thread. On completion or timeout: publishes AccuracyReport on /saltybot/uwb/accuracy_report + writes accuracy_.json. Per-anchor range stats included. CSV flushed every 5 s. Tests: 16/16 passing (test/test_accuracy_stats.py, no ROS2/hardware) Co-Authored-By: Claude Sonnet 4.6 --- .../config/uwb_logger_params.yaml | 7 + .../launch/uwb_logger.launch.py | 28 ++ .../src/saltybot_uwb_logger/package.xml | 31 ++ .../resource/saltybot_uwb_logger | 0 .../saltybot_uwb_logger/__init__.py | 0 .../__pycache__/__init__.cpython-314.pyc | Bin 0 -> 209 bytes .../accuracy_stats.cpython-314.pyc | Bin 0 -> 5463 bytes .../saltybot_uwb_logger/accuracy_stats.py | 99 +++++ .../saltybot_uwb_logger/logger_node.py | 409 ++++++++++++++++++ .../ros2_ws/src/saltybot_uwb_logger/setup.cfg | 4 + .../ros2_ws/src/saltybot_uwb_logger/setup.py | 29 ++ ...ccuracy_stats.cpython-314-pytest-9.0.2.pyc | Bin 0 -> 35723 bytes .../test/test_accuracy_stats.py | 116 +++++ .../saltybot_uwb_logger_msgs/CMakeLists.txt | 15 + .../msg/AccuracyReport.msg | 32 ++ .../src/saltybot_uwb_logger_msgs/package.xml | 21 + .../srv/StartAccuracyTest.srv | 15 + 17 files changed, 806 insertions(+) create mode 100644 jetson/ros2_ws/src/saltybot_uwb_logger/config/uwb_logger_params.yaml create mode 100644 jetson/ros2_ws/src/saltybot_uwb_logger/launch/uwb_logger.launch.py create mode 100644 jetson/ros2_ws/src/saltybot_uwb_logger/package.xml create mode 100644 jetson/ros2_ws/src/saltybot_uwb_logger/resource/saltybot_uwb_logger create mode 100644 jetson/ros2_ws/src/saltybot_uwb_logger/saltybot_uwb_logger/__init__.py create mode 100644 jetson/ros2_ws/src/saltybot_uwb_logger/saltybot_uwb_logger/__pycache__/__init__.cpython-314.pyc create mode 100644 jetson/ros2_ws/src/saltybot_uwb_logger/saltybot_uwb_logger/__pycache__/accuracy_stats.cpython-314.pyc create mode 100644 jetson/ros2_ws/src/saltybot_uwb_logger/saltybot_uwb_logger/accuracy_stats.py create mode 100644 jetson/ros2_ws/src/saltybot_uwb_logger/saltybot_uwb_logger/logger_node.py create mode 100644 jetson/ros2_ws/src/saltybot_uwb_logger/setup.cfg create mode 100644 jetson/ros2_ws/src/saltybot_uwb_logger/setup.py create mode 100644 jetson/ros2_ws/src/saltybot_uwb_logger/test/__pycache__/test_accuracy_stats.cpython-314-pytest-9.0.2.pyc create mode 100644 jetson/ros2_ws/src/saltybot_uwb_logger/test/test_accuracy_stats.py create mode 100644 jetson/ros2_ws/src/saltybot_uwb_logger_msgs/CMakeLists.txt create mode 100644 jetson/ros2_ws/src/saltybot_uwb_logger_msgs/msg/AccuracyReport.msg create mode 100644 jetson/ros2_ws/src/saltybot_uwb_logger_msgs/package.xml create mode 100644 jetson/ros2_ws/src/saltybot_uwb_logger_msgs/srv/StartAccuracyTest.srv diff --git a/jetson/ros2_ws/src/saltybot_uwb_logger/config/uwb_logger_params.yaml b/jetson/ros2_ws/src/saltybot_uwb_logger/config/uwb_logger_params.yaml new file mode 100644 index 0000000..552dcc1 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_logger/config/uwb_logger_params.yaml @@ -0,0 +1,7 @@ +uwb_logger: + ros__parameters: + log_dir: ~/uwb_logs + enable_csv: true + default_n_samples: 200 + default_timeout_s: 30.0 + flush_interval_s: 5.0 diff --git a/jetson/ros2_ws/src/saltybot_uwb_logger/launch/uwb_logger.launch.py b/jetson/ros2_ws/src/saltybot_uwb_logger/launch/uwb_logger.launch.py new file mode 100644 index 0000000..72dd954 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_logger/launch/uwb_logger.launch.py @@ -0,0 +1,28 @@ +"""uwb_logger.launch.py — Launch UWB position logger (Issue #634).""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + + +def generate_launch_description() -> LaunchDescription: + pkg_dir = get_package_share_directory("saltybot_uwb_logger") + params_file = os.path.join(pkg_dir, "config", "uwb_logger_params.yaml") + + return LaunchDescription([ + DeclareLaunchArgument("log_dir", default_value="~/uwb_logs"), + Node( + package="saltybot_uwb_logger", + executable="uwb_logger", + name="uwb_logger", + parameters=[ + params_file, + {"log_dir": LaunchConfiguration("log_dir")}, + ], + output="screen", + emulate_tty=True, + ), + ]) diff --git a/jetson/ros2_ws/src/saltybot_uwb_logger/package.xml b/jetson/ros2_ws/src/saltybot_uwb_logger/package.xml new file mode 100644 index 0000000..eb8b9e2 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_logger/package.xml @@ -0,0 +1,31 @@ + + + + saltybot_uwb_logger + 0.1.0 + + UWB position logger and accuracy analyzer (Issue #634). + Logs fused pose, raw UWB pose, per-anchor ranges to CSV. + Provides /saltybot/uwb/start_accuracy_test: collects N samples at a + known position and computes CEP50, CEP95, RMSE, max error. + + sl-uwb + Apache-2.0 + + rclpy + geometry_msgs + std_msgs + saltybot_uwb_msgs + saltybot_uwb_logger_msgs + + python3-numpy + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_uwb_logger/resource/saltybot_uwb_logger b/jetson/ros2_ws/src/saltybot_uwb_logger/resource/saltybot_uwb_logger new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_uwb_logger/saltybot_uwb_logger/__init__.py b/jetson/ros2_ws/src/saltybot_uwb_logger/saltybot_uwb_logger/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_uwb_logger/saltybot_uwb_logger/__pycache__/__init__.cpython-314.pyc b/jetson/ros2_ws/src/saltybot_uwb_logger/saltybot_uwb_logger/__pycache__/__init__.cpython-314.pyc new file mode 100644 index 0000000000000000000000000000000000000000..d40fd854adb1e34a7d970bead8ba611f8e8ce2be GIT binary patch literal 209 zcmdPqg+i?h(kHD1;f%JTY7CQrS@lkrZTkY3aPXpRs9zl(EaI;zBlV#JE^%l zM)sRGZ{EzjdGF24d%L?W5+qPQ{_k&d*8_w+!iHTu79y7Uh)q;sQ3rBadQj9gLA>C=+UB_UUi z8MzFTUOvfKS(Ca4UhR(ugCk|CNqV_Zs`N^FQIg*|la#VrNz=2Mo~iT&Ym@VfK4=c*MWi?;#i7Luejxr;k(>1*a-9TtE z0uW*fH+(LmsD?pvlVw9=ib7+c*%dT^X(Y}ujj{yOCKE?aCYYKxDtUEs56oMbRjHQv zo@TJ3o}fjRRAzyXW-O-3qM?*$CzX70dRn6inAb@3^|kD?z02`I>V>5_hyub61NeYE zjCNcaUJ9?f^tJ^;?Uzn1anPV~tkDtOP!l&S1*0FlMYq{AW(2_Sj|Gn^IGV813rLlu z$%N~``-EhHwvC(?W&~M~U9ubgqU@2q@b}67ga;fT@NYbTrfZH}4CV5665gpIEr9!=JUi%1$PX5*n+WVl(do9spB)5H zC7+W%!TSWklD}iw4fR8)Jy0)uN!$8mA7XyQ0*D2Pcm0zPV0`--5$3Q93IMzGth58Q zO2%wD8L3|^4gruZ(9D=KS{71up3;|(6IzZF#(ehtY0_V}MoCPIp#KigKbUOJm?T{`=S=f>!KS9g1G6wTyk@N1?uSwKgk*$Y(oT~jU1ZeN+eDvm_4*qeqwAmF zhu&isd$p2htbW~-W-WnqYYS@E9%a`h?+c7}G~kT(wH)2Hc4f=56M`(-7IkLuzrYMa z%NYV=>+JQ?V%nSbrTyu^7+BXTtu2uDuhx1xu)5t!)1#BkmKkNut(LpkV9U^`!3Lg* zr1MnPJ@SP{D_?2!#>Sd9hHKk;x9gTGH~D5`Y?Dv7_ASq94caTLtz}QDJ|1p2uj&I8 z1ix7hE*jFs?l~r@dKNcYC`12@`U4Ht&~O2*KHqJ@6tmglR6I=aa-nEb z8bW~Drx65rPAR^8v>gE+BZ}vSu1A308xNZw&xL?z$JZ9ICYqF8T)__=1;{muBa?-hU5!w+z+V4lnu-KaNDNjD9#e zKl$_g&3Es|UcPno(^Gd(-`_oSKQ>g09IhUF9O?Kdc`bD{wUGST>$S-C>ahpUp$uFd zSlDrM-@Wy_YLTAmn-<#|;^WZfrP#)$*jE1QT#9u*h;6xc;p&Bj@-5+)7jM0J=k%xF zy1#v(7E1xg{n#!GK8SUGlDsi^eel-CI~}#H{k7Qs=aL4l58Uke#ZWEwip7$;p89q2 zH>qExZVkZXuh+U$wXFwgvDcn%iG;n&B;u*R^q_O6I7!D|4m*FCD9nE{j{ds8aXM=_c(16H?^ z%N~R5BAV5v)Nt~p2pkx}fx${{{5O-7(zTSky;O-Va}!*<9;yUA?<(lR;(T&7noVeSTc6q%Nv zqTc`_!$o^~tW)k*vstsPX&cby(&P11B}@MBmB^1GfAw$svwz#d=wJN1EV=Pv#XlCD zj;nhcAkR~GLI72}xvIHxxv9TMxAZ8?_HFnxye%y~?m*4$m}k6d2m<}QI10q*0%BoX zK#0p)MZ@;X*-hvhFaT7x2P&?&+X6iXIJb9QQJxIEQij>yq$*CifP8qhSIOgO$9(^_ z!JiJ+qFsyru7-fNAYWNix0^ME;q#YjY68W;pv2PDaRGI7T$tCax*-d&@PgL~_qO){ zL94a$wy#^fB>M^v#vR{lBm*Ou;;ktd2)CNeR=8bvb8n3c|FXx&MXr782<`?d-HYHY z08OUgL^IlC2z(jGkqPCkl81ZyKZ|@8xfA(G`6Kz-=+)7Mvp2>+9?z@vHP{ShUxkb(k^XJea=Y_`lz{d+-e%vDSfP8iLg;eYJ zkS>AGL#owCRUEsFG$lXQLRPxa>PSEW;3S1jR>Ett?vZhrf}REd_oj1PnhdrN`_~!_ ziK{Ld2x3&Au_OREH>rbn`~NWbyTMxOU*Y z@$4u-t~##&;dqpyis>c0JCPm-VERk62v3nwp{J0> z{fx`L6`N>VYZc7p>1#GXuV&u`@B#UUIJAUcGX56pyw!h4yB9lDJ#jf(6AwKT;gW|m z-rM%n1-I3T8?a@>9NKZgAs!bT>UoQbgX-SF*M$HR?_>NrG*yNV5BN0!?-7c+Gegd| zH;qv|o_w>WhkwDN{0N~V@aWCj#_Fw^{zKL`ry=?s;KQT9&Hz|;3xe>(B?{uRFcG>R zk AccuracyStats: + """ + Compute accuracy statistics from position samples. + + Parameters + ---------- + xs, ys : 1-D arrays of measured x / y positions (metres) + truth_x/y : known ground-truth position (metres) + + Raises + ------ + ValueError if fewer than 2 samples or mismatched lengths. + """ + xs = np.asarray(xs, dtype=float) + ys = np.asarray(ys, dtype=float) + n = len(xs) + if n < 2: + raise ValueError(f"Need at least 2 samples, got {n}") + if len(ys) != n: + raise ValueError("xs and ys must have the same length") + + errors = np.sqrt((xs - truth_x) ** 2 + (ys - truth_y) ** 2) + + mean_x = float(np.mean(xs)) + mean_y = float(np.mean(ys)) + std_x = float(np.std(xs, ddof=1)) + std_y = float(np.std(ys, ddof=1)) + + return AccuracyStats( + n_samples = n, + mean_x = mean_x, + mean_y = mean_y, + std_x = std_x, + std_y = std_y, + std_2d = math.sqrt(std_x ** 2 + std_y ** 2), + bias_x = mean_x - truth_x, + bias_y = mean_y - truth_y, + cep50 = float(np.percentile(errors, 50)), + cep95 = float(np.percentile(errors, 95)), + max_error = float(np.max(errors)), + rmse = float(np.sqrt(np.mean(errors ** 2))), + ) + + +@dataclass +class RangeAccum: + """Running accumulator for per-anchor range statistics.""" + anchor_id: int + _samples: list = field(default_factory=list, repr=False) + + def add(self, range_m: float) -> None: + self._samples.append(range_m) + + @property + def count(self) -> int: + return len(self._samples) + + @property + def mean(self) -> float: + return float(np.mean(self._samples)) if self._samples else 0.0 + + @property + def std(self) -> float: + return float(np.std(self._samples, ddof=1)) if len(self._samples) >= 2 else 0.0 diff --git a/jetson/ros2_ws/src/saltybot_uwb_logger/saltybot_uwb_logger/logger_node.py b/jetson/ros2_ws/src/saltybot_uwb_logger/saltybot_uwb_logger/logger_node.py new file mode 100644 index 0000000..1101d2f --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_logger/saltybot_uwb_logger/logger_node.py @@ -0,0 +1,409 @@ +""" +logger_node.py — UWB position logger and accuracy analyzer (Issue #634) + +Subscriptions +───────────── + /saltybot/pose/fused geometry_msgs/PoseStamped — EKF-fused pose (200 Hz) + /saltybot/uwb/pose geometry_msgs/PoseStamped — raw UWB pose (10 Hz) + /uwb/ranges saltybot_uwb_msgs/UwbRangeArray — per-anchor ranges + +Service +─────── + /saltybot/uwb/start_accuracy_test StartAccuracyTest + Collects N fused-pose samples at known position, computes accuracy + metrics, publishes AccuracyReport, writes JSON summary. + +Publishes +───────── + /saltybot/uwb/accuracy_report AccuracyReport + +CSV logging (continuous, to log_dir) +───────────────────────────────────── + fused_pose_.csv — timestamp_ns, x_m, y_m, heading_rad + uwb_pose_.csv — timestamp_ns, x_m, y_m + uwb_ranges_.csv — timestamp_ns, anchor_id, range_m, raw_mm, rssi, tag_id + accuracy_.json — accuracy test results + +Parameters +────────── + log_dir ~/uwb_logs + enable_csv true + default_n_samples 200 + default_timeout_s 30.0 + flush_interval_s 5.0 +""" + +from __future__ import annotations + +import csv +import json +import math +import os +import threading +import time +from datetime import datetime, timezone +from pathlib import Path +from typing import Optional + +import numpy as np + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy + +from geometry_msgs.msg import PoseStamped +from saltybot_uwb_msgs.msg import UwbRangeArray +from saltybot_uwb_logger_msgs.msg import AccuracyReport +from saltybot_uwb_logger_msgs.srv import StartAccuracyTest + +from saltybot_uwb_logger.accuracy_stats import compute_stats, RangeAccum + + +_SENSOR_QOS = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + depth=10, +) + + +class UwbLoggerNode(Node): + + def __init__(self) -> None: + super().__init__("uwb_logger") + + # ── Parameters ──────────────────────────────────────────────────── + self.declare_parameter("log_dir", os.path.expanduser("~/uwb_logs")) + self.declare_parameter("enable_csv", True) + self.declare_parameter("default_n_samples", 200) + self.declare_parameter("default_timeout_s", 30.0) + self.declare_parameter("flush_interval_s", 5.0) + + self._log_dir = Path(self.get_parameter("log_dir").value) + self._enable_csv = self.get_parameter("enable_csv").value + self._default_n = self.get_parameter("default_n_samples").value + self._default_to = self.get_parameter("default_timeout_s").value + self._flush_iv = self.get_parameter("flush_interval_s").value + + if self._enable_csv: + self._log_dir.mkdir(parents=True, exist_ok=True) + + # ── CSV writers ─────────────────────────────────────────────────── + date_tag = datetime.now(timezone.utc).strftime("%Y%m%dT%H%M%SZ") + self._csv_lock = threading.Lock() + self._csv_files = [] + self._fused_csv = self._open_csv( + f"fused_pose_{date_tag}.csv", + ["timestamp_ns", "x_m", "y_m", "heading_rad"]) + self._uwb_csv = self._open_csv( + f"uwb_pose_{date_tag}.csv", + ["timestamp_ns", "x_m", "y_m"]) + self._ranges_csv = self._open_csv( + f"uwb_ranges_{date_tag}.csv", + ["timestamp_ns", "anchor_id", "range_m", "raw_mm", "rssi", "tag_id"]) + + # ── Accuracy test state ──────────────────────────────────────────── + self._test_lock = threading.Lock() + self._test_active = False + self._test_id = "" + self._test_truth_x = 0.0 + self._test_truth_y = 0.0 + self._test_n_target = 0 + self._test_xs: list = [] + self._test_ys: list = [] + self._test_ranges: dict[int, RangeAccum] = {} + self._test_t0 = 0.0 + self._test_timeout = 0.0 + self._test_done_event = threading.Event() + + # ── Publisher ───────────────────────────────────────────────────── + self._report_pub = self.create_publisher( + AccuracyReport, "/saltybot/uwb/accuracy_report", 10 + ) + + # ── Subscriptions ───────────────────────────────────────────────── + self.create_subscription( + PoseStamped, "/saltybot/pose/fused", + self._fused_cb, _SENSOR_QOS) + self.create_subscription( + PoseStamped, "/saltybot/uwb/pose", + self._uwb_pose_cb, _SENSOR_QOS) + self.create_subscription( + UwbRangeArray, "/uwb/ranges", + self._ranges_cb, _SENSOR_QOS) + + # ── Service ─────────────────────────────────────────────────────── + self._srv = self.create_service( + StartAccuracyTest, + "/saltybot/uwb/start_accuracy_test", + self._handle_start_test) + + # ── Periodic flush ──────────────────────────────────────────────── + self.create_timer(self._flush_iv, self._flush_csv) + + self.get_logger().info( + f"UWB logger ready — log_dir={self._log_dir} csv={self._enable_csv}" + ) + + def destroy_node(self) -> None: + self._flush_csv() + for fh in self._csv_files: + try: + fh.close() + except Exception: + pass + super().destroy_node() + + # ── Callbacks ───────────────────────────────────────────────────────── + + def _fused_cb(self, msg: PoseStamped) -> None: + ts = _stamp_ns(msg.header.stamp) + x = msg.pose.position.x + y = msg.pose.position.y + q = msg.pose.orientation + heading = 2.0 * math.atan2(float(q.z), float(q.w)) + + if self._enable_csv: + with self._csv_lock: + if self._fused_csv: + self._fused_csv.writerow([ts, x, y, heading]) + + with self._test_lock: + if self._test_active: + self._test_xs.append(x) + self._test_ys.append(y) + if len(self._test_xs) >= self._test_n_target: + self._test_active = False + self._test_done_event.set() + + def _uwb_pose_cb(self, msg: PoseStamped) -> None: + ts = _stamp_ns(msg.header.stamp) + x = msg.pose.position.x + y = msg.pose.position.y + if self._enable_csv: + with self._csv_lock: + if self._uwb_csv: + self._uwb_csv.writerow([ts, x, y]) + + def _ranges_cb(self, msg: UwbRangeArray) -> None: + ts = _stamp_ns(msg.header.stamp) + for r in msg.ranges: + if self._enable_csv: + with self._csv_lock: + if self._ranges_csv: + self._ranges_csv.writerow([ + ts, r.anchor_id, r.range_m, + r.raw_mm, r.rssi, r.tag_id, + ]) + with self._test_lock: + if self._test_active: + aid = int(r.anchor_id) + if aid not in self._test_ranges: + self._test_ranges[aid] = RangeAccum(anchor_id=aid) + self._test_ranges[aid].add(float(r.range_m)) + + # ── Service ──────────────────────────────────────────────────────────── + + def _handle_start_test( + self, + request: StartAccuracyTest.Request, + response: StartAccuracyTest.Response, + ) -> StartAccuracyTest.Response: + + with self._test_lock: + if self._test_active: + response.success = False + response.message = "Another test is already running" + response.test_id = self._test_id + return response + + n = int(request.n_samples) if request.n_samples > 0 else self._default_n + timeout = float(request.timeout_s) if request.timeout_s > 0.0 else self._default_to + test_id = ( + request.test_id.strip() + if request.test_id.strip() + else datetime.now(timezone.utc).strftime("%Y%m%dT%H%M%SZ") + ) + + self.get_logger().info( + f"Accuracy test '{test_id}' — " + f"truth=({request.truth_x_m:.3f}, {request.truth_y_m:.3f}) " + f"n={n} timeout={timeout}s" + ) + + with self._test_lock: + self._test_id = test_id + self._test_truth_x = float(request.truth_x_m) + self._test_truth_y = float(request.truth_y_m) + self._test_n_target = n + self._test_xs = [] + self._test_ys = [] + self._test_ranges = {} + self._test_t0 = time.monotonic() + self._test_timeout = timeout + self._test_done_event.clear() + self._test_active = True + + threading.Thread( + target=self._run_test, + args=(test_id, request.truth_x_m, request.truth_y_m, n, timeout), + daemon=True, + ).start() + + response.success = True + response.message = f"Test '{test_id}' started, collecting {n} samples" + response.test_id = test_id + return response + + def _run_test( + self, + test_id: str, + truth_x: float, + truth_y: float, + n_target: int, + timeout: float, + ) -> None: + finished = self._test_done_event.wait(timeout=timeout) + + with self._test_lock: + self._test_active = False + xs = list(self._test_xs) + ys = list(self._test_ys) + ranges_snap = dict(self._test_ranges) + t0 = self._test_t0 + + duration = time.monotonic() - t0 + n_got = len(xs) + + if n_got < 2: + self.get_logger().error( + f"Test '{test_id}' failed: only {n_got} samples in {duration:.1f}s" + ) + return + + if not finished: + self.get_logger().warn( + f"Test '{test_id}' timed out — {n_got}/{n_target} samples" + ) + + try: + stats = compute_stats(np.array(xs), np.array(ys), truth_x, truth_y) + except ValueError as exc: + self.get_logger().error(f"Stats failed: {exc}") + return + + self.get_logger().info( + f"Test '{test_id}' — n={stats.n_samples} " + f"CEP50={stats.cep50*100:.1f}cm CEP95={stats.cep95*100:.1f}cm " + f"RMSE={stats.rmse*100:.1f}cm max={stats.max_error*100:.1f}cm" + ) + + # Publish report + report = AccuracyReport() + report.header.stamp = self.get_clock().now().to_msg() + report.header.frame_id = "map" + report.truth_x_m = truth_x + report.truth_y_m = truth_y + report.n_samples = stats.n_samples + report.mean_x_m = stats.mean_x + report.mean_y_m = stats.mean_y + report.std_x_m = stats.std_x + report.std_y_m = stats.std_y + report.std_2d_m = stats.std_2d + report.bias_x_m = stats.bias_x + report.bias_y_m = stats.bias_y + report.cep50_m = stats.cep50 + report.cep95_m = stats.cep95 + report.max_error_m = stats.max_error + report.rmse_m = stats.rmse + report.test_id = test_id + report.duration_s = duration + + anchor_ids = sorted(ranges_snap.keys()) + report.anchor_ids = [int(a) for a in anchor_ids] + report.anchor_range_mean_m = [ranges_snap[a].mean for a in anchor_ids] + report.anchor_range_std_m = [ranges_snap[a].std for a in anchor_ids] + + self._report_pub.publish(report) + + # Write JSON + if self._enable_csv: + self._write_json(test_id, truth_x, truth_y, stats, + anchor_ids, ranges_snap, duration) + + # ── CSV / JSON helpers ───────────────────────────────────────────────── + + def _open_csv( + self, filename: str, headers: list + ) -> Optional[csv.writer]: + if not self._enable_csv: + return None + path = self._log_dir / filename + fh = open(path, "w", newline="", buffering=1) + writer = csv.writer(fh) + writer.writerow(headers) + self._csv_files.append(fh) + return writer + + def _flush_csv(self) -> None: + for fh in self._csv_files: + try: + fh.flush() + except Exception: + pass + + def _write_json(self, test_id, truth_x, truth_y, stats, + anchor_ids, ranges_snap, duration) -> None: + path = self._log_dir / f"accuracy_{test_id}.json" + data = { + "test_id": test_id, + "truth": {"x_m": truth_x, "y_m": truth_y}, + "n_samples": stats.n_samples, + "duration_s": round(duration, 3), + "mean": {"x_m": round(stats.mean_x, 4), + "y_m": round(stats.mean_y, 4)}, + "bias": {"x_m": round(stats.bias_x, 4), + "y_m": round(stats.bias_y, 4)}, + "std": {"x_m": round(stats.std_x, 4), + "y_m": round(stats.std_y, 4), + "2d_m": round(stats.std_2d, 4)}, + "cep50_m": round(stats.cep50, 4), + "cep95_m": round(stats.cep95, 4), + "rmse_m": round(stats.rmse, 4), + "max_error_m": round(stats.max_error, 4), + "anchors": { + str(aid): { + "range_mean_m": round(ranges_snap[aid].mean, 4), + "range_std_m": round(ranges_snap[aid].std, 4), + "n": ranges_snap[aid].count, + } + for aid in anchor_ids + }, + } + try: + with open(path, "w") as f: + json.dump(data, f, indent=2) + self.get_logger().info(f"Report written: {path}") + except OSError as exc: + self.get_logger().error(f"JSON write failed: {exc}") + + +# ── Helpers ──────────────────────────────────────────────────────────────── + +def _stamp_ns(stamp) -> int: + return stamp.sec * 1_000_000_000 + stamp.nanosec + + +def main(args=None) -> None: + rclpy.init(args=args) + node = UwbLoggerNode() + 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_logger/setup.cfg b/jetson/ros2_ws/src/saltybot_uwb_logger/setup.cfg new file mode 100644 index 0000000..0b9e8c8 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_logger/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/saltybot_uwb_logger +[install] +install_scripts=$base/lib/saltybot_uwb_logger diff --git a/jetson/ros2_ws/src/saltybot_uwb_logger/setup.py b/jetson/ros2_ws/src/saltybot_uwb_logger/setup.py new file mode 100644 index 0000000..0d654b7 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_logger/setup.py @@ -0,0 +1,29 @@ +import os +from glob import glob +from setuptools import setup + +package_name = "saltybot_uwb_logger" + +setup( + name=package_name, + version="0.1.0", + packages=[package_name], + data_files=[ + ("share/ament_index/resource_index/packages", + [f"resource/{package_name}"]), + (f"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-uwb", + maintainer_email="sl-uwb@saltylab.local", + description="UWB position logger and accuracy analyzer (Issue #634)", + license="Apache-2.0", + entry_points={ + "console_scripts": [ + "uwb_logger = saltybot_uwb_logger.logger_node:main", + ], + }, +) diff --git a/jetson/ros2_ws/src/saltybot_uwb_logger/test/__pycache__/test_accuracy_stats.cpython-314-pytest-9.0.2.pyc b/jetson/ros2_ws/src/saltybot_uwb_logger/test/__pycache__/test_accuracy_stats.cpython-314-pytest-9.0.2.pyc new file mode 100644 index 0000000000000000000000000000000000000000..2444f411d33a980753420396ea1eac80d1dc23b4 GIT binary patch literal 35723 zcmeHw3ve9AdFJdBg9R3gH%O5b0fHh4k^+bq0ZQUa6lKz~DQF}uSTe+|1+WC61A)ct zf+Q&GPL7=jw48|YS<=d)ORHFkKv_A_aU8*qis{@{Vpr|~5+JFpba%<6t`yZdSF+?p zSFXFN`~L3f?%BbB3(&HoRA~tQ(~rM+ahor}}O>>Ns$uybj; z^kKc{CUF6E?pdS4B6LK1xY(694SMK~K2hX_4f(Sf_6!|--^c}SeuUHA@G9;ixJ z9jH!MuhopO;jhz1Jn5PmtwA#ab(#?r-051DR)jQNrq!{uVx)y++KPu)8YSI_HEpm? z(^hK_m-ck<>)L69_Hcz!+Uymr!?it)?0-|)>3bg>9ZuJ$6RC8n{^*!lpNfy9C;P|J zv5AxYv5~Q%p@i8S9~hW0;{%hiR5}i}O$Sq{i9~(l_V$jZ=K4cp^~SyTx7Cj(5`&4s z=0m5w&CN~TTzFvYcyc10;AL_lBR)Em*o%se5BP13D?-?Z{~!Dey~i*1>I= z2ri|Lc5kEB50+7xefIOiJ} zOXYm2;nAi*&NG_xm^tsjDg2+zdEzO$j$VRf2goNDeZ%P($S=_2~1(z+1@EjI8er9*}eT4LZ6N|zcT(6CVgT4t1{ zBT+3~ZiMk(%5voo;O_VF?t4G=_yN%dYFrrq(PQ-wE}#c3p~nUckN8}Z9$VjW@81D6 zbR@lfvM={mQcvy>J=quax%K^|zpt2@NQdglNj#PDL^RlqD~_Ms$g{Qmo}qRcGWuj6 z?kTmhY$ps%WznK`j}hsOw#(}qek0IFZ=M}JVQ^Z5>?YC|~U;jHp}r&w7l25j2XH)mBUwh1(kSEodVffNf9HwT-GA+E!%f1^MN8 zWE2<#|G@1Y^91^?%QXFK?7QWk^xeES^hw{Z8=}OA8<1{*_!jPb2~*kiFE%m z5>EkRrt5Fng#2wyshpqAht7L)o;#X6r$0^UolU9GqgJ4PCu0TYCQ4}+SqNBJtxc)s zfkd)%Tj;T-&|~#?F#hhQ6lyzoW3`#bQ;CI@9lEi~j>k{M5~ew3I%|xWGKPjZrQbS!S>Dq;iik&&30NSY)U0g2~Iksa%w7#>Lj?dO6cW8g^T zR>V@r#wJDv8Eb54WUN0v5*v*lPvnYYsrb{#0V*XwG{omGe%f%$uo^CDaQ3Lfh<-4tu1_9@)+#{E9pH zZSY8IYl^_+hI(A&UQWdc?2?p zj9iMWyQW)z^ZL4}&&!w$d?h7=m#G37lL6FpwpGT!Bw|+)?BW@=$`Q;AN_KkyIn>nc zPj8>q8_?A4PdO- zl_QvWK(gBd$f2g*{`Bp$`UW)h_PNMfgtF@yxfI!OO}GB$^$oL;wK6IL)I5vjGF2gC zGJvY6d5THIt|H@ho?)*Y;p}?JZx5h=S%t2XC$2@1z{Is<_*M&vYduonLMiJwq6r|I z^h7;9MIv3DNi;h$*g^+sMT| zRnE1IsvO!DGD>WMs5`$B;YthgN)gTo7v$B&Gd9W!@=5`&C734!mKYJEJX+!q4NLnV zob$t0vkORuXjm#*A(A41ez!><5`&H=nJgGSGsz%f z))Rpa4NwzMyEPwy14gst_Cw}6;%g+b9^|~|^kD+L0MZq>UV<$&rbVG|lV#PsyBQ*mV z1CY3^{HftH!`I3hFO@ZB_s*4VnDV_DE`REwGY@5anFq4%bK#9s-q#;F|Hw?$%;3d+ zbIk{)D;GlL?x(wF^`^`KBhT!@2>@@`Tx1bxdTLA?Y{*?lyk?cGm4Hmf&g(u`b+ zG+xuKzj?iJ>MXu&~nPKRgPdLE!ph>@GnHBC3vL_ znbw2e5Hj_MF=#PlN|sTW4X%q!^LZS|)K?gpjz8dLIB+#M)GP^sma-W+yg>2w@kDhn zFxYTUAyfyW)W;N^27vJuR&u`&i8x#QdB~8VZWp7$(vhLWoT5i#RxRPk&^%$3kXi0s zQm|N)<@a{K`wK*^`4z)0n>v|711hRE(o=O1*?zjrrmVvJ$7az(Yde+QjsT zfSMkKKFU%?Z5K=kSWF#UX|!|2|zKm(2h=XiSt!ch|N>x zr>T7YJY}DSTJ4ced!(I5xK^f%O-QbqnOdfIVJfRwe_~|pWDMaQow<$f3YrCsgf~)e zfn0x5@?_exzPa#vOiH(%zwJUC)6&4)mOHaf{Y|9&sgq|;&g%6q);(W`Q>N}xr2d+2 z{mtw3QzvCi2ByYk@G>~8*i{#1q-84X%pyh^buz;qz%>_++l1cOxc%&nY80d=#z#7C zqgnjuk%m-}!c^zpBi^mHoTx89Zg+AJUm*+|G)u7qdJ6k&5Qqlc#{Fc_X5yCPiKv4N z(01t}srQkJkR4GeqmDltEzZ+(g!)LW!?{ATZ6$_({I8;=Oz!)y^qJAI-#awMW*Moa z=y8?kjTO-_c1FVMb)<#Asyj6>7%(YKX%T+kbA>w@xVYZ!W-0{5sihNRb7)_pClFG<%0 z`Rte0mt;BIN4Zf^@IK_YVN@36l{mwwD#%+R-iJ|LkXLH9T1xVeQJGO=)JDr3`i4j! zdNDt2HPdYBI51X5L@UG@$fyp;$fhq#79$?Qe;_9FZKDoo>85vKJ<;B2XUD(aJ^c5W zu*?UMWRmN};+lqDfTi+$T)hca+6jF$5)AbY*40=d?M61MX6jFGM)j%dmuJ53bmDwfI5U`SP@#f=+ z!QuF5=rLB14311MVb^s~VkJ{fIM5x8Y2#+vhVg>DGIM|)FpWw}hI{3%a4$hs&}eHi z-9$HqJ_C}cmT1k3wdch;@?u?ivF$u&+3oW58Evg88a*9)i+ZlOsJ0=-;Z%&@a_*Mv z;YGjn(@qZT&$kE_3ml!+Z9#M)qS-tQ<=H%8#UR= z?0tVw^Nlrg6&t653zZwp4!9H6!W}Q|nALY?cT7j;8aog=_uy<}$K0Au1ZUTDUa6wM zS5mW8opTYG<95zeGjb`i^O|n`&4V)nqnS$KVNw`aX_u)E8Iu82MG6DOBw|;Q0gGW? zjYvE9piD!JwwO#tjW4&!7??y1rL#L^1`i-@Ix5q^EMmyOL*O}dcWXLTSllVnXR2jk zM*tNpgvF$@WU%<KT6qF*&)6zZ?za0oW27%$3th zvefX7za0%T0QR9(lN zU-m{aIWFryIDl34yWWTDuVUJl_rn3KD(~99h2?Qy`KD3Vatg>3-2FA&m-r+X1G|Op zYe9M37s2uZ<)TX9C^I6iyPC^9i@yR5Qc}l%wa3o)}9ZIfjx<% zt@BWmpEY8L8v(!fBTK>g)7@6ePk$tom2WX+r3AMGua^j(xig$)%WIWTm_k+!i>NmV z;acF>6}9gWs2TwUw1KJ_rj}^De)DGTEC{ep*qZ*7Ik61}%%%$dI0JFhj8EnQREQc?!1@< zoGt%3%MfVAENIRlcAEvpt=RSyslA*K+rrqz3}Z9zy_?*DHRX|EQy2u{BX{`AhcDM| zoeSSO<^7X8-#GBc2WIa3&OOuSLS)R`&UT)=>&1i5A0#;0|H{xyLkL`GVC0VyIAw>X z6W<&~AagJ~I=7mE<<kZScss>T+9&kUMKFR>6IiMCp5Syf)5)a#eW49-#Qa@xw#g}Rn&TNOH$ZRF@e zm2+*QDu=casx8ECL$w8YrK%YCw@|$bwS)@tN}0;xeg^kBtV{|y5JO2HnPb>ttJxJ( zN2TOtLkzZ+NdhsL8RjWp!GXbU)LlSnR!=OX(KGiFxs%8~kV8#AUI$aWv5M9s8HUHt z(OeGI(sKQBq%*fvg#k&}=2GUGQ^wU*k5V%}PXsC#&1@w?n19J?snW&g&FxeV(@1qu z=pG`UA@W%whd@%)CF~O%*B_O!xlP_iZms(SCmHCwPW=LedxB$@`|23e4ZUW(e)!eH z7vkUg927$f+`yT`^44tCtPXOe3hv5VNy)Q*E^-Tk(|$%SMYdkkt-pDF>ulr}8I=K2 z>sTz8sSX*F0aQh59mOPKSCIktWy(P9*(w>77eey1Ulw%)aJlQu5dRmB%9tVO&9cLQ zqlZ{!dJ?K%v$79!2tS}B@jBLy`bC~PLq}31V#;l`z}iu%YjJZ~#%Nx_TsbNixN@lR z!gUs)w@!ymN1?7HM_0~66gmn*iL)82@4-txlXT%*1UoIvpFBb21l;NMb?^;x9*Xj6!hrzDxT$g^kSXObtkm(LANbG zl0`6*LGaHrl6}o>$W4a8`Tb}~9_LS^rCag%#4&yqDqcAxj{nbSS)S@4q6SvwA(1RM z{K<-Fd9;G2_heab1mWU7)PrMm&i1+uDZx%x~g z&kr%+i`H6GyeeNWo{BmH@>YwOD9?p_0K?_9l|xo_E!Vaxfk3vALsnJJwT-GA+NS3j z%G~*FFchohmUAB*R;l*5-iPYf;+e_&;bMX+@7lhF<#Av6hBDW3Hi!}a7{8w~n_o@KGXk>0nk_cCZ z{}KEz$N$P?g|Px&YOCHQB?Y&_ou&{|l+jneID+k^TtewKQ``xP?j^F1$Pkf}Ak4n5|gy4(XxxkIcvh*M6>Mz&OXFxECG^>S@kBxluu(2~q-+yQP8 zn;KP53-W|1jn?4RZ>i@tHlSk5;|(F}TCFgvHB%|9wUeb_@oQohIEBvJD6PuP!;L}e zT#()-tk`M2vtPBU>v zzWWg6JalE#Zk#j2vzvC$-Mk0E*_-!VsiFX>O!v%1_9A%kF3O1Py?EE9$boCR^>@We zlL<0Fno^7DGF2pFGJyI>Q>vIm>?$&_woY9IVWgcKlxe6@#E_90mKi*Nv}un_1G9)x zMiI}Lrw1c>WVs9X$uyJ`F_gQ|E6dHqWe`tFg(G`q_#&*4dF^*Z=kx;$g$LTXj|mTZ z0A34Q|Bh@HgK*xif-ag}_|7Tb8}OYwvY9M@-H@%vj|#Xn^p^}yOD&n3R~__a#nB+( zbf|Z~4NQ9tpKYO;tM)4oOml#$uH>rkQo2#mLd)+ym1hXx^yotDoY;n;d$3(0zV~;%vtwbNdL`)Med|%9jp<`+-d3Q5zl253BwwBXF zltT3&zqGH5!wkS&TCj!FFTFc=LRr(wra>{QC?9M1oHUPOAEM*Qkwgmf%1*wu6KQ3_ z+=IhEHkwE&vs12EvM<^0RP3sad5D_6ok$;ueSurvhD6;d`i|ttZMQnfmMmsr`OL&lSzoARDBUp)2oQ`6oTKA&0VFgjuF+M{Q%k57H3pAq}YwK=_>_VH0m z*?3s2RYT0Na?N7K>az@d5uD|a;<_%4KoJ{2e!*H!kBJ}dTj{E8bVVn+-4pXncqe=@ zX-;3ODJ8m1H7Y+6aRoBL7+SM*W?K#~RwzpcUhGT^Q9Mic(ZtD^Rl|Ds**%}@)2JkMCkXYPs;3_Q^26tDyB6MX zCAl%)X0N^QHSg^l4=q!HmDl;SIk` z(dxTz?%2B_@Z$|7d%KGBb~cjl`=8jge)I3!@Tfhdm-WVN=19C@tSZDRbCLvbP`uz9 z2Z!~>{rruSO^A(khsobirHQ$L`thzWbU;1g+_j}vkje#F;EZN&#d>%dn6*)b| zc6^J)a-mr4_}JjYNCM%~SZsVEK0jjf^+qhqm{c{|NfG&N zB2z@3B=QX+(?l*1`3^{~CdM}C9v+Cr(`j?KA3JuZP<=%#HZTI9JUBd%9!Oy4x)`?i z3)>P=j6Rk<>|-$rEi+PuK#CsE|9JvW=zVV>u<`w}Kw$m*W&S|J`=x=vuJ_A)AR%Ai z*7rmHz~=W${egp?_e*`q4tWE2dp^{?fqOjfX+#-FV)YK7N_o(Q>G*&YhIuEVIA?}@%OHhx}Q0swQ>-YS^2i1wCtV^Dp4k)6V@ zm9^q=Xm88+yq?yb_){_DKkltNKUo!bF-CY^=--gjfk^9y7|&)ho;_o)z0_Xy|A6 zk)80{yUhnEi4B%}D8zIwz}K3|v?@#vKTOdEBI}4Wf*fk{nzvAdNR#*URzioQ)!-X- zF5*0F8SXgu^?7VNep=2;G=#YAq+kP)Y>yuXa<>rkRwB(r$TPOda$cgj_&QrCY%@Pc z44p)}h|nZz+rWojw8WB70B~r)H?(ZsHgKHwOX%E|Ta!1a@ZBs`Ha+yz6%b4rd6NE} zb7dQszjaRpUQO$t8+sZ}yX(&mU5dcDX?+$RO(2L|)79UczJ4m5_4A}F6tU+bd=)i7 znzc$?Rx!$eXDnsRQ^k_U5x`X!3bN9T^)-y9g#=mGaa3xV7uz4q)1uy87o&Q$Ju9;k6kAbvS!u5y&8F~V`c;v~mCQJ#;T z7!j90t`}(QqT{+M=h{Y9&a%uD;+I1D5_VBkb3ox;6y+QMOG%+NNVg}4Q%MP}fB(pL zT&%R_3)>2E^Sn`sIiu#?BE0k=zG}R9kGTi2;jMsMYTG)qpE&q-c3)e{mURyA60T`C z??Nrt+coC^@|UMt4i``@Gu1&iRm%qYdnoi7BFbeojHrFI&l~P(o3_Lt{VI;@aIdmS zQIQJcp*e^9{GHnTnZfM7x$q_|b3T3kG=zlo#rx-44o*823qrBGIWst`gFFLm?PlzD zwtFtph)|YNvxAo+o3H8C-@LwgmUcf=DLhPCSS#%^RUuUI)b?} zHqWacGQvxihg9GGEWlxfZ0da5pJ%f)f?fD<31u}Jqiw^6yH=skQ z{&9T+g`JNj-h&)g=ncqu)$s-*YpPJ4bi4y82@hi6$#czhk*(R<$Kr4mbfLuo7cmov-tyDP78kLK@fl7fPp<`B8beHk*n}A|* zLyMMYN~vP|`7ZQnSSda_TirK_G<9CYr=c1MiO`W3|9$vR(lsqBrv0gDuN^YAV80#n zuZ(KU3oF@PzueRkMcpP1XI!ElMCf!8iOVqSn+CL6N)drr5o zi^EgEavwlQxDPfhZs+yeJ&VJCh@~~xiazN7X4#mBV$RMjBj}pu*XfPi%ahe<1w-R z$CUOg)n#A7+sPdXe#?>G)?sFd{}q{TZH}|e?kCXNWsVcqmx#RJU~S9WN2k5DkP|@& zx>o#V$MIx(GPg1xT5uf#%j(BfQWJm^z>}r5XUCuX?3CxP^<7V$ICElF-<5$j0OZVL z*K};*yDMAqO3h0(2uuSUg1oevr(bAzW8=3sB5CG6M!t<@NxS&pRejgg2^o_CvQAkX zm%(AhY|Qg?UCHAJ%%LeY(*U7X6*7>mkU>-{VyHG-!!uB=TREPIYAA%UJBz1qG8O98#{iXNywoCnNY7AVA|;|}Ry7#VMi z`WQUBOF)C6zhu*rT;b%l!wsgTP}GlUsie2jHYw#>v?V}LEpx1#<-kl`NeZs^5>a^{ zqJpHYknpp;JlD^Qlt0;?#*h9x#1)Kl~&A~YY!!Fme~zG@(qLoW@Zc_S~4 zp(Sh##qTl=4x0a&s@g=PiHOVKX&WSqxLm9T$?a5z={f(9LNwl`ia;#Mm#aAWK418` z(IMEL$a#r&WXLtT4N?xKqf{RakGA3J#m?tDvsLGAdlOdji8B+k`l^hNku&%uo>lyB zPG2=OA!9N?1CPaV85~yZs$!m}$|R3HfSiS2FTEOkoB9XO0^GTOy-AO;s$9Dpy&>tv zck1#bM`h1ktKCzpX~o*ji+$0SV3+5-9#BSgWgfxGH1GI{Xc1ePHYL8SC9h0d4YQ2L zSkpV?`1<8HzXY+Y!2y3=zhd>hi0?tP^;cM##@Ed+r>(y^5t7!;D}d@Qa>~)?BVLs$ z$DSn!MpB4!C=rX9U{vfF0Yhp5h3)Rpmz@VjKX94`462oA1T;l}9||6AfDL8%9|dAx0d@(muRteyE?18J~-W^S-Du+ZQFp>tw|6 zCd;FdXgOOBQW32XJr&B1Lyo@gF6ynuG7yikmg@NAyAtmKJz_BgQ0P06<$wT|_g&H6 zSj!n7mNu5H7WX82w&)>C9LpAz$9-Y31NP?n=-Vp4p&M-Lg7Ro<1(q}vdS}W&QdN*w zss@%bUSXfPLT#|`I97;PE*is9i9)<`b6umhAg@#zwUp+mAS#VIY$jgm7+LY=qKU&Qd%E_8&jp)}yKUvbaS0AmxxYs~6 zbb1v-Pe?Mz%GFQ<#6c1i-?{Dtg@%Y6Bk~kTo??l1 zWHV1uR+I=yE=>WOqaAD5E~HW&II8!EHRDkdX>Md+8@6NI;=eW(Fb!PCj1&1ikaztQ zssn*W>PJF@W+}07{iUzjfvSrC#+B;^chI2m6*g`vGN{n_6%nv52uY2LZmbk^ep~D5 zE$n11q$N8GHorrnP~P2o9eL29RwIe9oNLwIq9^ef5t@vaDi(4Xl648zIsOYh$`wjf z%y~|5ubAyrX*ZF@%%*(3&a=caN+d~SjL36D9tW{&(4g4-BBd*Zof1O{Jmwi-tu;4v z;&N%2smy2(K$1#xtK9@rk^2a0OZ|i?X{6%pt%}W4d*3V#KehkN{<96gci`tft#ZrX zz{bk9#oL(KH>-m@a|ceavEDHksYfV_jiW&>MW9Qv{^s?Kv-H~}Dusv1s%oWOrYdAi z22d4QRTYznT}1}=xuXo!p4lgZ@f{V!L(Qu|UZ01WP9i-y!Rd^LP^b_e3Xb`RNMeKc@*DQPVlD?lb8SJ-eR%P&T zS52MFG}v?*&^%CzT^Yf{LVtmnLz{7uGmtjJr5rM^SlSGe+t7<$ZRl^(m-RN0za#Q} zB0nJVLlCjgoe}<4d|93-5c2bK0I(&f2aootKZW=xHX-4$uaqOe0Ve#JMF#o z1e^jNL?Ro7PM*m0L;6X#$ZLkzf;&-aYQn|qB#PQ@LpTAzC9Dg#NK9&pOXS4A$eU54JFLx_`tw~86TKrUeDGRqarRb%iS0XP(F6eXhJ7zlOs`kuld;hz3 z;1@x!r|rYgPR~7_pKC-v+~Y0rZ27qc^5N=;XZ6oDkoVSW{?e(_^S+gD`a@6L^IP|v VU6JXSEw8`oZ@}2*zlpW^{{s{`*~9<< literal 0 HcmV?d00001 diff --git a/jetson/ros2_ws/src/saltybot_uwb_logger/test/test_accuracy_stats.py b/jetson/ros2_ws/src/saltybot_uwb_logger/test/test_accuracy_stats.py new file mode 100644 index 0000000..8d48d10 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_logger/test/test_accuracy_stats.py @@ -0,0 +1,116 @@ +"""Unit tests for saltybot_uwb_logger.accuracy_stats (Issue #634). No ROS2 needed.""" + +import math +import sys +import os + +import numpy as np +import pytest + +sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..")) +from saltybot_uwb_logger.accuracy_stats import compute_stats, RangeAccum + + +def _circle(n, r, cx=0.0, cy=0.0): + a = np.linspace(0, 2 * math.pi, n, endpoint=False) + return cx + r * np.cos(a), cy + r * np.sin(a) + + +class TestComputeStats: + def test_zero_error(self): + xs = np.full(5, 2.0); ys = np.full(5, 3.0) + s = compute_stats(xs, ys, 2.0, 3.0) + assert abs(s.cep50) < 1e-9 + assert abs(s.rmse) < 1e-9 + assert abs(s.max_error) < 1e-9 + + def test_known_bias(self): + rng = np.random.default_rng(0) + xs = rng.normal(4.0, 0.01, 100); ys = rng.normal(2.0, 0.01, 100) + s = compute_stats(xs, ys, 3.0, 2.0) + assert abs(s.bias_x - 1.0) < 0.05 + assert abs(s.bias_y) < 0.05 + + def test_cep50_below_cep95(self): + rng = np.random.default_rng(1) + xs = rng.normal(0, 0.1, 500); ys = rng.normal(0, 0.1, 500) + s = compute_stats(xs, ys, 0.0, 0.0) + assert s.cep50 < s.cep95 + + def test_cep50_is_median(self): + rng = np.random.default_rng(2) + xs = rng.normal(0, 0.15, 1000); ys = rng.normal(0, 0.15, 1000) + errors = np.sqrt(xs**2 + ys**2) + s = compute_stats(xs, ys, 0.0, 0.0) + assert abs(s.cep50 - float(np.median(errors))) < 1e-9 + + def test_rmse(self): + xs = np.array([0.1, -0.1, 0.2, -0.2]); ys = np.zeros(4) + s = compute_stats(xs, ys, 0.0, 0.0) + assert abs(s.rmse - math.sqrt(np.mean(xs**2))) < 1e-9 + + def test_max_error(self): + xs = np.array([0.0, 0.0, 1.0]); ys = np.zeros(3) + s = compute_stats(xs, ys, 0.0, 0.0) + assert abs(s.max_error - 1.0) < 1e-9 + + def test_circle_symmetry(self): + xs, ys = _circle(360, 0.12) + s = compute_stats(xs, ys, 0.0, 0.0) + assert abs(s.bias_x) < 1e-6 + assert abs(s.cep50 - 0.12) < 1e-6 + + def test_std_2d_pythagorean(self): + rng = np.random.default_rng(3) + xs = rng.normal(0, 0.1, 200); ys = rng.normal(0, 0.2, 200) + s = compute_stats(xs, ys, 0.0, 0.0) + assert abs(s.std_2d - math.sqrt(s.std_x**2 + s.std_y**2)) < 1e-9 + + def test_n_samples(self): + xs = np.ones(42); ys = np.zeros(42) + s = compute_stats(xs, ys, 0.0, 0.0) + assert s.n_samples == 42 + + def test_too_few_raises(self): + with pytest.raises(ValueError): + compute_stats(np.array([1.0]), np.array([1.0]), 0.0, 0.0) + + def test_length_mismatch_raises(self): + with pytest.raises(ValueError): + compute_stats(np.array([1.0, 2.0]), np.array([1.0]), 0.0, 0.0) + + +class TestRealistic: + def test_10cm_cep50(self): + rng = np.random.default_rng(42) + xs = rng.normal(3.0, 0.07, 500); ys = rng.normal(2.0, 0.07, 500) + s = compute_stats(xs, ys, 3.0, 2.0) + assert 0.05 < s.cep50 < 0.20 + + def test_biased_uwb(self): + rng = np.random.default_rng(7) + xs = rng.normal(3.15, 0.05, 300); ys = rng.normal(2.0, 0.05, 300) + s = compute_stats(xs, ys, 3.0, 2.0) + assert abs(s.bias_x - 0.15) < 0.02 + + +class TestRangeAccum: + def test_empty(self): + a = RangeAccum(0) + assert a.mean == 0.0 and a.std == 0.0 and a.count == 0 + + def test_single_std_zero(self): + a = RangeAccum(0); a.add(2.0) + assert a.std == 0.0 + + def test_mean_std(self): + a = RangeAccum(0) + for v in [1.0, 2.0, 3.0, 4.0, 5.0]: + a.add(v) + assert abs(a.mean - 3.0) < 1e-9 + assert abs(a.std - float(np.std([1,2,3,4,5], ddof=1))) < 1e-9 + assert a.count == 5 + + +if __name__ == "__main__": + pytest.main([__file__, "-v"]) diff --git a/jetson/ros2_ws/src/saltybot_uwb_logger_msgs/CMakeLists.txt b/jetson/ros2_ws/src/saltybot_uwb_logger_msgs/CMakeLists.txt new file mode 100644 index 0000000..8d50687 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_logger_msgs/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.8) +project(saltybot_uwb_logger_msgs) + +find_package(ament_cmake REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/AccuracyReport.msg" + "srv/StartAccuracyTest.srv" + DEPENDENCIES std_msgs +) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/jetson/ros2_ws/src/saltybot_uwb_logger_msgs/msg/AccuracyReport.msg b/jetson/ros2_ws/src/saltybot_uwb_logger_msgs/msg/AccuracyReport.msg new file mode 100644 index 0000000..47a9531 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_logger_msgs/msg/AccuracyReport.msg @@ -0,0 +1,32 @@ +# AccuracyReport.msg — UWB positioning accuracy test results (Issue #634) +# +# Published on /saltybot/uwb/accuracy_report after StartAccuracyTest completes. + +std_msgs/Header header + +float64 truth_x_m +float64 truth_y_m + +uint32 n_samples + +float64 mean_x_m +float64 mean_y_m + +float64 std_x_m +float64 std_y_m +float64 std_2d_m + +float64 bias_x_m +float64 bias_y_m + +float64 cep50_m +float64 cep95_m +float64 max_error_m +float64 rmse_m + +uint8[] anchor_ids +float64[] anchor_range_mean_m +float64[] anchor_range_std_m + +string test_id +float64 duration_s diff --git a/jetson/ros2_ws/src/saltybot_uwb_logger_msgs/package.xml b/jetson/ros2_ws/src/saltybot_uwb_logger_msgs/package.xml new file mode 100644 index 0000000..d5db3ea --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_logger_msgs/package.xml @@ -0,0 +1,21 @@ + + + + saltybot_uwb_logger_msgs + 0.1.0 + Message and service definitions for UWB logger (Issue #634). + sl-uwb + Apache-2.0 + + ament_cmake + rosidl_default_generators + + std_msgs + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/jetson/ros2_ws/src/saltybot_uwb_logger_msgs/srv/StartAccuracyTest.srv b/jetson/ros2_ws/src/saltybot_uwb_logger_msgs/srv/StartAccuracyTest.srv new file mode 100644 index 0000000..df33594 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_logger_msgs/srv/StartAccuracyTest.srv @@ -0,0 +1,15 @@ +# StartAccuracyTest.srv — trigger UWB accuracy test at a known position (Issue #634) + +# ── Request ──────────────────────────────────────────────────────────────── +float64 truth_x_m +float64 truth_y_m +uint32 n_samples +float64 timeout_s +string test_id + +--- + +# ── Response ─────────────────────────────────────────────────────────────── +bool success +string message +string test_id