🗺️ SLAM-Toolbox Lifelong Incremental Mapping (Lifelong Mapping)

1. Course Content

• Master the core concepts and engineering value of SLAM-Toolbox lifelong mapping (Lifelong / Continuing Mapping).

• Master the workflow of loading existing historical pose graph (.posegraph + .data), performing initial relocalization in RViz, and performing incremental mapping in unknown/changed areas.

• Master the pose graph smooth docking mechanism based on lidar point cloud gating (Scan Gating).

• Master the method for serializing and overwriting saving the latest incremental pose graph and 2D occupancy grid map.

2. SLAM-Toolbox Lifelong Mapping Introduction

In actual robot applications, the operating environment is often not static (for example, warehouse shelf rearrangements, office area expansions, or newly opened operating passages). Traditional 2D SLAM algorithms (such as Gmapping, Cartographer default configuration) mostly belong to Single-Session Mapping; once mapping is completed and a static map is generated, it cannot be directly expanded, and when the environment changes, it often requires completely re-scanning the entire site for mapping, which is extremely costly.

SLAM-Toolbox provides industry-leading Lifelong / Continuing Mapping capability:

• Pose Graph Deserialization: At system startup, historical topology graph and original laser scan data are directly loaded through the deserialization service, completely restoring the historical environment in the Ceres graph optimization back-end.

• Initial Pose Smooth Docking: Through the point cloud gating interception mechanism, laser data is released only after the robot completes initial alignment via RViz calibration or dock parking, avoiding pollution of the historical map by misaligned laser point clouds.

• Incremental Expansion and Global Loop Closure: When the robot explores unknown areas or changed environments, it real-time constructs new pose nodes and local scan buffers; when returning to known areas, it automatically triggers global loop closure detection and relaxation optimization between old and new nodes, generating a higher-precision globally unified map.

3. Preparation and Running

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

3.1 Start Lifelong Incremental Mapping Node

Run in Terminal 1 (specify loading previously saved map.posegraph and map.data through pose_graph:=map parameter):

ros2 launch yahboomcar_nav map_slam_toolbox_launch.py pose_graph:=map

3.2 Start RViz Map Visualization and Initial Pose Alignment

Run in Terminal 2:

xxxxxxxxxx
ros2 launch yahboomcar_nav display_map_launch.py

3.3 Start Keyboard Control to Explore New Areas

Run in Terminal 3 the keyboard control node:

xxxxxxxxxx
ros2 run yahboomcar_ctrl yahboom_keyboard

3.4 Incremental Mapping Operation and Map Save Instructions

1. Explore New Environment and Loop Closure Optimization:

   • Remote control the robot to slowly and smoothly drive from known areas into unknown expansion areas, observe the seamless splicing of historical map boundaries and new map in RViz.

   • After exploring new areas, guide the robot back to known areas; the system will automatically match historical features and trigger global loop closure optimization, eliminating long-distance accumulated drift.

   

2. Save Latest Incremental Pose Graph (.posegraph + .data): Run the pose graph save command in Terminal 4 to overwrite and update the pose graph file:

   xxxxxxxxxx
   ros2 launch yahboomcar_nav save_map_launch.py map_type:=posegraph map_name:=map
   

   

3. Save Latest 2D Raster Map (.yaml + .pgm): Run the raster map save command to generate the latest raster map for subsequent Nav2 navigation:

   xxxxxxxxxx
   ros2 launch yahboomcar_nav save_map_launch.py map_name:=map
   

   

4. Core Source Code and Algorithm Implementation Mechanism Analysis

4.1 Lifelong Mapping Launch and Scheduling Launch Analysis

Launch File Paths:

xxxxxxxxxx
$HOME/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_nav/launch/slam/map_slam_toolbox_launch.py
$HOME/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_nav/launch/slam/slam_toolbox_wit_launch.py

Launch Scheduling Core Logic

In slam_toolbox_wit_launch.py, the following key scheduling is performed for incremental mapping mode (incoming pose_graph):

​x
def _resolve_pose_graph(map_folder_dir, pose_graph):
    # 1. Automatically verify historical pose graph dual files (.posegraph and .data) existence
    graph_path = os.path.expanduser(pose_graph)
    if not os.path.isabs(graph_path):
        graph_path = os.path.join(map_folder_dir, graph_path)

    missing_files = [
        path for path in (f"{graph_path}.posegraph", f"{graph_path}.data")
        if not os.path.isfile(path)
    ]
    if missing_files:
        raise RuntimeError("Cannot continue mapping because pose graph files are missing: " + ", ".join(missing_files))
    return graph_path

def _launch_slam_toolbox(context, map_folder_dir, sync_slam_params_file, async_slam_params_file):
    ...
    if pose_graph:
        pose_graph_path = _resolve_pose_graph(map_folder_dir, pose_graph)
        runtime_parameters["map_file_name"] = pose_graph_path
        runtime_parameters["map_start_at_dock"] = True
        if initial_pose_mode == "rviz":
            # Redirect SLAM node point cloud input to gated and filtered safe topic
            runtime_parameters["scan_topic"] = "/scan_after_initial_pose"

    actions = [
        Node(
            package="slam_toolbox",
            executable=f"{slam_mode}_slam_toolbox_node",
            parameters=parameters,
            name="slam_toolbox",
            output="screen",
        )
    ]

    # When using RViz interactive relocalization, start dedicated pose initializer and point cloud gating node
    if pose_graph_path and initial_pose_mode == "rviz":
        actions.append(
            Node(
                package="yahboomcar_nav",
                executable="slam_pose_initializer",
                name="slam_pose_initializer",
                output="screen",
                parameters=[{
                    "pose_graph": pose_graph_path,
                    "map_frame": "map",
                    "initial_pose_topic": "/initialpose",
                    "scan_input_topic": "/scan",
                    "scan_output_topic": "/scan_after_initial_pose",
                    "deserialize_service": "/slam_toolbox/deserialize_map",
                }],
            )
        )
    return actions

4.2 Initial Relocalization and Point Cloud Gating Source Analysis (slam_pose_initializer)

Source Code Path:

xxxxxxxxxx
$HOME/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_nav/yahboomcar_nav/slam_pose_initializer.py

Point Cloud Gating (Scan Gating) and Service Asynchronous Interaction Source

xxxxxxxxxx
class SlamPoseInitializer(Node):
    def __init__(self):
        super().__init__("slam_pose_initializer")
        ...
        self._scan_enabled = False       # Lock point cloud gating in initial state
        self._request_in_flight = False
        self._pending_pose = None

        # 1. Subscribe to original lidar /scan, do not publish to /scan_after_initial_pose when locked
        self._scan_publisher = self.create_publisher(LaserScan, scan_output_topic, qos_profile_sensor_data)
        self._scan_subscription = self.create_subscription(
            LaserScan, scan_input_topic, self._scan_callback, qos_profile_sensor_data
        )

        # 2. Subscribe to 2D Pose Estimate initial pose topic published by RViz
        self._initial_pose_subscription = self.create_subscription(
            PoseWithCovarianceStamped, initial_pose_topic, self._initial_pose_callback, QoSProfile(depth=1)
        )

        # 3. Create SLAM-Toolbox pose graph deserialization service client
        self._deserialize_client = self.create_client(DeserializePoseGraph, service_name)

    def _scan_callback(self, message):
        # Only release lidar data after relocalization alignment is complete
        if self._scan_enabled:
            self._scan_publisher.publish(message)

    def _initial_pose_callback(self, message):
        if self._scan_enabled or self._request_in_flight:
            return
        ...
        # Extract 2D pose (x, y, yaw)
        self._pending_pose = (position.x, position.y, yaw)
        self._try_initialize()

    def _try_initialize(self):
        x, y, yaw = self._pending_pose
        request = DeserializePoseGraph.Request()
        request.filename = self._pose_graph
        request.match_type = request.START_AT_GIVEN_POSE
        request.initial_pose.x = x
        request.initial_pose.y = y
        request.initial_pose.theta = yaw

        # Asynchronously call deserialization service
        self._request_in_flight = True
        future = self._deserialize_client.call_async(request)
        future.add_done_callback(self._initialization_finished)

    def _initialization_finished(self, future):
        self._request_in_flight = False
        # After deserialization succeeds: enable point cloud gating, unsubscribe from initialpose
        self._scan_enabled = True
        self.destroy_subscription(self._initial_pose_subscription)
        self.get_logger().info("Pose graph initialized at RViz pose; laser scans are now enabled")

Design Highlights

• Preventing Historical Map Pollution: If the robot does not start at the historical origin, directly sending lidar point cloud to SLAM-Toolbox will cause current point cloud to be incorrectly matched near the origin, causing serious map distortion. Through the point cloud gating mechanism, mapping only begins after pose alignment succeeds.

• Seamless Smooth Docking: After deserialization succeeds, point cloud is immediately released; the entire process does not require restarting the node, completely transparent to upper-level control and visualization.

4.3 Lifelong Mapping Algorithm Core Parameter Interpretation

Configuration File Path:

xxxxxxxxxx
$HOME/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_nav/params/mapper_params_online_sync.yaml

Core Parameter Description Table