Skip to main content

SLAM

Chapter description​

In this tutorial, we will walk you through the map creation process and discuss the SLAM algorithm, which consists of two parts: mapping and localization. For this purpose, I will use the package slam_toolbox, which will be used to create a terrain map. Terrain mapping is usually a necessary element to create more advanced autonomous driving systems. In addition, thanks to the knowledge of the map and knowing that it does not change, we can use simple location algorithms such as amcl, which compares the current data from the sensors and tries to adjust the location in such a way that the readings from the point cloud match the map. To do this, we will use the LIDAR mounted on board our ROSbot.

This tutorial is dedicated to the ROSbot XL. However, most of the information and commands are compatible with all ROSbots, and most of the changes are simple and come down to launching the appropriate container, depending on your hardware configuration.

Available ROSbot platforms:

You can find the repository containing the final result after completing all the ROS 2 Tutorials here.

Introduction​

SLAM (simultaneous localization and mapping) is a technique for creating a map of environment and determining robot position at the same time. It is widely used in robotics. While moving, current measurements and localization are changing, in order to create map it is necessary to merge measurements from previous positions.

To perform an accurate and precise SLAM, one of the simplest solutions is to use a laser scanner and an odometry system (speed measurement system based on encoder data). However, due to the imperfections of the sensors, the map creation process is much more difficult. Why? Because various imperfections, e.g. caused by wheel slippage, can cause the same wall to be classified as two objects. To counteract unwanted changes, these algorithms use various tricks, such as the Monte Carlo method, which will be described in detail in Localize robot with amcl part. Note, however, that the slam_toolbox algorithm also performs similar operations to improve the quality of the map. For more details check this video.

Laser scanner​

In this example we will use RPLIDAR (laser scanner). For ROSbot, you can find a repository online containing rplidarNode from the rplidar_ros package to run RPLIDAR, but using a prepared docker is much simpler. The container will automatically configure and run rplidarNode. Upon startup, the node sends scans to topic /scan with a message of type sensor_msgs/LaserScan.

To test your sensor works type:

On ROSbot

Running the ROSbot is very easy thanks to the created docker services.

husarion@husarion:~$
docker compose up -d microros <rosbot_service> rplidar

After few seconds LiDAR should start spinning and new /scan topic should appear.

Depend on your ROSbot model, you should use appropriate name for your <rosbot_service>. To check available docker service use following command docker compose config --services, or find suitable Docker images prepared by Husarion.

You should have /scan topic in your system. You can examine it with ros2 topic list. It is possible to echo data but you will get lots of output, which is hard to read. Better method for checking the /scan topic is to use RViz. Run RViz and click Add. In new window select By topic tab and from the list select /scan. In visualized items list find position Fixed Frame and change it to base_link. To improve visibility of scanned shape, you may need to adjust one of visualized object options, set value of Style to Points. You should see many points which resemble shape of obstacles surrounding your robot. You can also add a RobotModel in RViz listen to /robot_description topic. Enable keyboard controls and watch how environment change.

ros2 run teleop_twist_keyboard teleop_twist_keyboard

image

SLAM​

To build the map we will use the async_slam_toolbox_node node from the slam_toolbox package. This node subscribes to the /scan_filtered topic to obtain data about the surrounding environment. /scan_filtered is an almost identical topic in relation to /scan, it differs in that points related to robot elements, such as a camera handle, are removed. In addition, it subscribes to /tf messages to obtain the position of the laser scanner and the robot relative to the starting point. Thanks to the position and laser data, the algorithm can determine the distance of the robot from the surrounding obstacles and on this basis create an occupation map. This map is published in topic /map in message nav_msgs/OccupancyGrid.

Create parameters file​

To run this node it is necessary to set parameters. There are many parameters provided by async_slam_toolbox_node node. All the parameters are in the slam_toolbox github repository, and the most important ones are described below.

  • base_frame - name of frame related with robot, in our case it will be base_link,
  • odom_frame - name of frame related with starting point, in our case it will be odom,
  • scan_topic - name of topic with sensor data, in our case it will be /scan_filtered,
  • resolution - resolution of the map in meters, in our case it will be set to 0.04,
  • max_laser_range - the maximum usable range of the laser, in our case it will be 12.0 meters
  • mode - mapping or localization mode for performance optimizations in the Ceres problem creation

All above parameters we set in dedicated yaml file in new config folder.

"~/ros2_ws/src/tutorial_pkg/config/slam.yaml
slam:
  ros__parameters:

    # Plugin params
    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

    # ROS Parameters
    odom_frame: odom
    map_frame: map
    base_frame: base_link
    scan_topic: /scan_filtered
    # mode: mapping # It will be set in launch file
    debug_logging: false
    throttle_scans: 1
    transform_publish_period: 0.02 # If 0 never publishes odometry
    map_update_interval: 2.0
    resolution: 0.04
    max_laser_range: 12.0
    minimum_time_interval: 0.1
    transform_timeout: 0.2
    tf_buffer_duration: 20.0
    stack_size_to_use: 40000000
    enable_interactive_mode: false

    # General Parameters
    use_scan_matching: true
    use_scan_barycenter: true
    minimum_travel_distance: 0.3
    minimum_travel_heading: 0.5
    scan_buffer_size: 10
    scan_buffer_maximum_scan_distance: 7.0
    link_match_minimum_response_fine: 0.1
    link_scan_maximum_distance: 1.0
    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 Parameters - Correlation Parameters
    correlation_search_space_dimension: 0.5
    correlation_search_space_resolution: 0.01
    correlation_search_space_smear_deviation: 0.1

    # Correlation Parameters - Loop Closure Parameters
    loop_search_space_dimension: 8.0
    loop_search_space_resolution: 0.05
    loop_search_space_smear_deviation: 0.03

    # Scan Matcher Parameters
    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

info

You can find detailed explanation of this parameters on official Github repository

The structure of config file

The configuration file should start with the node name (in our case amcl). Note: If the name is not exactly the same as the node name, the parameters will not be loaded. After the node name, enter ros__parameters (there are two _ characters in the name). From the next line, enter the names and values ​​of the parameters.

*If you are using namespace you should add name of namespace in the first line. Example:

namespace_name:
  node_name:
    ros__parameters:
      param1: value1
      param2: value2
      ...

Launch slam_toolbox​

Then create a new file slam.launch.py which will run async_slam_toolbox_node with the configuration contained in the file. The file also starts the node that runs RViz in the given configuration.

~/ros2_ws/src/tutorial_pkg/launch/slam.launch.py
import os

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time')
    slam_params_file = LaunchConfiguration('slam_params_file')

    use_sim_time_arg = DeclareLaunchArgument(
        'use_sim_time', default_value='false', description='Use simulation/Gazebo clock'
    )

    slam_params_file_arg = DeclareLaunchArgument(
        'slam_params_file',
        default_value=os.path.join(
            get_package_share_directory("tutorial_pkg"), 'config', 'slam.yaml'
        ),
        description='Full path to the ROS2 parameters file to use for the slam_toolbox node',
    )

    slam_node = Node(
        package='slam_toolbox',
        executable='async_slam_toolbox_node',
        name='slam',
        output='screen',
        parameters=[slam_params_file, {'use_sim_time': use_sim_time}],
    )

    rviz_config_path = os.path.join(
        get_package_share_directory("tutorial_pkg"), "rviz", "slam.rviz"
    )
    rviz2_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="screen",
        arguments=["-d", rviz_config_path],
    )

    ld = LaunchDescription()

    ld.add_action(use_sim_time_arg)
    ld.add_action(slam_params_file_arg)
    ld.add_action(slam_node)
    ld.add_action(rviz2_node)

    return ld

Let's make a few more changes before we run the script. Let's create a new maps folder where we will store information about the saved map and rviz where the default rviz configuration will be located. You can download rviz configurations from the repository.

It is still necessary to inform the compiler about the newly created folders. Add this folder to CMakeList.txt:

~/ros2_ws/src/tutorial_pkg/CMakeLists.txt
install(DIRECTORY
  config
  launch
  img_data
  maps
  rviz
  DESTINATION share/${PROJECT_NAME})

To run SLAM example build package and type:

husarion@husarion:~$
ros2 launch tutorial_pkg slam.launch.py

After starting, RViz should open. If you would like to manually configure RViZ, you should add the following topics: /scan_filtered and also /map topic from By topic tab. Initializing a node can take a while, so you may need to wait few second until you see generated map. Starting state should be similar to below picture:

image

Now drive your robot around and observe as new parts of map are added, continue until all places are explored. Final map should look like below:

image

In this way, a occupancy map was created, where white points indicate free space, black points are a wall or other obstacle, and transparently marked places not yet visited. More about the occupancy map can be found in the next chapter - path planning. Now let's learn how to save the map.

Map​

Saving the map​

The /map topic is available till node is running, when you close your node you will lose all your explored map. To deal with this you can save map, this can be done by SLAM RViz Plugin. To open this window click in Panels->Add new panel and select SlamToolboxPlugin. You should see new window.

image

You have to option to save map:

  • Save map - saves the map in the old format. Needed to run older apps
  • Serialize map - new type of map saving

We will use both approaches. In SlamToolboxPlugin type map next to Save Map and press Save Map. Similarly type map_serialized next to Serialize Map and press Serialize Map. Move the created files to the previously created maps folder in tutorial_pkg. The file structure should look like this:

tutorial_pkg/
  └── maps/
    ├── map.pgm
    ├── map.yaml
    ├── map_serialized.data
    └── map_serialized.posegraph

Load saved map​

In next example we will use Nav2 packages. Nav2 refers to the Navigation2 package, which is a comprehensive set of software modules for autonomous robot navigation. It is part of the ROS 2 Navigation Stack and is designed to provide advanced navigation capabilities for robots operating in real-world environments. It will be used Nav2 in next amcl chapter. Let's install necessary package:

"husarion@husrion:~$
sudo apt install ros-$ROS_DISTRO-navigation2
sudo apt install ros-$ROS_DISTRO-nav2-bringup

Nav2 use older map format (map.yaml & map.pgm). To load map, you will use map_server node with yaml_filename. Run map_server inside maps folder:

husarion@husrion:~/ros2_ws/src/tutorial_pkg/maps$
ros2 run nav2_map_server map_server --ros-args -p yaml_filename:=map.yaml

To start map_server you need to run one extra node from nav2.

husarion@husrion:~/ros2_ws/src/tutorial_pkg/maps$
ros2 run nav2_util lifecycle_bringup map_server

Test result in RViz.

Display Map

map_server node publish only one msg with map. To display map you have two option:

  1. Run RViZ before running map_server
  2. Change Map QoS Policy in RViz. Open Map setting and change Topic->Durability Policy to Transient Local.

image

Localize robot​

AMCL is a probabilistic 2D moving robot location system. It implements an adaptive (or KLD sampling) Monte Carlo location approach (described by Dieter Fox) that uses a particle filter to track the position of the robot on a known map.

Basically amcl is no longer a SLAM algorithm because it does not map the environment. However, the principle of operation of the locator can be helpful to better understand the SLAM group of algorithms. For the algorithm to work properly, it must have access to the map and odometry data. With this data, the algorithm of each iteration compares the appearance of the map, along with the current laser scans, and selects only those locations that best match the collected data with the map.

Create parameters file​

As in the case of the slam_toolbox package, amcl also has a lot of parameters necessary for proper operation. Among the most important are:

  • alphaX - a series of parameters to better match the location depending on the accuracy of the odometry
  • base_frame_id - name of frame related with robot, in our case it will be base_link,
  • global_frame_id - name of frame related with map starting point, in our case it will be map,
  • scan_topic - name of topic with sensor data, in our case it will be /scan_filtered,
  • max_particles - maximum allowed number of particles, in our case it will be set to 1000,
  • min_particles - minimum allowed number of particles, in our case it will be set to 100,
  • laser_max_range - the maximum usable range of the laser, in our case it will be 12.0 meters

Again, it is recommended to put all parameters in one amcl.yaml file.

~/ros2_ws/src/tutorial_pkg/launch/config.amcl.yaml
amcl:
  ros__parameters:
    alpha1: 0.2
    alpha2: 0.3
    alpha3: 0.3
    alpha4: 0.2
    alpha5: 0.3
    base_frame_id: "base_link"
    beam_skip_distance: 0.5
    beam_skip_error_threshold: 0.9
    beam_skip_threshold: 0.3
    do_beamskip: false
    global_frame_id: "map"
    lambda_short: 0.1
    laser_likelihood_max_dist: 2.0
    laser_max_range: 12.0
    laser_min_range: -1.0
    laser_model_type: "likelihood_field"
    max_beams: 60
    max_particles: 1000
    min_particles: 100
    odom_frame_id: "odom"
    pf_err: 0.05
    pf_z: 0.99
    recovery_alpha_fast: 0.05
    recovery_alpha_slow: 0.001
    resample_interval: 1
    robot_model_type: "nav2_amcl::DifferentialMotionModel"
    save_pose_rate: 0.5
    sigma_hit: 0.2
    tf_broadcast: true
    transform_tolerance: 1.0
    update_min_a: 0.3
    update_min_d: 0.2
    z_hit: 0.5
    z_max: 0.05
    z_rand: 0.5
    z_short: 0.05
    scan_topic: scan_filtered
    map_topic: map
    set_initial_pose: true
    always_reset_initial_pose: true
    first_map_only: false
    initial_pose:
      x: 0.0
      y: 0.0
      z: 0.0
      yaw: 0.0
info

You can find detailed explanation of this parameters on official Nav2 documentation

Launch amcl​

To run amcl you need to load map with map_server and run amcl node. All nodes in the Nav2 package are only initialized at startup, you need to use lifecycle_manager to run them. Using lifecycle_manager, Nav2 provides a unified way to manage the lifecycle of navigation-related components, making it easier to deploy and control your navigation system in a consistent and reliable manner.

~/ros2_ws/src/tutorial_pkg/launch/amcl.launch.py
import os

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    use_sim_time = LaunchConfiguration("use_sim_time")
    amcl_params_file = LaunchConfiguration("amcl_params_file")
    map_file = LaunchConfiguration("map_file")

    use_sim_time_arg = DeclareLaunchArgument(
        "use_sim_time", default_value="false", description="Use simulation/Gazebo clock"
    )

    map_file_arg = DeclareLaunchArgument(
        "map_file",
        default_value=os.path.join(get_package_share_directory("tutorial_pkg"), "maps", "map.yaml"),
        description="Full path to the yaml map file",
    )

    amcl_params_file_arg = DeclareLaunchArgument(
        "amcl_params_file",
        default_value=os.path.join(
            get_package_share_directory("tutorial_pkg"), "config", "amcl.yaml"
        ),
        description="Full path to the ROS2 parameters file to use for the amcl node",
    )

    map_server_node = Node(
        package="nav2_map_server",
        executable="map_server",
        name="map_server",
        output="screen",
        parameters=[{"use_sim_time": use_sim_time, "yaml_filename": map_file}],
    )

    amcl_node = Node(
        package="nav2_amcl",
        executable="amcl",
        name="amcl",
        output="screen",
        parameters=[amcl_params_file, {"use_sim_time": use_sim_time}],
    )

    nav_manager = Node(
        package="nav2_lifecycle_manager",
        executable="lifecycle_manager",
        name="nav_manager",
        output="screen",
        parameters=[
            {"use_sim_time": use_sim_time},
            {"autostart": True},
            {"node_names": ["map_server", "amcl"]},
        ],
    )

    rviz_config_path = os.path.join(
        get_package_share_directory("tutorial_pkg"), "rviz", "amcl.rviz"
    )
    rviz2_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="screen",
        arguments=["-d", rviz_config_path],
    )

    ld = LaunchDescription()

    ld.add_action(use_sim_time_arg)
    ld.add_action(amcl_params_file_arg)
    ld.add_action(map_file_arg)
    ld.add_action(map_server_node)
    ld.add_action(amcl_node)
    ld.add_action(nav_manager)
    ld.add_action(rviz2_node)

    return ld

To run amcl example type:

husarion@husarion:~$
ros2 launch tutorial_pkg amcl.launch.py

After starting, RViz should open. If you would like to manually configure RViZ, you should add the following topics: /scan_flitered, /map, /amcl_pose - remember to change the qos policy. Also add RobotModel subscribing to robot_description and ParticleCloud subscribing to particle_cloud and change the QoS policy Topic->Reliability Policy to Best Effort.

In order for amcl to start working, it needs to be given 2D Estimate Pose corresponding to the real position of the robot. You can now control the robot and watch the program run (you can control it with the teleop_twist_keyboard node). You should get a result similar to the image below, where the purple ellipse under the robot is the position uncertainty and the yellow triangle is the orientation uncertainty. Blue arrows represent potential points where the robot may be.

image

Task 1 - Inaccurate Initial Pose

In order to better understand the principle of operation of amcl, it is good to give an inaccurate position of the robot. To do this, you can use the 2D Estimate Pose tool in RViz, and then select a point about 1-2m away from the actual position of the robot (default grid size is 1m). The algorithm will automatically improve the position quality after driving a few meters. Remember to observe the actual robot, not the result in RViz, because the position estimation is now bogus - the robot is in a different position than in RViz.

Result and explanation

Below is a graphic that should help you better understand the issues. Description:

  1. The ROSbot is in a different position than the bet is the computer.
  2. Arrows represent potential points where the robot may be.
  3. The best points are selected and new ones are generated near them.

Summary​

After completing this tutorial, the concept of SLAM should no longer be foreign to you. From now on, you should know how to run rpLidar and know how to create a map, save it, and fix a location using slam_toolbox. Moreover, you should know how to change the slam_toolbox, map_server and amcl parameters. That's all for now and see you in the next tutorial! 😁

by Rafał Górecki, Husarion

Do you need any support with completing this tutorial or have any difficulties with software or hardware? Feel free to describe your thoughts on our community forum: https://community.husarion.com/ or to contact our support: support@husarion.com