移動ロボットの位置追跡問題
移動ロボットの制御をしたいときに意外と悩むのが位置情報をどのようにして取得するのかという問題です．
GPSやSLAMなどのようにロボット内部に搭載されたセンサで位置推定を行う手段もありますが，実験環境に精度が左右されやすい側面があると思います．
ロボット外部に設置する位置計測装置として有名なものとして，OptiTrackやVICONなどのようなモーショントラッカーが挙げられます．
Outside-Inの位置計測環境としてはこれらがベストだと思うのですが，いかんせん値段がお高めなので，気軽に試すとはなかなかいきません．
HTC Vive Trackerとは？
そこで今回注目したのが，今年発売のHTC vive Trackerです．
HTC ViveはもともとVRゲーム向けハードウェアで，部屋に固定されたBase StationからOutside-In方式の6-DOF(3次元位置＋姿勢)位置追跡が250Hz-1kHz [参考:Welcome to Steamworks]で可能なことが大きな特徴です．
参考：https://vr-maniacs.com/entry/htc-vive-roomscale/
HTC Viveはヘッドマウントディスプレイと２本のコントローラーで遊ぶものですが，わりと最近（といっても今年の３月）Vive Trackerと呼ばれる拡張コントローラーが発売されました．これを追跡させたいものに固定するだけで，簡単に位置情報を取り出すことができるというものです．
HTC Vive Tracker (www.vive.com)
バッテリー内蔵かつ無線通信ができ，サイズも小型なので，ロボット開発用途にも結構魅力的です．
何より一番気に入ってるのは、値段です．位置トラッキングを行うだけならヘッドマウントディスプレイとコントローラはいらないため，ベースステーション×２とVive Tracker１個でだいたい４万円くらいに収まります．OptiTrackやVICONだと値段は桁が２つくらい増えることを考えれば，6DOFの追跡装置として破格の値段であると言えるでしょう．あえて欠点を挙げるなら，80gとそれなりの重さはあるので，非力なロボットにはあまり載せられないくらいでしょうか．
今回は，Linux PC(Ubuntu)+ROS環境で実際にVive Trackerを動作させることを確認できたので，紹介します．
依存環境
- GPU搭載PC
- tracking処理自体には必要無いはずだが，SteamVR自体がGPUに依存しているっぽい？
- iGPU環境では試してないので，わかりません
- HTC vive Base Station ×２
- HTC vive tracker
- 広い部屋
テスト環境
- PC: Thinkpad p50s
- OS: Ubuntu 16.04LTS
- ROS: Kinetic
- GPU driver: nvidia-387
ドローンに載せたVive Trackerの位置情報をROSのtfメッセージとして得ることが目的です．
Trackerに付属する専用のUSBドングルでPC間の通信を行う仕組みです．
Parrot Bebop2に載っけてみた図
インストール
以下の手順は、基本的に 参考リンクの手順を踏襲しています。
これを書いている時点で、SteamVR for Linux自体まだベータのようですので、今後仕様が変更になる場合があります。使用する場合，常に最新の情報をチェックするようお願いします。
依存ライブラリを最初にインストールする
sudo apt-get install steam libsdl2-dev libvulkan-dev libudev-dev libssl-dev zlib1g-dev python-pip
sudo apt-get install libxtst6:i386 libxrandr-dev:i386 libglib2.0-dev:i386 libgtk2.0-0:i386 libpulse0:i386
pythonで使うためのライブラリ(pyopenvr)をインストールする
sudo pip install -U pip openvr
libudevのシンボリックリンクを作成
trackerと通信するUSBドングル関連の設定だと思うが，何やってるかわかんないです．とりあえず指示に従った．
sudo ln -s /lib/x86_64-linux-gnu/libudev.so.1 /lib/x86_64-linux-gnu/libudev.so.0
SteamVRのインストール
使用する環境は、SteamVRに依存しているので、最初にインストールを行う。
LIBRARYのタブに移動し、Searchの横の文字をクリックして、VRを選ぶ。
リストにSteamVRがあるので、インストールできる。
Steamの画面
ヘッドセットなしで使用できるようにする
初期状態だと，VRヘッドセットが未接続の状態でSteamVRを起動すると，トラッカーの追跡処理が行われないので，
設定ファイルを編集して，ヘッドセットを要求しないようにする．
gedit ~/.steam/steam/steamapps/common/SteamVR/resources/settings/default.vrsettings
でSteamVRの設定ファイルを編集。
3行目、"requireHmd" : true を "requireHmd" : false に変更することで、
Vive Tracker単体でのの使用ができる。
USBドングルとのペアリングを行う
PCとVive tracker間で通信ができるようにペアリングを行う。
SteamVR→Devices→PairController
からペアリングの画面に移行するので、指示に従う。
Vive tracker の場合、中心のボタンを長押しすると、ペアリングモードになる。
ペアリングが完了すると、図のような表示が現れるのでこれでSteamVR側の準備は完了。
NotReadyという表示が出ているが、これはHMDが繋がっていないからなので、tracker単体で使う場合には問題ない。
一旦設定が済んでしまえば，次回以降はUSBドングルを挿した状態でトラッカーのボタンを押せば自動的に認識し緑色のランプが点灯する．
（任意）端末上でSteamVRを起動する
いちいちSteamを開くのは面倒ですが，コマンドラインからも呼び出せるようです．
LD_LIBRARY_PATH=~/.steam/bin32/ ~/.steam/bin32/steam-runtime/run.sh ~/.steam/steam/steamapps/common/SteamVR/bin/vrstartup.sh
長くて覚えられないので，僕はエイリアスを.zshrcに登録してます．
alias steamvr='LD_LIBRARY_PATH=~/.steam/bin32/ ~/.steam/bin32/steam-runtime/run.sh ~/.steam/steam/steamapps/common/SteamVR/bin/vrstartup.sh'
ROSのメッセージとして，trackerのposeを取得
moon-wreckers/vive_trackerをrosのワークスペースに配置する。
ここでは、catkin_wsという名前のワークスペースを使用するとする。
$ cd ~/catkin_ws/src/
$ git clone https://github.com/moon-wreckers/vive_tracker.git
$ source ~/catkin_ws/devel/setup.bash # zshを使うならsetup.zsh
$ roslaunch vive_tracker vive_tracker.launch
座標系はBaseStation基準になってるので，地表固定に直すための座標系をpublishする．
$ rosrun vive_tracker vive_goal.py
これによって，実行したタイミングにおけるtrackerの位置・姿勢を基準とした座標系が/goal_locationとして定義されます．Tracker自体の座標系は機器固有のID名で定義されているので，tf_monitorかなんかで調べるといいでしょう．あとは，/goal_locationから/trackerの座標系をlookupすれば地表固定座標系における6DOF Poseが取得できます．
実際の動作
つまづいたポイント
nvidiaドライバーが古くて，SteamVRが正常起動しなかった．
ValveSoftware/SteamVR-for-Linux: Issue tracker for the Linux port of SteamVR
のサイトによると，現状（2017-12-11）では，GPUのドライバに387.12以降を要求するようです．
ドローン飛行時の振動をトラッカーのIMUが拾ってしまう
Bebop2特有の問題かもしれませんが，静止時はきちんと追跡するのに，
飛行時だけトラッキングがロストする現象がありました．
電波干渉の問題かなとも思ったのですが，飛行させてないときは問題なかったので，結構悩みました．
結果として，飛行時の振動がどうやら影響してたっぽかったので，免震ゲルをトラッカーと機体の間に３枚くらい挟んだら改善しました．
ちゃんと固定するなら，ドローン用のショックアブソーバを用意するのが良いかなと思ってます．
trackerのtfが30HzでしかPublishされない
vive_trackerのscripts/vive_tracker.py がrate = rospy.Rate(30) となってました．
本来もっとサンプリングレート出せるはずなので，
30のところをを250に変更すると，250HzでPublishされるようになりました．．
