はじめに

23年新卒入社の岡見(おかみ)です。大学院で使用していた、HoloLens2とロボット用のミドルウェアの通信をしていたので、この通信の話を軽く話していきたいと思います。

Unity-Ros間通信

Autoware.aiとUnityを通信する際に躓いた、UnityとRos間の通信について記述していきます。

Rosのノード名等はAutoware.aiを例に挙げて説明します。

RosBridgeと呼ばれるライブラリは、RosのノードをWebSocket通信を介してAutoware.aiのデータのやり取りを行うものです。 今回はその中でも特に購読(Subscriber)に注目をします。申し訳ないのですが、RosやRosBridgeについては今回は解説しません。今回はUnityアプリ側の実装方法について説明していきます。

下記の2つの事項を考慮して，２つある通信方法の内から１つでもYesであるならば１の手法を，２つともNoであるならば２の手法を利用するといいです。

1. ROSが動作するPCが無線回線に接続可能か？
2. Unityアプリが動作する端末が有線回線に接続可能か？

1.Ros#を利用する

以下では適宜、Unityのエディタ画面、およびUnity Asset Storeの画面のスクリーンショットを見せながら説明していきます。

また、UWP/HoloLensアプリで動作させたい場合は、https://github.com/EricVoll/ros-sharpからダウンロード後、Zipファイルの解凍し、UnityエディタのAssetsへドラッグアンドドロップをしてください。

UnityにアセットRos#を導入する

まず，UnityのメニューバーからWindow->Asset Storeと選択します。

その後，Asset Storeウィンドウでsearch onlineを選択する。Unityのバージョンによってはこの画面にならないかもしれません。その場合は、Webブラウザでhttps://assetstore.unity.com/?locale=ja-JPへアクセスする。その後、Unityで作成しているアカウントでログインする。

ブラウザでAssetStoreが開くので，検索ボックスにRos＃と入力して検索を行います。

Ros＃を選択し，Add to My Assetを選択。その後，Open in Unityを選ぶ。

すると，Unity Editor上でPackage Managerが開きます。そこでRos＃を選択し，Downloadを選択します。その後，Importを選択すると細長いウィンドウが出てくるので、そちらでもImportを選択するとUnity側でRosと通信するためのスクリプトがプロジェクトに入ります。

Ros#が.Net 4より大きいバージョンを使用しているため、設定を変更する必要があります。

その後、Unityツールバーの"Edit"->“Project Setting”->"Player"を開き，“Api Comparibility Level"を”.Net 4.x Equivalent"に変更してください。

UnityでRosとの通信をおこなうための設定

UnityでRosConnectorという空のオブジェクトを作成。 RosConnector.csスクリプトをRosConnectorのコンポーネントに追加します。

 RosConnectorオブジェクトを選択し，インスペクタのRosConnectorのRosBrideServerURLをws://<IPaddress>:9090に設定を変更します。

※SecondsTimeoutはRosへのソケット通信のタイムアウト時間を設定する箇所で時間は秒単位，シーン開始時ではなく実行からの時間でタイムアウトが発生すします。

Autowareの位置・姿勢情報について取得

RosConnectorオブジェクトにPoseStampedPublisher.csスクリプトを追加。 RosConnectorオブジェクトを選択し，インスペクタのPoseStampedPublisherにあるTopicを/current_poseに変更します。 また，PublishedTransformは受け取ったTransformの値を代入するオブジェクトを設定する項目となっています。

速度の取得

RosConnectorに適応するスクリプトをPoseStampedPublisherではなく，下記のmsgsSubscriber.csを作成し，Topic名は/linear_velocity_vizを参照にします。

using UnityEngine;

namespace RosSharp.RosBridgeClient
{

    public class msgsSubscriber : UnitySubscriber<MessageTypes.Std.Float32>
    {
        public string PublishedMessage;

        private string msgs;
        private bool isMessageReceived;

        protected override void Start()
        {
            base.Start();
        }

        private void Update()
        {
            if (isMessageReceived)
                ProcessMessage();
        }

        protected override void ReceiveMessage(MessageTypes.Std.Float32 message)
        {
            msgs = GetMessage(message);
            isMessageReceived = true;
        }

        private void ProcessMessage()
        {
            PublishedMessage = msgs;
        }

        private string GetMessage(MessageTypes.Std.Float32 message)
        {
            return message.data.ToString();
        }
    }
}

また、Unityで受け取りたい型は以下のURLなどから参照します。そして、スクリプトにおける変更箇所については以下に示します。

1. std_msgsドキュメント
2. geometry_msgsドキュメント
3. AutowareからPublishされるTopicの場合は、Topicsタブから確認可能
   Autowareのtopicの中身はGitHubで確認することが可能
   例.can_info

using UnityEngine;

namespace RosSharp.RosBridgeClient
{

    public class msgsSubscriber : UnitySubscriber<MessageTypes.ライブラリ名.変数の型名>
    {
        public string PublishedMessage;

        private string msgs;
        private bool isMessageReceived;

        protected override void Start()
        {
            base.Start();
        }

        private void Update()
        {
            if (isMessageReceived)
                ProcessMessage();
        }

        protected override void ReceiveMessage(MessageTypes.ライブラリ名.変数の型名 message)
        {
            msgs = GetMessage(message);
            isMessageReceived = true;
        }

        private void ProcessMessage()
        {
            PublishedMessage = msgs;
        }

        private string GetMessage(MessageTypes.ライブラリ名.変数の型名 message)
        {
            return message.data.ToString();
        }
    }
}

2.PythonでAutowareからデータを受け取り，UDPで端末に送信する

Rosを受け取るPythonスクリプトを作成する

pythonの環境はあるものとして進めていきます。 まず，roslibpyというライブラリが必要になるためインストールをします。インストール後，WindowsでVSCodeを使用している場合は立ち上げなおすと使用可能になります。

pip install roslibpy

※不足しているライブラリはその都度追加してください。

Rosから現在地のx座標を受け取る最低限のスクリプトが以下になります。

 from __future__ import print_function
import roslibpy

client = roslibpy.Ros(host='192.168.xxx.xxx', port=9090)
client.run()

listener = roslibpy.Topic(client, '/current_pose', 'geometry_msgs/PoseStamped')
listener.subscribe(lambda message: print('Heard talking: ' + str(message['pose']['position']['x'])))

try:
    while True:
        pass
except KeyboardInterrupt:
    client.terminate()

PythonからUnityに送るスクリプト

pythonにsocketライブラリを追加します。 現在地と現在の速度を同時に送信する場合は，スクリプトを２つ実行することで可能となります。この時の送信アドレスのポートは別にする必要があります。

例．

プログラム１．serverAddressPort = ("192.168.54.21", 11000)

プログラム2．serverAddressPort = ("192.168.54.21", 11001) 

155-156,159-161列をそれぞれいれかえることで現在の位置・姿勢の取得と速度の取得を切り替えることが可能となります。 また，同一PCに UDPを用いて送信する場合，IPAddressは127.0.0.1とするとループバックアドレスと呼ばれる、PCの自分自身のネットワークに接続します。

from __future__ import print_function
import roslibpy
import socket

client = roslibpy.Ros(host='192.168.2.105', port=9090)    #Ros側IPアドレス
client.run()
#serverAddressPort   = ("192.168.54.21", 11000)             #Unity側IPアドレス
serverAddressPort   = ("127.0.0.1", 11000)
cin = "string"
counter = 0
def UDP(Mess):
    global counter
    if counter>2:
        counter = 0
    else:
        cin = str(Mess)
        testString = cin.encode('ascii')
        hexVal = testString.hex() #hexLength = int(len(hexVal) / 2).to_bytes(4, 'big')
        hexLength = int(len(hexVal) / 2)
        hexLengthPlus4 = hexLength + 4
        fullMessage = hexLengthPlus4.to_bytes(4, 'big') + hexLength.to_bytes(4, 'big') + bytes.fromhex(hexVal)
        # Create a UDP socket at client side
        UDPClientSocket = socket.socket(family=socket.AF_INET, type=socket.SOCK_DGRAM)
        # Send to server using created UDP socket
        UDPClientSocket.sendto(fullMessage, serverAddressPort)
        print("sent")
        print(fullMessage)
        counter = counter+1

def TMP(Mess):
    a=1;
    
if __name__ == "__main__":
    listener = roslibpy.Topic(client, '/current_pose', 'geometry_msgs/PoseStamped') #現在の位置・姿勢の取得

    udpClntSock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

    listener.subscribe(lambda message: UDP(' '+ str(message['pose']['position']['x']) + ',' + str(message['pose']['position']['y']) + ',' + str(message['pose']['position']['z']) + ',' + str(message['pose']['orientation']['x']) + ',' + str(message['pose']['orientation']['y']) + ',' + str(message['pose']['orientation']['z']) + ',' + str(message['pose']['orientation']['w']) ) )
    #現在の位置・姿勢の取得

try:
    while True:
        pass
except KeyboardInterrupt:
    client.termina

UnityでUDPの受信スクリプトを作成する

Unity側では，UDPMessageReceiver.csのスクリプトを作成します。今回の#ifなどはHoloLensで使用していたため、 UnityエディタとUWP/HoloLensアプリで異なる挙動をしてくれという命令です。

UDPMessageReceiver.cs

using System.Collections.Generic;
using UnityEngine;
// UnityEvent を利用するため Events を追加
using UnityEngine.Events;

#if UNITY_UWP
using System.IO;
using System.Threading.Tasks;
using Windows.Networking.Sockets;
#endif


// 引数にByte列を受け取る UnityEvent<T0> の継承クラスを作成する
// UDP受信したバイト列を引数として渡す
// Inspector ビューに表示させるため、Serializable を設定する
[System.Serializable]
public class MyIntEvent : UnityEvent<byte[]>
{
}

public class UDPMessageReceiver : MonoBehaviour
{
    /// <summary>
    /// UDPメッセージ受信時実行処理
    /// </summary>
    [SerializeField, Tooltip("UDPメッセージ受信時実行処理")]
    private MyIntEvent UDPReceiveEventUnityEvent;

    /// <summary>
    /// UDP受信ポート
    /// </summary>
    [SerializeField, Tooltip("UDP受信ポート")]
    private int udpReceivePort = 11000;

    /// <summary>
    /// UDP受信データ
    /// </summary>
    [SerializeField]
    private byte[] p_UDPReceivedData;

    /// <summary>
    /// UDP受信イベント検出フラグ
    /// </summary>
    private bool p_UDPReceivedFlg;
    private string ipAddress;

    private System.Net.Sockets.UdpClient client = default;

    public string udpRecieve_message = default;


    /// <summary>
    /// 起動時処理
    /// </summary>
    void Start()
    {
        // 検出フラグOFF
        p_UDPReceivedFlg = false;

        // 初期化処理
        UDPClientReceiver_Init();
    }

    /// <summary>
    /// 定期実行
    /// </summary>
    void Update()
    {
        if (p_UDPReceivedFlg)
        {
            // UDP受信を検出すればUnityEvent実行
            // 受信データを引数として渡す
            UDPReceiveEventUnityEvent.Invoke(p_UDPReceivedData);
            udpRecieve_message = System.Text.Encoding.ASCII.GetString(p_UDPReceivedData);

            // 検出フラグをOFF
            p_UDPReceivedFlg = false;
        }
        
    }


    
    /// <summary>
    /// UDP受信時処理
    /// </summary>
    private void UDPReceiveEvent(byte[] receiveData)
    {
        // 検出フラグONに変更する
        // UnityEventの実行はMainThreadで行う
        p_UDPReceivedFlg = true;

        // 受信データを記録する
        p_UDPReceivedData = receiveData;
    }


#if WINDOWS_UWP
    /// <summary>
    /// UDP通信ポート
    /// </summary>
    Windows.Networking.Sockets.DatagramSocket p_Socket;
    
    /// <summary>
    /// ロック用オブジェクト
    /// </summary>
    object p_LockObject = new object();
    
    /// <summary>
    /// バッファサイズ
    /// </summary>
    const int MAX_BUFFER_SIZE = 1024;
    byte[] receiveBytes = new byte[MAX_BUFFER_SIZE];
    /// <summary>
    /// UDP受信初期化
    /// </summary>
    public async void UDPClientReceiver_Init()
    {
        try {
            // UDP通信インスタンスの初期化
            p_Socket = new Windows.Networking.Sockets.DatagramSocket();
            // 受信時のコールバック関数を登録する
            p_Socket.MessageReceived += OnMessage;
            // 指定のポートで受信を開始する
            p_Socket.BindServiceNameAsync(udpReceivePort.ToString());
        } catch (System.Exception e) {
            Debug.LogError(e.ToString());

        }
    }
    
    /// <summary>
    /// UDP受信時コールバック関数
    /// </summary>
    async void OnMessage
    (Windows.Networking.Sockets.DatagramSocket sender,
     Windows.Networking.Sockets.DatagramSocketMessageReceivedEventArgs args)
    {
        using (var reader = args.GetDataReader())
        {
            var buf = new byte[reader.UnconsumedBufferLength];
            reader.ReadBytes(buf);
            
            UDPReceiveEvent(buf);
        
        }
    }
#else
    /// <summary>
    /// UDP受信初期化
    /// </summary>
    public void UDPClientReceiver_Init()
    {
        // UDP受信ポートに受信する全てのメッセージを取得する
        System.Net.IPEndPoint endPoint =
            new System.Net.IPEndPoint(System.Net.IPAddress.Any, udpReceivePort);

        // UDPクライアントインスタンスを初期化
        System.Net.Sockets.UdpClient udpClient =
            new System.Net.Sockets.UdpClient(endPoint);

        // 非同期のデータ受信を開始する
        udpClient.BeginReceive(OnReceived, udpClient);
        client = udpClient;
    }

    /// <summary>
    /// UDP受信時コールバック関数
    /// </summary>
    private void OnReceived(System.IAsyncResult a_result)
    {
        // ステータスからUdpClientのインスタンスを取得する
        System.Net.Sockets.UdpClient udpClient =
        (System.Net.Sockets.UdpClient)a_result.AsyncState;

        // 受信データをバイト列として取得する
        System.Net.IPEndPoint endPoint = null;
        byte[] receiveBytes = udpClient.EndReceive(a_result, ref endPoint);

        // 受信データを受信時処理に引き渡す
        UDPReceiveEvent(receiveBytes);
        ipAddress = endPoint.Address.ToString();
        udpClient.Send(receiveBytes, receiveBytes.Length, ipAddress, endPoint.Port);

        // 非同期受信を再開する
        udpClient.BeginReceive(OnReceived, udpClient);
        client = udpClient;
    }

#endif

    ///<summary>
    ///メッセージ内容を別スクリプトから参照する用のGetter関数
    ///</summary>
    public string GetUDPMessage()
    {
        return udpRecieve_message;
    }
    
    ///<summary>
    ///シーン遷移等における際にポートを閉じる用の関数
    ///</summary>
    public void ClosePort()
    {
#if WINDOWS_UWP
        p_Socket.Dispose();
        p_Socket = null;

#else
        client.Close();
#endif
    }

}

Unityで空オブジェクトを作成。名前をUDPに変更し，UDPMessageReceiver.csのスクリプトをコンポーネントに追加して下さい。 Rosから複数ノードを受け取る場合は，pythonを複数実行するので，Unityで受け取りのUDPも複数ポート開ける必要があります。そのため，上記と同じ手順でオブジェクトを作成し，ポートを送信側に合わせる必要があります。