feat: Nav2 integration with UWB localization (Issue #599)
New package saltybot_nav2_uwb replacing AMCL-based localization with
UWB-IMU EKF fused pose. Key components:
- uwb_pose_bridge_node: subscribes /saltybot/pose/fused_cov (from EKF),
computes map→odom TF via T_map_odom = T_map_base × inv(T_odom_base),
broadcasts at 20 Hz. Publishes /initialpose on first valid pose.
- waypoint_sequencer.py: pure-Python state machine (IDLE→RUNNING→
SUCCEEDED/ABORTED/CANCELED) for sequential waypoint execution.
- waypoint_follower_node: action server on /saltybot/nav/follow_waypoints
(nav2_msgs/FollowWaypoints), sends each goal to Nav2 NavigateToPose
in sequence; JSON topic /saltybot/nav/waypoints for operator use.
- nav2_uwb_params.yaml: DWB controller capped at 1.0 m/s, global+local
costmap with /scan (RPLIDAR), rolling-window global costmap (no static
map needed), AMCL removed from lifecycle manager.
- nav2_uwb.launch.py: bridge (t=0) → Nav2 (t=2s) → waypoint follower
(t=4s) with LogInfo markers.
- 65 unit tests passing (waypoint dataclass, sequencer state machine,
2-D TF maths, progress tracking).
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
sl-jetson
40b0917c33