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 のバージョンを対象として検証しています。
システム要件
使用するハードウェアとソフトウェアの要件は下記リンクになります。
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 の準備
下記のリンクを参考にします。
- NVIDIA-ISAAC-ROS/isaac_ros_nvblox: Hardware-accelerated 3D scene reconstruction and Nav2 local costmap provider using nvblox
- NVIDIA-ISAAC-ROS/isaac_ros_visual_slam: Visual odometry package based on hardware-accelerated NVIDIA Elbrus library with world class quality and performance.
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 の起動
下記リンクにそって準備をしてください。
システムが要件を満たしているか確認してください。
必要なソフトウェアの要件は下記リンク先に記述されています。
Omniverse Launcher をダウンロードしてください。
Omniverse Launcher をインストールしてください。この Omniverse Launcher を通して Isaac Sim などのアプリケーションをインストールできます。
Nucleus をインストールしてください。Asset を管理するのに使用されます。この機能によって複数のユーザーが 3D Asset を編集などができます。
Cache をインストールしてください。Omniverse 専用の Cache になり、同一の Asset を読み込む際に処理が高速化されます。
Isaac Sim 2022.2.1 をインストールしてください。本チュートリアルはホストにインストールした場合の内容になります。インストール方法は下記を参照してください。
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 ]
}
}
ROS2 コンテナーが起動したら、ROS2 ワークスペースに必要な ROS2 の依存関係をインストール/ビルドする必要があります。Docker コンテナー内で以下のコマンドを入力します。
# 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