5. Mapping and Navigation Course

5.1 Preparation

Jetson Nano has limited processing performance. If the robot system becomes severely laggy when RViz is opened, RViz can be run in a virtual machine to assist with mapping and navigation.

5.1.1 Installing and Importing the Virtual Machine

Refer to 10. Movelt & Gazebo Simulation/10.1 Virtual Machine Installation and Import to install the virtual machine and import the image.

5.2.2 Copying Files to Virtual Machine

5.2.2.1 Exporting Files from the Robot

  1. Power on the robot and connect it to the NoMachine remote desktop software.

  2. Click the icon on the system desktop to open the ROS2 command-line terminal.

  3. Then enter the following command to go to the ros2_ws/src/ directory:

    cd ros2_ws/src/

  4. Enter the following command to package the navigation and slam folders into a compressed file:

    zip -r src.zip navigation slam

  5. Enter the following command to move the compressed file to the shared directory:

    mv src.zip /home/ubuntu/share/tmp

  6. Enter the following command to return to the ros2_ws directory:

    cd ..

  7. Enter the following command to view the .typerc file in this directory:

    ls -a

  8. Enter the following command to copy the .typerc file to the shared directory:

    cp .typerc /home/ubuntu/share/tmp

  9. Click the icon on the system desktop to open the file manager, then go to the home/docker/tmp directory.

  1. Press Ctrl + H to show the hidden .typerc file.

  1. Drag the two files in this directory to the computer desktop.

5.2.2.2 Importing Files into Virtual Machine

  1. Click the icon on the system desktop. The home directory opens by default.

  1. Drag the .typerc and src.zip files from the computer desktop into the virtual machine.

5.2.2.3 Creating and Compiling the Workspace

  1. Click the icon on the system desktop to open the virtual machine command-line terminal.

  2. Enter the following command to create the ros2_ws/src directory:

    mkdir -p ros2_ws/src

  3. Extract src.zip to the current /home/ubuntu directory:

    unzip src.zip

  4. Move the slam and navigation folders to the ros2_ws/src directory:

    mv slam navigation /home/ubuntu/ros2_ws/src/

  5. Move the .typerc file to the ros2_ws directory:

    mv .typerc ros2_ws/

  6. Navigate to the ros2_ws directory:

    cd ros2_ws/

  7. Enter the following command to compile the workspace:

    colcon build

  8. Modify the .bashrc file with the following command:

    gedit ~/.bashrc

    Copy the following content into the .bashrc file:

    source /home/ubuntu/ros2_ws/.typerc
source /home/ubuntu/ros2_ws/install/setup.bash

  9. After editing is complete, press Ctrl + S or click the Save button in the upper-right corner to save and exit.

  1. Enter the following command to refresh the environment configuration:

source ~/.bashrc

5.2.2.4 Setting the Robot to LAN Mode

  1. Click the icon on the system desktop to open the ROS2 command-line terminal.

  2. Enter the following command to modify the Wi-Fi configuration file, switch the robot to LAN mode, and update the Wi-Fi name and password:

    cd wifi_manager && gedit wifi_conf.py

  3. Change the configuration to LAN mode, then enter the Wi-Fi name and password. A Wi-Fi router or mobile hotspot is required for later connection.

  1. Press Ctrl + S to save the file and close it.

  2. For later connection, make sure the device and the virtual machine are on the same subnet. Enter the following command in the terminal to check:

ifconfig

Here, the virtual machine and the robot are both on the 192.168.11 subnet, so communication works properly.

  • Common connection methods:

  1. Router connection: Connect the computer and the Jetson controller board to the same router with Ethernet cables. Recommended

  2. LAN connection: Configure STA LAN mode as described in the tutorial, then connect the robot and the computer to the same Wi-Fi network or mobile hotspot. Recommended

  3. Direct connection: Set the robot to AP mode, then connect the computer to the robot’s Wi-Fi network. Not recommended

5.2 Mapping Tutorial

5.2.1 Principles of SLAM Map Construction

  • Introduction to SLAM

Taking human navigation as an example, before heading to a destination, the current location must be known, regardless of whether a map is available. While humans use their eyes, robots achieve this through LiDAR. SLAM, Simultaneous Localization and Mapping, refers to real-time positioning and map building.

Localization is the determination of the robot’s pose within a coordinate system. The origin and posture of the coordinate system can be obtained from the first keyframe, an existing global map, landmark points, or GPS.

Mapping refers to creating a map of the surrounding environment perceived by the robot, with points serving as the basic geometric elements of the map. The primary functions of the map are localization and navigation. Navigation can be divided into path planning and movement execution. Planning includes global and local planning, while execution involves controlling the robot’s movement after planning is complete.

  • Principles of SLAM Mapping

SLAM mapping mainly involves the following three processes:

  1. Preprocessing: Optimizing the raw point cloud data from the LiDAR, removing problematic data, or applying filtering.

Using a laser as the signal source, pulsed lasers emitted by the laser device hit surrounding obstacles, causing scattering.

A portion of the light waves will reflect back to the LiDAR receiver, and based on the principles of laser distance measurement, the distance from the LiDAR to the target point can be calculated.

Regarding point clouds: Generally speaking, the environmental information acquired by LiDAR is called a point cloud. It reflects a portion of what the robot’s “eyes” can see in its environment. The collected object information is presented as a series of scattered data points with accurate angles and distances.

  1. Matching: Finding the corresponding position of the current local environment’s point cloud data on the established map to perform matching.

Typically, laser SLAM systems calculate the change in relative movement distance and posture of the LiDAR by matching and comparing two point clouds from different times, thereby completing the robot’s self-localization.

  1. Map Fusion: Splicing the new round of data from the LiDAR into the original map, ultimately completing the map update.

  • Precautions for Map Construction

  1. When initiating the mapping process, it is recommended that the robot faces a straight wall, or a closed cardboard box is used as a substitute, allowing the LiDAR to scan as many points as possible.

  2. Ensure the completeness of the map as much as possible. In the paths the robot might traverse, all surrounding 360° areas need to be explored by the LiDAR to increase the map’s completeness.

  3. When mapping in large environments, it is optimal to allow the robot to complete a mapping loop closure first, before proceeding to scan the minor details of the environment thoroughly.

  • Evaluating Map Construction Results

Finally, after the map construction is completed, the following criteria can be used to determine whether the results meet navigation requirements:

  1. Whether the edges of obstacles in the map are distinct.

  2. Whether there are areas in the map that are inconsistent with the actual environment, e.g., presence or absence of loop closures.

  3. Whether there are gray areas within the robot’s action area on the map, e.g., unscanned areas.

  4. Whether there are obstacles in the map that will not exist during subsequent localization, e.g., dynamic obstacles.

  5. Whether it can be guaranteed that any position within the robot’s active area has been fully explored within a 360-degree field of view.

5.2.2 slam_toolbox Mapping Algorithm

  • Algorithm Concepts

The Slam Toolbox software package combines information from the laser rangefinder based on the format of LaserScan messages, and performs TF transformation from the odom-> base link, thereby creating a two-dimensional spatial map. This package allows the data and pose graph of the fully serialized, reloaded SLAM map to be used for continuous mapping, localization, merging, or other operations. It allows the SLAM Toolbox to run in both synchronous and asynchronous modes. The synchronous mode processes all valid sensor measurements regardless of lag, while the asynchronous mode processes them whenever possible.

In ROS2, it replaces packages like gmapping, cartographer, karto, and hector, offering fully featured SLAM. This functionality is built upon the powerful scan matcher of the Karto core, which has been extensively utilized and accelerated for this package. A new optimization plugin based on Google Ceres has also been introduced. Additionally, a new localization method called elastic pose-graph localization is introduced, which takes a sliding window of measurements and adds it to the graph for optimization and refinement. This allows for the tracking of changed local features in the environment rather than treating them as deviations. When leaving a certain area without affecting the long-term map, these redundant nodes are deleted.

Slam Toolbox is a suite of tools for 2D SLAM. Main features include:

  1. Mapping and saving map pgm files.

  2. Refining maps, re-mapping, or continuing to map on saved maps.

  3. Long-term mapping: Loading saved maps to continue mapping while deleting irrelevant information from new LiDAR point clouds.

  4. Optimizing localization mode on existing maps. It is also possible to use the LiDAR Odometry mode to run localization mode without mapping.

  5. Synchronous and asynchronous mapping.

  6. Dynamic map merging.

  7. Plugin-based optimization solvers, featuring a new optimization plugin based on Google Ceres.

  8. Interactive RVIZ plugin.

  9. Providing RVIZ graphical manipulation tools for operating nodes and connections during mapping.

  10. Map serialization and lossless data storage.

  • KARTO

Karto_SLAM is based on the concept of graph optimization, using highly optimized and non-iterative Cholesky decomposition for sparse system decoupling as the solution. Graph optimization methods use the mean of the graph to represent the map. Each node represents a position point of the robot’s trajectory and the sensor measurement data set. With the addition of each new node, calculations and updates are performed.

In the ROS version of Karto_SLAM, the Sparse Pose Adjustment (SPA) adopted is related to scan matching and loop closure detection. The more landmarks there are, the greater the memory requirement. However, compared to other methods, the graph optimization approach has a greater advantage in mapping large environments because it only contains a graph of points, which represent the robot’s pose, solving for the pose before solving for the map.

The algorithmic program framework of Karto SLAM is shown in the figure below:

As can be seen from the above figure, the process is quite straightforward. In the traditional soft real-time operating mechanism of SLAM, processing is performed every time a frame of data enters, and then it returns.

  • Source Code and WIKI Addresses related to KartoSLAM:

  1. KartoSLAM ROS Wiki: http://wiki.ros.org/slam_karto

  2. slam_karto package: https://github.com/ros-perception/slam_karto

  3. open_karto open-source algorithm: https://github.com/ros-perception/open_karto

  • SLAM Mapping Steps

  1. Click the icon on the system desktop to open the ROS2 command line terminal.

  2. Enter the command to disable the app auto-start service.

~/.stop_ros.sh

  1. Enter the command to start mapping.

ros2 launch slam slam.launch.py

  1. In the virtual machine, click the icon on the system desktop to open the command-line terminal, then enter the command to launch RViz.

ros2 launch slam rviz_slam.launch.py

  1. A wireless controller or keyboard can be used to control the movement of the robot. If keyboard control is selected, open a new command-line terminal on the robot, enter the command to start the keyboard control node, and press Enter:

ros2 launch peripherals teleop_key_control.launch.py

If the prompt shown in the figure below appears, the keyboard control service has started successfully.

  1. Control the robot to move around the current space to construct a more complete map. The table below lists the keyboard keys available for manipulating the robot’s movement and their corresponding functions:

Keyboard Key Robot Action
W Short press, switch to forward state
S Short press, switch to backward state
A Short press, turn left
D Short press, turn right
When controlling the robot's movement for mapping via the keyboard, the robot's moving speed can be appropriately reduced. The lower the robot's running speed, the smaller the relative error of the odometry, resulting in better mapping effects. As the robot moves, the map displayed in RVIZ will continuously increase until the map construction of the entire environmental scene is complete.

  • Saving the Map

Open a new command-line terminal on the robot, enter the command to save the map, and press Enter:

cd ~/ros2_ws/src/slam/maps && ros2 run nav2_map_server map_saver_cli -f "map_01" --ros-args -p map_subscribe_transient_local:=true

  • Effect Optimization

If a more precise mapping effect is desired, the odometry can be optimized. The robot requires the use of odometry during mapping, and odometry relies on the IMU.

The robot itself comes with loaded calibrated IMU data. This data enables the robot to successfully complete mapping and navigation functions normally, but the IMU can still be calibrated to achieve higher precision.

  • Parameter Description

Parameter files can be viewed under the path ~/ros2_ws/src/slam/config/slam.yaml.

For a detailed explanation of the many parameters, please refer to the official introduction: https://wiki.ros.org/slam_toolbox.

  • Launch File Analysis

This launch file is located at: /home/ubuntu/ros2_ws/src/slam/launch/slam.launch.py

  1. Importing libraries

import os
from ament_index_python.packages import get_package_share_directory

from launch_ros.actions import PushRosNamespace
from launch import LaunchDescription, LaunchService
from launch.substitutions import LaunchConfiguration
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction, OpaqueFunction, TimerAction

Detailed explanations for the launch library can be found in the official ROS documentation: https://docs.ros.org/en/humble/How-To-Guides/Launching-composable-nodes.html

  1. Setting paths

if compiled == 'True':
    slam_package_path = get_package_share_directory('slam')
else:
    slam_package_path = '/home/ubuntu/ros2_ws/src/slam'

Use get_package_share_directory to obtain the path of the slam feature package.

  1. Starting other launch files

base_launch = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(
        os.path.join(slam_package_path, 'launch/include/robot.launch.py')),
    launch_arguments={
        'sim': sim,
        'master_name': master_name,
        'robot_name': robot_name,
    }.items(),
)

slam_launch = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(
        os.path.join(slam_package_path, 'launch/include/slam_base.launch.py')),
    launch_arguments={
        'use_sim_time': use_sim_time,
        'map_frame': map_frame,
        'odom_frame': odom_frame,
        'base_frame': base_frame,
        'scan_topic': '{}/scan'.format(frame_prefix),
        'enable_save': enable_save
    }.items(),
)

if slam_method == 'slam_toolbox':
    bringup_launch = GroupAction(
     actions=[
         PushRosNamespace(robot_name),
         base_launch,
         TimerAction(
             period=5.0,  # Delay to wait for other nodes to start up (delay to enable other nodes)
             actions=[slam_launch],
         ),
      ]
    )

base_launch is the hardware launch file required to start the program, slam_launch is the mapping launch file, and bringup_launch is the initial pose launch file.

5.2.3 RTAB-VSLAM 3D Mapping

  • Introduction to RTAB-VSLAM

RTAB-VSLAM is an appearance-based, real-time 3D mapping approach, which is an open-source library that achieves loop closure detection through memory management methods. By limiting the size of the map, it ensures that loop closure detection is always processed within a fixed time limit, thereby meeting the requirements for long-term and large-scale online environmental mapping.

  • Principles of RTAB-VSLAM

RTAB-VSLAM 3D mapping utilizes feature-based mapping. The advantage is that general scenes can provide abundant feature points, offering good scene adaptability and allowing the use of feature points for relocalization. There are several disadvantages to this approach. First, feature point calculation algorithms are time-consuming. Second, the information utilized by feature points is too sparse, losing most of the information in the image. Third, the feature point method becomes ineffective for textureless areas. Finally, feature point matching is prone to mismatches, which significantly impacts the results.

Once the features in the images are obtained, features from different times need to be matched to form a loop closure detection. After matching is completed, the data is divided into two categories: Long-Term Memory (LTM) data and Short-Term Memory (STM) data. The LTM data is used for matching future data, while the STM data is used for matching continuously timed data.

When the RTAB-VSLAM algorithm runs, it first uses STM data to update the localization points and map. When data at a future moment successfully matches the LTM data, the corresponding LTM data is added to the STM data to update the localization points and map.

RTAB-VSLAM Software Package Link: https://github.com/introlab/rtabmap.

  • RTAB-VSLAM 3D Mapping Steps

  1. Click the icon on the system desktop to open the ROS2 command line terminal. Enter the command to disable the app auto-start service:

~/.stop_ros.sh

  1. Enter the command to start mapping:

ros2 launch slam rtabmap_slam.launch.py

  1. In the virtual machine, click the icon on the system desktop to open the command-line terminal, then enter the command to launch RViz.

ros2 launch slam rviz_rtabmap.launch.py

  1. A wireless controller or keyboard can be used to control the movement of the robot. If keyboard control is selected, open a new command-line terminal on the robot, enter the command to start the keyboard control node, and press Enter:

ros2 launch peripherals teleop_key_control.launch.py

If the prompt shown in the figure below appears, the keyboard control service has started successfully.

  1. Control the robot to move around the current space to construct a more complete map. The table below lists the keyboard keys available for manipulating the robot’s movement and their corresponding functions:

Keyboard Key Robot Action
W Short press, switch to forward state
S Short press, switch to backward state
A Short press, turn left
D Short press, turn right
When controlling the robot's movement for mapping via the keyboard, the robot's moving speed can be appropriately reduced. The lower the robot's running speed, the smaller the relative error of the odometry, resulting in better mapping effects. As the robot moves, the map displayed in RVIZ will continuously increase until the map construction of the entire environmental scene is complete.

  • Robot Map Saving

After mapping is completed, the shortcut Ctrl + C can be used in the respective command-line terminal windows to close the currently running program.

Note

Manual saving is not required for 3D mapping. The map is saved automatically when the mapping process is terminated using Ctrl+C.

  • Launch File Analysis

This launch file is located at: /home/ubuntu/ros2_ws/src/slam/launch/rtabmap_slam.launch.py

  1. Importing libraries

import os
from ament_index_python.packages import get_package_share_directory

from launch_ros.actions import PushRosNamespace
from launch import LaunchDescription, LaunchService
from launch.substitutions import LaunchConfiguration
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction, OpaqueFunction, TimerAction

Detailed explanations for the launch library can be found in the official ROS documentation: https://docs.ros.org/en/humble/How-To-Guides/Launching-composable-nodes.html

  1. Setting paths

if compiled == 'True':
    slam_package_path = get_package_share_directory('slam')
else:
    slam_package_path = '/home/ubuntu/ros2_ws/src/slam'

Use get_package_share_directory to obtain the path of the slam feature package.

  1. Starting other launch files

base_launch = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(
        os.path.join(slam_package_path, 'launch/include/robot.launch.py')),
    launch_arguments={
        'sim': sim,
        'master_name': master_name,
        'robot_name': robot_name,
        'rtabmap_name': rtabmap_name,
        'action_name': 'init_horizontal',

    }.items(),
)

rtabmap_launch = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(
        os.path.join(slam_package_path, 'launch/include/rtabmap.launch.py')),
    launch_arguments={
        'use_sim_time': use_sim_time,
    }.items(),
)

bringup_launch = GroupAction(
 actions=[
     PushRosNamespace(robot_name),
     base_launch,
     TimerAction(
         period=10.0,  # Delay to wait for other nodes to start up (delay to enable other nodes)
         actions=[rtabmap_launch],
     ),
  ]
)

base_launch is the hardware launch file required to start the program, rtabmap_launch is the rtab mapping launch file, and bringup_launch is the initial pose launch file.