IsaacSimでLIMO（無人走行UGV）でROS2 Navigationを試してみる

# 1. はじめに
[IsaacSimでLIMO（無人走行UGV）にLidarを追加してみた](https://qiita.com/bebebefu/items/cb2e0a8461f3412b7ca5)[[1]](#参考記事やサイト)ではLIMOのロボットモデルにLidarを追加し、そのLidarで周辺環境を確認することに成功しました。今回はこのLidarから取得した情報を使いつつ2D Mapを使った自律走行を試してみたいと思います。

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

# 3. 手順
[IsaacSimのROS2 Navigationを試してみる](https://qiita.com/bebebefu/items/d2437abba62493daf08e)[[2]](#参考記事やサイト)で自律走行をしたNVIDIA Carterをコピーしてきてその中のAction GraphをLIMO用に書き換えるという手順をとりました。

以下で紹介する手順はかなり我流のやり方ですので、もっとうまくやる方法はいくらでもあると思います。ただ、躓いてる部分も含めて記録・公開するのも大事かと思いますので、記載していきます。

## 3.1 NVIDIA Carterのコピー
LIMOにカメラを追加したりLidarを追加したりする作業を省略するため[IsaacSimでLIMO（無人走行UGV）にLidarを追加してみた](https://qiita.com/bebebefu/items/cb2e0a8461f3412b7ca5)[[1]](#参考記事やサイト)の記事で作成した、環境から壁などの余計なものを消した後に、limo_ROS_sample_v〇.usdといった適当な名前を付けて保存します（私はlocalhost>User>test/ROS2/Robotの中に保存しました）。そしてその環境にomniverse://localhost/NVIDIA/Assets/Isaac/2022.2.1/Isaac/Samples/ROS2/Robotの中にあるCarter_ROS.usdを以下の図で赤枠と赤線で示したようにドラッグアンドドロップしてきます。

![Carter_ROSの読み込み.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/af718efb-750c-4eaf-3066-8a6574f25465.png)

この状態だと、Carter_ROSとLIMOのモデルが併存している状態になります。

## 3.2 Action GraphとCamera Action Graphのコピー
Carter_ROSの中にあるAction GraphとCamera Action Graphを右クリックすると表示されるウィンドウに"duplicate"という項目が選択できますので、それをクリックします（以下の図の赤枠部分）。

![Screenshot from 2024-10-05 10-38-15.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/aff1984a-2428-0357-3ca7-02495c60e619.png)

そうすると以下の図のように"Action Graph_01"という項目が作成されます（以下の図の赤枠部分）。内容はコピーしたものなので"Action Graph"と同じです。

![Screenshot from 2024-10-05 10-38-59.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/427c1cc2-6996-6014-9035-14a4f04c3981.png)

次に"Action Graph_01"をLIMOモデルの直下にドラッグアンドドロップしてきます（以下の図の赤枠と赤線部分）。

![action_graphドラッグアンドドロップ.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/47cbca20-37a5-9999-9482-84aacdfc5f77.png)

この作業をCarter_ROSの中にある"ROS_Cameras"のAction Graphにも同じように行い、LIMOのモデルにコピーしてきます。そしてコピーしたらCarter_ROSは削除します。

## 3.3 Action Graphの設定
Carter_ROSとLIMOのモデルの構成は似てはいるのですが、完全には一緒ではないので、組み換えが必要です。以下ではAction Graphの各モジュールのどこをLIMOのモデルに合わせてどのように変えたかを説明します。

### 3.3.1 Isaac Read Lidar Beams Nodeの設定
[IsaacSimでLIMO（無人走行UGV）にLidarを追加してみた](https://qiita.com/bebebefu/items/cb2e0a8461f3412b7ca5)[[1]](#参考記事やサイト)の記事で設定したLidarの名前を"carter_lidar"に変えた上で（これを怠るとlidarのデータが取れませんでした。）、以下の図の"lidar_prim"フォルダマーク（赤枠部分）をクリックすると、ウィンドウ（黄枠部分）が表示されますので、先ほど名前を変えた"carter_lidar"（青枠部分）をクリックして"Select"（緑枠部分）を押して設定します。

![lidarの設定.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/219dcd05-6111-2f2a-2f1e-c0b0e52e1a0d.png)

### 3.3.2 ROS2 Publish Laser Scanの設定
ここは特段修正する必要はありませんが、以下の図で赤枠で囲んだ"frameId"が"carter_lidar"になっているか確認します。

![Screenshot from 2024-09-29 09-12-20.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/bf2ff153-3b75-8cca-bca2-6d7c9e0cbabc.png)

### 3.3.3 ROS2 Publish Raw Transform Treeの設定
ROS2 Publish Raw Transform Treeのオブジェクトは2つありますので、1つは以下の図のようにparentPrim(赤枠部分)とtargetPrims（青枠部分）のどちらにもbase_linkを読み込んでみます。
![base_linkのtree読み込み.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/3ab0a57e-e86f-5120-4a2c-fa62f19f6cd5.png)

もう1つは以下の図のようにparentPrim(赤枠部分)にはbase_linkを読み込み、targetPrims（青枠部分）にはLidarとCameraを読み込みます。
![lidarのtree読み込み.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/a678aa51-cfca-3cfe-4988-a55c4fcc46ce.png)

### 3.3.4 Isaac Compute Odometry Nodeの設定
Carter_ROSのchasis_linkが読み込まれていたところをbase_linkを読み込むようにします（以下の赤枠部分）。
![Isaac Compute Odometry Nodeの設定.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/c141a189-34c9-55c6-298a-77ec98d9f1b6.png)

### 3.3.5 Differential ControllerとArticulation Controllerなどの設定
ここの設定はコピーしてきたROS_carterの設定を流用はできませんでしたので、[IsaacSimでLIMO（無人走行UGV）をROS2で動かす](https://qiita.com/bebebefu/items/febee72d7cf60ffd08de)[[4]](#参考記事やサイト)の記事の3.5.2項～3.5.5項の内容を反映しました。

### 3.3.6 ROS2 Publish Odometryの設定
これは修正を加えませんでした。

### 3.4 ROS_Cameras(Action Graph)の設定
Carter_ROSには2つカメラがあり、それを使ってVisual Slamなどができるのですが、今回のLIMOのモデルにはCameraが1個だけですので、2つあるIsaac Set Cameraには、以下の図のようにcam_linkの直下にあるCameraをInputs>CameraPrimのところに設定します（以下の図の赤枠部分）。
![camera_left.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/823d36a7-1147-6c72-f707-1afa20bcd23d.png)
![camera_right.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/1c2f431a-35e0-4516-9704-d7df53c48daa.png)

# 4. 地図作成などのシミュレーションの準備
[IsaacSimでLIMO（無人走行UGV）にLidarを追加してみた](https://qiita.com/bebebefu/items/cb2e0a8461f3412b7ca5)[[1]](#参考記事やサイト)の記事で作成した環境からロボットを削除したusdファイルを一旦作成します。その中に前章で作成したLIMOのモデルをドラッグアンドドロップで読み込みます。
![limo読み込み.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/2661492b-e949-913a-62bb-5ead51db8ff5.png)

以下が読み込み後の様子です。

![limo読み込み後.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/ed053224-d526-672a-a85d-551d37903fc1.png)

次に地図の作成ですが、[IsaacSimでロボット走行用の2D地図を作ってみる](https://qiita.com/bebebefu/items/d566b98f75af41352b20)[[5]](#参考記事やサイト)の記事の方法をそのまま実行し、
地図はlimo_wall_test.pngとして保存しました。壁が両側にあるだけなので、地図は＝（イコール）みたいな形になっています。

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/4bbf1ded-d172-5deb-dcb0-15787737fcf6.png" width="80%">

そして[IsaacSimでLIMO（無人走行UGV）にLidarを追加してみた](https://qiita.com/bebebefu/items/cb2e0a8461f3412b7ca5)[[1]](#参考記事やサイト)の記事で作成した/ros2_isaacsim_test/src/carter_navigation の中にあるcarter_warehouse_navigation.yamlをコピーしてlimo_wall_test.yamlとして保存した上で内容を以下のように修正しました。

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

また[IsaacSimでLIMO（無人走行UGV）にLidarを追加してみた](https://qiita.com/bebebefu/items/cb2e0a8461f3412b7ca5)[[1]](#参考記事やサイト)の記事で作成した/ros2_isaacsim_test/src/carter_navigation の中にあるcarter_navigation.launch.pyをlimo_test.launch.pyとしてコピーして、以下のようにしました。修正した部分は"get_package_share_directory("carter_navigation"), "maps", "limo_wall_test.yaml""という部分で読み込むyamlファイルを先ほど保存したyamlファイルにしています。

```python:limo_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", "limo_wall_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(),
            ),
        ]
    )

```

# 5. シミュレーション結果
4章で地図を作成した環境にて▶ボタンを押してシミュレーションを始めます。

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

そうするとrivz2が開き、ちょっと時間はかかりますが、ロボットの状態が読み込まれます。

![rivz起動.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/0af7a71d-5741-e0c3-d31a-95607f4b3a6d.png)

👆の状態だと自己位置（上図で緑の丸の部分）がずれていますので、2D pose estimate（下図の赤枠部分）を使って現在地を修正して以下のようにします。

![自己位置修正.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/7d784d46-d615-7ffd-7ad7-6838463ba02d.png)

そしてrviz2上のNav2 goalで行きたいところを指定するとその地点にLIMOが自律走行して移動をしていきますが、これらの一連の動きの動画が以下です。

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

# 6. まとめ
上記の手順でLIMOのロボットモデルを読み込み、そのロボットモデルで自律走行や移動ができました。今後は四足歩行ロボットなどのロボットでも自律走行ができるかやViual SLAMなどを試していきたいと思います。

# 参考記事やサイト
[1][IsaacSimでLIMO（無人走行UGV）にLidarを追加してみた](https://qiita.com/bebebefu/items/cb2e0a8461f3412b7ca5)
[2][IsaacSimのROS2 Navigationを試してみる](https://qiita.com/bebebefu/items/d2437abba62493daf08e)
[3][agilexrobotics/Limo-Isaac-Sim](https://github.com/agilexrobotics/Limo-Isaac-Sim)
[4][IsaacSimでLIMO（無人走行UGV）をROS2で動かす](https://qiita.com/bebebefu/items/febee72d7cf60ffd08de)
[5][IsaacSimでロボット走行用の2D地図を作ってみる](https://qiita.com/bebebefu/items/d566b98f75af41352b20)