1. はじめに

IsaacSimでLIMO（無人走行UGV）でROS2 Navigationを試してみる [1]では、LIMOのロボットモデルにLidarを追加し、そのデータを使って2Dマップを基にした自律走行をシミュレーション環境上で実現しました。

その記事では次の目標としてVisual SLAMへの挑戦を挙げました。今回は、その第一歩として、Isaac SimとROS 2 RTAB-Mapを連携させ、LIMOロボットに搭載した単眼カメラの情報を使ったVisual Odometryの表示に取り組んだ内容を紹介したいと思います。

結果として、一応Visual Odometryの表示には成功しました。ただし、まだ理解が不十分な部分があり、課題が残っています。それでも、Isaac SimとRTAB-Mapの連携方法を示す事例として有意義だと考え、この記事を作成・投稿しました。

2. 実行環境

CPU: CORE i7 7th Gen
メモリ: 32GB
GPU: GeForce RTX 2070
OS: Ubuntu22.04(WSL2ではなくPCに直接インストール)
ROS2: Humble
Nvidia Driver: 535

3. 手順

3.1 RTAB-Map ROS 2パッケージ構築

IsaacSimのROS2 Navigationを試してみる [2]の記事で作成した"ros2_isaacsim_test"のワークスペースを使用して、rtabmap_rosパッケージを構築しました。

まず自分はこの構築までに色々と試してしまったので既存のRTAB-Mapバイナリを削除するため以下コマンドを打ちました。

terminal

sudo apt remove ros-humble-rtabmap*

次に必要な依存関係を以下のコマンドでインストールします。

terminal

sudo apt update
sudo apt install -y build-essential cmake git python3-colcon-common-extensions

そして以下のコマンドでRTAB-Map本体をクローンしてきます。

terminal

cd ~/ros2_isaacsim_test
git clone https://github.com/introlab/rtabmap.git src/rtabmap

次に以下のリポジトリのRTAB-MapパッケージのROS2ブランチを以下のコマンドでクローンしてきます。インストール方法については、このリポジトリのREADMEを参考にしています。

terminal

cd ~/ros2_isaacsim_test
git clone --branch ros2 https://github.com/introlab/rtabmap_ros.git src/rtabmap_ros

そして以下のコマンドよりrosdepを使って依存関係を解決します。

terminal

cd ~/ros2_isaacsim_test
rosdep update
rosdep install --from-paths src --ignore-src -r -y

ビルドプロセス中に、ログやDDSを最適化するために以下の環境変数を設定します。

terminal

export RCUTILS_LOGGING_USE_STDOUT=1
export RCUTILS_LOGGING_BUFFERED_STREAM=1
export RCUTILS_COLORIZED_OUTPUT=1
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export MAKEFLAGS="-j6"

そして以下のコマンドでビルドを実行します。

terminal

colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release

3.2 IsaacSim側のカメラの設定（Action Graphの修正）

IsaacSimでLIMO（無人走行UGV）にカメラを追加してみた [3]の記事で作成したAction Graphにある１個の"ROS 2 Camera Helper"をコピーアンドペーストして以下の図のように3つにします（赤枠部分）。

そしてそれぞれの"ROS 2 Camera Helper"の"topicName"と"type"を以下のように設定していきます。

"ROS 2 Camera Helper"のRGB画像トピックについての設定

下図で赤枠で囲んだところの項目を以下のように設定します。

topicName: /camera/color/image_raw
type:rgb

"ROS 2 Camera Helper"の深度画像トピックについての設定

下図で赤枠で囲んだところの項目を以下のように設定します。

topicName: /camera/depth/image_raw
type:depth

"ROS 2 Camera Helper"のカメラ情報トピックについての設定

下図で赤枠で囲んだところの項目を以下のように設定します。

topicName: /camera/camera_info
type:camera_info

4. シミュレーション結果

シミュレーション環境は[3]で作成した走行経路の両側に垂直な壁があるだけでしたので、特徴がつかみにくいということをがありました。そこで中央にキューブを１つ置いた以下の図の環境を作成しました。

次にIsaac Simの▶ボタンを押してシミュレーションを開始した後に、以下のコマンドでRTAB-MAPを起動しました。

terminal

ros2 launch rtabmap_launch rtabmap.launch.py \
    rgb_topic:=/camera/color/image_raw \
    depth_topic:=/camera/depth/image_raw \
    camera_info_topic:=/camera/camera_info \
    frame_id:=cam_link \
    map_frame_id:=map \
    odom_frame_id:=odom \
    approx_sync:=true \
    rtabmap_args:="--Odom/Strategy 0 --Vis/MinInliers 1 --Vis/EpipolarGeometry true"

ここで"Odom/Strategy 0"とすることで Visual Odometryを用いることを明示しています。
そして"Vis/MinInliers 1"とすることで特徴点マッチングの最小インライア数を1に設定して、今回のシミュレーション環境のように特徴点のあまりない環境でも特徴点のマッチングが成功しやすいようにしています。

これを起動すると以下のようなviewer(rtabmap viz)が起動します（以下の図は何の情報も読み込んでないデフォルト状態です）。

その後、LIMOに以下の前進と後進のコマンドを送信しました。

terminal（前進）

source /opt/ros/humble/setup.bash
ros2 topic pub /cmd_vel geometry_msgs/Twist '{linear:  {x: 0.1, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0,z: 0.0}}'

terminal（後進）

source /opt/ros/humble/setup.bash
ros2 topic pub /cmd_vel geometry_msgs/Twist '{linear:  {x: -0.1, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0,z: 0.0}}'

以下の動画は、その際のシミュレーション結果です。LIMOに搭載したカメラの映像は右端の"limo_cam"という表示が出ているIsaacSimにおけるViewerから確認することができます。そしてrtabmap vizの様子は左側の"RTAB-Map[ROS]"という表示が出ている画面から確認することができます。IsaacSimで確認できる画面では、どんどんキューブに近づいて行ったり離れて行ったりする画面がリアルタイムで確認できます。そしてrtabmap vizの3D MapにはLIMOの動いた軌跡が表示されていくことが確認できました。

5. まとめ

上記の手順を通じて、Isaac SimとROS 2 RTAB-Mapを連携させ、LIMOロボットに搭載した単眼カメラからの情報を使ってVisual OdometryによりLIMOの移動軌跡をrtabmap vizで表示することには一応成功しました。ただし、軌跡の表示が非常に遅かったり、途中で情報が取得できなかったのか、LIMOの周囲が黒く表示される現象が見られました。ただ、これらの事象の原因についてはまだ分かっておりません。そして目標であるVisual SLAMの実現には、まだ多くの課題が残っています。今後は少しずつ学びながら進めていきたいと考えています。

参考記事やサイト

[1]IsaacSimでLIMO（無人走行UGV）でROS2 Navigationを試してみる
[2]IsaacSimのROS2 Navigationを試してみる
[3]IsaacSimでLIMO（無人走行UGV）にカメラを追加してみた