sl-perception
2180b61440
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/<id> 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 <noreply@anthropic.com>
37 lines
1.3 KiB
XML
37 lines
1.3 KiB
XML
<?xml version="1.0"?>
|
|
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
|
<package format="3">
|
|
<name>saltybot_uwb_position</name>
|
|
<version>0.1.0</version>
|
|
<description>
|
|
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.
|
|
</description>
|
|
<maintainer email="sl-perception@saltylab.local">sl-perception</maintainer>
|
|
<license>MIT</license>
|
|
|
|
<buildtool_depend>ament_python</buildtool_depend>
|
|
|
|
<depend>rclpy</depend>
|
|
<depend>geometry_msgs</depend>
|
|
<depend>std_msgs</depend>
|
|
<depend>tf2_ros</depend>
|
|
<depend>saltybot_uwb_msgs</depend>
|
|
|
|
<!-- numpy is a system dep on Jetson (python3-numpy) -->
|
|
<exec_depend>python3-numpy</exec_depend>
|
|
|
|
<test_depend>ament_copyright</test_depend>
|
|
<test_depend>ament_flake8</test_depend>
|
|
<test_depend>ament_pep257</test_depend>
|
|
<test_depend>python3-pytest</test_depend>
|
|
|
|
<export>
|
|
<build_type>ament_python</build_type>
|
|
</export>
|
|
</package>
|