UnityでROS2の画像をサブスクライブする

# はじめに


この記事では、ROS2からパブリッシュされた画像データを、Unityでサブスクライブして描画する処理について説明します。ROS1用のROS#には`ImageSubscriber.cs`というスクリプトが用意されていますが、これをそのままROS2用に使ってもうまくいかないので、自分で書きます。

https://github.com/siemens/ros-sharp/blob/master/Unity3D/Assets/RosSharp/Scripts/RosBridgeClient/RosCommuncation/ImageSubscriber.cs

# 実行環境
- ubuntu 20.04.2 LTS
- ROS 2 Foxy Fitzroy
- Unity 2020.3.17f1



# コード

空のオブジェクトを作成して、以下のコードを貼り付けます。`TargetRenderer`には、送られてくる画像のアスペクト比に一致するように大きさを設定したQuadを、TopicNameには`/camera/image_raw`を入力します。これは、gazeboを起動したときに自動的に設定されるカメラ画像用のトピック名です。別のトピックを使う場合は適宜置き換えてください。



```ImageSubscriber.cs
using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using RosMessageTypes.Sensor;

public class ImageSubscriber : MonoBehaviour
{
    public Renderer targetRenderer;
    public string topicName;
    private ROSConnection ros;
    private Texture2D tex;
    
    void Start()
    {
        ros = ROSConnection.instance;
        ros.Subscribe<ImageMsg>(topicName, ReceiveMsg);

        // テクスチャを初期化
        tex = new Texture2D(640, 480, TextureFormat.RGB24, false);
    }

    void ReceiveMsg(ImageMsg msg)
    {
        // バイト列を取り出す
        byte[] imageData = msg.data;
        RenderTexture(imageData);
    }

    private void RenderTexture(byte[] data)
    {        
        // バイト列からtexture2dを生成
        tex.LoadRawTextureData(data);
        // テクスチャ割当
        targetRenderer.material.mainTexture = tex;
        tex.Apply();
    }
}
```

このトピックにパブリッシュされている`ImageMsg`型のメッセージには、時間や画像のサイズなどの情報と、画像を表すバイト列が格納されています。それをUnity側で取り出し、Texture2Dに変換しています。Unityのテクスチャフォーマットの詳細については[こちらのページ](https://docs.unity3d.com/ja/2018.4/ScriptReference/TextureFormat.html)を参考にしてください。バイト列からTexture2Dを復元する際には、以下のようにパラメータを適切に与えたうえでテクスチャを作成する必要があります。

```
tex = new Texture2D(640, 480, TextureFormat.RGB24, false);
```

```
$ ros2 topic echo /camera/image_raw
header:
  stamp:
    sec: 1630731322
    nanosec: 32497472
  frame_id: camera
height: 480
width: 640
encoding: rgb8
is_bigendian: 0
step: 1920
data:
- 22
- 4
- 19
- 24
- 6
- 21
- 20
- 9
```



# 動作確認

ターミナルからgazeboを起動します。

```
$ ros2 launch turtlebot3_gazebo empty_world.launch.py
```

別のターミナルで、ROS-TCP-Endpointを起動します。

```
$ ros2 run ros_tcp_endpoint default_server_endpoint --ros-args -p ROS_IP:=<your IP address>
```

この状態でUnityのシーンを実行すると、Quadにgazebo内のロボットのカメラ画像が描画されていることが確認できます。gazeboのロボットを動かすと、Unityで見えているカメラ画像もそれに合わせて変化します。

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

# 実機のカメラを使う場合

実機に接続されたwebカメラなどを使う場合は、以下の`v4l2_camera`というパッケージが利用可能です。

https://index.ros.org/r/v4l2_camera/gitlab-boldhearts-ros2_v4l2_camera/#foxy

以下のコマンドを実行してインストールするだけで使えます。

```
$ sudo apt install ros-foxy-v4l2-camera
```

画像トピックをパブリッシュするには以下のコマンドを叩きます。非圧縮および圧縮済みの画像データがパブリッシュされます。

```
$ ros2 run v4l2_camera v4l2_camera_node
```

以下のコードで、圧縮済みの画像データをサブスクライブして描画します。圧縮済みの場合は`CompressedImageMsg`型のメッセージを使ってやり取りします。上のコードと違って、テクスチャの復元には`LoadImage`メソッドを使います。

```ImageSubscriber.cs
using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using RosMessageTypes.Sensor;

public class ImageSubscriber : MonoBehaviour
{
    public Renderer targetRenderer;
    public string topicName;
    private ROSConnection ros;
    private Texture2D tex;
    
    void Start()
    {
        ros = ROSConnection.instance;
        ros.Subscribe<CompressedImageMsg>(topicName, ReceiveMsg);

        tex = new Texture2D(1, 1);
    }

    void ReceiveMsg(CompressedImageMsg compressedImage)
    {
        byte[] imageData = compressedImage.data;
        RenderTexture(imageData);
    }

    private void RenderTexture(byte[] data)
    {        
        tex.LoadImage(data);
        targetRenderer.material.mainTexture = tex;
        tex.Apply();
    }
    
}
```




# 参考資料
- https://tetorachaos.com/unity-texture-code
- https://qiita.com/Hiroaki-K4/items/53c2565c77319ecc76e2