RTAB-Map Localization and Navigation

1. Course Content

1. Run the sample program to perform navigation using RTAB-Map in localization mode.

2. Principle Introduction

• The default localization algorithm in Navigation 2 is AMCL (Adaptive Monte Carlo Localization). This involves replacing the default AMCL localization method with the RTAB-Map localization method, while other navigation functions remain unchanged.

3. Start the Agent

Note: If it is already running, you do not need to start it again.

Enter the command in the vehicle's terminal:

start_agent

The terminal will print the following information, indicating a successful connection:

4. Run the Example

4.1 Preparations

• You need to have a map in .db format built first. Refer to the 15-RTAB-Map SLAM Mapping section.

4.2 Start the Navigation Node

For Raspberry Pi 5 users, if the ROS container is not started, you need to start it first,

xxxxxxxxxx
bringup_m3

Open a terminal inside the container,

xxxxxxxxxx
exec_m3

(ORIN board)

xxxxxxxxxx
ros2 launch m3_bringup nav2.launch.py localization_mode:=rtabmap

(RDK & PI5 boards) Run in separate steps, with visualization launched on the accompanying virtual machine Rosmaster-M3-Ubuntu22.04,

xxxxxxxxxx
# On the virtual machine
rviz2 -d /home/yahboom/yahboomM3_ws/user_ws/src/m3_bringup/rviz/rtabmap_nav.rviz
# On the host machine
ros2 launch m3_bringup nav2.launch.py localization_mode:=rtabmap use_rviz:=False

• Navigation is also performed by giving a target pose with 2D Goal Pose.

5. Source Code Analysis

5.1 View TF Tree

xxxxxxxxxx
ros2 run rqt_tf_tree rqt_tf_tree

• Here, the TF transformation from the map coordinate frame to the odom coordinate frame is provided by the rtabmap node.

5.2 View Node Communication Graph

xxxxxxxxxx
ros2 run rqt_graph rqt_graph

• In the figure below, when the rtabmap node is in localization mode, it receives the Lidar /scan and odometry /odom topics, and publishes TF transformations and updates the /map.

5.3 Launch File

Source code path:

xxxxxxxxxx
~/yahboomM3_ws/user_ws/src/m3_bringup/launch/navigation/nav2.launch.py

• The key code snippet below starts the Navigation2 stack node and the RTAB-Map localization node.

xxxxxxxxxx
    # RTAB-Map positioning navigation
    rtabmap_localization_nav = GroupAction(
        condition=IfCondition(PythonExpression(["'", localization_mode, "' == 'rtabmap' and '", slam, "' == 'False' "])),
        actions=[
            PushRosNamespace(namespace=namespace),
            # Start navigation2 navigation file (no global positioning)
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(nav2_launch_mod),
                launch_arguments={
                                "map": map_dir,
                                "params_file": params_file,
                                "use_sim_time":use_sim_time,
                                }.items()),
            # Start slamtoolbox positioning node
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(rtabmap_localization_launch_file),
                launch_arguments={
                    "mode": "localization",
                }.items(),
            ),
            # Log Printing
            LogInfo(msg=[Fore.GREEN+'====================Robot-M3 Navigation Start with rtabmap localization====================',Fore.RESET])
        ]
    )

• RTAB-Map localization node launch file path:

xxxxxxxxxx
~/yahboomM3_ws/user_ws/src/m3_bringup/launch/slam/rtab_map.launch.py

• The key launch snippet below switches to localization mode via the localization launch parameter.

xxxxxxxxxx
    delay_start_rtabmap_localization= TimerAction(
        condition=IfCondition(PythonExpression(["'", mode, "' == 'localization'" ])),
        period=5.0,
        actions=[
            IncludeLaunchDescription(
                    PythonLaunchDescriptionSource(rtabmap_launch),
                    launch_arguments={
                        "rgb_topic":                    rgb_topic,
                        "depth_topic":                  depth_topic,
                        "camera_info_topic":            camera_info_topic,
                        "frame_id":                     'base_link',
                        "odom_topic":                   '/odom',
                        "use_sim_time":                 'false',
                        "rviz":                         'true',
                        "rviz_cfg":                      nav_cfg,
                        "database_path":                 database_path,     
                        "rtabmap_viz":                  'false',
                        "approx_sync":                  'true',
                        "approx_sync_max_interval":     '0.01',
                        "qos":                          '0',
                        "visual_odometry":              'false',
                        "icp_odometry":                 'false',
                        "subscribe_scan":               'true',
                        "sync_queue_size":              '50',
                        "topic_queue_size":             '50',
                        "localization":                 'true',# Use localization mode, provide map matching
                        "--Mem/IncrementalMemory":      'false',
                        "log_level":                    'info'
                    }.items(),
                )
        ]
     )