18000km離れたサンパウロのGCP上で動いているUnity HDRP/ROS2シミュレータのカメラをAWS Kinesis Video Streams WebRTC/DataChannelを使って遠隔操縦してみた

# はじめに

この記事は、[2021年ROSAdvent Calendar](https://qiita.com/advent-calendar/2021/ros)の24日目の記事です。
投稿が遅くなりすみません。

ROS2、遠隔操縦、Unityでのシミュレーションが流行ってきているように感じたので、勉強のためにタイトルのようなものを作って見ました。
下記のような内容に興味がある方の参考になれば幸いです。

* UnityのHDRPでROS2のシミュレータ環境構築([Ros2-For-Unity](https://github.com/RobotecAI/ros2-for-unity))
* GCP/DockerでのHeadlessなHDRP Unityアプリケーションの実行
* AWS Kinesis Video Streamsを使ったWebRTC C/JSのクライアントの作成

# 実行の様子

実際に遠隔操縦している様子が下記になります。
UnityのHDRP環境はサンプルで用意されたものをそのまま利用していますが、非常に綺麗で動かしていて楽しいです。サンパウロは東京から18000km離れていますが、相手がクラウド上なので、比較的安定的に遠隔操縦できました(解像度は640x480)。ただシミュレータなので、サンパウロで実行されていようが、あまり分からないのが残念です。
ROS2で構築しており、カメラトピックが出力できて、`cmd_vel` を受け付けるロボットだとロボットの種類を問わず操作できます。
<iframe width="560" height="315" src="https://www.youtube.com/embed/S-1238wVFfM" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>

動かすために必要なコードはこちらです。
https://github.com/otamachan/ros2-unity-kvm-webrtc
構築方法は、最後に記載したので興味のある方は見てみてください(すみません、徐々に追記します)。

# 構成

以下のような構成になっています。
<img width="70%" alt="system.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/15624/2aa36b47-fa87-6d54-9210-255d5ce44f0a.png">

通信の流れはおおよそ以下のようになっています。

まずは、カメラ画像です。
Unityシミュレータではロボットに配置されたカメラ画像を、定期的にキャプチャし、 `Ros2 For Unity`を使用して、`sensor_msgs/Image` として非圧縮でROS2のトピックとして出力します。
別のノードで、その画像を受信し、NVENCを利用しh264にエンコードした後、[Amazon Kinesis Video Streams C WebRTC SDK](https://github.com/otamachan/ros2-unity-kvm-webrtc) を使い作成されたWebRTCのPeerConnectionに流し込みます。
ブラウザ側では、同様に[Amazon Kinesis Video Streams WebRTC SDK for JavaScript](https://github.com/awslabs/amazon-kinesis-video-streams-webrtc-sdk-js)を使って作成したPeerConnection経由で画像を受信し、Videoとして表示しています。必要に応じて、TURNが利用されるため、P2Pで接続できない場合でも、転送できます。

操縦側です。
Unityシミュレータは、`Ros2 For Unity`を利用して、`geometry_msgs/Twist` をSubscribeして、速度に応じて移動できるようしておきます。
ブラウザ側では、仮想のJoystickで操作量を作成し、カメラ画像で使ったPeerConnectionにはったDataChannelにJSON形式で送信します。画像を配信するノードが同様に、DataChannel経由でJSONを受け取りそれをROS2の`geometry_msgs/Twist`として発行しなおすことで、ブラウザ側から操作できるようにしています。

UnityシミュレータをGCPで実行するには、Nvidiaドライバが有効になったxserverを起動する必要があります。今回は、xserverをDocker内に起動することで、ホストOSにxserverを起動する必要なく、UnityシミュレータをDockerで実行させています。この方法は、ホストOSにxserverを上げられないk8s等の環境でも、Unityのアプリケーション等xserverが必要なアプリケーションが利用できるようになるので、色々と便利です。
一方、コンテナ内にホストのNvidiaドライバとまったく同じバージョンのNvidiaドライバ(のユーザランドライブラリ)をインストールする必要があり、そこは注意が必要です。

# 終わりに

組み合わせるだけかと思いきや色々大変でした。特にUnityは慣れていない世界だったので、C#の書き方から始まって苦労しました。
それでは2022年もLet's enjoy robot programming!

ついでに。
**[PreferredRobotics社](https://www.pfrobotics.jp/)では、ROS/ROS2/AWS/GCPに興味があり、ロボットの製品化を一緒に推進していただける仲間を積極的に[募集](https://www.pfrobotics.jp/#careers)しております。**
興味のある方は是非話を聞きに来てください！

# 構築手順
## 構築環境

実行環境は、下記を想定しています。

* Ubuntu 20.04
* CUDA 10.2
* ROS2 Foxy
* Unity 2020.03(LTS)

## 必要なコードの取得

https://github.com/otamachan/ros2-unity-kvm-webrtc

GitHubから必要なコードをクローンしておきます。

```bash
git clone https://github.com/otamachan/ros2-unity-kvm-webrt.git
```

## UnityでHDRPサンプルプロジェクトの作成とビルド

HDRPをするために、Vulkan環境も構築しておきます。2020では、Vulkan環境がなくてもHDRPが実行できる気がしますが、2019では必要だったので念の為にいれておきます。

```bash
sudo apt install vulkan-utils
```

UnityHubの右上「新しいプロジェクト」をクリックし、HDRPのサンプルである、`3D Sample Scene(HDRP)` を選び、保存場所、プロジェクト名を適当につけて作成します。

<img width="50%" alt="unity2.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/15624/ce92e9da-837c-7c4c-9dcb-9fed0d9c6b76.png">

メニューの「File」→「Build and Run」からビルドして実行できることを確認してください。実行すると以下のような画面がでて、キーボードやマウスで視点の移動ができるようになります。
ビルドにはかなり時間を要するので、気長にまちます。

<img width="50%" alt="unity3.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/15624/ad3e72ef-46e2-c1f9-75ba-637fc3104a00.png">


## Ros2ForUnityのビルド環境準備

今回、ROS2とUnityの通信として、[Ros2 For Unity](https://github.com/RobotecAI/ros2-for-unity) を使用しました。Unityの公式は、[ROS TCP Connector](https://github.com/Unity-Technologies/ROS-TCP-Connector)を使う方法ですが、今回画像トピックを扱うため、容量やレイテンシを考慮し、直接ROS2のトピックが発行できるRos2 For Unityを使いました。

1. DotNetのインストール

    https://docs.microsoft.com/ja-jp/dotnet/core/install/linux-ubuntu#2004- に従ってdotnetのSDKをインストールします。

2. 各種ROS2のビルドツールのインストール

    また、Ros2ForUnityのビルドに必要なビルドツールをインストールしておきます。

     ```bash
     sudo apt install python3-vcstool python3-colcon-common-extensions
     ```

## Ros2 For Unityのビルド

https://github.com/RobotecAI/ros2-for-unity をクローンしビルドします。
以下の手順でビルドします。基本的には、[README-UBUNTU.md](https://github.com/RobotecAI/ros2-for-unity/blob/master/README-UBUNTU.md)の通りです。
`create_unity_package.sh`はUnityEditorがインストールされているパスを指定する必要があるので、注意してください。

```
source /opt/ros/foxy/setup.bash
./pull_repositories.sh
./build.sh
source install/setup.bash  # 重要
./create_unity_package.sh -u ~/<path to unity editors>/2020.3.25f1/Editor/Unity
```

ビルド後、先程のサンプルUnityプロジェクトで
メニュー「Assets」→「Import Package」→「Custom Package...」から、`ros2-for-unity/install/unity_package/Ros2ForUnity.unitypackage` を選択し、ビルドしたパッケージを取り込みます。
これもhttps://github.com/RobotecAI/ros2-for-unity#usage に書かれた手順通りです。

## Unityの再起動

`Ros2 For Unity` は、ROS2の実行環境に依存するため、ROS2の環境変数が設定された状態で、Unityを実行する必要があります。ターミナルから、以下のように、`/opt/ros/foxy/setup.bash` を読み込みUnityを再起動しておきます。UnityHubが常駐していた場合は、再起動前に一度終了させておいてください。

```bash
source /opt/ros/foxy/setup.bash
~/<path to unity editors>/2020.3.25f1/Editor/Unity
```

## 必要なスクリプトの配置

一番最初にクローンしておいた https://github.com/otamachan/ros2-unity-kvm-webrtc にある、[UnitySampleRobot](https://github.com/otamachan/ros2-unity-kvm-webrtc/tree/main/UnitySampleRobot) を、UnityプロジェクトのAssets以下にコピーします。

<img width="25%" alt="unity-folder.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/15624/a5a56a06-1414-0683-4e20-086c0228cc5e.png">

## カメラを配置し、ROS2の`geometry_msgs/Twist`型のトピックから動かせるようにする

1. シーンにカメラを配置します
1. 実行時に警告がでるので、「Audio Listener」のチェックをはずしておきます
1. 「Add Component」で「ROS2 Unity Component」を追加します

<img width="50%" alt="unity-camera.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/15624/63e1b5d4-81d5-b756-6e52-184de42958e0.png">

さらに「Add Component」で「RobotController」を追加します。これで`cmd_vel`でカメラを移動できるようになります。

<img width="25%" alt="unity-camera2.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/15624/25a997a0-1736-3ea1-b18c-3fd5d8641db4.png">


`cmd_vel` トピックでカメラが移動できるか、確認します。
Unityでアプリケーションを実行したあと、ターミナルで以下を実行しカメラがその場回転することを確認してください。

```bash
source /opt/ros/foxy/setup.bash
ros2 topic pub /cmd_vel geometry_msgs/Twist '{linear: {x: 0.0}, angular: {z: 0.3}}'
```

`RobotController`のコードはこちらです。
https://github.com/otamachan/ros2-unity-kvm-webrtc/blob/main/UnitySampleRobot/Assets/Scripts/UnitySampleRobot/RobotController.cs

## カメラの画像をROS2の`sensor_msgs/Image`型のトピックとして発行できるようにする

HDRPで、レンダリング結果を取得したり、加工する場合は、[CustomPass](https://docs.unity3d.com/ja/Packages/com.unity.render-pipelines.high-definition@10.5/manual/Custom-Pass.html)を利用し独自処理を書きます。今回は、`Blit` メソッドを使用しレンダリングテクスチャをY軸方向に反転して別のテクスチャにコピーします。別テクスチャにコピーしたあとは、`RequestAsyncReadback`を利用し、GPUからCPUメモリに非同期でコピーします。

`CameraPublisher`のコードはこちらです。
https://github.com/otamachan/ros2-unity-kvm-webrtc/blob/main/UnitySampleRobot/Assets/Scripts/UnitySampleRobot/CameraPublisher.cs


カメラのアンチエイリアスを有効にしておきます。
<img width="25%" alt="unity-camera3.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/15624/8c607835-945d-162c-e7fe-69db83d00b5f.png">

`CustomPassVolume`を追加し、InjectionPointを`AfterPostProcess`にし、`CameraPublisher`をCustomPassとして追加します。これで、Cameraの画像がPublishされるようになります。

<img width="25%" alt="unity-camera4.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/15624/433e5749-cc12-9607-8b0e-d21581befaf7.png">

Unityでアプリケーションを実行したあと、Rvizでカメラ画像がPublishされていることを確認してください。

```bash
ros2 run rviz2 rviz2
```

<img width="50%" alt="unity-camera3.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/15624/a76183ce-9f35-457a-0c38-384054902570.png">

## シミュレータのビルド

最後に、シミュレータをビルドします。
メニューから「File」→「Build Settings...」でビルドメニューを開き、「Build」でビルドします。
ここまでで、Unityシミュレータの作成は完了です。

↓にビルド結果をおいておきます。
https://drive.google.com/drive/u/0/folders/1QrvWHnIiJ9i2hPu0spXhjiOHHhJ8Lhsy



## ROS2/WebRTCプロキシノードのビルド

次は、ROS2の画像をSubscribeし、[AWS Kinesis Video Streams WebRTC](https://docs.aws.amazon.com/ja_jp/kinesisvideostreams-webrtc-dg/latest/devguide/what-is-kvswebrtc.html)を使いWebRTCとして、転送するノードをビルドします。

処理の概要を説明します。
AWS Kinesis Video Streams WebRTC自体は、画像のエンコード処理が入っていないため、[NVIDIA Video Codec SDK](https://developer.nvidia.com/nvidia-video-codec-sdk)を使い、h264にエンコーディングを行いました。エンコードAPIの呼び出しに関しては、Nvidiaが提供しているサンプル実装(https://github.com/NVIDIA/video-sdk-samples )をそのまま呼び出しています。
AWS Kinesis Video Streams WebRTCに関しても、提供されている[Amazon Kinesis Video Streams C WebRTC SDK](https://github.com/awslabs/amazon-kinesis-video-streams-webrtc-sdk-c)のサンプルである、`kvsWebRTCClientMaster`をほぼ再利用して、実装しています。

ポイントとしては、

* NVENCは、RGB系の入力として `NV_ENC_BUFFER_FORMAT_ARGB` の4byteフォーマットが必要であるため、Subscribeした画像はそのままエンコードできず、一度コピーを実施しています

* NVENCは、デフォルトでIDRフレームを生成しない設定になっていたので、適当なフレームで生成するような設定にしています(gopLength, idrPeriod, repeatSPSPPS)。これを設定しないと、途中から再接続した場合に、動画がデコードできなくなります。Amazon Kinesis Video Streams C WebRTC SDKは、FIR/PLIのコールバックが記述できるようになっているので、本来はこのコールバックでIDRフレーム等を生成させればよいのですが、今回はシンプルにするために、定期的に生成するだけにしています。

`ros2_kvm_webrtc_sample` のコードはこちらです。複数のサンプルを無理やりくっつけて実装しているので、きれいではないですが、500行程度で実装できました。
https://github.com/otamachan/ros2-unity-kvm-webrtc/blob/main/ros2_kvm_webrtc_sample/src/main.cpp

ビルド方法は、適当な場所にROS2のビルドワークスペースを作り、colconを使ってビルドします。
依存ライブラリでのビルド警告がでますが、無視します。

```bash
mkdir -p ws/src
cd ws/src
ln -s ../../ros2_kvm_webrtc_sample
source /opt/ros/foxy/setup.bash
colcon build
```

実行には、AWSのAWS Kinesis Video Streams WebRTC周りの権限が必要なので、必要な権限をもったIAMユーザを作成し、適当にキーを用意して実行してください。

  ```bash
export AWS_ACCESS_KEY_ID=XXXXXXXXXXXXXXXXXXXX
export AWS_SECRET_ACCESS_KEY=YYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYY
export AWS_DEFAULT_REGION=ap-northeast-1
source install/setup.bash
ros2 run ros2_kvm_webrtc_sample ros2_kvm_webrtc_sample_node
```

権限に問題がなければ、AWS Kinesis Video Streams WebRTCに「ScaryTestChannel」(SDKのサンプル通り)というチャネルが作られて、接続されます。

```bash
[KVS Master] Using trickleICE by default
[KVS Master] Created signaling channel ScaryTestChannel
[KVS Master] Finished setting audio and video handlers
[KVS Master] KVS WebRTC initialization completed successfully
[2021/12/31 12:08:10:9870] N: LWS: 4.2.1-v4.2.2, loglevel 7
[2021/12/31 12:08:10:9870] N: NET CLI H1 H2 WS ConMon IPv6-absent
[2021/12/31 12:08:10:9871] N:  ++ [wsi|0|pipe] (1)
[2021/12/31 12:08:10:9871] N:  ++ [vh|0|netlink] (1)
[2021/12/31 12:08:10:9871] N:  ++ [vh|1|default||-1] (2)
[KVS Master] Signaling client created successfully
[2021/12/31 12:08:10:9876] N:  ++ [wsicli|0|POST/h1/kinesisvideo.ap-northeast-1.amazonaws.com] (1)
[2021/12/31 12:08:11:0732] N:  -- [wsicli|0|POST/h1/kinesisvideo.ap-northeast-1.amazonaws.com] (0) 85.593ms
[2021/12/31 12:08:11:0735] N:  ++ [wsicli|1|POST/h1/kinesisvideo.ap-northeast-1.amazonaws.com] (1)
[2021/12/31 12:08:11:1478] N:  -- [wsicli|1|POST/h1/kinesisvideo.ap-northeast-1.amazonaws.com] (0) 74.288ms
[2021/12/31 12:08:11:1484] N:  ++ [wsicli|2|POST/h1/r-2c136a55.kinesisvideo.ap-northeast-1.amazo] (1)
[2021/12/31 12:08:11:3729] N:  -- [wsicli|2|POST/h1/r-2c136a55.kinesisvideo.ap-northeast-1.amazo] (0) 224.424ms
[2021/12/31 12:08:11:3735] N:  ++ [wsicli|3|WS/h1/m-26d02974.kinesisvideo.ap-northeast-1.amazona] (1)
[KVS Master] Signaling client connection to socket established
[KVS Master] Channel ScaryTestChannel set up done 
GPU in use: NVIDIA GeForce GTX 1080
```

## AWS Kinesis Video Streams WebRTCと接続確認

ROS2のトピックが、正しくPublishできているか確認します。
まず適当なUSBカメラから、`image_raw`トピックを発行します。

```bash
sudo apt install ros-foxy-v4l2-camera
source /opt/ros/foxy.setup.bash
ros2 run ros2 run v4l2_camera v4l2_camera_node
```

次に上述の通り、ROS2/WebRTCプロキシノードを起動します。

```bash
export AWS_ACCESS_KEY_ID=XXXXXXXXXXXXXXXXXXXX
export AWS_SECRET_ACCESS_KEY=YYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYY
export AWS_DEFAULT_REGION=ap-northeast-1
source install/setup.bash
ros2 run ros2_kvm_webrtc_sample ros2_kvm_webrtc_sample_node
```

AWSコンソールを開き、
<img width="50%" alt="aws1.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/15624/bd47307b-0827-2097-e1cd-567b4d4dfe6a.png">

該当チャネル(ScaryTestChannel)を選択後、メディア再生ビューワーで再生してみてください。

<img width="50%" alt="aws2.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/15624/b56a6ea6-03f2-cba8-01ec-5a20b3ac854b.png">

カメラ動画が再生できるのが、確認できると思います。

<img width="50%" alt="aws3.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/15624/a2f58fd3-284d-6083-a49d-70606fc2c0bd.png">

## ROS2/xserver/CUDA入りDockerイメージの作成

上記で作成した、UnityアプリをGCPで実行するための、Dockerイメージを作成します。

ポイントとしては

* ホストのNvidiaドライバと同じバージョンのNvidiaパッケージをインストールします。今回は、GCPの[common-cu110-ubuntu-2004](https://cloud.google.com/deep-learning-vm/docs/images) をホストのイメージとして使うため、予めインスタンスを一度起動し、パッケージのバージョンを調べておきました。`460.91.03-0ubuntu1` を今回は使用しています。

* xorg.confには、NVidiaグラフィックボードのBUS IDを指定する必要がありますが、インスタンスによって変わる可能性があるため、実行字に`nvidia-smi` を使用し取得し、実行時生成しています。

まず、Dockerイメージを作成し、Pushします。同等のイメージがすでに、https://hub.docker.com/r/otamachan/gcp-ros2-xserver にPushされているので、そちらを使用しても構いません。

```bash
cd gcp/docker
docker build -t 適当なpush先 .
docker push 適当なpush先
```

## GCP上でDockerを使ってxserverを起動し、Unityシミュレータを立ち上げる

次に、GCPのGPU入りインスタンスを起動します。予めProjectを作成し、GPUの割当申請を済ませて、gcloudコマンドラインのインストールをしておきてください(詳細は割愛します)

まず、GCPのインスタンスを起動して、Dockerを利用して、xserverを上げます。
`gcp/launch_instance.sh`を開いて、各種設定を確認して必要に応じて変更してください。以下の内容でインスタンスを起動します。

* インスタンス名: **ros2-unity-xserver**
* ゾーン: **asia-northeast1-a** (サンパウロ起動する場合は、southamerica-east1-cに変更します)
* マシンタイプ: **n1-standard-4**
* GPU: **nvidia-tesla-t4-vws, count=1** (T4を1つ使います)

確認ができたら、以下のコマンドでインスタンスを起動します。起動後から課金が始まるので注意してください。

```bash
cd gcp
./launch_instance.sh
```

起動すると、Nvidiaドライバのインストールや、Docker image pullと起動がされます。かなり時間がかかる(10分以上)なので、気長に待ってください。

以下のコマンドでログが確認できます。NVidiaドライバのインストール後、一度再起動されるので、注意してください。

```bash
gcloud compute ssh ros2-unity-xserver -- sudo journalctl -f
```

以下のコマンドでdockerでxserverが開始されていたら、起動完了です。

```bash
$ gcloud compute ssh ros2-unity-xserver -- sudo docker  ps

No zone specified. Using zone [asia-northeast1-a] for instance: [ros2-unity-xserver].
CONTAINER ID        IMAGE                        COMMAND             CREATED             STATUS              PORTS                    NAMES
843ae3d37ea1        otamachan/gcp-ros2-xserver   "bash /run.sh"      6 minutes ago       Up 6 minutes        0.0.0.0:8081->8081/tcp   gcp-ros2-xserver

```

起動ができたら、xserverに接続してみます。

```bash
gcloud compute ssh ros2-unity-xserver -- -L8081:localhost:8081
```

でインスタンスにログイン後、 http://localhost:8081/vnc.html にアクセスしてください。

<img width="50%" alt="vnc1.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/15624/fc16ad07-e37e-0bf0-e402-7b2e9bdd8f24.png">

起動したxserverにブラウザでアクセスできます。

<img width="50%" alt="vnc2.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/15624/d40c5b27-441e-e09c-1c97-2bffd8787169.png">

# 力付きてきたので、以下徐々に追記します。

## GCP上にUnityアプリとROS2/WebRTCプロキシノードを起動する

WS_ACCESS_KEY_ID, AWS_SECRET_ACCESS_KEYを指定して、プロキシのビルド、Unityアプリの実行、ノードの実行を行います。

```bash
gcloud compute ssh ros2-unity-xserver -- AWS_ACCESS_KEY_ID=XXXXXX AWS_SECRET_ACCESS_KEY=YYYYYYYY /opt/sim/run.sh -L8081:localhost:8081 
```

http://localhost:8081/vnc.html に起動し、シミュレータが起動していることを


## Webクライアントをブラウザから読み込み遠隔操作してみる

https://github.com/otamachan/ros2-unity-kvm-webrtc/tree/main/web-sample

https://github.com/awslabs/amazon-kinesis-video-streams-webrtc-sdk-js