Autonomous mobile robots must both build a map of their unknown environment and localize themselves within it. Tackling mapping and localization as separate tasks often leads to drift and inconsistencies. The hard reality is: if you treat mapping and localization independently, your robot’s map will be unreliable, and localization will degrade over time. Simultaneous Localization and Mapping (SLAM) solves both together, using each partial result to improve the other.
In this post, we:
Briefly recap the SLAM problem and why Graph SLAM is a powerful formulation.
Show how to launch and configure ROS 2’s slam_toolbox package for 2D LiDAR-based mapping.
Explain the most salient YAML parameters so you can tailor slam_toolbox to your robot and environment.
SLAM asks: “Given odometry inputs (noisy motion estimates) and sensor observations (e.g., LiDAR scans), can the robot build a consistent map while simultaneously estimating its own trajectory within that map?” This chicken-and-egg dilemma arises because:
Mapping needs accurate poses to place sensor measurements correctly.
Localization needs a map to correct odometry drift.
Graph-based SLAM addresses this by constructing a factor graph:
Nodes represent robot poses (at different time steps) and landmarks (features).
Edges (factors) encode constraints: odometry constraints between successive poses, and measurement constraints between a pose and observed landmarks (or scan-match factors).
Constraints aren’t rigid; think of them as springs that can stretch/compress to accommodate noise. An optimization backend “pulls” the graph into a configuration that best satisfies all constraints, yielding an optimized trajectory and map.
Compared to filter-based approaches, Graph SLAM:
Accumulates constraints over time and optimizes globally, reducing long-term drift.
Naturally incorporates loop closures (recognizing revisited places) by adding constraints between non-consecutive poses.
Is flexible: landmarks can be LiDAR features, visual features (for VSLAM), or even submaps. The same core machinery applies.
ROS 2’s slam_toolbox implements Graph SLAM for 2D LiDAR, providing:
Online mapping: build a map in real time as the robot moves.
Localization: load an existing map and localize within it.
Map merging: combine maps from multiple runs or robots.
Lifelong mapping: continuously update a saved map to reflect environment changes.
import os
from launch import LaunchDescription
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
def generate_launch_description():
# Use simulation time or real time
use_sim_time = LaunchConfiguration("use_sim_time")
slam_config = LaunchConfiguration("slam_config")
ros_distro = os.environ.get("ROS_DISTRO", "")
# Some lifecycle nodes differ by distro
lifecycle_nodes = ["map_saver_server"]
if ros_distro != "humble":
lifecycle_nodes.append("slam_toolbox")
# Declare arguments
use_sim_time_arg = DeclareLaunchArgument(
"use_sim_time",
default_value="true"
)
slam_config_arg = DeclareLaunchArgument(
"slam_config",
default_value=os.path.join(
get_package_share_directory("bumperbot_mapping"),
"config",
"slam_toolbox.yaml"
),
description="Full path to slam_toolbox YAML file"
)
# Node: map_saver_server to save maps periodically or on trigger
nav2_map_saver = Node(
package="nav2_map_server",
executable="map_saver_server",
name="map_saver_server",
output="screen",
parameters=[
{"save_map_timeout": 5.0},
{"use_sim_time": use_sim_time},
# Note: these should use colon syntax if needed; check Nav2 docs
{"free_thresh_default", "0.196"},
{"occupied_thresh_default", "0.65"},
],
)
# Node: slam_toolbox in synchronous mapping mode
slam_toolbox_node = Node(
package="slam_toolbox",
executable="sync_slam_toolbox_node",
name="slam_toolbox",
output="screen",
parameters=[
slam_config,
{"use_sim_time": use_sim_time},
],
)
# Lifecycle manager for Nav2/map saver/slam_toolbox
nav2_lifecycle_manager = Node(
package="nav2_lifecycle_manager",
executable="lifecycle_manager",
name="lifecycle_manager_slam",
output="screen",
parameters=[
{"node_names": lifecycle_nodes},
{"use_sim_time": use_sim_time},
{"autostart": True}
],
)
return LaunchDescription([
use_sim_time_arg,
slam_config_arg,
nav2_map_saver,
slam_toolbox_node,
nav2_lifecycle_manager,
])
ros2 lifecycle set /<SOME-NODE> configure
ros2 lifecycle set /<SOME-NODE> activate
slam_toolbox:
ros__parameters:
# Solver plugins (Ceres settings)
solver_plugin: solver_plugins::CeresSolver
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
ceres_preconditioner: SCHUR_JACOBI
ceres_trust_strategy: LEVENBERG_MARQUARDT
ceres_dogleg_type: TRADITIONAL_DOGLEG
ceres_loss_function: None
# Frame settings
odom_frame: odom
map_frame: map
base_frame: base_footprint
# Sensor topics
scan_topic: /scan
# Mode: mapping or localization
use_map_saver: true
mode: mapping # switch to 'localization' when using an existing map
# Timing and updates
debug_logging: false
throttle_scans: 1 # process every scan
transform_publish_period: 0.02 # seconds between publishing TF (0 disables)
map_update_interval: 5.0 # seconds between updated occupancy grid publications
resolution: 0.05 # occupancy grid resolution [m/cell]
max_laser_range: 12.0 # max range for rasterizing scans into submaps
minimum_time_interval: 0.5 # minimum seconds between scan integration
transform_timeout: 0.2 # TF lookup timeout [s]
tf_buffer_duration: 30.0 # TF buffer [s]
stack_size_to_use: 40000000 # increase if serialization issues
enable_interactive_mode: true # allow runtime parameter adjustments via service
# Scan-matching and submap settings
use_scan_matching: true
use_scan_barycenter: true
minimum_travel_distance: 0.5 # min robot translation [m] to integrate new scan
minimum_travel_heading: 0.5 # min rotation [rad] to integrate new scan
scan_buffer_size: 10
scan_buffer_maximum_scan_distance: 10.0
# Loop closure parameters
link_match_minimum_response_fine: 0.1
link_scan_maximum_distance: 1.5
loop_search_maximum_distance: 3.0
do_loop_closing: true
loop_match_minimum_chain_size: 10
loop_match_maximum_variance_coarse: 3.0
loop_match_minimum_response_coarse: 0.35
loop_match_minimum_response_fine: 0.45
# Correlation search (scan matching) parameters
correlation_search_space_dimension: 0.5
correlation_search_space_resolution: 0.01
correlation_search_space_smear_deviation: 0.1
# Loop closure search parameters
loop_search_space_dimension: 8.0
loop_search_space_resolution: 0.05
loop_search_space_smear_deviation: 0.03
# Scan matcher penalties
distance_variance_penalty: 0.5
angle_variance_penalty: 1.0
fine_search_angle_offset: 0.00349
coarse_search_angle_offset: 0.349
coarse_angle_resolution: 0.0349
minimum_angle_penalty: 0.9
minimum_distance_penalty: 0.5
use_response_expansion: true
Time to put all the boring theory into practice and create some cool maps!
Clone the Self-Driving-and-ROS-2-Learn-by-Doing-Map-Localization repository on your PC
git clone https://github.com/AntoBrandi/Self-Driving-and-ROS-2-Learn-by-Doing-Map-Localization.git
Go to the bumperbot workspace folder in the Section10_SLAM folder
cd Self-Driving-and-ROS-2-Learn-by-Doing-Map-Localization/Section10_SLAM/bumperbot_ws/
Install all the required dependencies using rosdep
rosdep install --from-paths src -y --ignore-src
Build the workspace to compile it
colcon build
In a new window of the terminal, source the bumperbot_ws
. install/setup.bash
Launch the simulation and start mapping!
ros2 launch bumperbot_bringup simulated_robot.launch.py use_slam:=True world_name:=small_house
Move your robot around either by connecting a joystick or by using the keyboard on your PC.
In that case, you can run the key_teleop node with the following command:
ros2 run key_teleop key_teleop --ros-args -p twist_stamped_enabled:=false
