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

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

Merged
sl-jetson merged 1 commits from sl-perception/issue-655-nav2-integration into main 2026-03-18 07:57:03 -04:00
Conversation 0 Commits 1 Files Changed 8 +1001 -46
sl-perception commented 2026-03-17 11:39:40 -04:00
Collaborator
Copy Link

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

## 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](https://claude.com/claude-code)
sl-perception added 1 commit 2026-03-17 11:39:40 -04:00 
AMCL-based autonomous navigation on pre-built static maps, wired to
VESC CAN differential-drive odometry (/odom, Issue #646) and RPLiDAR
(/scan) as the primary sensor sources.

New files (saltybot_nav2_slam):
- config/amcl_nav2_params.yaml — complete Nav2 + AMCL parameter file
  with inline global/local costmap configs (required by nav2_bringup):
  · AMCL: DifferentialMotionModel, 500–3000 particles, z-weights=1.0,
    odom_frame=/odom, scan_topic=/scan
  · Global costmap: static_layer + obstacle_layer (LiDAR) +
    inflation_layer (0.55m radius)
  · Local costmap: 4m rolling window, obstacle_layer (LiDAR) +
    inflation_layer, global_frame=odom
  · DWB controller: 1.0 m/s max, diff-drive constrained (vy=0)
  · NavFn A* planner
  · Recovery: spin + backup + wait
  · Lifecycle managers for localization and navigation
- launch/nav2_amcl_bringup.launch.py — orchestrates:
  1. sensors.launch.py (RealSense + RPLIDAR, conditional)
  2. odometry_bridge.launch.py (VESC CAN → /odom)
  3. nav2_bringup localization_launch.py (map_server + AMCL)
  4. nav2_bringup navigation_launch.py (full nav stack)
  Exposes: map, use_sim_time, autostart, params_file, include_sensors
- maps/saltybot_map.yaml — placeholder map descriptor (0.05m/cell)
- maps/saltybot_map.pgm — 200×200 P5 PGM, all free space (10m×10m)
- test/test_nav2_amcl.py — 38 unit tests (no ROS2 required):
  params structure, z-weight sum, costmap layers, DWB/NavFn validity,
  recovery behaviors, PGM format, launch file syntax checks

Updated:
- saltybot_bringup/launch/nav2.launch.py — adds nav_mode argument:
  nav_mode:=slam (default, existing RTAB-Map behaviour unchanged)
  nav_mode:=amcl (new, delegates to nav2_amcl_bringup.launch.py)
- saltybot_nav2_slam/setup.py — installs new launch, config, maps
- saltybot_nav2_slam/package.xml — adds nav2_amcl, nav2_map_server,
  nav2_behaviors, dwb_core, nav2_navfn_planner exec_depends

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

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
sl-jetson merged commit fdda6fe5ee into main 2026-03-18 07:57:03 -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#664
No description provided.
Delete Branch "sl-perception/issue-655-nav2-integration"

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?