はじめに

　本記事では、RTAB-Mapの機能のうち、ビジュアルオドメトリのみをROS2環境で使用する方法について簡単に紹介します。
　RTAB-Map (Real-Time Appearance-Based Mapping)とは、RGB-D(画像+深度)の特徴点情報をもとにしたSLAM手法のひとつで、データの計測にはKinectやRealsense D435などのデプスカメラやステレオカメラを使用します。Turtlebot3が標準で提供するような車輪オドメトリを使用しないため、路面状況や荷重に伴う車輪のスリップを考慮する必要がなくなります。一方で、特徴点が認識できるだけの明るい環境は必要になると思いますので、状況や用途に合わせて使い分けても良いのかもしれません。

RTAB-MapをROS2で使う

　本記事ではROS2(Humble)でRTAB-Mapを使用し、センサとしてIntel Realsense D435iを使用することを想定しています。
　まずは、rtabmap_rosを導入します。導入方法については、こちらの記事を参照してください。

　rtabmap_rosでは、rtabmap_examplesパッケージにて主要なセンサに合わせた設定を行うためのLaunchファイルが提供されています。そのうち、realsense_d435i_color.launch.pyを編集することでビジュアルオドメトリのみをパブリッシュするアプローチをとります。今回は、以下のLaunchファイルの中身を参考にします。

launchの編集

Launchの実行にあたって、新たにパッケージを作成しても、直接Launchファイルを呼び出してもかまいませんが、今回は、使い勝手の関係からパッケージを作成して呼び出すことにします。
パッケージにLaunchを含める方法の説明は省略します。以下の公式ドキュメントを参考にしてください。

　今回は、パッケージ名をmy_rtabmapとして想定で話を進めます。

　パッケージのディレクトリ直下にlaunchディレクトリを作成し、rtab_odom.launch.pyを新たに作成します。作成できたら、以下の内容を記述します。

rtab_odom.launch.py

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

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

   remappings=[
       ('imu', '/camera/imu'),
       ('rgb/image', '/camera/color/image_raw'),
       ('rgb/camera_info', '/camera/color/camera_info'),
       ('depth/image', '/camera/depth/image_rect_raw')]
   
   return LaunchDescription([
       # Nodes to launch       
       Node(
           package='rtabmap_odom', executable='rgbd_odometry', output='screen',
           parameters=parameters,
           remappings=remappings),

       # Compute quaternion of the 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')]),

       # The IMU frame is missing in TF tree, add it:
       Node(
           package='tf2_ros', executable='static_transform_publisher', output='screen',
           arguments=['0', '0', '0', '0', '0', '0', 'camera_gyro_optical_frame', 'camera_imu_optical_frame']),

   ])

　基本的には、元にしているLaunchファイルから不要な部分(SLAM機能、ビジュアライゼーション機能など)を削除し、ビジュアルオドメトリに関連する部分だけを残しています。

実行

　パッケージの設定(Cmakelists.txtなど)を完了させたうえで、ビルドさせていきましょう。

cd ~/(Your workspace directry)
colcon build --symlink-install

　ビルドしたターミナルから改めたターミナルにて、インストールと実行を行います。

cd ~/(Your workspace directry)
. install/setup.bash
ros2 launch my_rtabmap rtab_odom.launch.py

　実際に値がパブリッシュされているかについては、

ros2 topic echo /odom

などで確認しましょう。

課題

　今のところ、オドメトリは問題なくパブリッシュされているのですが、ROS2の座標系とセンサの座標系が異なります。そのため、座標系を合わせる中継ノードなどをもしかしたら立てる必要があるかもしれません。
　また、配線の接続なのかシステムの問題なのか判明していないのですが、センサを急激に動かすとデータが取れなくなる場合があります。コンソールにエラーが出力されるなどの症状はないため原因を究明しているところです。
今後は、今回紹介した手法によってNavigation2を実行する方法などの開発と紹介を行えたらと思っています。

参考