Isaac SimでVisual SLAMを動かしてみる

1. Nvidia Driver & CUDAのインストール
   省略
2. dockerのインストール
   省略
3. NVIDIA Container Toolkitのインストール
   以下のコマンドを実行

$ distribution=$(. /etc/os-release;echo $ID$VERSION_ID) \
      && curl -fsSL https://nvidia.github.io/libnvidia-container/gpgkey | sudo gpg --dearmor -o /usr/share/keyrings/nvidia-container-toolkit-keyring.gpg \
      && curl -s -L https://nvidia.github.io/libnvidia-container/$distribution/libnvidia-container.list | \
            sed 's#deb https://#deb [signed-by=/usr/share/keyrings/nvidia-container-toolkit-keyring.gpg] https://#g' | \
            sudo tee /etc/apt/sources.list.d/nvidia-container-toolkit.list
$ sudo apt-get update
$ sudo apt-get install -y nvidia-docker2
$ sudo systemctl restart docker

以下のコマンドを実行して、nvidia-smiの出力が表示されればOKです（このコマンドはdocker内でnvidia-smiコマンドを実行するため、docker内からGPUが使用できるかどうかがわかる）。

$ sudo docker run --rm --gpus all nvidia/cuda:11.0-base nvidia-smi

Dockerコンテナを起動する

/etc/docker/daemon.jsonを以下のように編集します。

    "runtimes": {
        "nvidia": {
            "path": "nvidia-container-runtime",
            "runtimeArgs": []
        }
    },
    "default-runtime": "nvidia"

上記の6行目まではすでに記載されていたので7行目（と6行目のカンマ）を追加しました。

上記の変更を以下のコマンドで反映します。

$ sudo systemctl daemon-reload
$ sudo systemctl restart docker

Dockerスクリプトを以下のコマンドでgitから取得します。ここではホームディレクトリ下にdockerというディレクトリを作成しています。

$ cd ~
$ mkdir docker
$ cd docker
$ git clone https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common

Dockerコンテナ用のディレクトリとして以下のディレクトリを作成しておきます（これは必須ではありません）。

$ mkdir -p ~/workspaces/isaac_ros-dev

Dockerコンテナの起動は~/docker/isaac_ros_common/に移動して以下のコマンドを入力します。

$ scripts/run_dev.sh ~/workspaces/isaac_ros-dev/

起動時にオプションとしてディレクトリパスを指定できます。Dockerコンテナを起動すると/workspaces/isaac_ros-devというディレクトリで起動します。このとき、起動時に指定したパスが/workspaces/isaac_ros-devにリンクされます。例えば、ホームディレクトリを指定して上記のコマンドを実行した場合、起動直後にlsコマンドを実行するとホームディレクトリの中身が表示されます。今回はコンテナ内のディレクトリ名とリンク先のディレクトリ名を合わせています。

Visual SLAMパッケージのビルド

パッケージのビルドをdockerコンテナ内で行います。まずは以下のコマンドでビルド用のワークスペースを作成します。

$ mkdir -p catkin_ws_ros2/src

Visual SLAMパッケージを以下のコマンドでgitから取得します。

$ cd catkin_ws_ros2/src
$ git clone https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_visual_slam
$ git clone https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_image_pipeline
$ git clone https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common

ビルドに必要な関連パッケージをインストールします。

$ rosdep install -i -r --from-paths src --rosdistro foxy -y --skip-keys "libopencv-dev libopencv-contrib-dev libopencv-imgproc-dev python-opencv python3-opencv"

次に以下のコマンドでこれらのパッケージをビルドします。

$ cd /workspaces/isaac_ros-dev/catkin_ws_ros2/
$ colcon build --symlink-install

ビルドがエラーなしで終了すると準備完了です。

Isaac SimでVisual SLAM

ここからIsaac SimでVisual SLAMを動かす手順になります。Isaac Simの環境はサンプルとして用意されている"carter_warehouse_apriltags_worker.usd"を使用します。これはCarterというNvidiaのロボットが倉庫に配置されている環境です。今回使用するVisual SLAMは"Elbrus SLAM"というもので2つのカメラを使用したステレオカメラシステムを使用します。

地図を作成する

まずはVisual SLAMで自己位置を推定するための3次元地図を作成します。内容的には以下の流れになります。

1. Isaac Simの起動
2. Dockerコンテナの起動
3. Visual SLAMのパッケージを起動する
4. ロボットを動かして地図を作成する
5. 作成した地図を保存する

Isaac Simを起動します。起動後、メニューのWindows -> Extensionsから"ROS"を検索し、ROS2 Bridgeを有効にします。

有効になっているROS1 Bridgeを一度無効にしてからROS2 Bridgeを有効にする

次にNucleusサーバから"Isaac/Samples/ROS/Scenario/carter_warehouse_apriltags_worker.usd"を開きます。

環境を開いた直後

開いた直後はCarterの右のカメラが有効になっていないので有効化します。

ROS_Camera_Stereo_Rightの"Raw USD Properties"のenabledにチェックする

Playボタンを押してシミュレーションを開始します。シミュレーションを開始すると2つのカメラ画像が表示されます。

見やすいように左右に並べる

Dockerコンテナを起動して、以下のコマンドでVisual SLAMを起動します。

$ cd catkin_ws_ros2
$ source install/setup.bash
$ ros2 launch isaac_ros_visual_slam isaac_ros_visual_slam_isaac_sim.launch.py

別の端末を立ち上げ、起動しているコンテナに接続します。以下のコマンド内の<container id>は$ docker psコマンドで表示されるものを使用します。

$ docker exec -it <container id> /bin/bash
$ cd catkin_ws_ros2
$ source install/setup.bash
$ ros2 topic pub /cmd_vel geometry_msgs/Twist '{linear:  {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 0.05}}'

このコマンドを実行することにより、Isaac Sim上のCarterがゆっくり回転し始めます。カメラ画像が回転します。

さらに別の端末を立ち上げ、先程同様コンテナに接続し、以下のコマンドを実行してrvizを立ち上げます。

$ docker exec -it <container id> /bin/bash
$ cd catkin_ws_ros2
$ source install/setup.bash
$ rviz2 -d src/isaac_ros_visual_slam/isaac_ros_visual_slam/rviz/default.cfg.rviz

立ち上げたrviz上には回転するロボットと点群のようなものが表示されます。点群のようなものはおそらくステレオ画像から生成した特徴点だと思われます。パッケージ起動直後からこの点群は蓄積されていくそうです。

回るロボット

またまた別の端末を立ち上げ、以下のコマンドを実行すると蓄積された点群を地図として出力します。

$ docker exec -it <container id> /bin/bash
$ cd catkin_ws_ros2
$ source install/setup.bash
$ ros2 action send_goal /visual_slam/save_map isaac_ros_visual_slam_interfaces/action/SaveMap "{map_url: /workspaces/isaac_ros-dev/catkin_ws_ros2/map}"

最後のコマンドでは、map_urlとしてディレクトリを指定します。指定したパスのディレクトリとその下に"data.mdb"というファイルが作成され、これが地図の実体となります。

Visual SLAMで自己位置認識してみる

地図を保存したらいったんすべての環境を終了させます。手順の流れは以下の通りです。

1. Isaac Simを起動する
2. Dockerコンテナを起動する
3. Visual SLAMのパッケージを起動する
4. 自己位置認識を実行するコマンドを実行する

1～3までは地図を作成したときと同じ操作になりますので省略します。

以下のコマンドで自己位置認識が実行されます。実行結果はコマンドを実行した端末に表示されます。

$ docker exec -it <container id> /bin/bash
$ cd catkin_ws_ros2
$ source install/setup.bash
$ ros2 action send_goal /visual_slam/load_map_and_localize isaac_ros_visual_slam_interfaces/action/LoadMapAndLocalize "{map_url: /workspaces/isaac_ros-dev/catkin_ws_ros2/map, localize_near_point: {x: 0.0, y: 0.0, z: 0.0}}"

実行結果としては以下の内容が表示されます。

• 自己位置認識が成功したかどうか
  ResultのSuccessがTrueの場合は自己位置認識成功、Falseの場合は自己位置認識失敗
• 認識結果の姿勢
  XYZ座標とクオータニオン

この最後のコマンドで何をやっているかというと、先程作成した地図を使用してIsaac Sim内のロボットの位置&姿勢を認識（推定）しています。試しにIsaac Sim内のロボットをteleop_keyboardなどで少し動かしてコマンドを実行すると認識結果が少し変わることが確認できます。

teleop_keyboardを実行する場合のコマンド例を以下に載せておきます。

$ docker exec -it <container id> /bin/bash
$ cd catkin_ws_ros2
$ source install/setup.bash
$ ros2 run turtlebot3_teleop teleop_keyboard

コマンド後部の"localize_near_point"は重要です。これは自己位置認識を開始する位置です。一般的に自己位置認識は地図のすべての位置から自分のいる位置を探すわけではなく、ある程度自分のいる位置を特定した上でその周辺を探索します。試しに"localize_near_point"を実際の位置より離れた場所を指定すると自己位置認識に失敗します。また"localize_near_point"はXYZ座標のみで向きなどは指定できないようです。

触ってみた感じだと、開始位置と実際の位置の差が0.7m程度のズレまで認識できました（地図の状況にもよると思いますが）。また、たまにX&Y座標は合っているのですがZ座標が明らかにズレた結果を返すことがありました。どのような自己位置認識手法が実装されているかまでは見ていません。