IsaacSimでLIMO（無人走行UGV）にカメラを追加してみた

# 1. はじめに
[IsaacSimでLIMO（無人走行UGV）をROS2で動かす](https://qiita.com/bebebefu/items/febee72d7cf60ffd08de)の記事[[1]](#参考記事やサイト)ではROSトピックを投げてLIMOを動かすことに成功しました。ただこれは単純にROS TOPICを投げて動かしているだけなので、つまらななと感じていました。そこでカメラを追加したり、Lidarを追加して自律走行したいなどと考えておりましたが、Isaac Sim向けLIMOモデルを公開しているgithubのリポジトリ[[2]](#参考記事やサイト)ではカメラを追加するデモがあるということが分かりました。

👇Isaac Sim向けLIMOモデルを公開しているgithubのリポジトリでのカメラを追加するデモのリンク

https://github.com/agilexrobotics/Limo-Isaac-Sim/blob/master/docs/camare2ros.mp4

なお、上記のIsaac Sim向けLIMOモデルを公開しているgithubのリポジトリではROS1のみの記載となっております。本記事ではそれを参考にROS2に対応させてカメラを追加しましたので、その手順も本記事で紹介したいと思います。

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

# 3. 手順
基本的には[[2]](#参考記事やサイト)で紹介されているカメラ追加のデモ動画の内容を追ってきます。なお、Isaac SimやROS2の起動については、[Ubunutu22.04の環境下においてIsaac Sim+ROS2を試そうとしてハマったところ](https://qiita.com/bebebefu/items/a89694659e650665e5db)で記載してますので、この部分については本記事では飛ばします。

## 3.1 LIMOモデルの読み込み
まず[[1]](#参考記事やサイト)で作成したLIMOのモデルを読み込みます。ROS2で動かせるようにしたAction Graph
などの設定はそのまま維持しています。以下が、モデルを読み込んだ際の様子です。
![モデル読み込み.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/9e77cddb-d2e1-6b6a-2196-c26ba192cc36.png)

## 3.2 カメラの追加
以下の図で赤枠で囲んだ"limo_xacro"をクリックします。
![手順１.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/4cc8da86-bebc-398e-a4b7-49712346bbb4.png)

次に"limo_xacro"をクリックした際に表示される"base_link"（以下の図で赤枠で囲んだところ、ちょっと隠れています）のところを右クリックすると以下の図で黄枠で囲んだウィンドウが出ますので、そこで"Create"（以下の図で黄緑枠で囲んだところ）をクリックします。そうするとまた別のウィンドウが出てきますので、そのウィンドウにある"Xform"（以下の図で青枠で囲んだところ）をクリックします。

![手順2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/57d17275-813d-0caa-9ef6-e90045523aa4.png)

"Xform"を押して追加されたオブジェクトの名前を以下の図で赤枠で囲んだところのように"cam_link"という名前にします。
![手順3.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/3ca661ce-d792-3a5f-0510-d77ff9f9408a.png)

そして"cam_link"のところを右クリックするとまた以下の図で黄色枠で囲んだウィンドウがでますので、そこで"Create"（以下の図で黄緑枠で囲んだところ）をクリックします。そうするとまた別のウィンドウが出てきますので、そのウィンドウにある"camera"（以下の図で青枠で囲んだところ）をクリックします。
![手順4.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/57d09c8e-538d-2d77-5e08-b482fccd7378.png)

そうすると以下の図で赤枠で囲んだところのように"cam_link"の直下に"Camera"オブジェクトが追加されます。またLIMOのモデルの方を見るとカメラの画角を表す線が出ていることが確認できます。
![手順5.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/f31f7268-9b06-2585-1a5c-eecc86bf7598.png)

## 3.3 カメラの調整
先ほど追加したカメラはLIMOの進行方向の真後ろを向いていますので、以下の図で赤枠で囲んだ"Transform"の"Orient"の"Y"の値のところを-90にします。（なお、後述しますが、これだけだとカメラ位置の設定は不十分であったことが後々わかります。）

![手順6.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/a62b3e80-de08-91bd-527a-a39bb477f046.png)

次にカメラのレンズの調整をするため、以下の図で赤枠で囲んだ"Lens"の"Focal Length"は20.0、"Focus Distance"は50.0に設定します。この値は[[2]](#参考記事やサイト)のデモで設定されている値をそのまま適用しました。
![手順7.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/272a8b50-dad2-da63-e29f-7dc62f2d7bb0.png)

## 3.4 Action Graphの構築
まず[[1]](#参考記事やサイト)で構築したAction Graphを以下の図のように読み込みます。この記事ではこのAction Graphに追加していく形にしてきます。

![手順8.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/2112df2f-1147-32cd-2af6-c8a85460e978.png)

以下の図で赤枠で囲んだところのように"Isaac Create Viewport"、"Isaac Get Viewport Render Product"、"Isaac Set Camera"、"ROS2 Camera Helper"のオブジェクトをAction Graphに追加し、以下の図のようにそれぞれのオブジェクトをつなげます。（以下の図だと"On Playback Tick"と"ROS2 Camera Helper"がつながっているように見えますが、実際はつながっていません）
![手順9.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/0a843f96-8529-21b4-339a-08cf9aa36a8d.png)

次に"ROS2 Camera Helper"のオブジェクトをクリックし、"frameId"を"map"とし、また"topic Name"のところを"/image_raw"としました（以下の図参照）。この設定は[[2]](#参考記事やサイト)のデモでの設定をそのまま適用しています。
![手順10.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/07888671-0eb9-acb4-9bfe-11a303deb863.png)

次に"Isaac Create Viewport"をクリックし、"Inputs"の"name"のところを"limo_cam"にします。
![手順11.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/63a76005-e6e6-2ee9-07e4-4496218fb72e.png)

次に"Isaac Set Camera"のオブジェクトをクリックし、以下の図で赤枠で囲んだ"Inputs"の"cameraPrim"の"Add Target"をクリックすると以下の図で黄枠で囲んだウィンドウが表示されます。そのウィンドウで緑枠で囲んだ"Camera"（3.2節で追加したカメラ）をクリックして選択した後に青枠で囲んだ"Select"をクリックして"cameraPrim"のところに上記の"Camera"を追加します。
![手順12.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/4873a44d-243d-7503-3afd-983634c55425.png)

## 3.5 カメラの再調整
3.4節までの設定を終え、Isaac Simの▶ボタンを押してシミュレーションを開始したときにLIMOに追加したカメラに映った映像が以下になります。

![手順13.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/340e5216-8a24-00b7-cba5-9bab910cd193.png)

これは何が映っているかというとLIMOの内部の様子が映っています。カメラの方向は調整しましたが、位置を調整していなかったのでカメラがLIMOの内部に埋まっている状態であることがこれで分かりました。

そのため3.3節で設定した"Transform"の"Orient"の上にある"Translate"の"X"を0.1、"Z"を0.05として、LIMOのカメラが実際にありそうなところに位置を調整しました。
![手順14.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/46f3408b-64f3-3cd5-409b-092b1ed6749a.png)

# 4. シミュレーション結果
シミュレーションでは、LIMOを静止させた状態で、壁を前後および上下方向に移動させました。以下の動画は、その際のシミュレーション結果です。LIMOに搭載したカメラの映像から、壁がLIMOに対して相対的に前後上下に動いている様子が確認できました。
<iframe width="560" height="315" src="https://www.youtube.com/embed/fOLCY0miwtc?si=FcI0fsrhNP4MBFfe" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" allowfullscreen></iframe>

次に、シミュレーション上で走行経路の両側に垂直な壁を作成しました。その後、シミュレーションを開始し、LIMOに以下の前進のコマンドを送信しました。
```:terminal
source /opt/ros/humble/setup.bash
ros2 topic pub /cmd_vel geometry_msgs/Twist '{linear:  {x: 0.2, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0,z: 0.0}}'
```
以下の動画は、その際のシミュレーション結果です。LIMOに搭載したカメラの映像から、LIMOが切り立った壁の間を走行している様子が確認できました。

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


# 5. まとめ
[[2]](#参考記事やサイト)の記事の手順を参考に、私の環境でもLIMOのロボットモデルにカメラを追加し、そのカメラで周辺環境を確認することができました。この件は今後、Visual SLAMの試行につなげていきたいと考えています。ただ、まだLidarを用いた2D SLAMすら実現できていないため、まずはそこに取り組みたいと思っています。そのため次回はIsaac Sim上でLIMOにLidarを追加し、周辺環境の情報を取得できるかを試行する予定です。

# 参考記事やサイト
[1][IsaacSimでLIMO（無人走行UGV）をROS2で動かす](https://qiita.com/bebebefu/items/febee72d7cf60ffd08de)
[2][agilexrobotics/Limo-Isaac-Sim](https://github.com/agilexrobotics/Limo-Isaac-Sim)