「Jetson Nano 超入門」を読んでSLAMやってみた

#はじめに
いつもJotson関連の情報でお世話になっているからあげさんがJetsonNanoの本を書いたので、いつものように真似してみるよ。
（注意！JetPackは4.2.2を使ってね）

Jetson Nano本のサポートページ

#教材のレベルも高い
MIDIや３Dカメラをつかうページでは、D435やポケミクが必要だったりで、さすが超入門。
どこのご家庭にも必ずあるJetsonNanoと違って、デバイス側の在庫がある人は少ないはず・・・だけど、Twitter見てるとみんな持っているから不思議。
ちなみに私のはD435じゃなくてD435iだからセーフ（謎）

#JetPack 4.3
本が届く前にJetPack4.3がリリースされて、OpenCV4.1がプリインストールされた。
これで長々とビルドする必要がなくなった！
（注意！JetPack4.3だとSLAMが動かせなかったので、現状ではJetPack4.2.xを使って続行してください）

#Part7 ROSを使ってロボットの眼を作ってみよう をやってみる
いきなりすべての章を吹っ飛ばして第7章から行きます。
JetPackを64GのSDに焼いてスタート。
（第３章を参考にパフォーマンスやスワップメモリが設定してないとビルドで詰まります。）
親切なスクリプトを起動して、全自動でインストールしてもらいます。
はい、亀が出ます。

#RealSenseがバージョンアップされてパッチ不要に！
D435はパフォーマンスが上がらず、D435iは全く動かないデフォルトのJetsonNano。ひと晩かけて超めんどくさいパッチを当てていたのですが、JetPack4.3にバージョンアップされたのと時同じくして、インテルがRealSenseのドライバを改良してくれました。
サメのおじさんも大感激ですね。

Install RealSense Camera in 5 minutes – Jetson Nano

５分でインストールできるとサメのおじさんが豪語するとおり、あっという間に終わります。
以前では信じられないほど楽なうえ、パフォーマンスも上がっているように見えます。
7-3のカーネルにパッチを当てるところはパスできるので、超入門を読んでいる皆さんにはおススメ。

#？realsense_rosがビルドできない
これで楽にセットアップできると思いきや、catkin buildでエラー発生。
エラーメッセージから、realsense_rosはOpenCV3.xじゃないと動かないよ。とのこと( ﾉД`)ｼｸｼｸ…。

#JetPack4.3 → JetPack4.2.2 ひとつ戻る・・・
OpenCVの再ビルトとダウングレードなんぞやってられないので、別のSDカードにJetPack4.2.2を焼いて、再度サメのおじさんの手順でRealSenseをインストール。
ありがとう、JetsonHacks

git clone https://github.com/JetsonHacksNano/installLibrealsense
cd installLibrealsense
./installLibrealsense.sh
realsense-viewer

Viewerは順調に起動。
D435iはあっさり動きます。

./buildLibrealsense.sh

realsenseのSDKのビルドには時間が小一時間ほどかかりました。
スクリプトにお任せなので放置でOK。

#realsense-rosをビルド

cd ~/catkin_ws/src
git clone https://github.com/IntelRealSense/realsense-ros
cd realsense-ros
catkin build

最新のrealsense-rosのバージョンでビルドします。
JetPack4.3だとここで詰まりましたが、今回はあっさり通りました。

roslaunch realsense2_camera demo_pointcloud.launch

ポイントクラウドのデモも動きますが、やはり重いです。
realsense-viewerに見慣れていると、この遅さはいかがなものかと。
思っていたらアドバイスをいただきました。

あとはlibrealsense2使うときにDecimation filter（間引き処理）使ってるかどうかですかね。realsense-viewerはデフォルトで使用してます。 https://t.co/kxKuo3AJbA

— dandelion (@dandelion1124) December 29, 2019

確かにdecimation属性で間引きができそうです。
ROSの書式が難しかったのであとでやるとして、車体を用意します。

#車体を用意
仮想空間で動いているだけでは机上の空論、リアルワールドのボディが必要です。

DokneyCarに改造していたTAMIYAのTT02Bを引っ張り出し、カメラマウントを単眼レンズからD435i+T265用に交換。
PWMドライバのPCA9685を載せてあるので、これを使ってモーターを動かします。
すでにDonkeyCarで動いた実績があるのでハードウェアは安心です。

#「D435iを使用したSLAM」
githubのrealsense-rosのwikiにこんな記事がありました。
D435iを使用したSLAM
これはまさに私の必要とするものではないか？

あまり深く考えず、この通りに実行すると、それっぽく動きました。
速度も遅くない、壁も作れる。
しかし、少し早く動かすとIMUがドリフトを起こすようです。
自己位置の推定がうまくいっていないということでしょう。

それでは、自己位置推定に長けたT265を使えば改善しそうです。

#チュートリアルにもどって基本的なところの確認
そもそも、ROSが何かよくわからないまま進めてきたので、チュートリアルレベルに戻る。
書籍「ROS2で始めよう次世代ロボットプログラミング」のROS1の章にD435iの使い方が記載されている。

roslaunch realsense2_camera rs_rgbd.launch

を実行すると、異常が出て止まってしまう。多分ドライバやlaunchファイルがインストールされていないのだろう。
（注意！あとで sudo apt install ros-melodic-rgbd-launchで不足分をインストールして実行できました）

realsense2_cameraについているデモを片っ端から実行してみる。

opensource_tracking.launch はそこそこいい速さで動く。（設定の変更が必要）
rs_rtabmap.launch はイメージも表示されるが、効能が分からない。pointcloudとの差は？

rs_camera.launch はopensource_tracking.launch から呼び出される。
rs_d435_camera_with_model.launch D435のイメージも表示される。かなりそれっぽい感じ。

・・・いろいろやってみましたが、SLAMがきっちりできている感じではありません。

#githubのissueを詳細に読んでみる
githubのlibRealseseのissueでは活発な意見が交わされていました。
こんな話が出てきました。

D435でraspbery pi 3bでopensouce_tracking.launchを使用中の問題 ＃1044

IMU-less D435を使用したSLAMマッピングの場合、SLAMソフトウェア「ORB_SLAM2」が適切な選択である可能性があります。以下のリンクには、ROSで使用するためのガイドがあります。

ORB_SLAM2とは？
ORB_SLAM2

とりあえず、ソースをダウンロードして動かしてみます。
https://github.com/appliedAI-Initiative/orb_slam_2_ros
クローンしたらcatkin buildで再構築。
Jetson-nanoでもかなり時間がかかります。
（うちでは15分程度。シャドウミストレスが変身してからモモがダウンするまで）

#ORB_SLAM2を動かしてみる

roslaunch realsense2_camera rs_rgbd.launch

で失敗するので、rgbd-launchパッケージを入れる。

sudo apt install ros-melodic-rgbd-launch

ORB_SLAM2を動かしてみる

roslaunch orb_slam2_ros orb_slam2_d435_rgbd.launch

これで表示させてみよう。

#表示してみる
やり方とか考え方が適当なので、詳しい方の突っ込み待ちです。

roslaunch realsense2_camera demo_pointcloud.launch
roslaunch orb_slam2_ros orb_slam2_d435_rgbd.launch
roslaunch realsense2_camera rs_rgbd.launch

この順番で別々のターミナルで実行する。
ポイントクラウドで画面が出てくることを確認してから、順番に立ち上げると、それっぽい画面で表示されました。

毎秒30fpsで動作しているようですが、CPUの負荷をほぼ使い切って動いています。
ゆっくり走らせるなら、この点群に当たらないように、動かせばよさそうです。

#次回の予定
点群を取ることがROSでできそうなので、マップを生成したり、衝突回避のようなものを実装していきます。

編集中の次の記事はこちら
ROSでRealSenseを真面目に動かしてみる