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

Merge pull request 'feat: Nav2 AMCL integration (Issue #655)' (#664) from sl-perception/issue-655-nav2-integration into main

Browse Source
This commit is contained in:
sl-jetson 2026-03-18 07:57:02 -04:00
commit fdda6fe5ee
8 changed files with 1001 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

141
jetson/ros2_ws/src/saltybot_bringup/launch/nav2.launch.py
View File

@ -1,76 +1,139 @@
"""
nav2.launch.py — Nav2 autonomous navigation stack for SaltyBot
Starts the full Nav2 navigation stack (controllers, planners, behavior server,
BT navigator, velocity smoother, lifecycle managers).
Supports two localization modes via the 'nav_mode' argument:
Localization is provided by RTAB-Map (slam_rtabmap.launch.py must be running):
/map — OccupancyGrid (static global costmap layer)
/rtabmap/odom — Odometry (velocity smoother + controller feedback)
TF map→odom — published by RTAB-Map
nav_mode:=slam (default)
Uses RTAB-Map for odometry and live map building.
Requires slam_rtabmap.launch.py to be running.
Sources: /rtabmap/odom (pose), /map (from RTAB-Map)
Output:
/cmd_vel — consumed by saltybot_bridge → STM32 over UART
nav_mode:=amcl
Uses VESC CAN odometry (/odom) + AMCL on a pre-built static map.
Launches: sensors, VESC odometry bridge, map_server, AMCL, Nav2 stack.
Sources: /odom (VESC CAN IDs 61/79), /scan (RPLIDAR)
Map: saltybot_nav2_slam maps/saltybot_map.yaml (override with map:=...)
Profile Support (Issue #506)
────────────────────────────
Supports profile-based parameter overrides via 'profile' launch argument:
profile:=indoor — conservative (0.2 m/s, tight geofence, aggressive inflation)
profile:=outdoor — moderate (0.5 m/s, wide geofence, standard inflation)
profile:=demo — agile (0.3 m/s, tricks enabled, enhanced obstacle avoidance)
nav_mode:=slam profile support (Issue #506)
profile:=indoor — conservative (0.2 m/s, tight geofence)
profile:=outdoor — moderate (0.5 m/s)
profile:=demo — agile (0.3 m/s, tricks enabled)
Run sequence on Orin:
1. docker compose up saltybot-ros2 # RTAB-Map + sensors
2. docker compose up saltybot-nav2 # this launch file
# SLAM mode (existing behaviour):
ros2 launch saltybot_bringup nav2.launch.py
# AMCL mode with saved map:
ros2 launch saltybot_bringup nav2.launch.py nav_mode:=amcl \
map:=/mnt/nvme/saltybot/maps/my_map.yaml
"""
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, LogInfo
from launch.actions import (
DeclareLaunchArgument,
GroupAction,
IncludeLaunchDescription,
LogInfo,
)
from launch.conditions import LaunchConfigurationEquals
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
def generate_launch_description():
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
bringup_dir = get_package_share_directory('saltybot_bringup')
def generate_launch_description() -> LaunchDescription:
nav2_bringup_dir = get_package_share_directory("nav2_bringup")
bringup_dir = get_package_share_directory("saltybot_bringup")
nav2_slam_dir = get_package_share_directory("saltybot_nav2_slam")
# ── Arguments ─────────────────────────────────────────────────────────────
nav_mode_arg = DeclareLaunchArgument(
"nav_mode",
default_value="slam",
choices=["slam", "amcl"],
description=(
"Localization mode. "
"slam: RTAB-Map live map (requires slam_rtabmap.launch.py running); "
"amcl: static map + AMCL particle filter + VESC odometry (Issue #655)."
),
)
# Profile argument (Issue #506)
profile_arg = DeclareLaunchArgument(
"profile",
default_value="indoor",
choices=["indoor", "outdoor", "demo"],
description="Launch profile for parameter overrides (Issue #506)"
description="Nav2 parameter profile for slam mode (Issue #506)",
)
profile = LaunchConfiguration('profile')
nav2_params_file = os.path.join(bringup_dir, 'config', 'nav2_params.yaml')
bt_xml_file = os.path.join(
bringup_dir, 'behavior_trees', 'navigate_to_pose_with_recovery.xml'
map_arg = DeclareLaunchArgument(
"map",
default_value=os.path.join(nav2_slam_dir, "maps", "saltybot_map.yaml"),
description="Map YAML path for AMCL mode",
)
nav2_launch = IncludeLaunchDescription(
use_sim_time_arg = DeclareLaunchArgument(
"use_sim_time",
default_value="false",
)
# ── Mode: SLAM (original behaviour — RTAB-Map + navigation_launch.py) ────
slam_nav2_params = os.path.join(bringup_dir, "config", "nav2_params.yaml")
slam_bt_xml = os.path.join(
bringup_dir, "behavior_trees", "navigate_to_pose_with_recovery.xml"
)
slam_mode = GroupAction(
condition=LaunchConfigurationEquals("nav_mode", "slam"),
actions=[
LogInfo(msg="[nav2] Mode: SLAM (RTAB-Map odometry + live map)"),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(nav2_bringup_dir, 'launch', 'navigation_launch.py')
os.path.join(nav2_bringup_dir, "launch", "navigation_launch.py")
),
launch_arguments={
'use_sim_time': 'false',
'autostart': 'true',
'params_file': nav2_params_file,
'default_bt_xml_filename': bt_xml_file,
# RTAB-Map publishes /map with transient_local QoS — must match
'map_subscribe_transient_local': 'true',
"use_sim_time": "false",
"autostart": "true",
"params_file": slam_nav2_params,
"default_bt_xml_filename": slam_bt_xml,
"map_subscribe_transient_local": "true",
}.items(),
),
],
)
profile_log = LogInfo(
msg=['[nav2] Loaded profile: ', profile]
# ── Mode: AMCL (static map + AMCL + VESC odometry, Issue #655) ───────────
amcl_mode = GroupAction(
condition=LaunchConfigurationEquals("nav_mode", "amcl"),
actions=[
LogInfo(msg="[nav2] Mode: AMCL (static map + VESC odometry, Issue #655)"),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(
nav2_slam_dir, "launch", "nav2_amcl_bringup.launch.py"
)
),
launch_arguments={
"map": LaunchConfiguration("map"),
"use_sim_time": LaunchConfiguration("use_sim_time"),
# Sensors are already started by saltybot_bringup GROUP A
"include_sensors": "false",
}.items(),
),
],
)
return LaunchDescription([
nav_mode_arg,
profile_arg,
profile_log,
nav2_launch,
map_arg,
use_sim_time_arg,
LogInfo(msg=["[nav2] Loaded nav_mode=", LaunchConfiguration("nav_mode"),
" profile=", LaunchConfiguration("profile")]),
slam_mode,
amcl_mode,
])

331
jetson/ros2_ws/src/saltybot_nav2_slam/config/amcl_nav2_params.yaml Normal file
View File

@ -0,0 +1,331 @@
# amcl_nav2_params.yaml — Complete Nav2 + AMCL parameter file (Issue #655)
#
# AMCL localises on a pre-built static map using /scan (RPLIDAR A1M8).
# Odometry source: /odom from vesc_can_odometry (VESC CAN IDs 61/79, Issue #646).
#
# Launch with:
# ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py
# ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py \
# map:=/mnt/nvme/saltybot/maps/my_map.yaml
#
# Costmaps (inline — required by nav2_bringup navigation_launch.py):
# global: static_layer + obstacle_layer (LiDAR) + inflation_layer
# local: obstacle_layer (LiDAR) + inflation_layer, 4m rolling window
# ── AMCL ─────────────────────────────────────────────────────────────────────
amcl:
ros__parameters:
use_sim_time: false
# Particle filter bounds
min_particles: 500
max_particles: 3000
# Motion model noise (alpha1–5: low = trust odometry more)
# Tuned for VESC differential drive (Issue #646): encoder slip ~5%
alpha1: 0.10 # rot noise from rot
alpha2: 0.10 # rot noise from trans
alpha3: 0.05 # trans noise from trans
alpha4: 0.05 # trans noise from rot
alpha5: 0.10 # (omni only)
# Sensor model — likelihood field
laser_model_type: "likelihood_field"
laser_likelihood_max_dist: 2.0
max_beams: 60
sigma_hit: 0.2
z_hit: 0.50 # beam hit weight
z_rand: 0.45 # random noise weight
z_max: 0.025 # max-range weight (z_hit+z_rand+z_max+z_short = 1.0)
z_short: 0.025 # short-read weight
sigma_short: 0.05
do_beamskip: false
beam_search_increment: 0.1
# Frame IDs
global_frame_id: "map"
odom_frame_id: "odom"
base_frame_id: "base_link"
# Update triggers
update_min_d: 0.20 # m — update after moving this far
update_min_a: 0.15 # rad — update after rotating this much
resample_interval: 1
# Particle filter convergence
pf_err: 0.05
pf_z: 0.99
# Transform
tf_broadcast: true
transform_tolerance: 0.5 # s — relaxed for slower Nav2 loop
save_pose_rate: 0.5 # Hz — save last pose for fast re-init
# Recovery particle injection (0 = disabled)
recovery_alpha_fast: 0.0
recovery_alpha_slow: 0.0
# Robot model
robot_model_type: "nav2_amcl::DifferentialMotionModel"
# Scan topic
scan_topic: /scan
# ── Map Server ────────────────────────────────────────────────────────────────
map_server:
ros__parameters:
use_sim_time: false
yaml_filename: "" # overridden by launch argument --map
# ── BT Navigator ─────────────────────────────────────────────────────────────
bt_navigator:
ros__parameters:
use_sim_time: false
global_frame: map
robot_base_frame: base_link
odom_topic: /odom
enable_groot_monitoring: false
default_nav_to_pose_bt_xml: >-
$(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml
default_nav_through_poses_bt_xml: >-
$(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml
plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node
- nav2_compute_path_through_poses_action_bt_node
- nav2_follow_path_action_bt_node
- nav2_spin_action_bt_node
- nav2_wait_action_bt_node
- nav2_back_up_action_bt_node
- nav2_drive_on_heading_bt_node
- nav2_clear_costmap_service_bt_node
- nav2_is_stuck_condition_bt_node
- nav2_goal_updated_condition_bt_node
- nav2_initial_pose_received_condition_bt_node
- nav2_reinitialize_global_localization_service_bt_node
- nav2_rate_controller_bt_node
- nav2_distance_controller_bt_node
- nav2_speed_controller_bt_node
- nav2_truncate_path_action_bt_node
- nav2_remove_passed_goals_action_bt_node
- nav2_goal_checker_selector_bt_node
- nav2_controller_selector_bt_node
- nav2_planner_selector_bt_node
# ── Controller Server (DWB local planner) ─────────────────────────────────────
controller_server:
ros__parameters:
use_sim_time: false
controller_frequency: 20.0
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.5
min_theta_velocity_threshold: 0.001
failure_tolerance: 0.3
progress_checker_plugin: "progress_checker"
goal_checker_plugins: ["general_goal_checker"]
controller_plugins: ["FollowPath"]
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.3
movement_time_allowance: 15.0
general_goal_checker:
stateful: true
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.20
yaw_goal_tolerance: 0.20
# DWB — tuned for VESC differential drive on Saltybot
FollowPath:
plugin: "dwb_core::DWBLocalPlanner"
debug_trajectory_details: false
min_vel_x: 0.0
min_vel_y: 0.0
max_vel_x: 1.0 # m/s — conservative indoor nav
max_vel_y: 0.0 # diff drive: no lateral
max_vel_theta: 1.5 # rad/s
min_speed_xy: 0.0
max_speed_xy: 1.0
min_speed_theta: 0.0
acc_lim_x: 2.0
acc_lim_y: 0.0
acc_lim_theta: 2.0
decel_lim_x: -2.0
decel_lim_theta: -2.0
vx_samples: 20
vy_samples: 1 # diff drive: no lateral
vtheta_samples: 40
sim_time: 1.7
linear_granularity: 0.05
angular_granularity: 0.025
transform_tolerance: 0.2
trans_stopped_velocity: 0.25
short_circuit_trajectory_evaluation: true
stateful: true
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign",
"PathAlign", "PathDist", "GoalDist"]
BaseObstacle.scale: 0.02
PathAlign.scale: 32.0
PathAlign.forward_point_distance: 0.1
GoalAlign.scale: 24.0
GoalAlign.forward_point_distance: 0.1
PathDist.scale: 32.0
GoalDist.scale: 24.0
RotateToGoal.scale: 32.0
RotateToGoal.slowing_factor: 5.0
RotateToGoal.lookahead_time: -1.0
# ── Planner Server (NavFn A*) ─────────────────────────────────────────────────
planner_server:
ros__parameters:
use_sim_time: false
expected_planner_frequency: 20.0
planner_plugins: ["GridBased"]
GridBased:
plugin: "nav2_navfn_planner/NavfnPlanner"
tolerance: 0.5
use_astar: true
allow_unknown: true
# ── Behavior Server (recovery behaviors) ──────────────────────────────────────
behavior_server:
ros__parameters:
use_sim_time: false
costmap_topic: local_costmap/costmap_raw
footprint_topic: local_costmap/published_footprint
cycle_frequency: 10.0
global_frame: odom
robot_base_frame: base_link
transform_timeout: 0.1
simulate_ahead_time: 2.0
behavior_plugins: ["spin", "backup", "wait"]
spin:
plugin: "nav2_behaviors/Spin"
backup:
plugin: "nav2_behaviors/BackUp"
wait:
plugin: "nav2_behaviors/Wait"
# Spin recovery
max_rotations: 2.0 # full rotations before giving up
# Backup recovery
max_backup_dist: 0.30 # m
backup_speed: 0.15 # m/s — slow for a balancing robot
# ── Global Costmap ────────────────────────────────────────────────────────────
# Uses /scan (RPLIDAR) for obstacle detection.
# Layers: static (from /map) + obstacle (LiDAR) + inflation.
global_costmap:
global_costmap:
ros__parameters:
use_sim_time: false
global_frame: map
robot_base_frame: base_link
update_frequency: 5.0 # Hz
publish_frequency: 2.0 # Hz
rolling_window: false
resolution: 0.05 # m/cell
robot_radius: 0.35 # m — inscribed circle
track_unknown_space: true
unknown_cost_value: -1
lethal_cost_threshold: 100
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: true
enabled: true
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: true
observation_sources: scan
scan:
topic: /scan
data_type: "LaserScan"
max_obstacle_height: 2.0
min_obstacle_height: 0.0
obstacle_max_range: 9.5
obstacle_min_range: 0.0
raytrace_max_range: 10.0
raytrace_min_range: 0.0
clearing: true
marking: true
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
enabled: true
inflation_radius: 0.55 # m — clears obstacles wider than robot
cost_scaling_factor: 10.0
inflate_unknown: false
inflate_around_unknown: true
# ── Local Costmap ─────────────────────────────────────────────────────────────
# Rolling 4m window in odom frame. LiDAR obstacle layer + inflation.
local_costmap:
local_costmap:
ros__parameters:
use_sim_time: false
global_frame: odom
robot_base_frame: base_link
update_frequency: 10.0 # Hz
publish_frequency: 5.0 # Hz
rolling_window: true
width: 4 # m
height: 4 # m
resolution: 0.05 # m/cell
robot_radius: 0.35
track_unknown_space: true
unknown_cost_value: 0 # treat unknown as free locally
lethal_cost_threshold: 100
plugins: ["obstacle_layer", "inflation_layer"]
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: true
observation_sources: scan
scan:
topic: /scan
data_type: "LaserScan"
max_obstacle_height: 2.0
min_obstacle_height: 0.0
obstacle_max_range: 5.0
obstacle_min_range: 0.0
raytrace_max_range: 6.0
raytrace_min_range: 0.0
clearing: true
marking: true
observation_persistence: 0.0
expected_update_rate: 5.5 # RPLIDAR A1M8 scan rate
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
enabled: true
inflation_radius: 0.55
cost_scaling_factor: 10.0
inflate_unknown: false
inflate_around_unknown: false
# ── Velocity Smoother ─────────────────────────────────────────────────────────
velocity_smoother:
ros__parameters:
use_sim_time: false
smoothing_frequency: 20.0
scale_velocities: false
feedback: "OPEN_LOOP"
max_velocity: [1.0, 0.0, 1.5]
min_velocity: [-1.0, 0.0, -1.5]
max_accel: [2.0, 0.0, 2.0]
max_decel: [-2.0, 0.0, -2.0]
odom_topic: "odom"
odom_duration: 0.1
deadband_velocity: [0.0, 0.0, 0.0]
velocity_timeout: 1.0
# ── Lifecycle Managers ────────────────────────────────────────────────────────
lifecycle_manager_localization:
ros__parameters:
use_sim_time: false
autostart: true
node_names: ["map_server", "amcl"]
lifecycle_manager_navigation:
ros__parameters:
use_sim_time: false
autostart: true
node_names: ["controller_server", "planner_server", "behavior_server",
"bt_navigator", "velocity_smoother"]

188
jetson/ros2_ws/src/saltybot_nav2_slam/launch/nav2_amcl_bringup.launch.py Normal file
View File

@ -0,0 +1,188 @@
"""
nav2_amcl_bringup.launch.py — Nav2 with AMCL localization (Issue #655).
Full autonomous navigation stack for SaltyBot using a pre-built static map
and AMCL particle-filter localization.
Odometry: /odom from vesc_can_odometry (VESC CAN IDs 61/79, Issue #646)
LiDAR: /scan from RPLIDAR A1M8 (via saltybot_bringup sensors.launch.py)
Map: static OccupancyGrid loaded by map_server (defaults to placeholder)
Startup sequence
────────────────
1. Sensors — RealSense D435i + RPLIDAR A1M8 drivers + static TF
2. VESC odometry — /odom (differential drive from dual CAN motors)
3. Localization — map_server + AMCL (nav2_bringup localization_launch.py)
4. Navigation — controller + planner + behaviors + BT navigator
(nav2_bringup navigation_launch.py)
Arguments
─────────
map path to map YAML (default: maps/saltybot_map.yaml placeholder)
use_sim_time (default: false)
autostart lifecycle manager autostart (default: true)
params_file Nav2 params override (default: config/amcl_nav2_params.yaml)
include_sensors launch sensors.launch.py (default: true — set false if
sensors are already running)
Usage
─────
# Minimal — sensors included, placeholder map:
ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py
# With a real saved map:
ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py \
map:=/mnt/nvme/saltybot/maps/my_map.yaml
# Without sensor bringup (sensors already running):
ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py \
include_sensors:=false
Integration with saltybot_bringup
──────────────────────────────────
The saltybot_bringup orchestrator (saltybot_bringup.launch.py) calls
nav2.launch.py at t=22 s in GROUP C. To use AMCL instead of RTAB-Map:
ros2 launch saltybot_bringup nav2.launch.py nav_mode:=amcl
"""
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
GroupAction,
IncludeLaunchDescription,
LogInfo,
)
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
def generate_launch_description() -> LaunchDescription:
nav2_slam_dir = get_package_share_directory("saltybot_nav2_slam")
bringup_dir = get_package_share_directory("saltybot_bringup")
nav2_bringup_dir = get_package_share_directory("nav2_bringup")
default_map = os.path.join(nav2_slam_dir, "maps", "saltybot_map.yaml")
default_params = os.path.join(nav2_slam_dir, "config", "amcl_nav2_params.yaml")
# ── Launch arguments ──────────────────────────────────────────────────────
map_arg = DeclareLaunchArgument(
"map",
default_value=default_map,
description="Path to map YAML file (Nav2 map_server format)",
)
use_sim_time_arg = DeclareLaunchArgument(
"use_sim_time",
default_value="false",
description="Use simulated clock from /clock topic",
)
autostart_arg = DeclareLaunchArgument(
"autostart",
default_value="true",
description="Automatically start lifecycle nodes after launch",
)
params_file_arg = DeclareLaunchArgument(
"params_file",
default_value=default_params,
description="Path to Nav2 parameter YAML file",
)
include_sensors_arg = DeclareLaunchArgument(
"include_sensors",
default_value="true",
description="Launch sensors.launch.py (set false if sensors already running)",
)
# ── Substitutions ─────────────────────────────────────────────────────────
map_file = LaunchConfiguration("map")
use_sim_time = LaunchConfiguration("use_sim_time")
autostart = LaunchConfiguration("autostart")
params_file = LaunchConfiguration("params_file")
include_sensors = LaunchConfiguration("include_sensors")
# ── Sensor bringup (conditional) ─────────────────────────────────────────
# RealSense D435i + RPLIDAR A1M8 drivers + base_link→laser/camera_link TF
sensors_launch = GroupAction(
condition=IfCondition(include_sensors),
actions=[
LogInfo(msg="[nav2_amcl] Starting sensors (RealSense + RPLIDAR)"),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(bringup_dir, "launch", "sensors.launch.py")
),
),
],
)
# ── VESC CAN odometry ────────────────────────────────────────────────────
# Differential drive from CAN IDs 61 (left) + 79 (right) → /odom + TF
odom_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(nav2_slam_dir, "launch", "odometry_bridge.launch.py")
),
launch_arguments={"use_sim_time": use_sim_time}.items(),
)
# ── Localization: map_server + AMCL ──────────────────────────────────────
# Publishes: /map, TF map→odom (AMCL), /amcl_pose, /particlecloud
localization_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(nav2_bringup_dir, "launch", "localization_launch.py")
),
launch_arguments={
"map": map_file,
"use_sim_time": use_sim_time,
"autostart": autostart,
"params_file": params_file,
"use_lifecycle_mgr": "true",
}.items(),
)
# ── Navigation: controller + planner + behaviors + BT navigator ──────────
# Subscribes: /odom, /scan, /map, /cmd_vel_nav (→ velocity_smoother → /cmd_vel)
# Provides: action servers for NavigateToPose + NavigateThroughPoses
navigation_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(nav2_bringup_dir, "launch", "navigation_launch.py")
),
launch_arguments={
"use_sim_time": use_sim_time,
"autostart": autostart,
"params_file": params_file,
"map_subscribe_transient_local": "true",
"use_lifecycle_mgr": "true",
}.items(),
)
return LaunchDescription([
# Arguments
map_arg,
use_sim_time_arg,
autostart_arg,
params_file_arg,
include_sensors_arg,
# Banner
LogInfo(msg="[nav2_amcl] Nav2 AMCL bringup starting (Issue #655)"),
# Startup sequence
sensors_launch, # step 1 — sensors
odom_launch, # step 2 — /odom from VESC
LogInfo(msg="[nav2_amcl] Starting AMCL localization"),
localization_launch, # step 3 — map_server + AMCL
LogInfo(msg="[nav2_amcl] Starting Nav2 navigation stack"),
navigation_launch, # step 4 — full nav stack
])

4
jetson/ros2_ws/src/saltybot_nav2_slam/maps/saltybot_map.pgm Normal file
View File

File diff suppressed because one or more lines are too long

16
jetson/ros2_ws/src/saltybot_nav2_slam/maps/saltybot_map.yaml Normal file
View File

@ -0,0 +1,16 @@
# SaltyBot placeholder map — 10m × 10m all free space at 0.05 m/cell.
#
# Replace with a real map saved by slam_toolbox or RTAB-Map:
# ros2 run nav2_map_server map_saver_cli -f /mnt/nvme/saltybot/maps/my_map
#
# Then launch with:
# ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py \
# map:=/mnt/nvme/saltybot/maps/my_map.yaml
image: saltybot_map.pgm
mode: trinary
resolution: 0.05 # m/cell — matches costmap resolution
origin: [-5.0, -5.0, 0.0] # lower-left corner (x, y, yaw)
negate: 0 # 0: white=free, black=occupied
occupied_thresh: 0.65 # p ≥ 0.65 → lethal
free_thresh: 0.25 # p ≤ 0.25 → free

15
jetson/ros2_ws/src/saltybot_nav2_slam/package.xml
View File

@ -4,9 +4,10 @@
<name>saltybot_nav2_slam</name>
<version>0.1.0</version>
<description>
Nav2 SLAM integration for SaltyBot autonomous navigation.
Combines SLAM Toolbox (RPLIDAR 2D SLAM), RealSense depth for obstacle avoidance,
VESC odometry, and DWB planner for autonomous navigation up to 20km/h.
Nav2 SLAM + AMCL integration for SaltyBot autonomous navigation (Issues #422, #655).
Combines SLAM Toolbox (RPLIDAR 2D SLAM), VESC CAN differential-drive odometry
(Issue #646), AMCL particle-filter localization on static maps, DWB local planner,
NavFn A* global planner, RealSense depth costmap, and spin/backup/wait recovery.
</description>
<maintainer email="sl-controls@saltylab.local">sl-controls</maintainer>
<license>MIT</license>
@ -20,6 +21,14 @@
<depend>tf2_ros</depend>
<exec_depend>nav2_bringup</exec_depend>
<exec_depend>nav2_amcl</exec_depend>
<exec_depend>nav2_map_server</exec_depend>
<exec_depend>nav2_bt_navigator</exec_depend>
<exec_depend>nav2_controller</exec_depend>
<exec_depend>nav2_planner</exec_depend>
<exec_depend>nav2_behaviors</exec_depend>
<exec_depend>dwb_core</exec_depend>
<exec_depend>nav2_navfn_planner</exec_depend>
<exec_depend>slam_toolbox</exec_depend>
<exec_depend>rplidar_ros</exec_depend>
<exec_depend>realsense2_camera</exec_depend>

6
jetson/ros2_ws/src/saltybot_nav2_slam/setup.py
View File

@ -14,6 +14,7 @@ setup(
"launch/nav2_slam_integrated.launch.py",
"launch/depth_to_costmap.launch.py",
"launch/odometry_bridge.launch.py",
"launch/nav2_amcl_bringup.launch.py",
]),
(f"share/{package_name}/config", [
"config/nav2_params.yaml",
@ -23,6 +24,11 @@ setup(
"config/local_costmap_params.yaml",
"config/dwb_local_planner_params.yaml",
"config/vesc_odometry_params.yaml",
"config/amcl_nav2_params.yaml",
]),
(f"share/{package_name}/maps", [
"maps/saltybot_map.yaml",
"maps/saltybot_map.pgm",
]),
],
install_requires=["setuptools"],

338
jetson/ros2_ws/src/saltybot_nav2_slam/test/test_nav2_amcl.py Normal file
View File

@ -0,0 +1,338 @@
"""
test_nav2_amcl.py — Unit tests for Nav2 AMCL integration (Issue #655).
Tests run without a ROS2 installation:
- amcl_nav2_params.yaml: parse, required sections, value ranges
- saltybot_map.yaml: parse, required keys, valid geometry
- saltybot_map.pgm: valid PGM header, correct dimensions
- nav2_amcl_bringup.launch.py: import (syntax) test
"""
import os
import struct
import pytest
import yaml
# ── Paths ─────────────────────────────────────────────────────────────────────
_PKG_ROOT = os.path.join(os.path.dirname(__file__), "..")
_CFG = os.path.join(_PKG_ROOT, "config", "amcl_nav2_params.yaml")
_MAP_YAML = os.path.join(_PKG_ROOT, "maps", "saltybot_map.yaml")
_MAP_PGM = os.path.join(_PKG_ROOT, "maps", "saltybot_map.pgm")
_LAUNCH = os.path.join(_PKG_ROOT, "launch", "nav2_amcl_bringup.launch.py")
# ── amcl_nav2_params.yaml ─────────────────────────────────────────────────────
def _load_params():
with open(_CFG) as f:
return yaml.safe_load(f)
def test_params_file_exists():
assert os.path.isfile(_CFG)
def test_params_top_level_sections():
params = _load_params()
required = [
"amcl",
"map_server",
"bt_navigator",
"controller_server",
"planner_server",
"behavior_server",
"global_costmap",
"local_costmap",
"velocity_smoother",
"lifecycle_manager_localization",
"lifecycle_manager_navigation",
]
for section in required:
assert section in params, f"Missing section: {section}"
def test_amcl_params_present():
amcl = _load_params()["amcl"]["ros__parameters"]
for key in ("min_particles", "max_particles", "alpha1", "alpha2",
"alpha3", "alpha4", "laser_model_type", "global_frame_id",
"odom_frame_id", "base_frame_id", "robot_model_type"):
assert key in amcl, f"Missing amcl param: {key}"
def test_amcl_particle_bounds():
amcl = _load_params()["amcl"]["ros__parameters"]
assert amcl["min_particles"] >= 100
assert amcl["max_particles"] >= amcl["min_particles"]
assert amcl["max_particles"] <= 10000
def test_amcl_frames():
amcl = _load_params()["amcl"]["ros__parameters"]
assert amcl["global_frame_id"] == "map"
assert amcl["odom_frame_id"] == "odom"
assert amcl["base_frame_id"] == "base_link"
def test_amcl_motion_model():
amcl = _load_params()["amcl"]["ros__parameters"]
for key in ("alpha1", "alpha2", "alpha3", "alpha4"):
assert 0.0 <= amcl[key] <= 1.0, f"{key} out of [0,1]"
def test_amcl_laser_model():
amcl = _load_params()["amcl"]["ros__parameters"]
assert amcl["laser_model_type"] in ("likelihood_field", "beam", "likelihood_field_prob")
z_sum = amcl["z_hit"] + amcl["z_rand"] + amcl["z_max"] + amcl["z_short"]
assert abs(z_sum - 1.0) < 1e-3, f"z weights sum {z_sum:.4f} ≠ 1.0"
def test_controller_server_params():
ctrl = _load_params()["controller_server"]["ros__parameters"]
assert ctrl["controller_frequency"] > 0
follow = ctrl["FollowPath"]
assert follow["max_vel_x"] > 0
assert follow["max_vel_x"] <= 10.0 # sanity cap
assert follow["max_vel_theta"] > 0
assert follow["min_vel_y"] == 0.0 # differential drive: no lateral
assert follow["vy_samples"] == 1 # differential drive
def test_planner_server_params():
planner = _load_params()["planner_server"]["ros__parameters"]
assert "GridBased" in planner["planner_plugins"]
grid = planner["GridBased"]
assert "NavfnPlanner" in grid["plugin"]
assert grid["use_astar"] is True
def test_behavior_server_recoveries():
behaviors = _load_params()["behavior_server"]["ros__parameters"]
plugins = behaviors["behavior_plugins"]
# Issue #655 requires: spin, backup, wait
for required in ("spin", "backup", "wait"):
assert required in plugins, f"Recovery behavior '{required}' missing"
# Check each has a plugin entry
for name in plugins:
assert name in behaviors, f"behavior_plugins entry '{name}' has no config block"
assert "plugin" in behaviors[name]
def test_global_costmap_layers():
gcm = _load_params()["global_costmap"]["global_costmap"]["ros__parameters"]
plugins = gcm["plugins"]
for layer in ("static_layer", "obstacle_layer", "inflation_layer"):
assert layer in plugins, f"Global costmap missing layer: {layer}"
# Each layer has plugin field
for layer in plugins:
assert layer in gcm, f"Layer {layer} has no config block"
assert "plugin" in gcm[layer]
def test_global_costmap_scan_source():
gcm = _load_params()["global_costmap"]["global_costmap"]["ros__parameters"]
obs = gcm["obstacle_layer"]
assert "scan" in obs["observation_sources"]
assert obs["scan"]["topic"] == "/scan"
assert obs["scan"]["data_type"] == "LaserScan"
def test_local_costmap_layers():
lcm = _load_params()["local_costmap"]["local_costmap"]["ros__parameters"]
plugins = lcm["plugins"]
# Local costmap: obstacle + inflation (no static — rolling window)
assert "obstacle_layer" in plugins
assert "inflation_layer" in plugins
assert lcm["rolling_window"] is True
def test_local_costmap_size():
lcm = _load_params()["local_costmap"]["local_costmap"]["ros__parameters"]
assert lcm["width"] >= 2
assert lcm["height"] >= 2
assert lcm["resolution"] == pytest.approx(0.05, abs=1e-6)
def test_costmap_frames():
gcm = _load_params()["global_costmap"]["global_costmap"]["ros__parameters"]
lcm = _load_params()["local_costmap"]["local_costmap"]["ros__parameters"]
assert gcm["global_frame"] == "map"
assert gcm["robot_base_frame"] == "base_link"
assert lcm["global_frame"] == "odom" # local uses odom frame
assert lcm["robot_base_frame"] == "base_link"
def test_inflation_radius_larger_than_robot():
gcm = _load_params()["global_costmap"]["global_costmap"]["ros__parameters"]
lcm = _load_params()["local_costmap"]["local_costmap"]["ros__parameters"]
robot_r = gcm["robot_radius"]
g_infl = gcm["inflation_layer"]["inflation_radius"]
l_infl = lcm["inflation_layer"]["inflation_radius"]
assert g_infl > robot_r, "Global inflation_radius must exceed robot_radius"
assert l_infl > robot_r, "Local inflation_radius must exceed robot_radius"
def test_lifecycle_manager_localization_nodes():
lm = _load_params()["lifecycle_manager_localization"]["ros__parameters"]
assert "map_server" in lm["node_names"]
assert "amcl" in lm["node_names"]
def test_lifecycle_manager_navigation_nodes():
lm = _load_params()["lifecycle_manager_navigation"]["ros__parameters"]
for node in ("controller_server", "planner_server", "behavior_server", "bt_navigator"):
assert node in lm["node_names"], f"lifecycle nav missing: {node}"
def test_velocity_smoother_limits_consistent():
vs = _load_params()["velocity_smoother"]["ros__parameters"]
ctrl = _load_params()["controller_server"]["ros__parameters"]
max_vx_ctrl = ctrl["FollowPath"]["max_vel_x"]
max_vx_vs = vs["max_velocity"][0]
# Smoother limit should be >= planner limit (or equal)
assert max_vx_vs >= max_vx_ctrl - 1e-3, (
f"velocity_smoother max_velocity[0]={max_vx_vs} < "
f"controller max_vel_x={max_vx_ctrl}"
)
# ── saltybot_map.yaml ─────────────────────────────────────────────────────────
def _load_map_yaml():
with open(_MAP_YAML) as f:
return yaml.safe_load(f)
def test_map_yaml_exists():
assert os.path.isfile(_MAP_YAML)
def test_map_yaml_required_keys():
m = _load_map_yaml()
for key in ("image", "resolution", "origin", "negate",
"occupied_thresh", "free_thresh"):
assert key in m, f"Map YAML missing key: {key}"
def test_map_yaml_thresholds():
m = _load_map_yaml()
assert 0.0 < m["free_thresh"] < m["occupied_thresh"] < 1.0
def test_map_yaml_resolution():
m = _load_map_yaml()
assert m["resolution"] == pytest.approx(0.05, abs=1e-6)
def test_map_yaml_origin_3d():
m = _load_map_yaml()
assert len(m["origin"]) == 3 # [x, y, yaw]
def test_map_yaml_image_name():
m = _load_map_yaml()
assert m["image"].endswith(".pgm")
# ── saltybot_map.pgm ─────────────────────────────────────────────────────────
def test_pgm_exists():
assert os.path.isfile(_MAP_PGM)
def test_pgm_valid_header():
"""P5 binary PGM: header 'P5\\nW H\\nMAXVAL\\n'."""
with open(_MAP_PGM, "rb") as f:
magic = f.readline().strip()
assert magic == b"P5", f"Expected P5 PGM, got {magic}"
def test_pgm_dimensions():
"""Parse width and height from PGM header; verify > 0."""
with open(_MAP_PGM, "rb") as f:
f.readline() # P5
wh_line = f.readline()
w_str, h_str = wh_line.decode().split()
w, h = int(w_str), int(h_str)
assert w > 0 and h > 0
assert w == 200 and h == 200
def test_pgm_pixel_count():
"""File size should match header dimensions + header bytes."""
with open(_MAP_PGM, "rb") as f:
lines = []
for _ in range(3):
lines.append(f.readline())
payload = f.read()
w, h = map(int, lines[1].decode().split())
assert len(payload) == w * h, (
f"Expected {w*h} pixels, got {len(payload)}"
)
def test_pgm_all_free_space():
"""All pixels should be 254 (near-white = free)."""
with open(_MAP_PGM, "rb") as f:
for _ in range(3):
f.readline()
data = f.read()
assert all(b == 254 for b in data), "Not all pixels are free (254)"
# ── Launch file syntax check ──────────────────────────────────────────────────
def test_launch_file_exists():
assert os.path.isfile(_LAUNCH)
def test_launch_file_imports_cleanly():
"""Compile launch file to check for Python syntax errors."""
import py_compile
py_compile.compile(_LAUNCH, doraise=True)
def test_launch_file_has_generate_function():
"""Launch file must define generate_launch_description()."""
with open(_LAUNCH) as f:
src = f.read()
assert "def generate_launch_description" in src
def test_launch_file_references_amcl_params():
"""Launch file must reference amcl_nav2_params.yaml."""
with open(_LAUNCH) as f:
src = f.read()
assert "amcl_nav2_params.yaml" in src
def test_launch_file_references_odometry_bridge():
"""Launch file must wire in VESC odometry bridge."""
with open(_LAUNCH) as f:
src = f.read()
assert "odometry_bridge.launch.py" in src
def test_launch_file_references_localization():
"""Launch file must include Nav2 localization_launch.py."""
with open(_LAUNCH) as f:
src = f.read()
assert "localization_launch.py" in src
def test_launch_file_references_navigation():
"""Launch file must include Nav2 navigation_launch.py."""
with open(_LAUNCH) as f:
src = f.read()
assert "navigation_launch.py" in src
def test_launch_file_has_map_argument():
"""Launch file must expose 'map' argument for map file path."""
with open(_LAUNCH) as f:
src = f.read()
assert '"map"' in src or "'map'" in src
if __name__ == "__main__":
pytest.main([__file__, "-v"])