UnityでROS#で独自のメッセージをSubscribeする

目的

前回導入したROS#で、前々々回作成した独自のメッセージをUnityで受け取ってみる。
結論から言うと、メッセージを受け取ることはできたが問題ありなので、ここではトラブルシュートの過程を記載する。

SubScriberを作る

Unity側でのメッセージのクラスとSubscriberのクラスを作成する。
メッセージのクラスは、ROS#のEditor拡張から作ることもできる。便利。

MsgGNSS.cs

/*
This message class is generated automatically with 'SimpleMessageGenerator' of ROS#
*/

using Newtonsoft.Json;

using RosSharp.RosBridgeClient.MessageTypes.Std;

namespace RosSharp.RosBridgeClient.MessageTypes
{
    public class MsgGNSS : Message
    {
        [JsonIgnore]
        public const string RosMessageName = "/ros_gnss_test/MsgGNSS";

        public Time stamp;
        public String data;

        public MsgGNSS()
        {
            stamp = new Time();
            data = new String();
        }

        public MsgGNSS(Time stamp, String data)
        {
            this.stamp = stamp;
            this.data = data;
        }
    }
}

GNSSSubscriber.cs

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

using RosSharp.RosBridgeClient;
using RosSharp.RosBridgeClient.MessageTypes;
using Newtonsoft.Json.Linq;
using Newtonsoft.Json;

namespace RosSharp.RosBridgeClient
{
    public class GNSSSubscriber : UnitySubscriber<MsgGNSS>
    {
        public string messageData;

        protected override void Start()
        {
            Debug.Log("SubscribeStart");
            base.Start();
        }

        protected override void ReceiveMessage(MsgGNSS message)
        {
            Debug.Log("ReceiveMsg "+ message);
            messageData = message.data.ToString();
        }
    }
}

SubscriberをROSConnectorをアタッチしたGameObjectのアタッチして動かしてみたが、メッセージを受信している気配がない。
さて困った。

うまくいかないからパケットキャプチャで確認する

ラズパイのROS側のコンソールログを見てみると、

2019-12-22 14:32:34+0000 [-] [INFO] [1577025154.734286]: [Client 20] Subscribed to /ros_gnss_msg
2019-12-22 14:32:39+0000 [-] [INFO] [1577025159.331651]: [Client 20] Unsubscribed from /ros_gnss_msg

ちゃんとSubscribeできているみたい。
しかたがないので、WireSharkでパケットキャプチャしてみたところ、それらしきWebSocketのパケットは送受信されている。
つまり、ROS#の側まではデータがきているが、自前のSubscriberまで来ていないということに。

C#のソースコードの形でライブラリを導入してデバッグする

Asset Storeや、UnityPackageでの導入だとROS#はRosBridgeClientをdllとして持っていてそれを使う。そのため、そもそもデバッグがしづらい。
そのため、リポジトリをcloneして、DLLの代わりにRosBridgeClientのソースから導入してみる。
ほぼそのまま動くようになったので、Visual Studioでブレークポイントはったり、Debug.Logで値を出力しながらデバッグしていく。

判明した原因

独自メッセージの中の、RosSharp.RosBridgeClient.MessageTypes.St.StringがうまくJSONから変換できていない。Execption出しているのだが、握りつぶされてすごくわかりにくい状態になっていた。

ライブラリ側に手を入れて、むりやりメッセージを受け取る

一旦正攻法での解決は後回しにして、ライブラリ側に手を入れて、メッセージをなんとか無理やり受け取れるようにしてみる。

RosSocket.cs内の、メッセージをディスパッチしているところに、どうせ他のメッセージこないからと、以下のようなコードを入れてみる。

                case "publish":
                    {
                        string topic = jObject.GetValue("topic").ToString();
                        foreach (Subscriber subscriber in SubscribersOf(topic))
                        {
                            //subscriber.Receive(jObject.GetValue("msg"));
                            GNSSMsgBind.sec = uint.Parse((string)jObject["msg"]["stamp"]["secs"]);
                            GNSSMsgBind.nsec = uint.Parse((string)jObject["msg"]["stamp"]["nsecs"]);
                            GNSSMsgBind.data = (string)jObject["msg"]["data"];
                        }
                        return;
                    }

我ながらひどいやりかただ。もちろん同時に以下のようなこれまたひどい設計も何もあったもんじゃないクラスを作っておく。

GNSSMsgBind.cs

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class GNSSMsgBind
{
    public static string data;
    public static uint sec;
    public static uint nsec;
}

さらに、表示用として、以下のコードを作成して、GameObjectにアタッチしておく。

GNSSSubscDisplay.cs

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class GNSSSubscDisplay : MonoBehaviour
{
    public string data;

    // Update is called once per frame
    void Update()
    {
        data = GNSSMsgBind.data;
    }
}

なかなかひどいやり方だが、メインスレッドとは別スレッドなので、直接呼出しがしにくく、一番手っ取り早い思いついた方法がこれだった。

ちゃんとラズパイのROSからのメッセージが受信できている。

何が問題か分かったが、納得いかないので今後の課題

この解決方法ではさすがに厳しいので、デシリアライズをちゃんとすることを考えなければいけないが、この確認のためにかなり時間を取ってしまったので、いったんここでおしまい。