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:
- ROSbot
- Gazebo simulator
Running the ROSbot is very easy thanks to the created docker services.
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 commanddocker compose config --services
, or find suitable Docker images prepared by Husarion.
Running the simulator is very easy thanks to the created aliases. If you have not already done so, configure your environment for simulations.
ROSBOT_SIM
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
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 bebase_link
,odom_frame
- name of frame related with starting point, in our case it will beodom
,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 to0.04
,max_laser_range
- the maximum usable range of the laser, in our case it will be12.0
metersmode
- 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.
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
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.
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
:
install(DIRECTORY
config
launch
img_data
maps
rviz
DESTINATION share/${PROJECT_NAME})
To run SLAM example build package and type:
- ROSbot
- Gazebo simulator
ros2 launch tutorial_pkg slam.launch.py
ros2 launch tutorial_pkg slam.launch.py use_sim_time:=true
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:
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:
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.
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:
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:
- ROSbot
- Gazebo simulator
ros2 run nav2_map_server map_server --ros-args -p yaml_filename:=map.yaml
ros2 run nav2_map_server map_server --ros-args -p yaml_filename:=map.yaml -p use_sim_time:=true
To start map_server
you need to run one extra node from nav2
.
ros2 run nav2_util lifecycle_bringup map_server
Test result in RViz.
map_server
node publish only one msg with map. To display map you have two option:
- Run RViZ before running
map_server
- Change Map QoS Policy in RViz. Open Map setting and change Topic->Durability Policy to Transient Local.
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 odometrybase_frame_id
- name of frame related with robot, in our case it will bebase_link
,global_frame_id
- name of frame related with map starting point, in our case it will bemap
,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 to1000
,min_particles
- minimum allowed number of particles, in our case it will be set to100
,laser_max_range
- the maximum usable range of the laser, in our case it will be12.0
meters
Again, it is recommended to put all parameters in one amcl.yaml
file.
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
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.
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:
- ROSbot
- Gazebo simulator
ros2 launch tutorial_pkg amcl.launch.py
ros2 launch tutorial_pkg amcl.launch.py use_sim_time:=true
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.
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:
- The ROSbot is in a different position than the bet is the computer.
- Arrows represent potential points where the robot may be.
- 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