sl-perception created pull request seb/saltylab-firmware#664

sl-perception
Tue, 17 Mar 2026 11:39:40 -0400

664#feat: Nav2 AMCL integration (Issue #655)#

Summary

AMCL-based autonomous navigation stack for SaltyBot, wired to VESC CAN odometry (Issue #646) and RPLiDAR as sensor sources.

New: config/amcl_nav2_params.yaml

Complete Nav2 params with inline costmap configs (required by nav2_bringup):

AMCL: DifferentialMotionModel, 500–3000 particles, z-weights sum=1.0, odom_frame=/odom, scan_topic=/scan

Global costmap: static_layer (from /map) + obstacle_layer (LiDAR /scan) + inflation_layer (0.55m)

Local costmap: 4m rolling window, obstacle_layer (LiDAR) + inflation_layer, global_frame=odom

DWB controller: 1.0 m/s max, diff-drive constrained (vy_samples=1, min_vel_y=0)

NavFn A* planner

Recovery behaviors: spin + backup + wait (per spec)

Lifecycle managers: localization (map_server+amcl), navigation (controller+planner+behaviors+bt_nav+smoother)

New: launch/nav2_amcl_bringup.launch.py

Orchestrates 4-step startup:

1. sensors.launch.py (conditional — skip if already running)

2. odometry_bridge.launch.py → /odom (VESC CAN IDs 61/79)

3. localization_launch.py → map_server + AMCL → TF map→odom

4. navigation_launch.py → controller + planner + behaviors + BT nav

Arguments: map, use_sim_time, autostart, params_file, include_sensors

New: maps/saltybot_map.{yaml,pgm}

10m×10m placeholder free-space map (200×200 P5 PGM, 0.05m/cell). Replace with saved SLAM map via map_saver_cli.

Updated: saltybot_bringup/launch/nav2.launch.py

Adds nav_mode argument:

nav_mode:=slam (default) — existing RTAB-Map behaviour unchanged

nav_mode:=amcl — delegates to nav2_amcl_bringup.launch.py

Tests: test/test_nav2_amcl.py — 38 unit tests (no ROS2 required)

Covers: params structure, z-weight sum=1.0, costmap layers, recovery behaviors, inflation > robot radius, lifecycle node lists, DWB/NavFn validity, PGM format + dimensions, launch file syntax checks.

All 58 tests pass (38 new + 20 from Issue #646).

Test plan

pytest jetson/ros2_ws/src/saltybot_nav2_slam/test/ -v → 58 passed

ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py — starts without errors

ros2 topic hz /odom → ~50 Hz from VESC

ros2 topic hz /scan → ~5.5 Hz from RPLIDAR

ros2 topic echo /amcl_pose — pose updates as robot moves

ros2 run tf2_tools view_frames — map→odom→base_link chain present

Send NavigateToPose goal → DWB generates /cmd_vel trajectory

Recovery: block path → spin → backup → wait triggers

AMCL mode via bringup: ros2 launch saltybot_bringup nav2.launch.py nav_mode:=amcl

🤖 Generated with Claude Code