📍 SLAM-Toolbox Pure Localization and Autonomous Navigation

1. Course Content

• Master using SLAM-Toolbox to load offline pose graph (.posegraph + .data) for high-precision global pure localization (Pure Localization Mode).

• Combine SLAM-Toolbox pure localization pose output with Nav2 navigation framework (TEB local trajectory optimization planner) to achieve autonomous obstacle-avoiding single-point/multi-point navigation in known map environments.

• Understand the collaborative workflow of graph optimization sliding window global localization and Nav2 path planning stack in non-AMCL mode.

2. SLAM-Toolbox Pure Localization Navigation Principle Analysis

In actual industrial and research applications, after map building is complete, there is usually no need to continue incrementally expanding the map during navigation. SLAM-Toolbox provides a dedicated pure localization mode (Localization Mode):

1. Graph Structure Freezing and Sliding Window Optimization (Sliding Window): After loading offline pose graph (.posegraph and .data), the system uses it as read-only prior, no longer inserting permanent new nodes into the global pose graph; only performs real-time nonlinear optimization of the robot's current pose within a very small sliding window (scan_buffer_size: 3) using the Ceres solver, greatly reducing CPU and memory load.

2. Advantages Over Traditional AMCL Particle Filter: Traditional AMCL relies on Monte Carlo particle filter, which is prone to particle dispersion degradation, jumping, or losing track (Pose Jump) in long corridors or symmetric feature environments; SLAM-Toolbox, based on laser point cloud and dense pose graph matching, outputs high-frequency smooth, jitter-free map -> odom coordinate transformation.

3. Seamless Nav2 Navigation Protocol Stack Integration: The system shields native AMCL node through modified slamtoolx_bringup.launch.py, with localization_slam_toolbox_node solely responsible for publishing global localization, while Nav2 focuses on costmap 2D updates, global path planning (NavfnPlanner), and TEB local trajectory obstacle avoidance control.

3. Preparation and Running

This example requires opening 3 terminals to start the following nodes in sequence:

3.1 Start Chassis + Lidar

Run in Terminal 1 (start A1 Pro chassis driver, wheel odometry, and lidar driver):

xxxxxxxxxx
ros2 launch yahboomcar_nav laser_bringup_launch.py

3.2 Start RViz Navigation Visualization

Run in Terminal 2 (it is recommended to open RViz on the virtual machine if the VM and robot are on the same LAN):

xxxxxxxxxx
ros2 launch yahboomcar_nav display_nav_launch.py

3.3 Start SLAM-Toolbox Pure Localization Navigation Core

Run in Terminal 3 (one-click pull up SLAM-Toolbox pure localization node and Nav2 path planning stack):

xxxxxxxxxx
ros2 launch yahboomcar_nav navigation_slamtoolx_launch.py map:=map.yaml pose_map:=map

4. Relocalization Navigation Operation Flow and Effect Description

4.1 Initial Relocalization Calibration (2D Pose Estimate)

1. Click the 2D Pose Estimate tool in the top toolbar of RViz2;

2. Click at the approximate actual robot stopping position on the map, and hold and drag the mouse along the chassis heading direction to release the green arrow to publish initial pose estimate;

3. After SLAM-Toolbox receives the pose, it will immediately complete local matching and alignment with surrounding geometric features; observe that the red laser point cloud in RViz completely coincides with the black obstacle contours on the map, representing successful relocalization.

4.2 Single-Point Autonomous Obstacle-Avoiding Navigation (Nav2 Goal)

1. Click the Nav2 Goal (or 2D Goal Pose) tool in the top toolbar of RViz2;

2. Click in the free traversable area on the map and drag out the expected goal heading arrow;

3. Nav2 global planner will generate an optimal path, local TEB planner real-time controls Ackermann chassis front-wheel steering and vehicle speed, dynamically avoids obstacles on the travel route, and smoothly arrives at the goal point.

4.3 Multi-Point Autonomous Continuous Cruising (Waypoint Following)

If you need the robot to continuously cruise multiple goal points:

1. In the Navigation2 panel in the lower-left corner of RViz2 interface, switch the mode from Navigate to Pose to Waypoint / Nav Through Poses Mode;

2. Use the Nav2 Goal tool to sequentially click and calibrate multiple target waypoints on the map;

3. Click Start Waypoint Following, and the robot will autonomously travel through all waypoints in the set sequence and steadily align and stop at each point.

5. Core Source Code and Localization Parameter Analysis

5.1 Pure Localization and Navigation Integration Launch Analysis

1. Main Launch Scheduling Launch

File Path:

xxxxxxxxxx
$HOME/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_nav/launch/nav2/navigation_slamtoolx_launch.py

xxxxxxxxxx
def generate_launch_description():
    ...
    # Map and parameter configuration
    map_dir = PathJoinSubstitution([map_folder_dir, LaunchConfiguration("map")])
    pose_map = PathJoinSubstitution([map_folder_dir, LaunchConfiguration("pose_map")])
    params_file = LaunchConfiguration("params_file")

    # 1. Start Navigation2 stack (using modified slamtoolx_bringup to shield AMCL)
    nav2_bringup = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(nav2_launch_mod),
        launch_arguments={
            "map": map_dir,
            "params_file": params_file,
            "use_sim_time": use_sim_time,
        }.items()
    )

    # 2. Start SLAM-Toolbox pure localization node
    slamtoolbox_localization = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(slamtoolbox_localization_launch_file),
        launch_arguments={
            "map": pose_map,
            "use_sim_time": use_sim_time,
        }.items()
    )

    return LaunchDescription([
        *declared_arguments,
        slamtoolbox_localization_nav,
    ])

2. SLAM-Toolbox Pure Localization Node Launch

File Path:

xxxxxxxxxx
$HOME/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_nav/launch/nav2/localization_slamtoolx_launch.py

xxxxxxxxxx
# Dynamically replace pose graph path through RewrittenYaml and start localization_slam_toolbox_node
configured_params = RewrittenYaml(
    source_file=slam_params_file,
    root_key='',
    param_rewrites={
        'use_sim_time': use_sim_time,
        'map_file_name': map_file,
    },
    convert_types=True
)

slam_toolbox_node = Node(
    package='slam_toolbox',
    executable='localization_slam_toolbox_node',
    name='slam_toolbox',
    output='screen',
    parameters=[configured_params],
)

5.2 Core Algorithm and Navigation Parameter Interpretation

Parameter Configuration File Paths:

xxxxxxxxxx
$HOME/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_nav/params/mapper_params_localization.yaml
$HOME/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_nav/params/teb_nav_params.yaml

Core Parameter Description Table

Parameter File Core Configuration Snippet

​x
# 1. SLAM-Toolbox pure localization configuration (mapper_params_localization.yaml)
slam_toolbox:
  ros__parameters:
    solver_plugin: solver_plugins::CeresSolver
    odom_frame: odom
    map_frame: map
    base_frame: base_footprint
    scan_topic: /scan
    mode: localization               # Pure localization mode
    scan_buffer_size: 3              # Sliding window size is 3 frames
    resolution: 0.05
    min_laser_range: 0.05
    max_laser_range: 12.0
    do_loop_closing: true 

# 2. TEB local trajectory optimization and obstacle avoidance configuration (teb_nav_params.yaml)
controller_server:
  ros__parameters:
    controller_frequency: 20.0
    FollowPath:
      plugin: "teb_local_planner::TebLocalPlannerROS"
      max_vel_x: 0.25                # Maximum cruising speed (m/s)
      min_vel_x: -0.05               # Limit reverse speed
      max_vel_theta: 1.0             # Maximum turning angular velocity (rad/s)
      min_obstacle_dist: 0.1         # Minimum obstacle hard distance (m)
      xy_goal_tolerance: 0.25        # Arrival distance accuracy (m)
      yaw_goal_tolerance: 0.25       # Arrival angle accuracy (rad)

local_costmap:
  local_costmap:
    ros__parameters:
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 10.0
        inflation_radius: 0.18       # Local inflation radius (m)