jetson/ros2_ws/src/saltybot_bringup/launch/nav2.launch.py

@@ -1,76 +1,139 @@
 """
 nav2.launch.py — Nav2 autonomous navigation stack for SaltyBot
 
-Starts the full Nav2 navigation stack (controllers, planners, behavior server,
+Supports two localization modes via the 'nav_mode' argument:
-BT navigator, velocity smoother, lifecycle managers).
 
-Localization is provided by RTAB-Map (slam_rtabmap.launch.py must be running):
+  nav_mode:=slam  (default)
-  /map             — OccupancyGrid (static global costmap layer)
+    Uses RTAB-Map for odometry and live map building.
-  /rtabmap/odom    — Odometry (velocity smoother + controller feedback)
+    Requires slam_rtabmap.launch.py to be running.
-  TF map→odom      — published by RTAB-Map
+    Sources: /rtabmap/odom (pose), /map (from RTAB-Map)
 
-Output:
+  nav_mode:=amcl
-  /cmd_vel         — consumed by saltybot_bridge → STM32 over UART
+    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)
+  nav_mode:=slam  profile support (Issue #506)
-────────────────────────────
+    profile:=indoor  — conservative (0.2 m/s, tight geofence)
-  Supports profile-based parameter overrides via 'profile' launch argument:
+    profile:=outdoor — moderate  (0.5 m/s)
-    profile:=indoor  — conservative (0.2 m/s, tight geofence, aggressive inflation)
+    profile:=demo    — agile     (0.3 m/s, tricks enabled)
-    profile:=outdoor — moderate (0.5 m/s, wide geofence, standard inflation)
-    profile:=demo    — agile (0.3 m/s, tricks enabled, enhanced obstacle avoidance)
 
 Run sequence on Orin:
-  1. docker compose up saltybot-ros2   # RTAB-Map + sensors
+  # SLAM mode (existing behaviour):
-  2. docker compose up saltybot-nav2   # this launch file
+  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():
+def generate_launch_description() -> LaunchDescription:
-    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
+    nav2_bringup_dir   = get_package_share_directory("nav2_bringup")
-    bringup_dir = get_package_share_directory('saltybot_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')
+    map_arg = DeclareLaunchArgument(
-
+        "map",
-    nav2_params_file = os.path.join(bringup_dir, 'config', 'nav2_params.yaml')
+        default_value=os.path.join(nav2_slam_dir, "maps", "saltybot_map.yaml"),
-    bt_xml_file = os.path.join(
+        description="Map YAML path for AMCL mode",
-        bringup_dir, 'behavior_trees', 'navigate_to_pose_with_recovery.xml'
     )
 
-    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',
+                    "use_sim_time":                "false",
-            'autostart': 'true',
+                    "autostart":                   "true",
-            'params_file': nav2_params_file,
+                    "params_file":                 slam_nav2_params,
-            'default_bt_xml_filename': bt_xml_file,
+                    "default_bt_xml_filename":     slam_bt_xml,
-            # RTAB-Map publishes /map with transient_local QoS — must match
+                    "map_subscribe_transient_local": "true",
-            'map_subscribe_transient_local': 'true',
                 }.items(),
+            ),
+        ],
     )
 
-    profile_log = LogInfo(
+    # ── Mode: AMCL (static map + AMCL + VESC odometry, Issue #655) ───────────
-        msg=['[nav2] Loaded profile: ', profile]
+
+    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,
+        map_arg,
-        nav2_launch,
+        use_sim_time_arg,
+        LogInfo(msg=["[nav2] Loaded nav_mode=", LaunchConfiguration("nav_mode"),
+                     "  profile=", LaunchConfiguration("profile")]),
+        slam_mode,
+        amcl_mode,
     ])

jetson/ros2_ws/src/saltybot_nav2_slam/config/amcl_nav2_params.yaml

@@ -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"]

jetson/ros2_ws/src/saltybot_nav2_slam/launch/nav2_amcl_bringup.launch.py

@@ -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
+    ])

jetson/ros2_ws/src/saltybot_nav2_slam/maps/saltybot_map.yaml

@@ -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

jetson/ros2_ws/src/saltybot_nav2_slam/package.xml

@@ -4,9 +4,10 @@
   <name>saltybot_nav2_slam</name>
   <version>0.1.0</version>
   <description>
-    Nav2 SLAM integration for SaltyBot autonomous navigation.
+    Nav2 SLAM + AMCL integration for SaltyBot autonomous navigation (Issues #422, #655).
-    Combines SLAM Toolbox (RPLIDAR 2D SLAM), RealSense depth for obstacle avoidance,
+    Combines SLAM Toolbox (RPLIDAR 2D SLAM), VESC CAN differential-drive odometry
-    VESC odometry, and DWB planner for autonomous navigation up to 20km/h.
+    (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>

jetson/ros2_ws/src/saltybot_nav2_slam/setup.py

@@ -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"],

jetson/ros2_ws/src/saltybot_nav2_slam/test/test_nav2_amcl.py

@@ -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"])