5. ROS2-Mapping & Navigation Courses

5.1 Mapping

5.1.1 URDF Model

  • Introduction to URDF Model

The Unified Robot Description Format (URDF) is an XML file format widely used in ROS (Robot Operating System) to comprehensively describe all components of a robot.

Robots are typically composed of multiple links and joints. A link is defined as a rigid object with certain physical properties, while a joint connects two links and constrains their relative motion.

By connecting links with joints and imposing motion restrictions, a kinematic model is formed. The URDF file specifies the relationships between joints and links, their inertial properties, geometric characteristics, and collision models.

  • Comparison between Xacro and URDF Model

The URDF model serves as a description file for simple robot models, offering a clear and easily understandable structure. However, when it comes to describing complex robot structures, using URDF alone can result in lengthy and unclear descriptions.

To address this limitation, the xacro model extends the capabilities of URDF while maintaining its core features. The xacro format provides a more advanced approach to describe robot structures. It greatly improves code reusability and helps avoid excessive description length.

For instance, when describing the two legs of a humanoid robot, the URDF model would require separate descriptions for each leg. On the other hand, the xacro model allows for describing a single leg and reusing that description for the other leg, resulting in a more concise and efficient representation.

  • Basic Syntax of URDF Model

(1) XML Basic Syntax

The URDF model is written using XML standard.

Elements:

An element can be defined as desired using the following formula:

<element>

</element>

Properties:

Properties are included within elements to define characteristics and parameters. Please refer to the following formula to define an element with properties:

<element

property_1="property value1"

property_2="property value2">

</element>

Comments:

Comments have no impact on the definition of other properties and elements. Please use the following formula to define a comment:

<!– comment content –>

(2) Link

The Link element describes the visual and physical properties of the robot’s rigid component. The following tags are commonly used to define the motion of a link:

<visual>: Describe the appearance of the link, such as size, color and shape.

<inertial>: Describe the inertia parameters of the link, which will used in dynamics calculation.

<collision>: Describe the collision inertia property of the link

Each tag contains the corresponding child tag. The functions of the tags are listed below.

Tag Function
origin Describe the pose of the link. It contains two parameters, including xyz and rpy. Xyz describes the pose of the link in the simulated map. Rpy describes the pose of the link in the simulated map.
mess Describe the mess of the link
inertia Describe the inertia of the link. As the inertia matrix is symmetrical, these six parameters need to be input, ixx, ixy, ixz, iyy, iyz and izz, as properties. These parameters can be calculated.
geometry Describe the shape of the link. It uses mesh parameter to load texture file, and em[ploys filename parameters to load the path for texture file. It has three child tags, namely box, cylinder and sphere.
material Describe the material of the link. The parameter name is the required filed. The tag color can be used to change the color and transparency of the link.

(3) Joint

The “Joint” tag describes the kinematic and dynamic properties of the robot’s joints, including the joint’s range of motion, target positions, and speed limitations. In terms of motion style, joints can be categorized into six types.

The following tags will be used to write joint motion.

<parent_link>: Parent link

<child_link>: Child link

<calibration>: Calibrate the joint angle

<dynamics>: Describes some physical properties of motion

<limit>: Describes some limitations of the motion

The function of each tag is listed below. Each tag involves one or several child tags.

Tag Function
origin Describe the pose of the parent link. It involves two parameters, including xyz and rpy. Both xyz and rpy describe the pose of the link in simulated map.
axis Control the child link to rotate around any axis of the parent link.
limit The motion of the child link is constrained using the lower and upper properties, which define the limits of rotation for the child link. The effort properties restrict the allowable force range applied during rotation (values: positive and negative; units: N). The velocity properties confine the rotational speed, measured in meters per second (m/s).
mimic Describe the relationship between joints.
safety_controller Describes the parameters of the safety controller used for protecting the joint motion of the robot.

(4) Robot Tag

The complete top tags of a robot, including the <link> and <joint> tags, must be enclosed within the <robot> tag. The format is as follows:

(5) gazebo Tag

This tag is used in conjunction with the Gazebo simulator. Within this tag, you can define simulation parameters and import Gazebo plugins, as well as specify Gazebo’s physical properties, and more.

(6) Write Simple URDF Model

Name the model of the robot

To start writing the URDF model, we need to set the name of the robot following this format: “<robot name=’robot model name’>”. Lastly, input “</robot>” at the end to represent that the model is written successfully.

(7) Set links

① To write the first link and use indentation to indicate that it is part of the currently set model. Set the name of the link using the following format: <link name=”link name”>. Finally, conclude with “</link>” to indicate the successful completion of the link definition.

② Write the link description and use indentation to indicate that it is part of the currently set link, and conclude with “</visual>”.

③ The “<geometry>” tag is employed to define the shape of a link. Once the description is complete, include “</geometry>”. Within the “<geometry>” tag, indentation is used to specify the detailed description of the link’s shape. The following example demonstrates a link with a cylindrical shape: “<cylinder length=’0.01’ radius=’0.2’/>”. In this instance, “length=’0.01’” signifies a length of 0.01 meters for the link, while “radius=’0.2’” denotes a radius of 0.2 meters, resulting in a cylindrical shape.

④ The “<origin>” tag is utilized to specify the position of a link, with indentation used to indicate the detailed description of the link’s position. The following example demonstrates the position of a link: “<origin rpy=’0 0 0’ xyz=’0 0 0’ />”. In this example, “rpy” represents the roll, pitch, and yaw angles of the link, while “xyz” represents the coordinates of the link’s position. This particular example indicates that the link is positioned at the origin of the coordinate system.

⑤ The “<material>” tag is used to define the visual appearance of a link, with indentation used to specify the detailed description of the link’s color. To start describing the color, include “<material>”, and end with “</material>” when the description is complete. The following example demonstrates setting a link color to yellow: “<color rgba=’1 1 0 1’ />”. In this example, “rgba=’1 1 0 1’” represents the color threshold for achieving a yellow color.

(8) Set joint

① To write the first joint, use indentation to indicate that the joint belongs to the current model being set. Then, specify the name and type of the joint as follows: “<joint name=”joint name” type=”joint type”>”. Finally, include “</joint>” to indicate the completion of the joint definition.

Note

To learn about the type of the joint, please refer to “joint”.

② Write the description section for the connection between the link and the joint. Use indentation to indicate that it is part of the currently defined joint. The parent parameter and child parameter should be set using the following format: “<parent link=’parent link’/>”, and “<child link=’child link’ />”. With the parent link serving as the pivot, the joint rotates the child link.

③ “<origin>” describes the position of the joint using indention. This example describes the position of the joint: “<origin xyz=’0 0 0.1’/>”. xyz is the coordinate of the joint.

④ “<axis>” describes the position of the joint adopting indention. “<axis xyz=’0 0 1’ />” describes one posture of a joint. xyz specifies the pose of the joint.

⑤ “<limit>” imposes restrictions on the joint using indention. The below picture The “<limit>” tag is used to restrict the motion of a joint, with indentation indicating the specific description of the joint angle limitations. The following example describes a joint with a maximum force limit of 300 Newtons, an upper limit of 3.14 radians, and a lower limit of -3.14 radians. The settings are defined as follows: “effort=’joint force (N)’, velocity=’joint motion speed’, lower=’lower limit in radians’, upper=’upper limit in radians’”.

⑥ “<dynamics>” describes the dynamics of the joint using indention. “<dynamics damping=’50’ friction=’1’ />” describes dynamics parameters of a joint.

The complete codes are as below.

5.1.2 Explanation of ROS Robot URDF Model

  • Preparation

To grasp the URDF model, check out “Basic Syntax of URDF Model” for the key syntax. This part quickly breaks down the robot model code and its components.

  • Check Code of Robot Model

(1) Start the robot, and access the robot system desktop using NoMachine.

(2) Click-on to open the command-line terminal.

(3) Run the following command to enable the app auto-start service, and hit Enter key.

sudo systemctl stop start_app_node.service

(4) Enter the command and press Enter to access the startup program directory.

colcon_cd Jetauto_description

(5) Enter the command to navigate to the robot simulation model folder.

cd urdf

(6) Execute the command to access the robot simulation model folder.

vim jetauto.xacro

(7) Locate the code below:

Several URDF models are combined to create a full robot:

File Name Device
materials Color
inertial_matrix Inertia Matrix
lidar_a1 A1 Lidar
lidar_g4 G4 Lidar
jetauto_arm Exclusive to JetAutoPro Robot
jetauto_car JetAuto Robot
Imu Inertial Measurement Unit
depth_camera Depth Camera
usb_camera USB Camera
common Common Components or Attributes
connect Connectors, detailing the physical connections between robot components
gripper Gripper Assembly
arm.transmission Robotic Arm Transmission Structure
gripper.transmission Gripper Transmission Structure

  • Brief Analysis of Robot’s Main Body Model

Open a new command prompt and enter the following command to open the robot model file, which contains descriptions of each part of the robot:

vim jetauto_car.urdf.xacro

This is the start of the URDF file. It specifies the XML version and encoding while defining a robot model named jetauto. The xmlns:xacro namespace is also included to facilitate the generation of URDF using Xacro macros. This line of code defines a Xacro attribute called M_PI and assigns it the value of the mathematical constant π.

In this section, a link named base_footprint is defined as the robot’s chassis.

Various characteristics of the robot such as mass, width, height, and depth are specified.

Next, a joint called base_joint is defined with a type of “fixed”, indicating it’s a stationary joint. It connects the parent link base_footprint with the child link base_link.

The joint’s position (origin) is determined using an xyz attribute.

<link name="base_footprint"/>

<joint name="base_joint" type="fixed">

 <parent link="base_footprint"/>

 <child link="base_link"/>

 <origin xyz="0.0 0.0 0.005" rpy="0 0 0"/>

</joint>

The following code is an XML snippet that defines a link in a robot model. Let’s break down and analyze its structure and purpose.

The code begins with the <link> tag, which defines a link within the robot model. This link is named base_green_link. Inside the <link> tag, there are three sections: <inertial>, <visual>, and <collision>.

The <inertial> section defines the link’s inertial properties, such as mass and inertia. It contains an <origin> tag that specifies the position and orientation of the inertial frame relative to the link frame. The <mass> tag specifies the link’s mass, while the <inertia> tag defines the inertia matrix around the link’s principal axes.

The <visual> section defines the visual representation of the link. It also contains an <origin> tag that specifies the position and orientation of the visual frame relative to the link frame. The <geometry> tag defines the shape of the visual representation, which, in this case, is a mesh. The <mesh> tag specifies the filename of the mesh file that represents the visual appearance of the link. Lastly, the <material> tag defines the color or texture of the visual representation, here represented by a material called ‘green.’

The <collision> section defines the collision properties of the link. It is similar to the <visual> section but is used for collision detection rather than visualization. It includes an <origin> tag and a <geometry> tag that define the position, orientation, and shape of the collision representation.

Overall, this code snippet defines a link in the robot model, including its inertial properties, visual representation, and collision attributes. In simulation or visualization environments, the mesh files specified in the <visual> and <collision> sections are used for visual representation and collision detection with the link.

The following code describes a joint named base_green_joint of type fixed, which means it is a fixed joint. The joint’s parent link is named base_link, and its child link is named base_green_joint. The joint’s coordinate origin is at (0, 0, 0), the Euler angles (rpy) are (0, 0, 0), and the joint has no defined axis.

Next, let’s take a look at the description of the link:

The above code describes a link named wheel_right_front_link, which contains information about its inertia (inertial), visual representation (visual), and collision shape (collision).

The inertial part describes the link’s mass and inertia matrix. The mass is 0.124188560741815, and specific values are provided for the inertia matrix components.

The visual part details the link’s appearance, defined by a three-dimensional model (mesh) with the filename package://jetauto_description/meshes/wheel_left_front_link.stl. The link uses a material named black.

The collision part specifies the link’s collision shape, also defined by a three-dimensional model (mesh) with the same filename as the visual part.

The following code snippet is an XML fragment used to define a joint and a link in a robot model. The joint definition includes:

Name: wheel_left_back_joint

Type: fixed (indicating a fixed joint that does not allow movement)

Origin: Specifies the position and orientation of the joint relative to the parent link (”base_link”).

Parent Link: Specifies the link to which the joint is attached.

Child Link: Specifies the link that the joint connects to.

Axis: Specifies the rotation axis of the joint. In this case, the axis is set to (0, 0, 0), indicating that it is a fixed joint with no rotation capability.

The link definition includes:

Name: wheel_left_back_link

Inertia: Specifies the inertial properties of the link, including mass, center of mass, and moment of inertia.

Visual: Specifies the visual representation of the link, including its position, orientation, geometry (mesh), and material.

Collision: Specifies the collision properties of the link, including its position, orientation, and geometry (mesh).

Another link definition is provided with the name

Name: wheel_right_front_link

Inertia: Specifies the inertial properties of the link, including mass and inertia.

Visualization: Specifies the visual representation of the link, including its position, orientation, geometry (mesh), and material.

Collision: Specifies the collision properties of the link, including its position, orientation, and geometry (mesh).

Detailed Explanation:

Inertial Properties (inertial): Defines the mass and inertia matrix of the link. In this example, the mass of the link is 0.124186629923608, and the individual components of the inertia matrix (ixx, ixy, ixz, iyy, iyz, izz) are also specified with precise values.

Visual Properties (visual): Defines the visual representation of the link. Here, the link’s visualization uses a mesh with the file name package://jetauto_description/meshes/wheel_right_front_link.stl. Additionally, a material named black is applied for visualization.

Collision Properties (collision): Defines the collision properties of the link. In this case, the collision properties are identical to the visual properties, using the same mesh.

The following code defines a fixed joint that connects two links: base_link and wheel_right_front_link. The joint’s initial position and rotation are specified using the <origin> tag.

name=wheel_right_front_joint: This attribute sets the joint’s name to wheel_right_front_joint.

type=fixed: This attribute indicates that the joint is of type fixed, meaning it is stationary and does not allow movement.

<origin> tag: This tag specifies the joint’s initial position and rotation. The joint’s xyz coordinates are set to -0.10658 0.091851 -0.065487, and the rpy (roll, pitch, yaw) rotation angles are set to 0 0 3.1416.

<parent> tag: This tag designates base_link as the parent link of the joint, meaning the joint is connected to base_link.

<child>tag:This tag designates wheel_right_front_link as the child link of the joint, meaning the joint is connected to wheel_right_front_link.

<axis> tag: This tag defines the joint’s axis. In this case, the axis is set to (0, 0, 0), indicating that the joint has no specific axis of rotation.

The following describes a link named "wheel_right_back_link", including its inertial, visual, and collision properties. The link’s geometry is defined using a mesh file, and its visual properties also specify a black material.

name="wheel_right_back_link": This attribute sets the name of the link to "wheel_right_back_link".

<inertia> tag: Defines the inertial properties of the link, including its mass and inertia matrix.

<origin> tag: Specifies the position and rotation of the link’s inertia origin. Specifically, the xyz coordinates are 0.21333 0.20224 0.00032742, and the rpy (roll, pitch, yaw) rotation angles are “0 0 0”.

<mass> tag: Defines the mass of the link, which is "0.13231".

<inertia> tag: Specifies the inertia matrix of the link, including the values for ixx, ixy, ixz, iyy, iyz, and izz.

<visual> tag: Defines the visual properties of the link, including its appearance and material.

<origin> tag: Specifies the position and rotation of the link’s visual origin. The xyz coordinates are 0 0 0, and the rpy rotation angles are 0 0 0.

<geometry> tag: Defines the geometry of the link, including a mesh file.

<mesh> tag: Specifies the mesh file used for the link’s geometry, located at "package://jetauto_description/meshes/wheel_right_back_link.stl".

<material> tag: Defines the material of the link, specified as black.

<collision> tag: Specifies the collision properties of the link, including its geometry.

<origin> tag: Specifies the position and rotation of the link’s collision origin. The xyz coordinates are 0 0 0, and the rpy rotation angles are 0 0 0.

<geometry> tag: Defines the geometry of the link, including a mesh file.

<mesh> tag: Specifies the mesh file used for the link’s geometry, located at "package://jetauto_description/meshes/wheel_right_back_link.stl".

The following code snippet describes a joint named wheel_right_back_joint:

<joint>: This is the opening tag for the joint element, defining a joint with name='wheel_right_back_joint'.

type="fixed": Specifies that the joint is of type fixed, meaning it is a stationary joint that does not permit movement.

<origin>: This tag defines the joint’s origin, including its position and orientation.

xyz="0.10675 0.091848 -0.065821": Indicates the position of the joint’s origin in 3D space as (0.10675, 0.091848, -0.065821).

rpy="0 0 3.1416": Represents the orientation of the joint’s origin using Euler angles, with roll, pitch, and yaw set to 0, 0, and 3.1416 respectively.

<parent>: This tag specifies the joint’s parent link.

link="base_link": Identifies the parent link of this joint as base_link.

<child>: This tag specifies the joint’s child link.

link="wheel_right_back_link": Identifies the child link of this joint as wheel_right_back_link.

<axis>: This tag defines the axis of the joint.

xyz="0 0 0": Sets the joint’s axis to (0, 0, 0), indicating that the joint does not have a defined axis of rotation.

</joint>: This is the closing tag for the joint element.

5.1.3 Principles of SLAM Mapping

  • Introduction to SLAM

SLAM stands for Simultaneous Localization and Mapping.

Localization involves determining the pose of a robot in a coordinate system,The origin of orientation of the coordinate system can be obtained from the first keyframe, existing global maps, landmarks or GPS data.

Mapping involves creating a map of the surrounding environment perceived by the robot. The basic geometric elements of the map are points. The main purpose of the map is for localization and navigation. Navigation can be divided into guidance and control. Guidance includes global planning and local planning, while control involves controlling the robot’s motion after the planning is done.

  • SLAM Mapping Principle

SLAM mapping mainly consists of the following three processes:

(1) Preprocessing: Optimizing the raw data from the radar point cloud, filtering out problematic data or performing filtering. Using laser as a signal source, pulses of laser emitted by the laser are directed at surrounding obstacles, causing scattering.

Some of the light waves will reflect back to the receiver of the lidar, and then, according to the principle of laser ranging, the distance from the lidar to the target point can be obtained.

Regarding point clouds: In simple terms, the surrounding environment information obtained by lidar is called a point cloud. It reflects a portion of what the ‘eyes’ of the robot can see in the environment where it is located. The object information collected presents a series of scattered, accurate angle, and distance information.

(2) Matching: Matching the point cloud data of the current local environment with the established map to find the corresponding position.

(3) Map Fusion: Integrating new round data from the lidar into the original map, ultimately completing the map update.

  • Notes

(1) Begin the mapping process by positioning the robot in front of a straight wall or within an enclosed box. This enhances the Lidar’s capacity to capture a higher density of scanning points.

(2) Initiate a 360-degree scan of the environment using the Lidar to ensure a comprehensive survey of the surroundings. This step is crucial to guarantee the accuracy and completeness of the resulting map.

(3) For larger areas, it’s recommended to complete a full mapping loop before focusing on scanning smaller environmental details. This approach enhances the overall efficiency and precision of the mapping process.

  • Judge Mapping Result

Finally, assess the robot’s navigation process against the following criteria once the mapping is complete:

(1) Ensure that the edges of obstacles within the map are distinctly defined.

(2) Check for any disparities between the map and the actual environment, such as the presence of closed loops or inconsistencies.

(3) Verify the absence of gray areas within the robot’s motion area, indicating areas that haven’t been adequately scanned.

(4) Confirm that the map doesn’t incorporate obstacles that won’t exist during subsequent localization.

(5) Validate the map’s coverage of the entire extent of the robot’s motion area.

5.1.4 slam_toolbox Mapping Algorithm

  • Mapping Definition

Slam Toolbox software package combines information from laser rangefinders in the form of LaserScan messages and performs TF transformation from odom-> base link to create a two-dimensional map of space. This software package allows for fully serialized reloadable data and pose graphs of SLAM maps, used for continuous mapping, localization, merging, or other operations. It allows Slam Toolbox to operate in synchronous (i.e., processing all valid sensor measurements regardless of delay) and asynchronous (i.e., processing valid sensor measurements whenever possible) modes.

ROS replaces functionalities like gmapping, cartographer, karto, hector, providing comprehensive SLAM functionality built upon the powerful scan matcher at the core of Karto, widely used and accelerated for this package. It also introduces a new optimization plugin based on Google Ceres. Additionally, it introduces a new localization method called ‘elastic pose-graph localization,’ which takes measured sliding windows and adds them to the graph for optimization and refinement. This allows for tracking changes in local features of the environment instead of considering them as biases, and removes these redundant nodes when leaving an area without affecting the long-term map.

Slam Toolbox is a suite of tools for 2D Slam, including:

① Mapping and Saving Maps (pgm Files): Create and save maps in the pgm file format.

② Refining and Continuing Mapping: Refine existing maps, remap, or extend mapping on previously saved maps.

③ Long-Term Mapping: Continue mapping with a previously saved map, removing irrelevant data from new laser point clouds.

④ Localization Optimization on Existing Maps: Optimize localization using an existing map. Alternatively, use “Laser Odometry” mode for localization without mapping.

⑤ Synchronous and Asynchronous Mapping: Supports both synchronous and asynchronous mapping.

⑥ Dynamic Map Merging: Merge maps dynamically.

⑦ Plugin-Based Optimization Solver: Includes a new optimization plugin based on Google Ceres.

⑧ Interactive RVIZ Plugin: Provides an interactive RVIZ plugin for map manipulation.

⑨ RVIZ Graphical Tools: Offers graphical tools in RVIZ for managing nodes and connections during the mapping process.

⑩ Map Serialization and Lossless Data Storage: Supports map serialization and ensures lossless data storage.

KARTO:

Karto_SLAM is based on graph optimization techniques, utilizing highly efficient and non-iterative Cholesky decomposition to solve sparse systems. In this method, the map is represented as a graph where each node represents a specific position in the robot’s trajectory along with its associated sensor measurements. As new nodes are added, the computations are updated accordingly.

The ROS version of Karto_SLAM incorporates Sparse Pose Adjustment (SPA), which is linked to scan matching and loop closure detection. Although including more landmarks increases memory requirements, graph optimization methods like Karto_SLAM are particularly advantageous in large environments. This is because they only represent the robot’s poses in the graph and determine the map after computing these poses.

The algorithmic framework of Karto_SLAM is shown in the diagram below:

From the diagram above, it can be seen that the process is quite straightforward. The SLAM system operates with a traditional soft real-time mechanism: each new frame of data is processed as it arrives and then returned.

KartoSLAM Source Code and Wiki Address:

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

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

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

  • Mapping Operation Steps

(1) Click-on to initiate the ROS2 command-line terminal.

(2) Execute the command below to disable the app auto-start service.

sudo systemctl stop start_app_node.service

(3) Execute the following command to initiate mapping.

ros2 launch slam slam.launch.py

(4) Create a new command-line terminal, and enter the command to launch the rviz tool and display the mapping results.

ros2 launch slam rviz_slam.launch.py

(5) Open a new command line terminal, enter the command to start the keyboard control node, and press Enter:

ros2 launch peripherals teleop_key_control.launch.py

When you receive the following prompt, it means that the keyboard control service is enabled successfully.

(6) Control the robot to move within the current space to build a more complete map. The table below lists the keyboard keys and their corresponding functions for controlling the robot’s movement:

Key Robot's Movement
W Short press; The robot will keep moving forward
S Short press; The robot will keep moving backward
A Long press; Turn left
D Long press; Turn right

When controlling the robot for mapping, you can reduce its speed. A slower speed results in smaller relative errors in odometry, which leads to better mapping outcomes. As the robot moves, the RVIZ display will continuously update the map until the entire environment is mapped.

  • Save Map

Open a new command-line terminal, and run the following command to save the map, then hit Enter key.

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

  • Outcome Optimization

If you want to achieve more precise mapping results, you can optimize the odometry. The robot relies on odometry for mapping, which in turn depends on the IMU.

The robot already has calibrated IMU data loaded, allowing it to perform mapping and navigation functions effectively. However, we can further calibrate the IMU to obtain higher precision. For methods and steps on calibrating the IMU, please refer to the section 2. ROS2-Motion Control Course -> 2.2 Motion Control -> 2.2.1 IMU, Linear Velocity, and Angular Velocity Calibration.

  • Parameter Description

The parameter file can be found at the path ros2_ws/src/slam/config/slam.yaml.

For more detailed information about the parameters, please refer to the official documentation: https://wiki.ros.org/slam_toolbox.

  • Launch File Analysis

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

  • Import Library

The launch library can be explored in detail in the official ROS documentation: https://docs.ros.org/en/humble/How-To-Guides/Launching-composable-nodes

  • Set the Storage Path

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

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

  • Initiate Other Launch File

35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
    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,  
                 actions=[slam_launch],
             ),
          ]
        )

base_launch: Launch for hardware initialization

slam_launch: Launch for basic mapping

bringup_launch: Launch for initial pose setup

5.1.5 RTAB-VSLAM 3D Vision Mapping & Navigation

  • RTAB-VSLAM Description

RTAB-VSLAM is a appearance-based real-time 3D mapping system, it’s an open-source library that achieves loop closure detection through memory management methods. It limits the size of the map to ensure that loop closure detection is always processed within a fixed time limit, thus meeting the requirements for long-term and large-scale environment online mapping.

  • RTAB-VSLAM Working Principle

RTAB-VSLAM 3D mapping employs feature mapping, offering the advantage of rich feature points in general scenes, good scene adaptability, and the ability to use feature points for localization. However, it has drawbacks, such as a time-consuming feature point calculation method, limited information usage leading to loss of image details, diminished effectiveness in weak-texture areas, and susceptibility to feature point matching errors, impacting results significantly.

After extracting features from images, the algorithm proceeds to match features at different timestamps, leading to loop detection. Upon completion of matching, data is categorized into long-term memory and short-term memory. Long-term memory data is utilized for matching future data, while short-term memory data is employed for matching current time-continuous data.

During the operation of the RTAB-VSLAM algorithm, it initially uses short-term memory data to update positioning points and build maps. As data from a specific future timestamp matches long-term memory data, the corresponding long-term memory data is integrated into short-term memory data for updating positioning and map construction.

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

  • RTAB-VSLAM 3D Mapping Instructions

(1) Click on on the system desktop to open the ROS2 command-line terminal. Then run the command to disable the app auto-start service:

sudo systemctl stop start_app_node.service

(2) Execute the command to start mapping:

ros2 launch slam rtabmap_slam.launch.py

(3) Create a new command-line terminal, and enter the command to open the RViz tool and display the mapping effect:

ros2 launch slam rviz_rtabmap.launch.py

(4) Create a new command-line terminal, and 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 image below appears, it means the keyboard control service has started successfully.

(5) Control the robot to move in the current space to build a more complete map. The table below shows the keyboard keys available for controlling robot movement and their corresponding functions:

Button Robot's Action
W Short press to switch to the forward state and continuously move forward
S Short press to switch to the backward state and continuously move backward
A Long press to interrupt the forward or backward state and turn left
D Long press to interrupt the forward or backward state and turn right

When controlling the robot’s movement for mapping using the keyboard, it’s advisable to appropriately reduce the robot’s movement speed. The smaller the robot’s running speed, the smaller the relative error of the odometry, resulting in a better mapping effect. As the robot moves, the map displayed in RVIZ will continuously expand until the entire environmental scene’s map construction is completed.

  • Map Saving

After mapping is completed, you can use the shortcut “Ctrl+C” in each command-line terminal window to close the currently running program.

Note

For 3D mapping, there’s no need to manually save the map. When you use “Ctrl+C” to close the mapping command, the map will be automatically saved.

  • lanunch File Analysis

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

(1) Import Library

You can refer to the ROS official documentation for detailed analysis of the launch library: “https://docs.ros.org/en/humble/How-To-Guides/Launching-composable-nodes.html”

1
2
3
4
5
6
7
8
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

(2) Set the Storage Path

Use get_package_share_directory to obtain the path of the slam package.

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

(3) Initiate Other Launch File

37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
    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,  # Delay for enabling other nodes(延时等待其它节点启动好)
             actions=[rtabmap_launch],
         ),
      ]
    )

base_launch: Launch for hardware initialization

slam_launch: Basic mapping launch

rtabmap_launch: RTAB mapping launch

bringup_launch: Initial pose launch