Oculus Quest 2でROS Sharpを使う

Oculus Quest 2でROS#を使ってみたところ、Oculus Quest 2からのデータ送信がそのままでは正しく動きませんでした。
一部コードに変更を加えることで動くようになったので、その方法をまとめました。

(2021/2/14 追記)
Githubのリンクを追記しました
(追記ここまで)

今回はROS#を使って、
- ヘッドセットの位置・向きの出力
- コントローラ入力の出力
- カメラ画像の受信
をします。
一部コードに変更を加えた以外はOculus QUestと同じ手順で、ほぼ[こちらの記事](https://qiita.com/Spritaro/items/21a2e11fd6d09c8a5aac)そのままです。

# 環境
- Unity
  - 2019.4.15f1 LTS
- ROS#
  - [version 1.6](https://github.com/siemens/ros-sharp/releases/tag/v1.6)

# 環境構築
1. Unityの開発環境構築
  - UnityHubを起動する
  - メニューで「インストール」を選択し、右上の「インストール」ボタンを選択
  - Unity 2019.4.17f1 (LTS)を選択する。その際、「Android Build Support」プラグインを追加する
1. Oculus Quest2の開発者モードを有効にする
1. プロジェクトの作成
  - UnityHubからプロジェクトを新規作成する
  - テンプレートに3Dを選択する

# ビルド設定
1. Build Settingの変更
  - File -> Build Settingを選択
  - PlatformでAndroidを選択し、Switch Platformボタンを押す
1. Oculus Integrationをインポート
  - Asset Storeから*Oculus Integration for Unity*をインポートする
1. XR Plug-in Managementのインストール
  - Window -> Package Managerを選択
  - XR Plug-in Managementを選択し、インストール
1. Player Settingの変更
  - Edit -> Project Settingsを選択
  - Player -> Other Settingsを選択
      - *Package Name*に任意の名前を入力
      - *Minimum API Level*をAndroid 7.1 'Nougat' (API level 25)にする
      - *Api Compatibility Level*を.NET 4.xにする
  - XR Plug-in Managementを選択
      - Android と Standalone の “Oculus” にチェック 入れる
1. ROS#のインポート
  - [こちら](https://github.com/siemens/ros-sharp/releases/tag/v1.6)から*RosSharp.unitypackage*をダウンロード
  - Asset -> Import Package -> Custome Packageからダウンロードしたパッケージを選択しインポート

# アプリ実装
ソースコードは[こちらのGitHub](https://github.com/hiro-han/ar-car-oculus-quest2)にあげました。

## Oculus Quest2のカメラとコントローラ
1. カメラ
  - HierarchyからMain Cameraを削除
  - ProjectからAssets/Oculus/VR/Prefabsを選択し、OVRCameraRigをHierarchyドラッグ＆ドロップ
  - OVRCameraRigのOVR Managerスクリプトで、Target Devicesで、Quest2にチェック
1. コントローラ
  - ProjectからAssets/Oculus/VR/Prefabsを選択し、OVRControllerPrefabをHierarchyのLeftControllerAnchorの下にドラッグ＆ドロップ
      - LeftControllerAnchorにOVRControllerPrefabをドラッグ＆ドロップ
      - ControllerをL Touchに変更
  - ProjectからAssets/Oculus/VR/Prefabsを選択し、OVRControllerPrefabをHierarchyのRightControllerAnchorの下にドラッグ＆ドロップ
      - RightControllerAnchorにOVRControllerPrefabをドラッグ＆ドロップ
      - ControllerをR Touchに変更
  - AndroidManifest.xmlを変更
      - <category android:name="android.intent.category.INFO"/>を<category android:name="android.intent.category.LAUNCHER"/>に変更

## ROS Sharp
1. RosConnectorの設定
  - Hierarchyで右クリック -> Create EmptyでGameObjectを作成、名前をRosConnectorに変更
  - ProjectでAssets/RosSharp/Scripts/RosBridgeClient/RosCommunicationを開き、RosConnectorスクリプトをRosConnectorオブジェクトにアタッチ
  - RosConnectスクリプトで、ROSのIPアドレスを設定
1. カメラ画像の受信
  - Hierarchyで右クリック > 3D Object > Planeで平面を作成、名前をImageReceiverに変更
  - InspectorでPositionを(0, 0, 10)に、Rotationを(90, 0, 0)に、Scaleを(-1, -1, 0.75)に設定
  - ProjectでAssets/RosSharp/Scripts/RosBridgeClient/RosCommunicationを開き、ImageSubscriberスクリプトをRosConnectorオブジェクトにアタッチ
  - ImageSubscriberスクリプトのTopicを設定（自分の環境では*/image_raw/compressed*としました）
  - ImageSubscriberスクリプトのMessageReceiverに、ImageReceiverオブジェクトをドラッグ＆ドロップ
1. ヘッドセットの位置・向きの出力
  - ProjectからAssets/RosSharp/Scripts/RosBridgeClient/RosCommunicationを開き、PoseStampedPublisherスクリプトをRosConnectorオブジェクトにアタッチ
  - PoseStampedPublisherスクリプトのTopicを設定（自分の環境では*/oculus/head_set/pose*としました）
  - PoseStampedPublisherスクリプトのPublished TransformにCenterEyeAnchorをドラッグ＆ドロップ
1. コントローラ入力の送信
  - ProjectからAssets/RosSharp/Scripts/RosBridgeClient/RosCommunicationを開き、JoyPublisherスクリプトをRosConnectorオブジェクトにアタッチ
  - JoyPublisherスクリプトのTopicのTopicを設定（自分の環境では*/oculus/controller*としました）
  - ProjectのAssets/RosSharp/Scripts/RosBridgeClient/MessageHandlingからJoyAxisReaderを4つアタッチ
      - 自分は左右コントローラのジョイスティックのみ使うためこのようにしました。ボタンを使う場合はJoyButtonReaderを使用するボタンの数だけアタッチします
  - 4つのJoyAxisReaderのNameにそれぞれ以下の値を設定
      - Oculus_CrossPlatform_PrimaryThumbstickHorizontal
      - Oculus_CrossPlatform_PrimaryThumbstickVertical
      - Oculus_CrossPlatform_SecondaryThumbstickHorizontal
      - Oculus_CrossPlatform_SecondaryThumbstickVertical
  - 使用可能なボタンのNameの一覧は、Edit -> Project Settings -> Input Managerから確認できます

## ROS Sharpスクリプトの変更
以上でまでのコードをビルドして動かしたところカメラ画像は表示されるのですが、*/oculus/head_set/pose*と*/oculus/controller*が出力されませんでした。
ログを確認すると、

```
Error Unity NullReferenceException: Object reference not set to an instance of an object
Error Unity   at RosSharp.RosBridgeClient.UnityPublisher`1[T].Start () [0x00018] in <c1466e84f5f0406bad7dd0f2189d032c>:0 
Error Unity   at RosSharp.RosBridgeClient.PoseStampedPublisher.Start () [0x00000] in <c1466e84f5f0406bad7dd0f2189d032c>:0 

Error Unity NullReferenceException: Object reference not set to an instance of an object
Error Unity   at RosSharp.RosBridgeClient.PoseStampedPublisher.UpdateMessage () [0x00000] in <c1466e84f5f0406bad7dd0f2189d032c>:0 
Error Unity   at RosSharp.RosBridgeClient.PoseStampedPublisher.FixedUpdate () [0x00000] in
```
という例外が発生していました。

調査したところ、UnityPublisher<T>のStart()内にある`rosConnector.RosSocket.Advertise<T>(Topic);`で例外が発生していることがわかりました。
`rosConnector`の初期化等に時間がかかっているのかと考え、直前にスリープを入れたり、PoseStampedPublisher.csスクリプトとJoyPublisher.csスクリプトの実行順を一番最後にしたりと試したのですが、解決できませんでした。
そこで、UnityPublisherのPublish()内で、rosConnectorがnullの場合は、GetComponent()とAdvertise()を呼ぶようにしました。

また、Start()で例外が発生したままにするとPoseStampedPublisherとJoyPublisherのStart()でのメッセージの初期化が行われないため、Publish()でメッセージのNullReferenceExceptionも発生してしまいます。そのためStart()内で例外をcatchしメッセージを初期化するようにしました。
以下が変更したソースコードになります。

```UnityPublisher.cs
namespace RosSharp.RosBridgeClient
{
    [RequireComponent(typeof(RosConnector))]
    public abstract class UnityPublisher<T> : MonoBehaviour where T : Message
    {
        public string Topic;
        private string publicationId;

        private RosConnector rosConnector;
        protected bool initialized = false;

        protected virtual void Start()
        {
            try
            {
                rosConnector = GetComponent<RosConnector>();
                publicationId = rosConnector.RosSocket.Advertise<T>(Topic);
            } catch (Exception e)
            {
                // nop
            }  
        }

        protected void Publish(T message)
        {
            if (!rosConnector)
            {
                rosConnector = GetComponent<RosConnector>();
            }
            if (string.IsNullOrEmpty(publicationId)) {
                publicationId = rosConnector.RosSocket.Advertise<T>(Topic);
            }
            rosConnector.RosSocket.Publish(publicationId, message);
        }
    }
}
```

# Build & 実行
1. Oculus Quest 2を接続してBuild & Run
2. Oculus Quest 2で、カメラ画像が表示されることを確認する
3. ROS側で、`$ topic list`コマンドを実行し、*/oculus/head_set/pose*と*/oculus/controller*があることを確認する
4. `$ rostopic echo /oculus/head_set/pose`、`$ rostopic echo /oculus/controller`をそれぞれ実行し、値が出力されていることを確認する(/oculus/controllerの出力例)

```
$ rostopic echo /oculus/controller
---
header: 
  seq: 177
  stamp: 
    secs: 1612795853
    nsecs:  23462057
  frame_id: "Unity"
axes: [0.0, 0.0, 0.0, 0.0]
buttons: []
---
header: 
  seq: 178
  stamp: 
    secs: 1612795853
    nsecs:  30868053
  frame_id: "Unity"
axes: [0.0, 0.0, 0.0, 0.0]
buttons: []
---

```

# 最後に
UnityとOculus Quest 2について詳しくないため以上の対応をしましたが、根本的な解決方法があるのではないかと思います。
もじご存じの方がいれば教えていただけると嬉しいです。
また、ROS#の最新版では解決されているかもしれないので、最新のソースコードをダウンロードしてビルドし、試してみてもいいかと思います。（自分はビルド環境がないため試していません）


# 参考
[OculusQuestをROSと通信させる](https://qiita.com/Spritaro/items/21a2e11fd6d09c8a5aac)
[ROS Sharp v1.6](https://github.com/siemens/ros-sharp/releases/tag/v1.6)