Meta Quest 3で ROS2 (ros2-for-unity)を使う

以前MetaQuest2+ROS(ROS#)で[ラジコンのカメラ画像を表示＋ラジコンを操縦するアプリ](https://qiita.com/hiro-han/items/8a6a419950d164b4ae52)を作りましたが、それをMetaQuest3+ROS2対応しました。

環境はこちらです。
- ROS2 humble
- Unity 2022.3.11.f1


（※）2024/3/6 事前準備とエラー対応を追記
（※）2024/4/8 ソースコードとビルドしたパッケージをGithubで公開し、Githubnへのリンクを追記（[Link](https://github.com/hiro-han/ros2-for-unity/releases/tag/v1.0.0)）

# ros2-for-unity
UnityでROS2を使うにはRobotecAIが開発した[***ros2-for-unity***](https://github.com/RobotecAI/ros2-for-unity)とUnity公式が開発した[***Unity-Robotics-Hub***](https://github.com/Unity-Technologies/Unity-Robotics-Hub)があります。

後者の***Unity-Robotics-Hub***は通信の際TCPに変換をするため通信が遅くなります。そのため***ros2-for-unity***を使用しました。

詳しく解説している記事は多くあるのでそちらをご参照ください。例えばこちら。
- [ROS2 for Unityで始めるUnityとROS2間の高速データ通信](https://zenn.dev/hakuturu583/articles/ros2_for_unity)

# ros2-for-unityをMetaQuest3で使う
ros2-for-unityはwindowsもしくはubuntu上のunityで動作します。一方MetaQuest3はAndroidなので対応が必要となります。
ありがたいことにros2-for-unityのAndroid対応をしている方がいるので、この方が公開しているソースコードををベースに開発しました。
- [AndroidをROS2ノードにしてロボットのコントローラを作る](https://qiita.com/Kotakku/items/cdc3eca89dd8aec4ee86)


# ros2-for-unityのビルド
## ビルド済みパッケージ
以下で説明するビルドエラーの修正を行ったソースコードと、ビルド済みのUnity PackageをGihubにアップしました。参考にしてください。
- Unity Package: [Link](https://github.com/hiro-han/ros2-for-unity/releases/tag/v1.0.0)
- ソースコード: [Link](https://github.com/hiro-han/ros2-for-unity)

## ビルドエラーの修正
基本的にはこの方が公開している[Githubリポジトリ](https://github.com/Kotakku/ros2-for-unity)を使うのですが、Meta Quest 3のアプリ開発で必要となるUnity 2022.3.11.f1 + Android 10.0でビルドするとエラーが出るので修正する必要があります。

- エラーメッセージ
```
gmake[2]: *** [src/cpp/CMakeFiles/fastrtps.dir/build.make:874: src/cpp/CMakeFiles/fastrtps.dir/fastdds/publisher/DataWriter.cpp.o] Error 1
gmake[2]: *** [src/cpp/CMakeFiles/fastrtps.dir/build.make:580: src/cpp/CMakeFiles/fastrtps.dir/rtps/reader/RTPSReader.cpp.o] Error 1
In file included from /app/3rdlib/ros2-for-unity/src/ros2cs/src/eProsima/Fast-DDS/src/cpp/rtps/writer/StatefulWriter.cpp:48:
In file included from /app/3rdlib/ros2-for-unity/src/ros2cs/src/eProsima/Fast-DDS/src/cpp/rtps/RTPSDomainImpl.hpp:23:
/app/3rdlib/ros2-for-unity/src/ros2cs/src/eProsima/Fast-DDS/thirdparty/filewatch/FileWatch.hpp:511:30: error: no viable overloaded '+='
                current_time += std::chrono::nanoseconds(result.st_mtim.tv_nsec);
```

- 修正内容

`Fast-DDS/thirdparty/filewatch/FileWatch.hpp`で、`std::chrono::time_point<std::chrono::system_clock>`のように変数を宣言している箇所があるので、ここすべて修正します。

```
(修正前)
std::chrono::time_point<std::chrono::system_clock> current_time

（修正後）
std::chrono::time_point<std::chrono::system_clock, std::chrono::nanoseconds > current_time;
```

- 補足
ros2-for-unityのビルドスクリプトを実行するとros2csがクローンされ、さらにros2csでいくつかのパッケージがクローンされますが、そのうちの１つがFast-DDSです。
ros2-for-unityをクローンしたディレクトリから今回修正するファイルの相対パスは`src/ros2cs/src/eProsima/Fast-DDS/thirdparty/filewatch/FileWatch.hpp`となります。

## ビルド手順
前提として、ROS2がビルドでき、`. /opt/ros/humble/setup.bash`を実行した環境である必要があります。

### 事前準備
[もとのGithubリポジトリの手順](https://github.com/RobotecAI/ros2cs/blob/master/README-UBUNTU.md#prerequisites)にあるように事前にいくつかインストールする必要があります。
手順と同じ内容ですが、以下のようにソフトをインストールします。

```
# Install rmw and tests-msgs for your ROS2 distribution
sudo apt-get update
sudo apt install -y ros-humble-test-msgs
sudo apt install -y ros-humble-fastrtps ros-humble-rmw-fastrtps-cpp
sudo apt install -y ros-humble-cyclonedds ros-humble-rmw-cyclonedds-cpp

# Install vcstool package
curl -s https://packagecloud.io/install/repositories/dirk-thomas/vcstool/script.deb.sh | sudo bash
sudo apt-get update
sudo apt-get install -y python3-vcstool

# Install dotnet
sudo apt-get install -y apt-transport-https

sudo apt remove dotnet* aspnetcore* netstandard*
sudo rm /etc/apt/sources.list.d/microsoft-prod.list
sudo apt update
sudo apt install -y dotnet-sdk-6.0
```


### ビルド

1. ros2-for-unityのclone
    ```
    $ git clone https://github.com/Kotakku/ros2-for-unity.git
    ```
1. 使用するパッケージのpull
    ```
    $ cd ros2-for-unity
    $ ./pull_repositories.sh
    ```
1. 上で説明したビルドエラーを修正
1. ビルド
    ```
    $ ./build.sh -p {NDKのパス} --clean-install --standalone

    (例)
    $ ./build.sh -p ~/2022.3.18f1/Editor/Data/PlaybackEngines/AndroidPlayer/NDK --clean-install --standalone
    ```
1. unityパッケージのビルド（オプション）
    この手順は必須ではありませんが、やっていた方がunityにros2-for-unityを追加するのが簡単になります
    ```
    $ ./create_unity_package.sh -u {unity editorのパス}

    (例)
    $ ./create_unity_package.sh -u ~/Unity/2022.3.18f1/Editor/Unity
    ```
    成功すると`install/unity_package/Ros2ForUnity.unitypackage`が出力されます。

### エラー対応
自分の環境では以下のビルドエラーがでました。環境によっては出ない人もいると思いますが、参考にしてください。

- エラー1
    ```
    /usr/lib/python3/dist-packages/setuptools/command/install.py:34: SetuptoolsDeprecationWarning: setup.py install is deprecated. Use build and pip and other standards-based tools.
      warnings.warn(
    ```

    対応
    setuptoolsをインストール
    ```
    pip install setuptools==58.2.0
    ```

- エラー2
    ```
    CMake Error at CMakeLists.txt:170 (find_package):
      By not providing "Findfoonathan_memory.cmake" in CMAKE_MODULE_PATH this
      project has asked CMake to find a package configuration file provided by
      "foonathan_memory", but CMake did not find one.
    
      Could not find a package configuration file provided by "foonathan_memory"
      with any of the following names:
    
        foonathan_memoryConfig.cmake
        foonathan_memory-config.cmake
    
      Add the installation prefix of "foonathan_memory" to CMAKE_PREFIX_PATH or
      set "foonathan_memory_DIR" to a directory containing one of the above
      files.  If "foonathan_memory" provides a separate development package or
      SDK, be sure it has been installed.
    ```
    対応
    根本解決方法はわかっていないのですが、CMakeLists.txtに以下のように追記をすることで回避できました
    ``` ros2-for-unity/src/ros2cs/src/eProsima/foonathan_memory_vendor/CMakeLists.txt
    ...(略)...

    execute_process(COMMAND
        ${CMAKE_COMMAND}
        --find-package
        -DNAME=foonathan_memory
        -DCOMPILER_ID=$<IF:$<BOOL:${WIN32}>,MSVC,GNU>
        -DLANGUAGE=CXX
        -DMODE=EXIST
        -DCMAKE_FIND_DEBUG_MODE=ON
      ERROR_QUIET
      RESULT_VARIABLE _EXIT_CODE)
    
    SET(_EXIT_CODE 1) # <- 追加
    
    if(NOT _EXIT_CODE EQUAL 0)

    ...(略)...
    ```

- 警告
    ```
        build/statistics_msgs/rosidl_generator_cs/statistics_msgs/msg/metrics_message_s.c:58:36: warning: address of 'ros_message->metrics_source.data' will always evaluate to 'true' [-Wpointer-bool-conversion]
      if (&ros_message->metrics_source.data)
    ```
    対策
    ビルドオプションを追加します
    ``` ros2-for-unity/src/ros2cs/build_android.sh

    ...(略)...

    -DCMAKE_SHARED_LINKER_FLAGS="-Wl,-rpath,'\$ORIGIN',-rpath=.,--disable-new-dtags" \
    -DCMAKE_FIND_ROOT_PATH=${PWD}/install/ \
    -DCMAKE_HAVE_LIBC_PTHREAD=1 \
    --no-warn-unused-cli \
    -Wno-deprecated \
    -Wno-pointer-bool-conversion"

    ...(略)...

    ```

## カスタムメッセージの追加
独自に実装したカスタムメッセージを追加することができます。方法は２つです。

1. ros2_for_unity_custom_messages.reposにリポジトリを追加する
    カスタム決セージをgitリポジトリに上げている場合は、このファイルにリポジトリを追加することで自動的にリポジトリをクローン、ビルドしてくれます
    (例)
    ```ros2_for_unity_custom_messages.repos
    repositories:
      src/ros2cs/custom_messages/original_message:
        type: git
        url: https@github.com:xxxx/original_message.git
        version: main
    ```
    - ２行目のsrc/...: パッケージをクローンするディレクトリ
    - url: リポジトリのurl
    - version: ブランチ

# Unityアプリのビルド
前提として、Meta Quest 3アプリの開発ができるよう設定をしておきます。

## ros2-for-unityのインポート
エディタの上部のメニューで`Asset -> ImportPackage -> CustomPackage`を選択し、上の手順で作成したパッケージ`Ros2ForUnity.unitypackage`を選んでインポートします

## アプリ実装
ここでは例として、カメラの画像（sensor_msgs/msg/CompressedImage）を受信し表示するアプリの実装をします。

1. Hierarchyウィンドウで空のGameObjectを作成します。ここでは`RosManager`とします
1. Hierarchyウィンドウで`Plane`を作成します。ここでは`CameraImage`とします
1. `CameraImage`のPosition, Rotation, Scaleを設定します。
    ```
    Position: (0, 0, 10)
    Rotation: (90, 0, 0)
    Scaleを設定します: (-1, -1, 0.75)
    ```
1. Assetsフォルダ以下の任意の場所に`CompressedImageSubscriber.cs`を作成します
1. `CompressedImageSubscriber.cs`の実装をします
    ```
    using System;
    using System.Collections;
    using System.Collections.Generic;
    using UnityEngine;
    using ROS2;
    
    [RequireComponent(typeof(ROS2UnityComponent))]
    public class CompressedImageSubscriber : MonoBehaviour
    {
        public string topicName;
        public Renderer targetRenderer;
        private ROS2UnityComponent ros2Unity = null;
        private ROS2Node ros2Node = null;
        private ISubscription<sensor_msgs.msg.CompressedImage> image_sub = null;
        private Texture2D tex;
        static private byte[] recievedData = null;
        static object lockObject = new object();
    
        void Start()
        {
            ros2Unity = GetComponent<ROS2UnityComponent>();
    
            if (ros2Unity != null) {
                if (ros2Unity.Ok()) {
                    ros2Node = ros2Unity.CreateNode("ROS2UnityListenerNode");
                }
                tex = new Texture2D(1, 1);
            } else {
                Debug.Log("ros2Unity is null");
            }
        }
    
        void Update()
        {
            if (ros2Unity != null) {
                if (image_sub == null) {
                    Debug.Log("image_sub is null");
                } else {
                    Debug.Log("image_sub is not null");
                }
    
                if (ros2Unity.Ok() && image_sub == null) {
                    image_sub = ros2Node.CreateSubscription<sensor_msgs.msg.CompressedImage>(
                        topicName, CallbackCompressedImage);
                }
            } else {
                Debug.Log("ros2Unity is null");
            }
    
            lock (lockObject) {
                if (recievedData != null) {
                    tex.LoadImage(recievedData);
                    targetRenderer.material.mainTexture = tex;
                    tex.Apply();                
                } else {
                    Debug.Log("recievedData is null");
                }
            }
        }
    
        static void CallbackCompressedImage(sensor_msgs.msg.CompressedImage msg) {
            if (msg != null) {
                if (msg.Data != null) {
                    lock (lockObject) {
                        recievedData = new byte[msg.Data.Length];
                        Buffer.BlockCopy(msg.Data, 0, recievedData, 0, msg.Data.Length);
                    } 
                } else {
                    Debug.Log("msg.Data is null");
                }
            } else {
                Debug.Log("msg is null");
            }
        }
    }

    ```
    CreateSubscriptionで、データを受信したときに呼び出されるcallback関数を登録しています。
    このコールバック関数は別プロセスで動作するので、このコールバック関数内でテクスチャの設定をしようとするとエラーが発生していまいます。
    そのため、いったんバッファ（recievedData）に格納し、メインプロセスで実行されるUpdate関数内でテクスチャの設定をしています。
    注意点としてrecievedDataへのデータの書き込みと、データの読み込みが同時に行われることを防ぐため排他制御をする必要があります。
1. Inspectorウィンドウで`RosManager`に、`Ros2UnityComponent`と上で作成した`CompressedImageSubscriber`を追加します。
1. Inspectorウィンドウで、CompressedImageSubscriberのパラメータを設定します
    Topic Name: /image_raw/compressed
    Target Renderer: CameraImageをドラッグ&ドロップ

これをビルド・実行し、PCなどからCommpressdImageを出力すると、画像が表示されます。
[表示例](https://x.com/hirohan6/status/1761056276765942206?s=20)

## 任意のROS_DOMAIN_IDを使う方法
Meta Quest 3でも任意のROS_DOMAIN_IDを使うことができます。
そのために環境変数の設定とスクリプトの実行順を設定する必要があります。

1. `RosInitializer.cs`というスクリプトを追加し、以下の実装をします
    ```
    using System;
    using System.Collections;
    using System.Collections.Generic;
    using UnityEngine;
    
    public class RosInitializer : MonoBehaviour
    {
    
        public int rosDomainId = 1;
    
        // Start is called before the first frame update
        void Start()
        {
            // ROS_DOMAIN_ID設定
            Environment.SetEnvironmentVariable("ROS_DOMAIN_ID", rosDomainId.ToString());
            Debug.Log("current ROS_DOMAIN_ID:" + Environment.GetEnvironmentVariable("ROS_DOMAIN_ID") + "\n");
        }
    
        // Update is called once per frame
        void Update()
        {
            // Nothing
        }
    }
    ```
1. Inspectorウィンドウ上で`RosManager`に`RosInitializer`を追加します
1. Inspectorウィンドウ上で`RosInitializer`のパラメータを設定します
    Ros Domain Id: {使用したいROS_DOMAIN_IDの値}
1. スクリプトの実行順を指定します
    ROS_DOMAIN_IDは、ROS2UnityComponentやCompressedImageSubscriberより先に設定する必要があります。
    何も指定しないと実行順がランダムとなるのでROS_DOMAIN_IDを設定する前にROSが立ち上がってしまい、意図したROS_DOMAIN_IDで送受信ができません。
    設定方法は以下のとおりです。
    1. 上部のメニューで、`File -> Build Settings -> Player Settings -> Script Execution Order`を選択します
    1. 設定画面で以下の順でスクリプトが実装されるように設定します
        1. RosInitializer
        1. Ros2UnityComponent
        1. CompressedImageSubscriber