slam-toolbox Mapping

1. Course Content

Basic: Run the example program, build and save a grid map with the slam-toolbox mapping algorithm.

Advanced: Understand slam-toolbox's configuration parameters

2. slam-toolbox Introduction

slam-toolbox is a ROS-based open-source 2D SLAM (simultaneous localization and mapping) algorithm package, mainly used for the mapping and localization of mobile robots in unknown environments. It is based on the Graph Optimization framework, combining sensor data such as LiDAR (e.g., 2D LiDAR) and an inertial measurement unit (IMU) to build a 2D grid map of the environment and update the robot's pose in real time.

2.1 Frontend Processing (Laser Scan Matching)

• GICP algorithm: estimates the robot's relative pose change by iteratively computing the optimal match between the current laser scan and the map. Compared with traditional ICP, GICP introduces a probabilistic model and is more robust to outliers.

• Motion compensation: uses IMU data to compensate for the laser scan distortion during the robot's motion, improving the matching accuracy in dynamic environments.

2.2 Backend Optimization (Graph Optimization)

• Pose graph construction: constructs a graph structure from the robot's pose nodes and the relative pose constraints obtained from scan matching.

• Global optimization: uses a nonlinear optimization algorithm (such as Ceres Solver) to optimize the pose graph, eliminating accumulated error and achieving global consistency of the map.

3. Running the Cases

• Launch method 1: GUI launch

• Launch method 2: CLI command launch in VNC

xxxxxxxxxx
ros2 launch yahboomcar_bringup slam_toolbox.launch.py

• Control the car's movement. You can use keyboard control or handle control; here, keyboard control is used as an example

xxxxxxxxxx
ros2 run common_demo keyboard_control

Click once on the terminal window with the mouse. Press z to appropriately reduce the speed

• Press I, <, J, L to control the car's forward, backward, left turn, and right turn respectively, controlling the car to move slowly to complete mapping

3.1 Saving the Grid Map

• Start RobotManager

xxxxxxxxxx
robotmanage gui

• Click Save Map. You can also customize the map name and map type. If the terminal indicates that saving failed, simply save again

• The map save path is as follows:

xxxxxxxxxx
~/map/map.yaml

• A pgm image and a yaml file map.yaml

xxxxxxxxxx
image: map.pgm
mode: trinary
resolution: 0.05
origin: [-10, -10, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25

3.2 Saving the posegraph Pose Graph

• The pose graph can be used in localization mode during navigation

4. Configuration Parameters

• Parameter file path for synchronous mapping mode:

xxxxxxxxxx
~/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_bringup/param/mapper_params_online_async.yaml

• See the comments for the role of each parameter

sync_slam_toolbox:
  ros__parameters:

    # Plugin parameters (optimization    solver_plugin: solver_plugins::CeresSolver  # Optimization solver plugin, using the Ceres solver
    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY  # Ceres linear solver type, sparse normal Cholesky decomposition
    ceres_preconditioner: SCHUR_JACOBI  # Ceres preconditioner type, Schur-Jacobi preconditioner
    ceres_trust_strategy: LEVENBERG_MARQUARDT  # Ceres trust region strategy, Levenberg-Marquardt method
    ceres_dogleg_type: TRADITIONAL_DOGLEG  # Ceres dogleg type, traditional dogleg (takes effect only when the trust region strategy is DOGLEG)
    ceres_loss_function: None  # Ceres loss function, none (uses the default squared loss)

    # ROS parameters
    odom_frame: odom  # Name of the odometry coordinate frame
    map_frame: map  # Name of the map coordinate frame
    base_frame: base_footprint  # Name of the robot base coordinate frame
    scan_topic: /scan  # LiDAR scan data topic (absolute path)
    use_map_saver: true  # Whether to enable the map saving service and subscribe to the map topic
    mode: mapping  # Operating mode, mapping or localization

    # Map loading related parameters (if enabled, it will load the map from the specified file and continue mapping)
    map_file_name: ""  # The pose graph file name to load (if empty, nothing is loaded)
    # map_start_pose: [0.0, 0.0, 0.0]  # Starting pose [x, y, yaw] when loading the map (mutually exclusive with map_start_at_dock; this parameter takes priority)
    map_start_at_dock: true  # Whether to start loading from the first node of the map (the dock position)

    debug_logging: false  # Whether to enable debug log output
    throttle_scans: 1  # Scan data throttling count in synchronous mode (process 1 frame every n frames)
    transform_publish_period: 0.02  # Publishing period of the map-to-odometry coordinate transform (seconds); 0 means do not publish
    map_update_interval: 5.0  # Update interval of the 2D occupancy grid map (seconds, used for visualization or other applications)
    resolution: 0.05  # Resolution of the 2D occupancy grid map (meters/pixel)
    min_laser_range: 0.05  # Minimum valid laser distance used for grid map generation (meters)
    max_laser_range: 12.0  # Maximum valid laser distance used for grid map generation (meters)
    minimum_time_interval: 0.5  # Minimum time interval for processing two frames of scan data in synchronous mode (seconds)
    transform_timeout: 0.2  # Timeout for TF coordinate transform lookup (seconds)
    tf_buffer_duration: 30.  # Cache duration of TF messages (seconds)
    stack_size_to_use: 40000000  # Program stack size (bytes), used for serialization/deserialization of large maps
    enable_interactive_mode: true  # Whether to enable interactive mode (retains laser scan cache for RVIZ interaction, which increases memory usage)

    # General parameters
    use_scan_matching: true  # Whether to use scan matching
    use_scan_barycenter: true  # Whether to use the scan barycenter (improves matching stability)
    minimum_travel_distance: 0.5  # Minimum distance the robot must move before processing new scan data (meters)
    minimum_travel_heading: 0.5  # Minimum angle the robot must turn before processing new scan data (radians)
    scan_buffer_size: 10  # Scan data buffer size (keeps the most recent n scans)
    scan_buffer_maximum_scan_distance: 10.0  # Maximum distance of laser points in the scan buffer (meters; beyond this they are filtered out)
    link_match_minimum_response_fine: 0.1  # Minimum response value for link matching in fine matching (used for adjacent node constraints)
    link_scan_maximum_distance: 1.5  # Maximum distance between adjacent scans (meters, used to build links between nodes)
    loop_search_maximum_distance: 3.0  # Maximum search distance for loop closure detection (meters)
    do_loop_closing: true  # Whether to enable loop closure detection
    loop_match_minimum_chain_size: 10  # Minimum node chain length required for loop closure detection (to avoid mismatches caused by short chains)
    loop_match_maximum_variance_coarse: 3.0  # Maximum variance threshold during coarse matching (used to filter candidate loop closures)
    loop_match_minimum_response_coarse: 0.35  # Minimum response value for loop closures in coarse matching
    loop_match_minimum_response_fine: 0.45  # Minimum response value for loop closures in fine matching

    # Correlation parameters - scan matching parameters
    correlation_search_space_dimension: 0.5  # Search space size for scan matching (meters; the optimal match is searched within this range)
    correlation_search_space_resolution: 0.01  # Resolution of the scan matching search space (meters; affects matching accuracy and speed)
    correlation_search_space_smear_deviation: 0.1  # Smear deviation of laser points in scan matching (meters; increases robustness)

    # Correlation parameters - loop closure detection parameters
    loop_search_space_dimension: 8.0  # Search space size for loop closure detection (meters)
    loop_search_space_resolution: 0.05  # Resolution of the loop closure detection search space (meters)
    loop_search_space_smear_deviation: 0.03  # Smear deviation of laser points in loop closure detection (meters)

    # Scan matcher parameters
    distance_variance_penalty: 0.5  # Distance variance penalty coefficient (affects the weight of distance error in matching)
    angle_variance_penalty: 1.0  # Angle variance penalty coefficient (affects the weight of angle error in matching)

    fine_search_angle_offset: 0.00349  # Angular offset for fine search (radians, about 0.2 degrees)
    coarse_search_angle_offset: 0.349  # Angular offset for coarse search (radians, about 20 degrees)
    coarse_angle_resolution: 0.0349  # Angular resolution for coarse search (radians, about 2 degrees)
    minimum_angle_penalty: 0.9  # Minimum angle penalty coefficient (filters out matches with excessive angle deviation)
    minimum_distance_penalty: 0.5  # Minimum distance penalty coefficient (filters out matches with excessive distance deviation)
    use_response_expansion: true  # Whether to use response expansion (improves matching stability)
    min_pass_through: 2  # Minimum number of points passing through in scan matching (filters out invalid matches)
    occupancy_threshold: 0.1  # Threshold of the occupancy grid (used to determine whether a grid cell is occupied)