はじめに

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

実行環境

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のテクスチャフォーマットの詳細についてはこちらのページを参考にしてください。バイト列から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で見えているカメラ画像もそれに合わせて変化します。

実機のカメラを使う場合

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

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

$ 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();
    }
    
}

参考資料