ROS2下部署并运行rtab-map（d455相机）

🚀ROS2-Humble下部署 rtab-map

适用系统：Ubuntu 22.04
ROS2版本：Humble
相机设备：深度相机d455，自带IMU
GitHub源码链接：https://github.com/introlab/rtabmap_ros/tree/ros2

一、源码安装

可以直接进行二进制安装

sudo apt install ros-$ROS_DISTRO-rtabmap-ros

若要进行源码安装，请先确保二进制清除干净，运行下行命令

sudo apt remove ros-$ROS_DISTRO-rtabmap*

接下来进行安装编译

mkdir -p ~/rtab_ws/src
cd ~/rtab_ws
git clone https://github.com/introlab/rtabmap.git src/rtabmap
git clone --branch ros2 https://github.com/introlab/rtabmap_ros.git src/rtabmap_ros
rosdep update && rosdep install --from-paths src --ignore-src -r -y
export MAKEFLAGS="-j4" # 若运行内存大于16G，可忽视本行
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --parallel-workers 4

二、相机配置

根据自己的相机类型选择配置文件，官方配置例子存放在~/rtab_ws/src/rtabmap_ros/rtabmap_examples/launch文件夹下，本章节采用d455相机，在运行才先确保下了realsense的ros包，命令如下：

sudo apt-get install ros-humble-realsense2-camera

创建自己的launch文件，可参考官方的d435i配置，参考realsense_d435i_stereo.launch.py文件后，d455配置文件如下：

# Requirements:
#   A realsense D455
#   Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera)
# Example:
#   $ ros2 launch rtabmap_examples realsense_d455_stereo.launch.py

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch_ros.actions import Node, SetParameter
from launch.actions import IncludeLaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration

def generate_launch_description():
    parameters=[{
          'frame_id':'camera_link',
          'subscribe_stereo':True,
          'subscribe_odom_info':True,
          'wait_imu_to_init':True
          }]

    remappings=[
          ('imu', '/imu/data'),
          ('left/image_rect', '/camera/infra1/image_rect_raw'),
          ('left/camera_info', '/camera/infra1/camera_info'),
          ('right/image_rect', '/camera/infra2/image_rect_raw'),
          ('right/camera_info', '/camera/infra2/camera_info')]

    return LaunchDescription([

        # Launch arguments 启动参数：控制 IMU 同步策略
        DeclareLaunchArgument(
            'unite_imu_method', default_value='2',
            description='0-None, 1-copy, 2-linear_interpolation. Use unite_imu_method:="1" if imu topics stop being published.'),

        #Hack to disable IR emitter 关闭红外投射器以避免干扰
        SetParameter(name='depth_module.emitter_enabled', value=0),

        # Launch camera driver 启动 D455 相机驱动
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource([os.path.join(
                get_package_share_directory('realsense2_camera'), 'launch'),
                '/rs_launch.py']),
                launch_arguments={'camera_namespace': '',
                                  'enable_gyro': 'true',
                                  'enable_accel': 'true',
                                  'unite_imu_method': LaunchConfiguration('unite_imu_method'),
                                  'enable_infra1': 'true',
                                  'enable_infra2': 'true',
                                  'enable_color': 'false',
                		          'enable_depth': 'false',
                                  'enable_sync': 'true'}.items(),
        ),
	
	# 启动 stereo odometry
        Node(
           package='rtabmap_odom', executable='stereo_odometry', output='screen',
           parameters=parameters,
           remappings=remappings),
	
	# 启动 RTAB-Map 主建图节点
        Node(
            package='rtabmap_slam', executable='rtabmap', output='screen',
            parameters=parameters,
            remappings=remappings,
            arguments=['-d']),
	
	# 可视化工具：RTAB-Map 自带 GUI
        Node(
            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
            parameters=parameters,
            remappings=remappings),
                
        # Compute quaternion of the IMU 可选：IMU 预处理
        Node(
            package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen',
            parameters=[{'use_mag': False, 
                         'world_frame':'enu', 
                         'publish_tf':False}],
            remappings=[('imu/data_raw', '/camera/imu')]),
    ])

可以根据自己的参数和相机实际发布的话题名称修改文件内容，由于d455 相机通过 /camera/imu 发布的 sensor_msgs/msg/Imu 消息中：只有 angular_velocity（角速度）和 linear_acceleration（加速度）字段，没有 orientation 字段（为空）。而imu_filter_madgwick_node节点，它可以基于 IMU 原始数据估算 orientation，然后生成带 orientation 的新 /imu/data 话题，故imu话题名称采用/imu/data 话题名。

三、运行程序 

运行自己相机的配置文件，举例如下：

source ~/rtab_ws/install/setup.bash
ros2 launch rtabmap_examples realsense_d455_stereo.launch.py 

运行后就可以看到rtab-map自带的GUI，地图数据默认保存在 ~/.ros/rtabmap.db，可通过databaseViewer工具查看数据库：

rtabmap-databaseViewer ~/.ros/rtabmap.db