Isaac ROS Nvblox と Isaac Sim を活用したシミュレーション環境でのコストマップ作成

Reading Time: 5 minutes

Isaac ROS Nvblox の仕組み (概要)

Isaac ROS Nvblox には、3D 再構成とナビゲーション用コストマップのための ROS 2 パッケージが含まれています。nvblox_nav2 は、深度と姿勢を処理してリアルタイムで 3D シーンを再構成し、Nav2 用の 2D コストマップを作成します。コストマップは、障害物を回避するビジョンベースのソリューションとしてナビゲーション中のプランニングで使用します。

nvblox_nav2 は、深度画像を提供するステレオカメラで動作するように設計されており、対応するポーズは GPU アクセラレーションを使用して、nvblox を使用して 3D 再構成と 2D コストマップを計算します。

下記は nvblox_nav2 を用いた典型的なグラフです。奥行き画像に対応する入力カラー画像は unet で処理され、PeopleSemSegNet DNN モデルを用いてカラー画像内の人物に対するセグメンテーションマスクを推定しています。深度画像に対応するポーズは、visual_slam を用いて計算されます。得られた人物マスクとポーズは、カラー画像と深度画像とともに、3D シーン再構成に使用されます。出力されたコストマップは、Nav2 のコストマッププラグインを通じて提供され、オプションとして、メッシュ可視化プラグインを用いて Rviz にカラー化された 3D 再構成が行われます。

nvblox_nav2 は、3D ボクセルグリッドに格納された TSDF (Truncated Signed Distance Function) の形で再構築された地図を構築します。このアプローチは、各ボクセルで占有確率を保存する 3D 占有グリッドマッピングのアプローチに似ています。しかし、nvblox のような TSDF ベースのアプローチでは、各ボクセルに最も近い表面までの (符号付き) 距離が格納されます。一般的に、TSDF ベースの再構成は、より高品質なサーフェス再構成を提供します。

また、距離場は再構成に利用するだけでなく、将来のロボットの位置が衝突するかどうかを即座に確認できるため、経路計画にも有効です。

人は移動ロボットにとって一般的な障害物であり、コストマップの一部ではありますが、3D 再構成に人を含めるべきではありません。人との距離に応じて異なるナビゲーションを行うことで行動認識を提供するプランナーは、人のコストマップが有益です。人物のセグメンテーションはカラーイメージを使用して計算され、結果として得られるマスクが深度画像に適用され、深度がシーン深度画像と人物深度画像に分離されます。シーンの深度画像は、上で説明したように TSDF マッピングに転送され、人の深度画像は占有グリッドマップに処理されます。

占有グリッドマップが静的なオブジェクトのみを捉えるという仮定を緩和するために、Nvblox は占有率の減衰ステップを適用しています。固定された周波数で、すべてのボクセルの占有確率は時間の経過とともに 0.5 に向かって減衰します。つまり、マップの状態 (占有か空きか) は、視野から外れた後、不明 (占有確率 0.5) になるまで確率が低くなるのです。

Quick Start

v0.30.0-dp のバージョンを対象として検証しています。

システム要件

使用するハードウェアとソフトウェアの要件は下記リンクになります。

NVIDIA-ISAAC-ROS/isaac_ros_nvblox: Hardware-accelerated 3D scene reconstruction and Nav2 local costmap provider using nvblox

Platform	Hardware	Software	Notes
Jetson	Jetson Orin Jetson Xavier	JetPack 5.1.1	For best performance, ensure that power settings are configured appropriately.
x86_64	NVIDIA GPU	Ubuntu 20.04+ CUDA 11.8+

Isaac ROS Nvblox の準備

下記のリンクを参考にします。

Github ページから Isaac ROS のソースコードをクローンします。

# Clone source code from Isaac ROS Github.
# Here is the expected source tree.
#   ~/workspaces/isaac_ros-dev/src/
#       |-isaac_ros_common/
#       +-isaac_ros_nvblox/

# use ‘v0.30.0-dp’ branch instead of ‘main(default)’.
mkdir -p ~/workspaces/isaac_ros-dev/src/
cd ~/workspaces/isaac_ros-dev/src/
git clone -b v0.30.0-dp --recurse-submodules https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common.git
cd isaac_ros_common
git lfs pull

cd ~/workspaces/isaac_ros-dev/src/

# use ‘v0.30.0-dp’ instead of ‘main(default)’.
git clone -b v0.30.0-dp --recurse-submodules　https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_nvblox
cd isaac_ros_nvblox
git lfs pull

Github ページから Isaac ROS Visual SLAM と Isaac ROS NITROS のソースコードをクローンします。

# Clone source code from Isaac ROS Github.
# Here is the expected source tree.
#   ~/workspaces/isaac_ros-dev/src/
#       |-isaac_ros_common/
#       |-isaac_ros_nvblox/
#       |-isaac_ros_visual_slam/
#       +-isaac_ros_nitros/

# use ‘v0.30.0-dp’ branch instead of ‘main(default)’.
cd ~/workspaces/isaac_ros-dev/src/
git clone -b v0.30.0-dp https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_visual_slam.git
cd isaac_ros_visual_slam
git lfs pull

# Clone source code
cd ~/workspaces/isaac_ros-dev/src/

# use ‘v0.30.0-dp’ branch instead of ‘main(default)’.
git clone -b v0.30.0-dp https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_nitros.git
cd isaac_ros_nitros
git lfs pull

Linux ホスト PC で ROS2 コンテナーイメージを Build/Run します。

# Build/Run a ROS2 container
cd ~/workspaces/isaac_ros-dev/src/isaac_ros_common/
./scripts/run_dev.sh

ROS2 コンテナーが起動したら、ROS2 ワークスペースに必要な ROS2 の依存関係をインストール/ビルドする必要があります。Docker コンテナー内で以下のコマンドを入力します。

# Inside the container, install package-specific dependencies via rosdep:
cd /workspaces/isaac_ros-dev/ && \
    rosdep install -i -r --from-paths src --rosdistro humble -y --skip-keys "libopencv-dev libopencv-contrib-dev libopencv-imgproc-dev python-opencv python3-opencv nvblox"


# Install the dependencies for your ROS workspace
cd /workspaces/isaac_ros-dev

# Build code
colcon build --symlink-install

# Source the ros_ws
source /workspaces/isaac_ros-dev/install/setup.bash

Isaac Sim の起動

下記リンクにそって準備をしてください。

isaac_ros_common/isaac-sim-sil-setup.md at main · NVIDIA-ISAAC-ROS/isaac_ros_common

システムが要件を満たしているか確認してください。

Isaac Sim Requirements — Omniverse Robotics documentation

必要なソフトウェアの要件は下記リンク先に記述されています。

Isaac Sim Requirements — Omniverse Robotics documentation

Omniverse Launcher をダウンロードしてください。

Omniverse Platform for creating and operating metaverse applications

Omniverse Launcher をインストールしてください。この Omniverse Launcher を通して Isaac Sim などのアプリケーションをインストールできます。

Installing Launcher — Omniverse Launcher documentation

Nucleus をインストールしてください。Asset を管理するのに使用されます。この機能によって複数のユーザーが 3D Asset を編集などができます。

Installation — Omniverse Nucleus documentation

Cache をインストールしてください。Omniverse 専用の Cache になり、同一の Asset を読み込む際に処理が高速化されます。

Workstation — Omniverse Nucleus documentation

Isaac Sim 2022.2.1 をインストールしてください。本チュートリアルはホストにインストールした場合の内容になります。インストール方法は下記を参照してください。

Workstation Installation — Omniverse Robotics documentation

Isaac Sim を使用する方法は複数、存在しますが、今回は Stand Alone で動作するスクリプトを使用して動作します。

Isaac SIM を起動します。Isaac ROS Nvblox で Isaac Sim を起動するスクリプトが用意されています。スクリプトの詳細はこちらで説明されています。起動するのに時間がかかります。

# Start up Isaac Sim with the correct sensors on the host machine.
~/.local/share/ov/pkg/isaac_sim-2022.2.1/python.sh ~/workspaces/isaac_ros-dev/src/isaac_ros_nvblox/nvblox_examples/nvblox_isaac_sim/omniverse_scripts/start_isaac_sim.py

下のようなウィンドウが表示されます。

以上で Isaac Sim の準備は終わります。

Isaac ROS Nvblox との連携

Docker を起動したターミナルに戻り、下記コマンドを実行します。
Terminal 1: 起動ファイルを使って Nvblox ノードを起動します:

cd /workspaces/isaac_ros-dev/
source /workspaces/isaac_ros-dev/install/setup.bash
ros2 launch nvblox_examples_bringup isaac_sim_example.launch.py

Rviz 上で 2D Goal Pose ボタンをクリックします。任意の場所にロボットを移動させてみます。このページの上部にあるように、メッシュとコストマップ、そしてロボットがゴール地点に向かって移動しているのが確認できるはずです。

Isaac Sim Human Reconstruction

nvblox の Human Reconstruction のために Isaac Sim を使用する方法を示します。人間を検出するための入力として、シミュレーションから得られる正解のセマンティックセグメンテーションを使用します。このデモでは、ロボットが動いている間に人間が環境内を移動するために、拡張機能 omni.anim.people と omni.anim.navigation に依存しています。(Note: 終了時にコンテナーターミナルを閉じないこと)

Rviz ウィンドウを閉じます。
ターミナル 1 の終了コマンド (ctrl + c)
Isaac Sim の Stop () を押す。
Isaac SIM の window を閉じる。

Github ページから Isaac ROS のソースコードをクローンします。

# Clone source code from Isaac ROS Github.
# Here is the expected source tree.
#   ~/workspaces/isaac_ros-dev/src/
#       |-isaac_ros_common/
#       |-isaac_ros_nvblox/
#       |-isaac_ros_visual_slam/
#       |-isaac_ros_nitros/
#       |-isaac_ros_image_segmentation/
#       |-isaac_ros_dnn_inference/
#       +-isaac_ros_image_pipeline/

# use ‘v0.30.0-dp’ branch instead of ‘main(default)’.
mkdir -p ~/workspaces/isaac_ros-dev/src/
cd ~/workspaces/isaac_ros-dev/src/
git clone -b v0.30.0-dp https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_image_segmentation
cd isaac_ros_image_segmentation
git lfs pull

# Change directory
cd ~/workspaces/isaac_ros-dev/src/

# use ‘v0.30.0-dp’ branch instead of ‘main(default)’.
git clone -b v0.30.0-dp https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_dnn_inference.git
cd isaac_ros_dnn_inference
git lfs pull

# Change directory
cd ~/workspaces/isaac_ros-dev/src/

# use ‘v0.30.0-dp’ branch instead of ‘main(default)’.
git clone -b v0.30.0-dp https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_image_pipeline.git
cd isaac_ros_image_pipeline
git lfs pull

ROS2 コンテナーイメージを Build/Run します。

# Build/Run a ROS2 container
cd ~/workspaces/isaac_ros-dev/src/isaac_ros_common/
./scripts/run_dev.sh

Docker コンテナー内で以下のコマンドを入力します。
PeopleSemSegNet ETLT ファイルと int8 推論モードキャッシュファイルをダウンロードしてください:

mkdir -p /workspaces/isaac_ros-dev/models/peoplesemsegnet/1
cd /workspaces/isaac_ros-dev/models/peoplesemsegnet
wget 'https://api.ngc.nvidia.com/v2/models/nvidia/tao/peoplesemsegnet/versions/deployable_quantized_vanilla_unet_v2.0/files/peoplesemsegnet_vanilla_unet_dynamic_etlt_int8.cache'
wget 'https://api.ngc.nvidia.com/v2/models/nvidia/tao/peoplesemsegnet/versions/deployable_quantized_vanilla_unet_v2.0/files/peoplesemsegnet_vanilla_unet_dynamic_etlt_int8_fp16.etlt'

ETLT ファイルを TensorRT のプランファイルに変換します。

/opt/nvidia/tao/tao-converter -k tlt_encode -d 3,544,960 -p input_1:0,1x3x544x960,1x3x544x960,1x3x544x960 -t int8 -c peoplesemsegnet_vanilla_unet_dynamic_etlt_int8.cache -e /workspaces/isaac_ros-dev/models/peoplesemsegnet/1/model.plan -o argmax_1 peoplesemsegnet_vanilla_unet_dynamic_etlt_int8_fp16.etlt

以下の内容で、/workspaces/isaac_ros-dev/models/peoplesemsegnet/config.pbtxt という triton 設定ファイルを作成してください:

name: "peoplesemsegnet"
platform: "tensorrt_plan"
max_batch_size: 0
input [
  {
    name: "input_1:0"
    data_type: TYPE_FP32
    dims: [ 1, 3, 544, 960 ]
  }
]
output [
  {
    name: "argmax_1"
    data_type: TYPE_INT32
    dims: [ 1, 544, 960, 1 ]
  }
]
version_policy: {
  specific {
    versions: [ 1 ]
  }
}

# Install the dependencies for your ROS workspace
cd /workspaces/isaac_ros-dev

# Build code
colcon build --symlink-install

# Source the ros_ws
source /workspaces/isaac_ros-dev/install/setup.bash

Isaac SIM を起動します。エラーが出ますが無視して待ちます。スクリプトの詳細はこちらで説明されています。アニメーションでは Python スクリプトを実行する必要があるため、UI でシーンを実行すると、スクリプト実行を有効にするかどうかを確認するウィンドウが表示されます。[Yes] をクリックすると、シーンと人間のアニメーションを開始することができるようになります。

# Start up Isaac Sim with the correct sensors on the host machine.
#   alias omni_python='~/.local/share/ov/pkg/isaac_sim-2022.2.1/python.sh'
~/.local/share/ov/pkg/isaac_sim-2022.2.1/python.sh  ~/workspaces/isaac_ros-dev/src/isaac_ros_nvblox/nvblox_examples/nvblox_isaac_sim/omniverse_scripts/start_isaac_sim.py --with_people --scenario_path=/Isaac/Samples/NvBlox/carter_warehouse_navigation_with_people.usd

Docker を起動したターミナルで下記コマンドを実行します。
Terminal 1: 起動ファイルを使って NVBlox ノードを起動します

cd /workspaces/isaac_ros-dev/
source /workspaces/isaac_ros-dev/install/setup.bash
ros2 launch nvblox_examples_bringup isaac_sim_humans_example.launch.py

Human command の生成

先程のデモでは Human が動作していないため、Human が動作するためのコマンドを生成します。

Isaac ROS Nvblox と Human Reconstruction の連携

Rviz ウィンドウを閉じます。
ターミナル 1 の終了コマンド (ctrl + c)
Isaac Sim の Stop () を押す。
Isaac SIM の window を閉じる。

下記コードにより、ヘッドレスで Isaac Sim が起動し、<path_to_folder> に新しい human_cmd_file.txt が生成されます。デフォルトでは、人間 1 人につき 5 つのウェイポイントを生成しますが、オプション --num_waypoints=<number_of_waypoints> で変更することが可能です。その後、スクリプトファイルを手動でアップロードしてデフォルトのものと置き換えるか、ステップ 2. と同じコマンドに --use_generated_command_file --anim_people_waypoint_dir <path_to_folder> を加えてスクリプトファイルを自動的に設定することができます。

# Start up Isaac Sim with the correct sensors on the host machine.
#   alias omni_python='~/.local/share/ov/pkg/isaac_sim-2022.2.1/python.sh'
# Example
# ~/workspaces/isaac_ros-dev/src/isaac_ros_nvblox/nvblox_examples/nvblox_isaac_sim/omniverse_scripts/start_isaac_sim.py \
#    --with_people --scenario_path=/Isaac/Samples/NvBlox/carter_warehouse_navigation_with_people.usd \
#    --anim_people_waypoint_dir <path_to_folder> --with_people --random_command_generation
mkdir /tmp/way_point_dir

~/.local/share/ov/pkg/isaac_sim-2022.2.1/python.sh ~/workspaces/isaac_ros-dev/src/isaac_ros_nvblox/nvblox_examples/nvblox_isaac_sim/omniverse_scripts/start_isaac_sim.py \
    --with_people --scenario_path=/Isaac/Samples/NvBlox/carter_warehouse_navigation_with_people.usd \
    --anim_people_waypoint_dir /tmp/way_point_dir --with_people --random_command_generation

Isaac SIM を再度起動します。先程作成したコマンドファイルを適用するように --use_generated_command_file --anim_people_waypoint_dir <path_to_folder> を設定しています。

# Start up Isaac Sim with the correct sensors on the host machine.
#   alias omni_python='~/.local/share/ov/pkg/isaac_sim-2022.2.1/python.sh'
# ~/workspaces/isaac_ros-dev/src/isaac_ros_nvblox/nvblox_examples/nvblox_isaac_sim/omniverse_scripts/start_isaac_sim.py #--with_people --scenario_path=/Isaac/Samples/NvBlox/carter_warehouse_navigation_with_people.usd #--use_generated_command_file --anim_people_waypoint_dir <path_to_folder>

~/.local/share/ov/pkg/isaac_sim-2022.2.1/python.sh ~/workspaces/isaac_ros-dev/src/isaac_ros_nvblox/nvblox_examples/nvblox_isaac_sim/omniverse_scripts/start_isaac_sim.py --with_people --scenario_path=/Isaac/Samples/NvBlox/carter_warehouse_navigation_with_people.usd --use_generated_command_file --anim_people_waypoint_dir /tmp/way_point_dir

[Yes] をクリックすると、シーンと人間のアニメーションを開始することができるようになります。人が行動するようになります。

Docker を起動したターミナルで下記コマンドを実行します。
Terminal 1: 起動ファイルを使って NVBlox ノードを起動します。Human が動作した状態でのデモが確認できます。

cd /workspaces/isaac_ros-dev/
source /workspaces/isaac_ros-dev/install/setup.bash
ros2 launch nvblox_examples_bringup isaac_sim_humans_example.launch.py

Isaac ROS Nvblox と Isaac Sim を活用したシミュレーション環境でのコストマップ作成

Isaac ROS Nvblox の仕組み (概要)

Quick Start

システム要件

Isaac ROS Nvblox の準備

Isaac Sim の起動

Isaac ROS Nvblox との連携

Isaac Sim Human Reconstruction

Human command の生成

Isaac ROS Nvblox と Human Reconstruction の連携

Tags

About the Authors

Isaac ROS Nvblox と Isaac Sim を活用したシミュレーション環境でのコスト マップ作成

Isaac ROS Nvblox の仕組み (概要)

Quick Start

システム要件

Isaac ROS Nvblox の準備

Isaac Sim の起動

Isaac ROS Nvblox との連携

Isaac Sim Human Reconstruction

Human command の生成

Isaac ROS Nvblox と Human Reconstruction の連携

Tags

About the Authors

関連記事

NVIDIA GB200 NVL72 は兆単位パラメーターの LLM トレーニングとリアルタイム推論を実現

NeMo Framework で日本語 LLM を簡単デプロイ - オンライン推論編 -

cuDLA による NVIDIA Jetson Orin 上での YOLOv5 の紹介

NeMo Framework で日本語 LLM をファインチューニング - PEFT 編 -

TAO Toolkit 5.0 に追加された Data Service を活用

Isaac ROS Nvblox と Isaac Sim を活用したシミュレーション環境でのコストマップ作成