期待から始まるが・・・
UnityからROSを使いたくて、ROS#に手を出してみた。独シーメンス社のオープンソースということで、勤め先内部的にインパクトがあるっていう下心はもちろんあるんだけど。ARCore/Hololens上のUnityからロボットにアクセスしたかったので、UWPとかにも対応させたかった。ROS.NETのほうがTFも使えて多機能なんだけど、その辺の移植が死にそうなので諦めて、同様にROSSharpも諦めた。（うろ覚えだけどXML周り）
TFが使えないので同期がとれてるフレームを切りだすとかできないけど、ROS側でTFを解決してPoseStampで投げつけるかぁ。という感じで割り切りました。
ROS#のしくみ
ROS#がUWPに移植出来たりするのは中身が単純だからなのです。ROSデータの中身は単純なTCPで垂れ流し状態なのですが、それが何処にあるのか？を問い合わせるのにROS_MASTER(roscore)にお伺いを立てる必要があります。その時に使うXML-RPCのライブラリが.NETFrameworkにゴリゴリに依存してるとUWP(.NET Core)に移植しづらいという事になります。ここでrosbridge_serverの登場です。rosbridge_serverは全ての通信のwebsocketで受けとめてくれる。proxyサーバです。通信すべき相手の場所がどこか？という事は気にする必要はありません。また通信内容はJSON(オプションでBSON)なのでUWPだろうがUnityだろうが実績がある方法が転がっています。
第一の壁NTP on Android in 化石社内ネットワーク
ロボットは複数のセンサの結果の整合性を保つため、センサ値や座標情報にはミリ秒精度のタイムスタンプが打たれます。この時間を同期するシステムは実はROSにはなくOSの時間をNTP等でミリ秒単位で合わせろという無茶振りをしてくるのがROSなのです。(ROS2はどうだか知らない）さて、化石のような頭の硬い会社でROSを使いましょう。社内ネットワークにNTPは立っているが何故か平気で秒単位でNTP時刻からずれています。社外のNTPは使えません。ここまでは問題はないです。そこにAndroid端末がやってきます、Andoroidは社内の怪しいNTPサーバで時刻合わせなんてしてくれません。つまり、タイムスタンプが合わないのです。古すぎるor未来の時刻のデータは捨てられてしまい、通信不能に陥ってしまいます。
そこでOSの時間を合わせるのを諦めて、アプリケーション(Unity)内でNTPをつかってUNIX時間を作ります。なお、NTPサーバはpythonのサンプルがあったので流用しました。（myntp.py）
メッセセージヘッダにセットするのはROS#がよく考えられていて、StandardHeaderExtensions.cs にExtensionとして時刻をセットする関数を作るだけです。なお、NTPに時刻同期はNtpTime.csのように10秒に一回程度の頻度で時差を取り寄せています。
第二の壁 座標合わせ
壁というよりは、どうしようもなく出てくる仕様なのですが、何らかの方法で座標を合わせる必要があります。その座標計算に使うTransform行列がUnityがMatrix*Vectorな順番なのに対して、ROSのTF2ライブラリでつかえるPyKDLがVector*Matrixな順番で頭が混乱するといった程度なのですが、その辺の行列を生で触るのは初めてだったので半日潰してます。第三の壁 マイペースなgazebo
実機を持ち出すとぬくぬくしたパソコンの前じゃなくて、クソ寒い工場に追い出されます。なのでギリギリめんえgazeboシミュレータでの物理シミュレーションを使った開発をします。当面gazeboで物理シミュレーション、ROSでロボット制御、Unityで可視化という流れです。折角NTPに対応したので、use_sim_timeパラメータをfalseにして、リアルタイムのシミュレーション結果にROSの時間をあてがうようにします。普通に動いたので安心してLIDAR(LaserScan)の可視化をしようとすると、データが見当たりません。gazeboからの位置情報などは正しい時間をさしているものの、センサだけはシミュレーション時間をさしています。設定で逃げれるものではなく、ソースをかきかえなければいけない状況だったので、こんな感じにセンサーの時刻を現在時刻に書き換えるROSノードをでっちあげました。第四の壁 JSON、NaN食べない。
これでようやく、LIDAR可視化できるぞ！とおもったらUnity 上ではウンともスンとも言いません。Subscribeしてるのに、一つもメッセージがきません。と思ったらWebsocketを流れるJSON に観測値に混ざって「null」という見慣れない文字。JSONの数値はNaN,Infが扱えないので、rosbridge側で勝手にnull値をいれていました。このnull問題、LIDARの性質上10cm以下や10m以上は欠測なのでInfやNaNが結構入ります。ros-sharpのissues読んでるとこれもproxyノードで適当な値をいれて逃げてますが、LIDARはともかく他のセンサまで考えると、欠測は欠測だろ。数字を入れるのは美しくない・・・。という気もします。そこで馬鹿正直な方法としては「float」型を「float?」型にするという手があります。するとJSON.NETではnullable(null値を取る事ができる）floatとして処理されます（ただし処理オーバーヘッドがある）。確かにこれで解決はできるのですが、ソースコード中のfloatを片っ端からfloat?に書き換えて、nullチェックを全てに足すのは流石に面倒。ということで、rosbridgeの実装を眺めてるとBSONという文字をソースコードに発見。rosbridgeにbson_only_modeという機能があるのに気が付いたのでした。BSONの場合数値をdouble型にするので、NaNもInfも表現できます。ただ、rosbridgeがBSONの時も無駄にnull置換をしていたので、BSONモードの時にかぎってnull置換をしないようにしました。ROS#側はJSON.NETがそもそもBSON使えるというのがあって、Serialize/Deserialize部分をちょっと書き換えるだけでした。
0 件のコメント:
コメントを投稿