Configure Mapping and Localization

The problem

Your robot has to work in the same building every day. You will map the TurtleBot 3 world once, save the map in both forms, restart everything, and make the robot localise in the saved map from its start position.

The localization configuration you are given has mistakes. The pre-flight tool finds some of them; the simulator finds the rest.

Expected behaviour. After the restart, SLAM Toolbox runs in localization mode, loads the serialised graph, and the pose of the robot in the map frame is within 0.25 m and 10° of where it was spawned (the same place where mapping started, which is the origin of the saved map).

Observable failure. The node starts in mapping mode and builds a new map over the old one; or it fails to load the file; or it loads it and the robot appears far from the right place because no initial pose was given.

This is a real ROS 2 project, not a browser lab. You run it on your own machine with ROS 2, a simulator and the tools of Module 7. The website does not run ROS 2 and does not grade it; you grade yourself with the criteria below and the check.sh script.

Time: about 90 minutes · Level: Intermediate · Theory: M7-T4 SLAM Toolbox and Mapping Workflows, M7-T5 Mapping vs Localization Mode, M7-T6 Map Saving and Reuse

Prerequisites and setup

You need a computer with ROS 2 Humble on Ubuntu 22.04 (a virtual machine or container with a display is fine). Jazzy works for the SLAM and Nav2 parts; the simulator launch differs, and the project notes say where. Install once:

sudo apt update
sudo apt install ros-humble-turtlebot3-gazebo ros-humble-turtlebot3-teleop ros-humble-slam-toolbox \
  ros-humble-navigation2 ros-humble-nav2-bringup ros-humble-tf2-tools ros-humble-tf2-ros ros-humble-rviz2
source /opt/ros/humble/setup.bash
export TURTLEBOT3_MODEL=burger

Get the project files: ros2_labs/l4-configure-mapping-and-localization in the site’s repository (clone it, or copy the files listed below from this page). The folder ros2_labs/common holds scripts shared by several labs.

1. Mapping run. mkdir -p /tmp/slam_lab. Terminal 1: export TURTLEBOT3_MODEL=burger and ros2 launch launch/lab4.launch.py params:=config/mapping.yaml. Terminal 2: teleop. Drive around the whole world, finishing where you started.
2. Save. ros2 run nav2_map_server map_saver_cli -f /tmp/slam_lab/lab_map and ros2 service call /slam_toolbox/serialize_map slam_toolbox/srv/SerializePoseGraph "{filename: '/tmp/slam_lab/lab_graph'}".
3. Restart. Stop everything (Ctrl-C in all terminals; killall gzserver gzclient if needed). Do not move anything: the next launch spawns the robot at the same place.

Verification status. The project files follow the documented Humble interfaces but have not been executed by the site’s automated tests (they cannot run ROS 2). If a command or parameter is rejected by your version, the error names it: treat that as part of the exercise and tell us.

The project files

The mapping parameters are correct. The localization parameters are not; the launch file is generic.

launch/lab4.launch.py

"""Lab M7-L4: TurtleBot 3 world plus SLAM Toolbox with a parameter file chosen by the 'params' argument.

Mapping:       ros2 launch launch/lab4.launch.py params:=config/mapping.yaml
Localization:  ros2 launch launch/lab4.launch.py params:=config/localization.yaml
"""
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    tb3 = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch', 'turtlebot3_world.launch.py')
    return LaunchDescription([
        DeclareLaunchArgument('params', default_value='config/mapping.yaml'),
        IncludeLaunchDescription(PythonLaunchDescriptionSource(tb3)),
        Node(package='slam_toolbox', executable='async_slam_toolbox_node', name='slam_toolbox', output='screen',
             parameters=[LaunchConfiguration('params')]),
    ])

config/mapping.yaml

# SLAM Toolbox parameters for the mapping run of lab M7-L4 (correct as given).
slam_toolbox:
  ros__parameters:
    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

    use_sim_time: true
    odom_frame: odom
    map_frame: map
    base_frame: base_footprint
    scan_topic: /scan
    mode: mapping

    debug_logging: false
    throttle_scans: 1
    transform_publish_period: 0.02
    map_update_interval: 2.0
    resolution: 0.05
    max_laser_range: 3.5
    minimum_time_interval: 0.3
    transform_timeout: 0.2
    tf_buffer_duration: 30.
    stack_size_to_use: 40000000
    use_scan_matching: true
    use_scan_barycenter: true
    minimum_travel_distance: 0.2
    minimum_travel_heading: 0.2
    do_loop_closing: true
    loop_search_maximum_distance: 3.0
    loop_match_minimum_response_coarse: 0.35
    loop_match_minimum_response_fine: 0.45

config/localization.yaml

# SLAM Toolbox parameters for the localization run of lab M7-L4. This file contains mistakes on purpose.
slam_toolbox:
  ros__parameters:
    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

    use_sim_time: true
    odom_frame: odom
    map_frame: map
    base_frame: base_footprint
    scan_topic: /scan
    mode: mapping
    map_file_name: /tmp/slam_lab/lab_graph.posegraph

    debug_logging: false
    throttle_scans: 1
    transform_publish_period: 0.02
    map_update_interval: 2.0
    resolution: 0.05
    max_laser_range: 3.5
    minimum_time_interval: 0.3
    transform_timeout: 0.2
    tf_buffer_duration: 30.
    stack_size_to_use: 40000000
    use_scan_matching: true
    use_scan_barycenter: true
    minimum_travel_distance: 0.2
    minimum_travel_heading: 0.2
    do_loop_closing: true
    loop_search_maximum_distance: 3.0
    loop_match_minimum_response_coarse: 0.35
    loop_match_minimum_response_fine: 0.45

check_pose.py

#!/usr/bin/env python3
"""Checks that map -> base_footprint is close to the expected pose (default 0, 0, 0: the robot was spawned where the map was started).
Usage: check_pose.py [x y yaw_deg [tol_m tol_deg]]"""
import math
import sys
import rclpy
from rclpy.node import Node
from rclpy.parameter import Parameter
from rclpy.time import Time
from tf2_ros import Buffer, TransformListener


def main():
    a = [float(v) for v in sys.argv[1:]] + [0.0, 0.0, 0.0, 0.25, 10.0][len(sys.argv) - 1:]
    ex, ey, eyaw, tol_m, tol_deg = a[:5]
    rclpy.init()
    node = Node('check_pose', parameter_overrides=[Parameter('use_sim_time', Parameter.Type.BOOL, True)])
    buf = Buffer()
    TransformListener(buf, node)
    end = node.get_clock().now().nanoseconds + 15 * 10**9
    t = None
    while rclpy.ok() and node.get_clock().now().nanoseconds < end:
        rclpy.spin_once(node, timeout_sec=0.2)
        try:
            t = buf.lookup_transform('map', 'base_footprint', Time())
            break
        except Exception:
            pass
    if t is None:
        print('no transform map -> base_footprint')
        return 2
    q = t.transform.rotation
    yaw = math.degrees(math.atan2(2 * (q.w * q.z + q.x * q.y), 1 - 2 * (q.y * q.y + q.z * q.z)))
    dp = math.hypot(t.transform.translation.x - ex, t.transform.translation.y - ey)
    dy = abs((yaw - eyaw + 180) % 360 - 180)
    print(f'pose in map: x={t.transform.translation.x:.2f} y={t.transform.translation.y:.2f} yaw={yaw:.1f} deg; error {dp:.2f} m, {dy:.1f} deg')
    return 0 if dp <= tol_m and dy <= tol_deg else 1


if __name__ == '__main__':
    raise SystemExit(main())

What to do

Follow the loop of this track: Problem → Experiment → Diagnose → Fix → Evaluate → Understand.

1. Map. Complete the mapping run. Watch RViz: close the loop by returning to the start. Note the size of the map.
2. Save both forms. Save the grid and serialise the graph. List the files in /tmp/slam_lab. Which one will AMCL use, and which one will SLAM Toolbox use? Run ./check.sh now: the first three checks should pass.
3. Diagnose the localization config. Paste config/localization.yaml into the tool below. Then read M7-T5 and M7-T6 and list, in your notes, everything that would stop localization from working.
4. Fix. Edit the file: the mode, the file name (without extension), and a start pose (map_start_pose: [0.0, 0.0, 0.0], since the robot is at the origin of the map). Launch with params:=config/localization.yaml.
5. Evaluate. With the robot not moved since spawn, run ./check.sh. Then drive a few metres and confirm in RViz that the scan stays on the saved walls.
6. Break it on purpose. Relaunch with map_start_pose: [3.0, 3.0, 0.0]. What happens to the scan and to the pose? Compare with the worked example of M7-T5.

Pre-flight tool

Expected output

Typical output when things are right. Exact numbers, timestamps and wording differ between machines and versions.

Files after saving

ls /tmp/slam_lab
lab_map.pgm  lab_map.yaml  lab_graph.posegraph  lab_graph.data

The map yaml

image: lab_map.pgm
mode: trinary
resolution: 0.05
origin: [-5.2, -4.9, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25

The mode in the running node

ros2 param get /slam_toolbox mode
String value is: localization

The pose check

pose in map: x=0.03 y=-0.02 yaw=0.4 deg; error 0.04 m, 0.4 deg

check.sh when everything is fixed

PASS  occupancy grid saved
PASS  pose graph serialised
PASS  the yaml has resolution 0.05
PASS  localization: the node runs with mode: localization
PASS  localization: map -> base_footprint is within 0.25 m and 10 degrees of the spawn pose

5 passed, 0 failed

Troubleshooting

Evaluation criteria

You are done when every criterion passes. check.sh automates most of them (run it from the project folder, with the launch file running and ROS 2 sourced). The remaining ones you judge yourself.

cd ros2_labs/l4-configure-mapping-and-localization
./check.sh

Worked explanation

Try the lab before opening this.

Why it happens

The occupancy grid is a picture drawn from the pose graph; localization in SLAM Toolbox matches new scans against the stored scans, so it needs the graph (M7-T6). A localiser also needs a prior: without it, it can only search near wherever it believes it is (M7-T5). Both are configuration facts that cannot be guessed from the behaviour of the algorithm.

What this project does not cover

• The TurtleBot 3 world is small and feature-rich; large symmetric buildings need global localization.
• The mapping and the localization run in the same simulation session type; a real robot adds sensor differences between sessions.
• AMCL is not part of this project; run Nav2’s localization on the saved yaml for comparison.
• The project files have not been run by the site’s tests.

What’s next

Other projects in this module

• M7-L1: Debug a ROS 2 SLAM Pipeline
• M7-L2: Fix a TF Configuration Error
• M7-L3: Diagnose Sensor Timestamp Problems
• M7-L4: Configure Mapping and Localization: this project.
• M7-L5: Debug SLAM-to-Nav2 Integration