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

feat: Nav2 with UWB localization (Issue #599) #607

Merged
sl-jetson merged 1 commits from sl-jetson/issue-599-nav2-uwb into main 2026-03-14 15:54:53 -04:00
Conversation 0 Commits 1 Files Changed 11 +1766
sl-jetson commented 2026-03-14 15:04:06 -04:00
Collaborator
Copy Link

Summary

Implements Issue #599 — Nav2 navigation stack with UWB-based localization replacing AMCL.

New package: saltybot_nav2_uwb

uwb_pose_bridge_node — Subscribes /saltybot/pose/fused_cov (UWB-IMU EKF) + /odom, 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 — Pure-Python state machine (IDLE→RUNNING→SUCCEEDED/ABORTED/CANCELED), ROS2-free, unit-testable.

waypoint_follower_node — Action server /saltybot/nav/follow_waypoints (nav2_msgs/FollowWaypoints). Drives each goal to Nav2 NavigateToPose sequentially. JSON topic /saltybot/nav/waypoints for operator use. Cancel topic /saltybot/nav/cancel.

nav2_uwb_params.yaml — DWB capped at 1.0 m/s, RPLIDAR-only costmap (rolling window), AMCL removed from lifecycle manager.

nav2_uwb.launch.py — bridge (t=0) → Nav2 (t=2s) → waypoint follower (t=4s).

Tests: 65 passing

Waypoint dataclass, sequencer state machine, 2-D TF math, quaternion↔yaw roundtrip.

## Summary Implements Issue #599 — Nav2 navigation stack with UWB-based localization replacing AMCL. ### New package: `saltybot_nav2_uwb` **`uwb_pose_bridge_node`** — Subscribes `/saltybot/pose/fused_cov` (UWB-IMU EKF) + `/odom`, 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`** — Pure-Python state machine (IDLE→RUNNING→SUCCEEDED/ABORTED/CANCELED), ROS2-free, unit-testable. **`waypoint_follower_node`** — Action server `/saltybot/nav/follow_waypoints` (nav2_msgs/FollowWaypoints). Drives each goal to Nav2 NavigateToPose sequentially. JSON topic `/saltybot/nav/waypoints` for operator use. Cancel topic `/saltybot/nav/cancel`. **`nav2_uwb_params.yaml`** — DWB capped at 1.0 m/s, RPLIDAR-only costmap (rolling window), AMCL removed from lifecycle manager. **`nav2_uwb.launch.py`** — bridge (t=0) → Nav2 (t=2s) → waypoint follower (t=4s). ### Tests: 65 passing Waypoint dataclass, sequencer state machine, 2-D TF math, quaternion↔yaw roundtrip.
sl-jetson added 1 commit 2026-03-14 15:04:06 -04:00 
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 merged commit 2460ba27c7 into main 2026-03-14 15:54:53 -04:00
Sign in to join this conversation.
Reviewers
No Reviewers
Labels
No items
No Label
Milestone
No items
No Milestone
Projects
Clear projects
No project
Assignees
Clear assignees
shaytac sl-controls sl-firmware sl-jetson sl-mechanical sl-perception sl-uwb sl-webui seb
No Assignees
1 Participants
Notifications
Due Date
No due date set.
Dependencies

No dependencies set.

Reference: seb/saltylab-firmware#607
No description provided.
Delete Branch "sl-jetson/issue-599-nav2-uwb"

Deleting a branch is permanent. Although the deleted branch may continue to exist for a short time before it actually gets removed, it CANNOT be undone in most cases. Continue?