5. Mapping & Navigation Course

5.1 Mapping Tutorial

5.1.1 Getting Started with URDF Model

5.1.1.1 URDF Model Introduction

URDF is an XML-based format used to describe the structure of a robot. The purpose of this format is to provide a robot description standard that is as general as possible.

A robot is typically modeled as a structure composed of multiple links and joints. Links can be understood as rigid bodies with mass, and joints can be understood as elements that connect and constrain the relative motion between two links.

When multiple links are connected and constrained through joints, they form a robot motion model. A URDF document describes the relative relationships of these joints and links, along with their inertial properties, geometric features, and collision models.

5.1.1.2 Comparison Between Xacro Model and URDF Model

The URDF model is a relatively simple robot description file. It features a clear structure and is easy to understand. However, when describing more complex robot structures using URDF, the description file can become lengthy and unable to concisely represent the robot model.

The Xacro model is an extended form of the URDF model. They are essentially the same. When using the Xacro format to describe a robot, the description file can be made more concise, enhancing code reusability and alleviating the verbosity problem common in URDF models.

For example, when the legs of a humanoid robot are described, URDF requires each leg to be described individually, whereas Xacro allows a single leg to be described once and reused.

5.1.1.3 Basic URDF Syntax

  • XML Basic Syntax

Since URDF models are written based on the XML standard, it is necessary to understand the basic structure of XML.

  1. Elements:

Elements are defined with assigned names. The following rules apply when these elements are created:

<element>

</element>

  1. Attributes:

Attributes are included within an element to define its properties and parameters. The following format can be used when an element is defined:

<element attribute_1=”value1” attribute_2=”value2”>

</element>

  1. Comments:

Comments do not affect other attributes or elements. They can be defined using the following syntax:

<! – Comment content –>

  • Links

In URDF models, links are represented by the <link> tag. They describe the appearance and physical properties of a rigid part of the robot. When defining a link, the following tags are used:

<visual>: Specifies the visual properties of the robot link, such as size, color, and shape.

<inertial>: Specifies the inertial properties of the link, mainly used in the robot’s dynamics calculations.

<collision>: Specifies the collision properties of the link.

Each tag contains corresponding sub-tags with specific functions, as summarized in the table below:

Tag Description
origin Describes the pose of the link. It contains two parameters: xyz specifies the link’s position in the simulation map, and rpy specifies the link’s orientation in the simulation map.
mess Defines the mass of the link.
inertia Defines the inertia of the link. Due to the symmetry of the inertia matrix, six parameters—ixx, ixy, ixz, iyy, iyz, and izz—must be provided as attributes. These parameters are obtained through calculation.
geometry Describes the shape of the link. It uses the mesh parameter to load texture files and the filename parameter to specify the file path. There are three sub-tags: box, cylinder, and sphere, representing a rectangular box, a cylinder, and a sphere respectively.
material Defines the material of the link. The name parameter is required. The color sub-tag can be used to adjust the color and transparency.

  • Joint

In a URDF model, joints are defined using the joint tag. They describe the kinematic and dynamic properties of the robot’s joints, as well as limits on position and velocity. Based on the type of motion, joints are classified into six types as shown in the table below:

Type and Description Tag
Rotational joint: Allows unlimited rotation around a single axis continuous
Rotational joint: Similar to continuous but with angular limits revolute
Prismatic joint: Moves along a single axis with positional limits prismatic
Planar joint: Allows translation and rotation within a plane planar
Floating joint: allows both translational and rotational movement floating
Fixed joint: A special joint that does not allow any movement fixed

The following tags are used when defining joint actions:

<parent_link>: Defines the parent link.

<child_link>: Defines the child link.

<calibration>: Used to calibrate the joint angle.

<dynamics>: Describes certain physical properties of the motion.

<limit>: Defines the motion limits.

Each tag contains corresponding sub-tags with specific functions, as summarized in the table below:

Tag Description
origin Describes the pose of the parent link. It contains two parameters: xyz specifies the link’s position in the simulation map, and rpy specifies the link’s orientation in the simulation map.
axis Defines the rotation of the child link around one of the X, Y, or Z axes of the parent link.
limit Constrains the child link. The lower and upper attributes define the rotation range in radians, the effort attribute limits the force applied during rotation (positive and negative values in newtons, N), and the velocity attribute limits the rotational speed in meters per second (m/s).
mimic Defines the relationship between this joint and other joints.
safety_controller Specifies the safety controller parameters used to protect the robot’s joint movements.

  • robot tag

The top-level tag for a complete robot. <link> and <joint> tags must be contained within <robot>, formatted as follows:

  • gazebo tag

Used with the Gazebo simulator to set simulation parameters. This tag allows the inclusion of Gazebo plugins, physical property settings, and more.

  • Create a simple URDF model

  1. Set the robot model name

At the beginning of a URDF model, set the robot’s name: <robot name=”robot_model_name”>. At the end of the model, enter </robot> to indicate the completion of the robot model.

  1. Sett a Link

(1) Define the first link. Use indentation to indicate that this link belongs to the current model. Set the link name: <link name=”link_name”>. At the end of the link definition, enter </link> to indicate the link is complete.

(2) Define the link’s visual description. Use indentation to indicate that this description belongs to the current link. Start with <visual> and end with </visual>.

(3) <geometry> describes the shape of the link. Close it with </geometry>. Use indentation to indicate that its content provides the detailed description of the link’s shape. The example below describes a link shape: <cylinder length=”0.01” radius=”0.2”/>, where length=”0.01” indicates the link’s length is 0.01 meters, and radius=”0.2” indicates the link’s radius is 0.2 meters, forming a cylinder.

(4) <origin> describes the position of the link. Use indentation to indicate the detailed description of the link’s position. The example below describes a link’s position: <origin rpy=”0 0 0” xyz=”0 0 0”/>, where rpy defines the link’s angles, and xyz defines the link’s coordinates. This indicates that the link is positioned at the origin of the coordinate system.

(5) <material> describes the link’s appearance. Use indentation to indicate the detailed description of the link’s color. Start with <material> and end with </material>. The example below sets the link color to yellow: <color rgba=”1 1 0 1”/>, where rgba=”1 1 0 1” specifies the color values.

  1. Set a Joint

(1) Define the first joint. Use indentation to indicate that this joint belongs to the current model. Set the joint name and type: <joint name=”joint_name” type=”joint_type”>. At the end of the joint definition, enter </joint> to indicate the joint is complete.

(2) Define the joint’s connected links. Use indentation to indicate that this description belongs to the current joint. Set the parent and child parameters. They can be set as follows: <parent link=”parent_link”/>, <child link=”child_link”/>. When the joint rotates, the parent link serves as the pivot, and the child link rotates accordingly.

(3) <origin> describes the joint’s position. Use indentation to indicate the detailed description of the joint’s position. The example below describes a joint position: <origin xyz=”0 0 0.1”/>, where xyz specifies the joint coordinates, indicating that the joint is located at x=0, y=0, z=0.1 in the coordinate system.

(4) <axis> describes the joint’s orientation. Use indentation to indicate the detailed description of the joint’s pose. The example below describes a joint orientation: <axis xyz=”0 0 1”/>, where xyz specifies the joint’s orientation.

(5) <limit> constrains the joint’s movement. Use indentation to indicate the detailed description of the joint’s angular limits. The example below shows a joint limited to a maximum effort of 300 N, an upper rotation limit of 3.14 radians, and a lower rotation limit of -3.14 radians. It is set as follows: effort=”joint effort (N)”, velocity=”joint speed”, lower=”lower rotation limit (rad)”, upper=”upper rotation limit (rad)”.

(6) <dynamics> describes the joint’s dynamics. Use indentation to indicate the detailed description of the joint’s properties. The example below shows a joint’s dynamic parameters: <dynamics damping=”50” friction=”1”/>, where damping=”damping value” and friction=”friction force”.

The complete code is as follows:

5.1.2 ROS Robot URDF Model

5.1.2.1 Getting Ready

To understand the URDF model, refer to 5.1.1.3 Basic URDF Syntax.

This section provides a brief analysis of the robot model code and component models. This section provides a brief analysis of the robot model code and component models.

5.1.2.2 Access the Robot Model Code

  1. Start the robot and connect it to the remote control software NoMachine. For connection instructions, refer to 1.7.2 AP Mode Connection Steps.

  2. Click the terminal icon in the system desktop to open a new command-line window.

  3. Stop the app auto-start service by entering the following command and press Enter:

    sudo systemctl stop start_app_node.service

  4. Enter the command to navigate to the robot simulation model folder.

    cd ~/ros2_ws/src/simulations/rosorin_description/urdf

  5. Use the cat command to inspect the robot’s description file:

    cat rosorin.xacro

  6. The robot’s relevant descriptions are as follows:

(1) base_link: Base coordinate joint, used to connect various hardware URDF joints on the robot.

(2) lidar_frame: LiDAR joint.

(3) left_front_wheel_link: Left front wheel joint.

(4) left_back_wheel_link: Left rear wheel joint.

(5) right_front_wheel_link: Right front wheel joint.

(6) right_back_wheel_link: Right rear wheel joint.

(7) mic_link: Voice device joint description.

5.1.3 SLAM Map Construction Principle

5.1.3.1 SLAM Introduction

Take humans as an example: before reaching a destination, one needs to know their current position, whether or not a map is available. Humans rely on their eyes, whereas robots rely on LiDAR. SLAM refers to simultaneous localization and mapping.

Localization determines the robot’s pose within a coordinate system. The origin and orientation of the coordinate system can be obtained from the first keyframe, an existing global map or landmarks, or GPS.

Mapping refers to creating a map of the environment perceived by the robot, where the basic geometric elements of the map are points. The primary purpose of the map is for localization and navigation. Navigation can be divided into guiding and moving: guiding includes global and local path planning, while moving refers to controlling the robot’s motion according to the planned path.

5.1.3.2 SLAM Mapping Principle

SLAM mapping mainly involves the following three processes:

  1. Preprocessing: Optimize the raw point cloud data from the LiDAR, remove problematic data, or apply filtering.

Using laser as the signal source, pulses emitted by the LiDAR hit surrounding obstacles, causing scattering.

Part of the reflected light returns to the LiDAR receiver. Using the laser ranging principle, the distance from the LiDAR to the target point can be calculated.

Regarding point clouds: Simply put, the information captured by the LiDAR about the surrounding environment is called the point cloud. It represents the part of the environment that the robot’s “eyes” can see. The captured object information is presented as a series of discrete points with precise angles and distances.

  1. Matching: Match the current local point cloud data to the existing map to find the corresponding position.

Typically, a LiDAR SLAM system compares point clouds captured at different times to calculate the LiDAR’s relative movement and pose change, thereby completing the robot’s localization.

  1. Map fusion: Merge the new data from the LiDAR into the existing map, updating the map continuously.

5.1.3.3 Note on Map Construction

  1. When constructing the map at startup, it is best for the robot to face a straight wall, or use a closed cardboard box instead, so that the LiDAR can capture as many points as possible.

  2. Try to ensure the completeness of the map. For all 360° areas surrounding the robot along its possible paths, the LiDAR needs to scan them to increase map integrity.

  3. When mapping in a large environment, it is recommended that the robot first completes the map’s loop closure, and then scans the smaller details of the environment.

5.1.3.4 Evaluate Map Construction Result

After the map construction is complete, the results can be evaluated using the following items:

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

  2. Whether there are areas in the map that are inconsistent with the real environment, such as missing loop closure.

  3. Whether there are gray areas in the robot operational region, such as unscanned areas.

  4. Whether there are obstacles in the map that will not exist during subsequent localization, such as movable obstacles.

  5. Whether the map ensures that at any position within the robot’s operational area, the full 360-degree view has been scanned.

5.1.4 slam_toolbox Mapping Algorithm

5.1.4.1 Algorithm Concept

The Slam Toolbox package combines information from a LiDAR in the form of LaserScan messages and performs TF transformation from odom to the base link to create a 2D spatial map. This package allows full serialization of reloaded SLAM map data and pose graphs for continuous mapping, localization, merging, or other operations. Slam Toolbox can operate in synchronous mode, processing all valid sensor measurements, regardless of lag, and asynchronous mode, processing valid sensor measurements whenever possible.

ROS replaces functionalities of gapping, cartographer, karto, hector, etc., providing a fully-featured SLAM capability built on the powerful scan matcher at the core of Karto, widely used and optimized for this package. The package also introduces a new optimization plugin based on Google Ceres. Additionally, it provides a new localization method called elastic pose-graph localization, which uses a sliding window of measurements added to the pose graph for optimization and refinement. This approach allows the system to track locally changed features in the environment without treating them as errors and to remove redundant nodes when leaving a region, preserving the integrity of the long-term map.

Slam Toolbox is a set of tools for 2D SLAM. Its main functions are as shown below:

  • Build maps and save map PGM files.

  • Refine maps, rebuild maps, or continue building on saved maps.

  • Long-term mapping: continue building on saved maps while removing irrelevant information from new laser point clouds.

  • Optimize localization mode on existing maps. Run localization mode without mapping using LiDAR odometry mode.

  • Synchronous and asynchronous mapping.

  • Dynamic map merging.

  • Plugin-based optimization solver with a new Google Ceres-based optimization plugin.

  • Interactive RVIZ plugins.

  • Provide RVIZ graphical operation tools to manipulate nodes and connections during mapping.

  • 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 to decouple sparse systems as a solution. The graph optimization method represents the map using the mean of the graph, where each node represents a position point in the robot’s trajectory and a set of sensor measurements. Each new node added triggers a computational update.

The ROS version of Karto_SLAM employs Sparse Pose Adjustment (SPA) related to scan matching and loop closure detection. The more landmarks there are, the higher the memory requirement. However, compared to other methods, the graph optimization approach has greater advantages in large environments because it only includes the robot pose in the graph. After obtaining the poses, the map is generated.

The algorithmic framework of Karto_SLAM is as shown below:

From the figure above, the process is actually quite simple. Following the traditional soft real-time operation mechanism of SLAM, each incoming frame of data is processed immediately and then returned.

  • KartoSLAM related source code and WIKI address:

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

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

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

5.1.4.2 Mapping Steps

  1. Start the robot and connect it to the remote control software NoMachine. For connection instructions, refer to 1.7.2 AP Mode Connection Steps.

  2. Click the terminal icon in the system desktop to open a new command-line window.

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

sudo systemctl stop start_app_node.service

  1. Enter the command to start mapping.

ros2 launch slam slam.launch.py

  1. Right-click the system terminal icon and select New Window to open a new terminal window.

  1. Enter the following command to launch RViz and display the mapping result.

ros2 launch slam rviz_slam.launch.py

  1. Right-click the system terminal icon and select New Window to open a new terminal window.

  1. Next, enter the following command to start the keyboard control node, then press Enter:

ros2 launch peripherals teleop_key_control.launch.py

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

  1. Control the robot to move around the current area to build a more complete map. The table below lists the keyboard keys used to control robot movement and their corresponding functions:

Keyboard Key Robot Action
W Press once to switch to forward mode and continue moving forward
S Press once to switch to reverse mode and continue moving backward
A Press and hold to interrupt forward or reverse movement and turn left
D Press and hold to interrupt forward or reverse movement and turn right

5.1.4.3 Save Map

  1. Right-click the system command-line terminal icon and select New Window to open a new command-line terminal.

  2. 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**

If the prompt shown above appears, the map has been saved successfully.

5.1.4.4 Exit Mapping

  1. Right-click the system command-line terminal icon and select New Window to open another command-line terminal.

  2. Then enter the command and press Enter to execute the program for all ROS nodes in the current system environment. If it does not close successfully, repeat the command as needed.

~/.stop_ros.sh

5.1.4.5 Effect Optimization

To achieve more accurate mapping results, optimize the odometer. The odometer is required for robot mapping, and it depends on the IMU.

The robot is preloaded with calibrated IMU data, which allows it to perform mapping and navigation normally. However, the IMU can still be recalibrated to achieve higher precision. For IMU calibration methods and procedures, refer to section 2.1.1 IMU Calibration of 2. Chassis Motion Control Course.

5.1.4.6 Parameter Description

The parameter file can be found at: ~/ros2_ws/src/slam/config/slam.yaml

For detailed parameter information, refer to the official documentation at: https://wiki.ros.org/slam_toolbox

5.1.4.7 Launch File Analysis

The path of the launch file is:

/home/ubuntu/ros2_ws/src/slam/launch/slam.launch.py

  • Import Libraries:

The launch library can be referenced in the official ROS documentation for detailed explanations:

https://docs.ros.org/en/humble/How-To-Guides/Launching-composable-nodes.html

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

  • Set Path

Use get_package_share_directory to obtain the path of the slam package.

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

  • Launch Other 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': f'{frame_prefix}scan_raw',  # Using scan_raw topic
            'enable_save': enable_save
        }.items(),
    )

    if slam_method == 'slam_toolbox':
        bringup_launch = GroupAction(
            actions=[
                PushRosNamespace(robot_name),
                base_launch,
                TimerAction(
                    period=10.0,
                    actions=[slam_launch],
                ),
            ]
        )

    return [sim_arg, master_name_arg, robot_name_arg, slam_method_arg, bringup_launch]

base_launch: Hardware launch required for the program.

slam_launch: Basic mapping launch.

bringup_launch: Initial pose launch.

5.1.5 RTAB-VSLAM 3D Mapping

5.1.5.1 RTAB-VSLAM Overview

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

5.1.5.2 RTAB-VSLAM Working Principle

RTAB-VSLAM 3D mapping uses a feature-based mapping method. Its advantage lies in the abundance of feature points available in most environments, which provides good scene adaptability and enables relocalization based on feature points. However, it also has several drawbacks. Feature point extraction is time-consuming. Feature points use limited information and lose most image details. The method becomes ineffective in weak-texture areas. Feature matching is also prone to mismatches, which can significantly affect the results.

After extracting image features, features from different time frames are matched to form loop closure detection. Once matching is completed, data are categorized into two types — long-term memory data and short-term memory data. Long-term memory data are used for matching future data, while short-term memory data are used for matching temporally continuous data.

During operation, the RTAB-VSLAM algorithm first uses short-term memory data to update pose estimation and mapping. When future data can be matched with long-term memory data, the corresponding long-term memory data are incorporated into short-term memory to further update pose and mapping.

RTAB-VSLAM package link: https://github.com/introlab/rtabmap

5.1.5.3 3D Mapping Steps

  1. Start the robot and connect it to the remote control software NoMachine. For connection instructions, refer to 1.7.2 AP Mode Connection Steps.

  2. Click the terminal icon in the system desktop to open a ROS2 command-line window. Enter the command to disable the app auto-start service.

sudo systemctl stop start_app_node.service

  1. Entering the following command to start mapping.

ros2 launch slam rtabmap_slam.launch.py

  1. Press Ctrl + Shift + E to open a new command-line terminal, then enter the command to start the RVIZ tool for displaying the mapping process.

ros2 launch slam rviz_rtabmap.launch.py

  1. Press Ctrl + Shift + O to open another command-line terminal, then enter the command to start the keyboard control node and press Enter.

ros2 launch peripherals teleop_key_control.launch.py

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

  1. Control the robot to move within the current space to build a more complete map. Table of keyboard keys and the robot action functions.

Keyboard Key Robot Action
W Short press to switch to forward mode, keep moving forward.
S Short press to switch to backward mode, keep moving backward.
A Long press to interrupt forward or backward movement and turn left.
D Long press to interrupt forward or backward movement and turn right.

When controlling the robot via keyboard for mapping, it is recommended to reduce the robot’s movement speed appropriately. The lower the movement speed, the smaller the odometry error, resulting in better mapping performance. As the robot moves, the map displayed in RVIZ will continue to expand until the entire environment is fully mapped.

5.1.5.4 Save Map

After completing the mapping, press Ctrl + C in each command-line terminal window to stop the currently running program.

Note

For 3D mapping, manual map saving is not required. When the mapping command is stopped using Ctrl + C, the map is automatically saved.

5.1.5.5 Exit Mapping

  1. Right-click the system command-line terminal icon and select New Window to open a new command-line terminal.

  2. Then enter the command and press Enter to execute the program for all ROS nodes in the current system environment. If it does not close successfully, repeat the command as needed.

~/.stop_ros.sh

5.1.5.6 Launch File Analysis

The path of the launch file is:

/home/ubuntu/ros2_ws/src/slam/launch/rtabmap_slam.launch.py

  • Import Libraries:

The launch library can be referenced in the official ROS documentation for detailed explanations:

https://docs.ros.org/en/humble/How-To-Guides/Launching-composable-nodes.html

import os
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import PushRosNamespace
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction, OpaqueFunction, TimerAction

  • Set Path

Use get_package_share_directory to obtain the path of the slam package.

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

  • Launch Other 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,
            'action_name': '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, 
             actions=[rtabmap_launch],
         ),
      ]
    )

base_launch: Hardware launch required for the program.

slam_launch: Basic mapping launch.

rtabmap_launch rtab: Mapping launch.

bringup_launch: Initial pose launch.