自作 Turtlebot3 自律走行に向けたプログラム。#5
--- Turtlebot3 SBC(Jetson Nano 2G) で、rtabmap_ros で、Kp/DetectorStrategy=6(GFTT/BRIEF) を使う方法。 ---
副題: OpenCVにcontrib を組み込んで、rtabmap の再ビルド。
自作 Turtlebot3 自律走行に向けたプログラム。#4 の続きです。
rtabmap_ros で、Kp/DetectorStrategy=6(GFTT/BRIEF) を使うには、OpenCV に contrib を組み込まないといけない。とのことなので、
組み込んでみました。結構大変ぞね!!!
参考は、下記ページ。
introlab/rtabmap_ros
開発環境
Jetson Nano 2G JetPack 4 (Ubuntu 18.04)
Ros: Melodic
1. Turtlebot3 SBC に最小限必要なパッケージにする。
Turtlebot3 SBC(Jetson Nano 2G) で、rtabmap_ros を動かすのに必要なパッケージにのみにします。
$ sudo apt remove ros-melodic-desktop
$ sudo apt install ros-melodic-ros-base
当初、パッケージ版の、OpenCV4.1.1 との共存を試しましたが、rtabmap_ros の動作が変なので、
パッケージ版 OpenCV4.1.1 は、削除します。
$ sudo apt remove libopencv*
注)上記に替えて、下記でも良いかも。
$ sudo apt remove ros-melodic-perception
$ sudo apt remove ros-melodic-vision-opencv
$ sudo apt remove ros-melodic-cv-bridge
2. OpenCV 4.1.1 に、contrib and CUDA を組み込む。
下記を参考にして、組み込んでください。
Jetson Nano 2G OpenCV4.1.1 Build
3. OpenCV を使っていなそうな、必要なパッケージのインストール。
$ sudo apt install ros-melodic-geometry
$ sudo apt install ros-melodic-octomap
$ sudo apt install ros-melodic-octomap-ros (注) 不要かも!
$ sudo apt install liboctomap-dev (注) 不要かも!
4. cv_bridge のビルド (ブランチ:melodic)
$ cd ~/catkin_ws/src
$ git clone https://github.com/ros-perception/vision_opencv.git
$ cd vision_opencv
$ git checkout melodic
此処で、Jetson Nano に向けた変更をします。参考は、下記ページ
Compile vision_opencv on Jetson Nano
Step 4 は、不要。
$ cd ~/catkin_ws
$ catkin_make -j1
注) build は、Jetson Nano 2G 上で、テキストログインで行うと、早いし、問題がでません。
おんちゃんは、当初、PC から、ssh2 でログインして行っていましたが、遅いし、なぜか問題ない箇所でエラーになりました。
5. pcl_conversions のビルド。
$ cd ~/catkin_ws/src
$ git clone https://github.com/ros-perception/perception_pcl.git
$ cd perception_pcl
(ブランチ:melodic-devel)
$ git checkout melodic-devel
$ cd ~/catkin_ws/src
$ git clone https://github.com/ros-perception/pcl_msgs.git
$ cd ~/catkin_ws/src
$ git clone https://github.com/ros/geometry2.git
$ cd geometry2
(ブランチ:melodic-devel)
$ git checkout melodic-devel
$ cd ~/catkin_ws
$ catkin_make -j1
注) $ sudo apt install libbullet-dev が必要かも?
6. stereo_image_proc のビルド。(ブランチ:melodic)
$ cd ~/catkin_ws
$ git clone https://github.com/ros-perception/image_pipeline.git
$ cd image_pipeline
$ git checkout melodic
$ cd ~/catkin_ws
$ catkin_make -j1
8. turtlebot3、turtlebot3_msgs ビルド。 (ブランチ:melodic-devel)
注) turtlebot3/turtlebot3_example は、削除します。
$ cd ~/catkin_ws/src
$ git clone https://github.com/ROBOTIS-GIT/turtlebot3.git
$ cd turtlebot3
$ git checkout melodic-devel
$ cd ../
$ git clone https://github.com/ROBOTIS-GIT/turtlebot3_msgs.git
$ cd turtlebot3_msgs
$ git checkout melodic-devel
$ cd ~/catkin_ws
$ catkin_make -j1
9. rtabmap のビルド。(ブランチ:melodic-devel)
$ cd ~/usr/local/src
$ git clone https://github.com/introlab/rtabmap.git
$ cd rtabmap
$ git checkout melodic-devel
$ cd build
$ cmake ..
注) With OCTOMAP=YES がある事を確認します。
$ make
$ sudo make install
10. rtabmap_ros のビルド。(ブランチ:melodic-devel)
$ cd ~/catkin_ws/src
$ git clone https://github.com/introlab/rtabmap_ros.git
$ cd rtabmap_ros
$ git checkout melodic-devel
$ cd ~/catkin_ws
$ catkin_make -j1
11. お疲れ様。これで動くと思います。
注1) やっと、rtabmap_ros の動作が安定したか?
以前は、突然ワープしたり、起点の odometry の場所が移動したりしていたが、それがなくなったみたいぞね。
注2) Kp/DetectorStrategy と Vis/FeatureType は、同じ値にしないといけないみたいぞね!
Kp/DetectorStrategy = Vis/FeatureType = 1 (SHIFT) の方が結果がよいのか。
注3) 自立走行となると、将来は、move_base も必要になるが、move_base は、SBC 上で動くのだろうか?
12. その後、
やっと、自作 turtlebot3 での rtabmap_ros の動作が安定したので、じっくり rtabmapviz の画面を眺めていました。
Loop closure detection のフレームの意味は何か考えていましたが、おんちゃんが思うに、
これは、
カメラフレームデータと特徴点を１つのランドマークデータとして、逐次保存して行き、
移動場所での周囲の同様のカメラフレームデータと特徴点を用いて、
保存されているランドマークデータと突き合わせして、現在位置を特定(SLAM) しているのでは。と理解した。
その特徴点の作り方や、ランドマークの突合方法の処理の方法が、SHFIT、GFTT/BRIEF、... だけあると言うことか。
2(ORB) もあるみたいだが、これは、別途 ORB をビルドして、rtabmap のビルドで組み込めば良いのか?
此処に、記述があるみたいだ。
How does ORB SLAM work in RTABMAP? #707
13. 新たな問題発生。
下記エラーがでているみたい。
google で検索すると、下記ページがあった。
navigation (move_base) でロボットが一時停止を繰り返す
どうやら、map の出力が間に合っていないとの事。
Jetson Nano で、rtabmap_ros の処理が間に合っていないのか。
試しに。下記設定をしてみました。
1) rtabmap_ros/rtabmap パラメータ追加
2) Jetson Nano 2G のパフォーマンスをモードを上げる。
$ sudo nvpmodel -m 0
注) こちらは、すでに 0 : Max モードでした。
3) Jetson Nano 2G の クロックを上げる。
$ sudo jetson_clocks
注) こちらは、クロックがあがりました。起動時、毎回設定しないといけないみたいじゃ。
4) 此処に来て、新たな不具合発生です。どうやら、IMUが通信断になるようです。
odometry の pose.pose.orientation の部分に、ICM20948 の SparkFun_ICM-20948_ArduinoLibrary を使って、
DMP 6軸フュージョンの値を使用しているのですが、ロボットは、回転しているのに、Rviz 上のStereo PointClouds の投影方向が同じままになります。
odometry の pose.pose.orientation が更新されていないとしか考えられません。
原因が、ESP32 - Jetson Nano の Serial通信なのかもしれませんが。
また、ESP32 が壊れたかもしれません。完全に壊れては、いないようなので、新しいのを購入する前に、
もしかしたら、esp32 の SPI I/O の PULL UP が必要なのかもしれないと思って、
SPI通信の MOSI(23) だけ、INPUT_PULLUP を指定してみました。
MPU_SPI.begin(18,19,23,5);
// test by nishi MOSI(23) Pull UP 2023.12.13
pinMode(23,INPUT_PULLUP);
本当は、外付けにすれば良いのかもしれませんが。今は、様子見です。
やっぱり、これでは改善しませんでした。
ESP32 か壊れたか!!
試しに、odometry の pose.pose.orientation を、IMUを止めて、本来のモータ回転からにして、同じテストをしてみました。
6回、テストしてみて、問題は出ませんでした。エラー表示は出ましたが、無事、毎回、同じように完走しました。
ESP32 - Jetson Nano の Serial通信 の問題ではなくて、やはり、また、ESP32 の SPI通信の不具合のようです。!!
この問題が解決すれば、自作 Turtlebot3 & rtabmap_ros で何回テストしても同じような結果が得られる事が、確認できそうです。
何回テストしても、おかしな動きをせずに、同じように完走できる事が、今後の自立走行プログラムに向けて、一番必要です。
試しに、odometry の pose.pose.orientation を、turtlebot3_core.ino IMU のオリジナルのmadgwick の 6軸フュージョンにしてみました。
こちらでは、 IMU のデータ断はでないようです。但し、平滑化不足と誤差が大きのか、rtabmap_ros で、
自己の位置決めが出来なくて、ロボットが完走できません。
どやら、SparkFun_ICM-20948_ArduinoLibrary の DMP 6軸フュージョンのデータ取得の問題のようです。
こちらは、6軸フュージョンの性能は、すこぶる良いのですが、データ取得に安定せいがないのか。これが改善できれば完了だが。
どうやら、ESP32 から、 ICM20948 へのアクセス頻度が大きすぎたようです。
ICM20948 へのデータアクセスを、 arduino の loop() からを止めて、別 core のタスクからにして、かつ、アクセスRateを、90[Hz] にしました。
オリジナルの turtlebot3_core.ino が、 loop()のなかから、常にIMUのデータを取得させていたので、それを踏襲していました。
また、IMU のパブリッシュRateも、200[Hz] だったので、それを使っていましたが。
しかし、ESP32や、ICM20948 には過酷ではなかったか。と思います。
今は、60[Hz] にしていますが、IMUデータの取り込みと、Odometryのパブリッシュが別コアタスクになったので、
排他制御のミューテックスを組み込みました。
因みに、Odometry のパブリッシュRate は、StereoCamera のFPS と同じにしているので 15[Hz] です。
ICM20948 のマグネットの Data Output Rate が、100[Hz] なので、DMP 9軸フュージョンも、100[Hz] 以下で取り込めばうまくいったのかも?
時間があれば、再度、DMP 9軸フュージョンで確認してみたいぞね!!
以上で、どうやら安定して、rtabmap_ros が動かせるようになったみたいぞね!!
また、不具合がでたら、書きます。
やっぱり、IMUのデータ取得に問題がありました。
ICM20948 SparkFun_ICM-20948_ArduinoLibrary の DMP 6軸フュージョンのデータアクセスは、どうしてもデータが取れない事があります。
プログラムを、これを前提に作らないといけない様です。
なので、ICM20948 への DMP 6軸フュージョン のアクセスRate を高くして、200[Hz] 、100[Hz] でデータアクセスを実行して、
その中で、正常に取得出来たデータの直近のものを、Publish に使う様にしました。
オリジナルの Turtlebot3_core.ino がこの発想でした。
注1) 200[Hz] と言えば、5[ms] ごとにアクセスすることになります。Arduino のシングル loop()処理ので中でこれを実現するのは所詮無理です。
注2) なので、おんちゃんは、別タスクで、DMP 6軸フュージョンを、100[Hz] でデータ アクセスを実行して、正常に取れたデータだけを取得クオータアニオンとして更新します。
うまく取れなかった時は、古いデータのままにしてきます。その後、ミューテックスで排他制御をして、データの受け渡し領域へコピーします。
注3) loop() の中で、Publish する時は、同じミューテックスで排他制御をして、データの受け渡し領域から、クオータアニオン のデータを取り込んで、Publish します。
これで、やっと、うまく動くようになりました。
注4) DMP 6軸フュージョン のデータ アクセス時に、エラーの時は、LED をON/OFF 反転させて、どれくらいエラーが生じているのか、見える化しましたが、
結構、LED は点滅しています。これでも下手な鉄砲も、数打てば当たる原理で、なんとか動いています。
その後。
上記までの設定で、うまく動いていましたが、また、突然、IMU のデータが取れなくなる症状がでました。
さっきまで、うまく動いていたに、突然症状が出るのは、今までの経験から、ハード不良を疑ってみました。
試しに、思い切って、ESP32 の SPI Pin の全てに外付けの PullUp 1[KΩ] をつけてみました。ブレッドボードで試したら、
問題ないようなので、ロボットにもつけて様子見です。
外付けPullUp をつけないで、IMUのデータが取れない症状の出た ESP32 は、もう内部で、SPIが傷んでいるみたいです。
新品の ESP32 に交換して様子見です。
やはり、再発しました。
たまたま、ArduinoのMadgwickライブラリの使い方 を見つけて、見ていたら、
MadgwickAHRS の設定値に、センサーのサンプリング周波数 と 補正ゲイン が有ること知りました。
早速、見捨てていた、MadgwickAHRS.cpp で試してみました。
どうやら、補正ゲイン 0.1f を、0.001f にしてみました。
//#define betaDef 0.1f // 2 * proportional gain
// test by nishi 2022.1.8
#define betaDef 0.001f // 2 * proportional gain
これで大夫、ロボットの微細な揺れが収まりました。
早速、MadgwickAHRS の 6軸 Fusion で、自作 Turtlebot3 をうごかしてみます。
大丈夫みたいですが、ロボットがカーブを描く動作で、Rviz 上で横滑りしているようです。
やはり、6 軸 fusion では、限界かと思い、諦めていた MadgwickAHRS の 9軸 Fusion を再度ためす事にしました。
今回は、サンプリング周波数=100 としました。ICM-20948 の Magnet の出力Rate=100[Hz] なので、それに合わせます。
手始めに、ブレッドボードで、90度ずつ回転させて、Rviz で、正確に 90度ずつ回転するのか確かめました。OK のようです。
Spark Fun Libraliy の DMP 9軸フージョンや、他のサンプルで試した時は、この段階で、NG でした。
次に、実際に 自作 Turtlebot3(foxbot_core3) で試した見ました。
残念ながら、起動時点で、Rviz でロボットの向きをみると、へちを向いています。
それでも、Python プログラムでロボットを、90度ずつ1回転させると、ちゃんと追随します。
以前は、この時点で、Rviz 上で、ロボットが突然あっちこっち向いていました。
かなりの改善です。
さて、あとは、この起動時のロボットの姿勢のずれをどうするか? です。
ロボットの IMU の取り付け位置がずれていないか、じっとみていたら、どうやら、 IMU の Magneto は、正確な方向を示しているようです。
ICM-20948 の取り付けのズレや、ロボットの傾きを正確に、しかしちょっと大きく表示しているようです。
どやら、データの扱い方に問題がある点に気が付きました。
IMU の Magnet は、地磁気の絶対値を使うのだから、起動させた時のロボットの向きを、毎回、忠実に反映するのだろう。
この事から、考えて、
結論を言えば、起動時の IMU からのデータを起点として、それを下駄として扱い、それからの差の値を使う。
どうやら、これが、キャリブレーションと言う事のようです。
早速、オリジナルの acc のキャリブレーション処理を参考に、maganet のキャリブレーション処理を組み込んでみました。
処理は、オリジナルにならって、起動に、512 回 magnet データを取り込んで、
その平均値を、基準値とします。
以後は、取り込んだ、maganet データから、基準値を引いた値をセンサーデータとする。
こうする事で、ロボットの起動時の向きが、どんな方向でも、Rviz で同じ向きに表示される事になります。
これで、実際に自作 Turtlebot3 and Stereo Camera で、rtbamap_ros の Mapping を試してみました。
Python プログラムで、ロボットを動かして、部屋の 3D Map を撮ってみましたが、感動です。
部屋の壁が、真四角に、向かい合う壁が平行に、隣合う壁が直角に、Rviz に表示されました。
6軸 Fusion では、此処まで正確にできませんでした。さすが 9軸 Fusion です。
地磁気の絶対値が使えるので、ロボットがどんな動きをしても、ロボットの姿勢は
、正確に取れます。
Thank You MadgwickAHRS. 今まで信用していなかったのを勘弁しとうせ。
これで、Spark Fun Libraliy の DMP 6軸フージョンとは、おさらばです。
その後、MadgwickAHRS + 9軸 Fusion で、ブレッドボードで、机の上に置いたままにして、
Rviz で、 TF を見ていたら、Z軸を中心に、少しずつ回転しているみたいです。
残念ながら、これでは使えない。
なぜ、こうも IMU は、問題がでるのか?
どうやら、MadgwickAHRS.cpp で、q0,q1,q2,q3 にちょっとずつ誤差が蓄積されていっているようです。
小数点以下、第4桁までを有効として、四捨五入にしました。
MadgwickAHRS.cpp
後、これで、gain は、0.0025f まで上げれました。
//#define betaDef 0.1f // 2 * proportional gain
// test by nishi 2022.1.8
#define betaDef 0.0025f // 2 * proportional gain
あと、Magnet データは、自分で組み込んだ処理では、取れない時があるみたいなので、
Spark Fun の Libraliy の方を使うこととしました。
Magnet データが正常に取れた時は、LED をOn/Off させて、見てわかるようにしました。
やはり、あまり芳しく無いので、SparkFun の DMP 6軸 Fusion をしばらく使うようにします。
こちらの方でテストしていて、IMUデータが突然、取れなくなると思っていた現象ですが、どうやら、
ESP32 - Jetson Nano 2G との間の通信の問題のような気がします。
TTL-Serial を止めて、Wi-Fi で試すことも考えられますが、その前に、Jetson Nano 2G の不要なサービスを止めてみました。
whoopsie
注) whoopsie は、サービスをDisable にしても効果がないので、パッケージを削除しました。
ModemManager (package: modemanager)
rpcbind
dnsmasq