IsaacSimのROS2 Navigationを試してみる

# はじめに
先日の投稿でロボット走行用の2Dmapを作成してみましたので、今度は作成した地図をベースとした自律走行を試してみたいと思います。ちなみにQiita内の記事を調べたところ、今回私がやろうとしていることは、すでにManyan3さんが以下のリンクの記事[[1]](#参考記事やサイト)で実施されております。

https://qiita.com/Manyan3/items/d1c3f311b57109772470

上記の記事と今回の記事との違いはOSがUbuntuが22.04であるというところぐらいで、あまり違いはないのですが、自分で作成した2D Mapを使った自律走行もしてますので、そこも記載していきたいと思います。

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

**Nvidia Driverについて余談：**
ISAAC SIMでは、525.85.05が推奨されていますが、私の環境ではインストールできませんでした。そして525.147.05をrunファイルでインストールすると入りはするんですが、何回もドライバーが飛ぶので嫌になり、以下のコマンドで推奨されたドライバーをaptでインストールしました。

```:terminal
ubuntu-drivers devices
```

なお、インストール方法は[ubuntuにCUDA、nvidiaドライバをインストールするメモ](https://qiita.com/porizou1/items/74d8264d6381ee2941bd)([[2]](#参考記事やサイト))を参考にしました。

# 3. 手順
この章では基本的には記事[[1]](#参考記事やサイト)で述べられている内容を追っていきますが、
ROS2とIsaac Simについては、[Ubunutu22.04の環境下においてIsaac Sim+ROS2を試そうとしてハマったところ](https://qiita.com/bebebefu/items/a89694659e650665e5db)[[3]](#参考記事やサイト)でインストールをしておりますので、この部分の設定については本記事では飛ばします。

## 3.1 Navigation2などのインストール
[[1]](#参考記事やサイト)の記事はfoxy向けの記事でしたので、ここではManyan3さんの[ROS2 Humble Navigation２ 設定メモ](https://qiita.com/Manyan3/items/e13a19d14a25cdc16425)[[4]](#参考記事やサイト)の記事の2章「Navigation２その他コンポーネント追加」を参考に以下のコマンドを実行してインストールしました。

```:Navigation2のインストール
sudo apt install ros-humble-navigation2
sudo apt install ros-humble-nav2-bringup
```
```:Moveitのインストール
sudo apt install ros-humble-moveit
```

ちなみにこれを実行しない場合、後述します"ros2 launch carter_navigation carter_navigation.launch.py"のコマンドを打っても"PackageNotFoundError: "package 'nav2_bringup' not found"というエラーがでます。

## 3.2 ワークスペースの作成
[[1]](#参考記事やサイト)の記事の2章「ワークスペース作成」を参考に、以下のコマンドを実行してワークスペースを作成しました。

```:terminal
mkdir ros2_isaacsim_test
cd ros2_isaacsim_test
mkdir src
```

## 3.3 パッケージ取り出し
[[1]](#参考記事やサイト)の記事の3章「パッケージ取り出し」を参考に、私の環境では以下のところにCarterのパッケージがあることを確認しました。
```:terminal
cd ~/.local/share/ov/pkg/isaac_sim-2022.2.1/ros2_workspace/src

ls 

isaac_ros2_messages  isaac_tutorials  navigation
```

そして"~/.local/share/ov/pkg/isaac_sim-2022.2.1/ros2_workspace/src"を先ほど作成したワークスペースに以下のコマンドでコピーしました。

```:terminal
sudo cp -r ./src  ~/ros2_isaacsim_test/src
```

そして"/ros2_isaacsim_test"のフォルダに移動した上で以下のコマンドを実行し、ビルドを開始しました。
```:terminal
source /opt/ros/humble/setup.bash
colcon build
```

これでstd errは出ましたが、ビルドには成功しました。

## 3.3 自律走行の試行
まず[[3]](#参考記事やサイト)の記事の手順で「omniverse://localhost/NVIDIA/Assets/Isaac/2022.2.1/Isaac/Samples/ROS2/Scenario/carter_warehouse_apriltags_worker.usd」をIsaac Simで読み込ました。

そして別ターミナルを開き以下のコマンド実行しました。
```:terminal
cd ~/ros2_isaacsim_test
source ~/ros2_isaacsim_test/install/setup.bash
ros2 launch carter_navigation carter_navigation.launch.py
```

そうしましたところ、以下の図のようにrviz2が起動し、ロボット走行用地図が読み込まれるとともにCarterの現在地と障害物の認識状況が表示されました。ロボット走行用地図はコピーしてきたパッケージの中にありましたので、今回は作成する必要はありません。
![Screenshot from 2024-05-26 08-43-57.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/6e98c2fc-f25a-1214-bb35-a6284ea4406f.png)

"2D Pose Estimate"ボタンでロボットの位置の修正もできるのですが、今回は必要ありませんでしたので"Navigation2 Goal"でゴール地点を選択して自律走行させた動画が以下になります。なお、左の画面がIsaac Simの様子で右側の画面がrviz2の様子です。

<iframe width="560" height="315" src="https://www.youtube.com/embed/4W3PIAQpTSk?si=FcI0fsrhNP4MBFfe" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" allowfullscreen></iframe>

# 4. 自分で作成した2D Mapを使った自律走行
## 4.1 ロボット走行用地図の作成
[IssacSimでロボット走行用の2D地図を作ってみる](https://qiita.com/bebebefu/items/d566b98f75af41352b20)[[5]](#参考記事やサイト)の記事で紹介した手順でロボット走行用の地図を以下のように作成しました。先ほど試行したパッケージに入っている地図よりも少し高い平面を抽出しましたので、段ボールが積まれている箇所が若干異なっています。
　なお、この地図は"VISUALIZE MAP"のボタンで表示されるダイアログで操作して"carter_warehouse_navigation_test.png"として保存します。また"VISUALIZE MAP"のボタンで表示されるダイアログではこの地図の情報を見ることができますが"Bottom Left: (-11.975, -17.975)"という情報が得られました、これは後述する原点合わせの時に用いますのでどこかにメモしておきました。

![carter_warehouse_navigation_test.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/3ba5e3a9-e0a9-bd31-32ea-2d4e13faafc3.png)

## 4.2 yamlファイルとlaunchファイルの作成
次に先ほど作成したワークスペースの"/ros2_isaacsim_test/src/navigation/carter_navigation/maps/"の中に先ほど保存した"carter_warehouse_navigation_test.png"をコピーして貼り付けます。次にcarter_warehouse_navigation.yaml
をコピーしてcarter_warehouse_navigation_test.yamlとして保存した上で内容を以下のように修正しました。

```yaml:carter_warehouse_navigation_test.yaml
image: carter_warehouse_navigation_test.png
resolution: 0.05
origin: [-11.975, -17.975, 0.0000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
```

"image"の項目を先ほど作成したpngファイルの名前にしました。そしてoriginのところを先ほど地図の情報としてメモした"Bottom Left: (-11.975, -17.975)"に合わせる必要がありましたが、基となったcarter_warehouse_navigation.yamlもoriginが [-11.975, -17.975, 0.0000]でしたので修正する必要はありませんでした。

次に"/ros2_isaacsim_test/src/navigation/carter_navigation/launch/"の中にあるcarter_navigation.launch.pyをコピーしてcarter_navigation_test.launch.pyとして保存した上で内容を以下のように修正しました。

```python:carter_navigation_test.launch.py
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():

    use_sim_time = LaunchConfiguration("use_sim_time", default="True")

    map_dir = LaunchConfiguration(
        "map",
        default=os.path.join(
            get_package_share_directory("carter_navigation"), "maps", "carter_warehouse_navigation_test.yaml"
        ),
    )

    param_dir = LaunchConfiguration(
        "params_file",
        default=os.path.join(
            get_package_share_directory("carter_navigation"), "params", "carter_navigation_params.yaml"
        ),
    )

    nav2_bringup_launch_dir = os.path.join(get_package_share_directory("nav2_bringup"), "launch")

    rviz_config_dir = os.path.join(get_package_share_directory("carter_navigation"), "rviz2", "carter_navigation.rviz")

    return LaunchDescription(
        [
            DeclareLaunchArgument("map", default_value=map_dir, description="Full path to map file to load"),
            DeclareLaunchArgument(
                "params_file", default_value=param_dir, description="Full path to param file to load"
            ),
            DeclareLaunchArgument(
                "use_sim_time", default_value="true", description="Use simulation (Omniverse Isaac Sim) clock if true"
            ),
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(os.path.join(nav2_bringup_launch_dir, "rviz_launch.py")),
                launch_arguments={"namespace": "", "use_namespace": "False", "rviz_config": rviz_config_dir}.items(),
            ),
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource([nav2_bringup_launch_dir, "/bringup_launch.py"]),
                launch_arguments={"map": map_dir, "use_sim_time": use_sim_time, "params_file": param_dir}.items(),
            ),
        ]
    )
```

修正したところは読み込むyamlファイルをcarter_warehouse_navigation.yamlからcarter_warehouse_navigation_test.yamlに修正しただけです。

## 4.3 ロボットの自律走行
3.3節で述べた手順と同じように、[[3]](#参考記事やサイト)の記事の手順で「omniverse://localhost/NVIDIA/Assets/Isaac/2022.2.1/Isaac/Samples/ROS2/Scenario/carter_warehouse_apriltags_worker.usd」をIsaac Simで読み込ました。

そして別ターミナルを開き以下のコマンド実行しました。
```:terminal
cd ~/ros2_isaacsim_test
source ~/ros2_isaacsim_test/install/setup.bash
ros2 launch carter_navigation carter_navigation_test.launch.py
```

そうしましたところ、rviz2が起動し、ロボット走行用地図が読み込まれるとともにCarterの現在地と障害物の認識状況が表示されました。また"Navigation2 Goal"でゴール地点を選択して自律走行させた様子が以下になります。なお、左の画面がIsaac Simの様子で右側の画面がrviz2の様子です。
![Screenshot from 2024-05-26 09-43-20.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/340fbb4d-9ffb-82fb-fdca-1c3a8f20f591.png)

![Screenshot from 2024-05-26 09-43-46.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/c273020d-8444-1d17-e76a-6766acf59edf.png)

また"Navigation2 Goal"でゴール地点を選択して自律走行させた動画は以下になります。なお、左の画面がIsaac Simの様子で右の画面がrviz2の様子です。

<iframe width="560" height="315" src="https://www.youtube.com/embed/rDriiUQfCR8?si=FcI0fsrhNP4MBFfe" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" allowfullscreen></iframe>

# 5. まとめ
[[1]](#参考記事やサイト)の記事の手順を参考に、私の環境でもロボット走行用の2D地図を作成し、その地図を使用してROS2のNavigationによりロボットモデル(NVIDIA Carter)を自律走行させられることを確認できました。今後は自分の環境でも以下のことが可能なのか試していきたいと考えています。

- もう少し複雑な環境でも読み込んで自律走行させることができるのか
- VISUAL SLAMを試せるのか
- 他のロボットモデルを読み込み、そのロボットモデルで自律走行や移動ができるのか

# 参考記事やサイト
[1][ROS2 Navigation（Isaac SIM）チュートリアル実行](https://qiita.com/Manyan3/items/d1c3f311b57109772470)
[2][ubuntuにCUDA、nvidiaドライバをインストールするメモ](https://qiita.com/porizou1/items/74d8264d6381ee2941bd)
[3][Ubunutu22.04の環境下においてIsaac Sim+ROS2を試そうとしてハマったところ](https://qiita.com/bebebefu/items/a89694659e650665e5db)
[4][ROS2 Humble Navigation２ 設定メモ](https://qiita.com/Manyan3/items/e13a19d14a25cdc16425)
[5][IssacSimでロボット走行用の2D地図を作ってみる](https://qiita.com/bebebefu/items/d566b98f75af41352b20)