2019年2月21日木曜日

続ROS#と戦ってみた

とりあえずの戦況報告。
前回、BSON化してLaserScanを送るという所までできました。
その後、細々とした修正をに加え、PointCloudを扱うコードを追加したりしてました。

とりあえずの移動ロボット可視化には十分つかえそうな感じです。
もし要望があるようならば、そのうち解説をかこうかなと思っています。

2019年1月26日土曜日

ROS#と戦ってみた

期待から始まるが・・・

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だろうが実績がある方法が転がっています。

デメリットもあって、JSONなのでデータがデカくなる上に、折角分散通信できるようなROSの設計ですが、rosbridgeというproxyに一旦データが集中して集まる事になります。平たくいって余り通信速度を求めてはいけません。

第一の壁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部分をちょっと書き換えるだけでした。

 第五の壁 まともにテストされてない・・・

これで晴れてLIDARが表示されました!でもなんかへん?LIDARの表示は球体・線・エリア塗りつぶしの3種類あるんだけど、球体だけがまともで残り二つは明後日の向きになる。と思ったら、残り二つはUnityのtransformの使い方、つまりlocalPositionとPositionの使い分けがきちんとできてない感じでした。そして二つの間違え方が違うので、不慣れなメンバーが混じってたのでしょう・・・。映ったからOK!でいろんな角度でのテストまではしなかったのかな?

 まとめ

 色々はまり所はありましたが、ROS-sharpは単機能な分トラブルシュートは簡単なのかなという感じです。ちゃんとARCore実機でカクつくこともなく動いたし。Buggyなのを除けば十分選択肢にはいるのかなという感じです。今回作ったのをプルリクするのか細々とforkしていくのかどっちが良いかは悩み所です。(BSONはオプションで切り替えとかじゃないとだめなかなぁ?)ご意見があれば嬉しいです。