Donkey Carに6DoFを追加する

やりたいこと　Raspberry Piで自己位置推定

部屋の中ではDonkey Carがブイブイ回せたので、もうちょっと高度なことがしたい

CPU負荷の大きいことをやろうとしたけど、JETSON-nanoがまだ届かない

自己位置推定はインテルのトラッキングカメラは全部中でやってくれるのでCPU負荷は低い

自己位置推定をトラッキングカメラ任せにして、同じ場所を強化学習しながら走り続ける車が作れないか？

とりあえず、自己位置推定のサンプルをRaspberryPiで回してみよう。

CPU負荷の低いことをしよう

あんなに苦労したトラッキングを簡単にやってしまうT265なるトラッキングカメラをIntelが発売していた。
カメラで重い処理を全部やってくれるので、ラズパイ３でも余裕で動くよね？

同じことをやってる先達はいっぱいいるので、参考にさせて頂きます。

走行させるのはいつものTT-02Bではなく、最近出番の少ないCC-01。
今回は後から何載せるのかわからないので、安定感のあるボディを採用。

「フィードバックしてね」

と、やっていたら、英文メッセージ着信。（英訳はGoogle先生に頼りっぱなし）
いつもSlackでお世話になっている人からメッセージ。

使わせていただくのだから、フィードバックはすべき。
けど、英語もまともに読めない私の感想が役に立つのかどうか？
そして今、日本は平成最後のゴールデンウィークで大混乱してる最中で、しばらくコワーキングスペースも使えない。

まずは、あなたのブログをよく読んでみる。
そして、1つ問題がある。
日本は明々後日、皇帝の退位と新皇帝の即位があり、平成から令和にeraがChangeするので、
10日間、パーツショップも工房も休みだ。（だから回答は遅くなる）

と返答。意味は通じたはず。通じるといいな・・・。

ブログをよく読んでみよう

T265をDonkeyで動かすブログはこちら。
まずは読んでみよう。

結論　素晴らしい
・・・早えーよ

一言で言えば、それが機能するときそれは本当に素晴らしいと正確です。それは時々それがその弱い瞬間を持っていてそれが混乱するのです。そして位置追跡はインクリメンタルなので、わずかなエラーでもその後のすべてに影響します

なるほど。
一度おかしくなったら、どうやって復旧するかが問題だな。

また、位置データの原点が常にカメラが起動された場所にあることもかなりの制限です。これはカメラ自体の問題ではありませんが、これを使用して何かを実装するときにはまだ考慮すべき点があります

周辺の映像やGPSの情報をもとに、原点復帰ができるといいんだけどな。

Donkeycarコミュニティのメンバーが、彼のユニットがRaspbianに取り組んでいると伝えました。彼は彼の魔術を分かち合うのに十分に素晴らしく、そして私たちはRaspbian上でrealsense SDKをコンパイルしました。

私はリポジトリの中にRealsenseとRaspbianのための公式の指示があるというヒントを得ました。あなたはそれらをここで見つけることができます。これらはOpenCVのようなもので、ヘッドレスのラズビアンランニングロボットには必要ありません。そのため、それらが機能する限り、私たちのより簡単な指示に従うこともできます。

ずいぶん見つけにくいところに、RealSenseのD435用のRaspbian(RaspberryPi3) Installationが置いてあるものだ。

とりあえず、指示に沿ってRaspberryPiのセットアップをしてみよう。

Donkey Carに環境をインストール

適当なバージョンで動くDonkey CarのROMを用意して、スワップファイルを拡張します。
コンパイルには必要です。

Blogの内容に沿って設定して、コンパイルの準備。

# Toggle swap off
sudo dphys-swapfile swapoff

# Edit the config file and increase the swap size to 2048
# by editing variable CONF_SWAPSIZE=2048
sudo nano /etc/dphys-swapfile

# Toggle swap back on
sudo dphys-swapfile swapon

# Reboot your raspberry
sudo reboot

# After reboot, check that swap size changed
free

# Should show something like Swap: 2097148

ファイル/etc/dphys-swapfileのCONF_SWAPSIZEを2048に変更する。

/etc/dphys-swapfile]

CONF_SWAPSIZE=2048

ここから後は、原文通りにインストールできるのだが、最後に詰まったのはここ。

#ファイルを消去する指示は、初回は無視。
#コンパイルに失敗した時は、buildディレクトリごと消去したほうがいいので私は使わなかった 
xarg sudo rm < install_manifest.txt
rm CMakeCache.txt

export CC=/usr/bin/gcc-6
export CXX=/usr/bin/g++-6
cmake -D CMAKE_BUILD_TYPE="Release"\
     -D FORCE_LIBUVC=ON \
     -D BUILD_PYTHON_BINDINGS=ON \
     -D BUILD_EXAMPLES=OFF  ..

#問題はここのmake -j4 私の環境では失敗する
make -j4
sudo make install
sudo ldconfig

-j4オプション禁止

コンパイルをかけると、途中で止まってしまいます。
一晩ほっといても70%程度で先に進まなくなる。
先方にも連絡してみたのですが、彼の環境では正常動作したが、他の環境では再現したらしい。
ということで、色々聞いて回ったところ、どうやら-j4オプションで全部のコアを使ってコンパイルをすると、メモリを使い果たして止まってしまうらしい。

そこで、

make

オプションなし。
これで一晩放置することとした。

寝る前に放っておいたら、正常にコンパイルが終わっていた。
インストールもすんなり完了。

サンプル実行

のサンプルプログラムをそのままコピーして動かしてみたところ、そのままで何の問題もなく動作。
さらに、RealSenseのサンプルプログラムも、~/test　にコピーして動かすと、問題なく動作した。

画像が出てくるわけではなく、6DoFの座標がどんどん出てくるだけなので、面白みは全然ないが、T265を動かしたら、数値が追従するのでほぼ正常に動いているようだ。

次の目標

とりあえず、RaspberryPi3B+でRealSenseT265を動かすことはできた。
次はDonkeyCarのソフトに組み込んで、自己位置推定させながら動作させてみたい。

ハードウェアはもう出来上がっているんだけど、なかなかソフトが追いつかないです。

謝辞

いつもSlackで助言をして頂き、今回も協力して頂いた、Mikko Pohja氏に感謝します。

Mikko Pohja氏のBlogはこちら。
https://markku.ai/post/realsense-t265/