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

@@ -1,139 +1,76 @@
 """
 nav2.launch.py — Nav2 autonomous navigation stack for SaltyBot
 
-Supports two localization modes via the 'nav_mode' argument:
+Starts the full Nav2 navigation stack (controllers, planners, behavior server,
+BT navigator, velocity smoother, lifecycle managers).
 
-  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)
+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:=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:=...)
+Output:
+  /cmd_vel         — consumed by saltybot_bridge → STM32 over UART
 
-  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)
+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)
 
 Run sequence on Orin:
-  # 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
+  1. docker compose up saltybot-ros2   # RTAB-Map + sensors
+  2. docker compose up saltybot-nav2   # this launch file
 """
 
 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 LaunchConfigurationEquals
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, LogInfo
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 
 
-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)."
-        ),
-    )
+def generate_launch_description():
+    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
+    bringup_dir = get_package_share_directory('saltybot_bringup')
 
+    # Profile argument (Issue #506)
     profile_arg = DeclareLaunchArgument(
         "profile",
         default_value="indoor",
         choices=["indoor", "outdoor", "demo"],
-        description="Nav2 parameter profile for slam mode (Issue #506)",
+        description="Launch profile for parameter overrides (Issue #506)"
     )
 
-    map_arg = DeclareLaunchArgument(
-        "map",
-        default_value=os.path.join(nav2_slam_dir, "maps", "saltybot_map.yaml"),
-        description="Map YAML path for AMCL mode",
+    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'
     )
 
-    use_sim_time_arg = DeclareLaunchArgument(
-        "use_sim_time",
-        default_value="false",
+    nav2_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            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',
+        }.items(),
     )
 
-    # ── 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")
-                ),
-                launch_arguments={
-                    "use_sim_time":                "false",
-                    "autostart":                   "true",
-                    "params_file":                 slam_nav2_params,
-                    "default_bt_xml_filename":     slam_bt_xml,
-                    "map_subscribe_transient_local": "true",
-                }.items(),
-            ),
-        ],
-    )
-
-    # ── 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(),
-            ),
-        ],
+    profile_log = LogInfo(
+        msg=['[nav2] Loaded profile: ', profile]
     )
 
     return LaunchDescription([
-        nav_mode_arg,
         profile_arg,
-        map_arg,
-        use_sim_time_arg,
-        LogInfo(msg=["[nav2] Loaded nav_mode=", LaunchConfiguration("nav_mode"),
-                     "  profile=", LaunchConfiguration("profile")]),
-        slam_mode,
-        amcl_mode,
+        profile_log,
+        nav2_launch,
     ])

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

@@ -1,331 +0,0 @@
-# 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

@@ -1,188 +0,0 @@
-"""
-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

@@ -1,16 +0,0 @@
-# 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,10 +4,9 @@
   <name>saltybot_nav2_slam</name>
   <version>0.1.0</version>
   <description>
-    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.
+    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.
   </description>
   <maintainer email="sl-controls@saltylab.local">sl-controls</maintainer>
   <license>MIT</license>
@@ -21,14 +20,6 @@
   <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,7 +14,6 @@ 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",
@@ -24,11 +23,6 @@ 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

@@ -1,338 +0,0 @@
-"""
-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"])