自作 Turtlebot3 自律走行に向けたプログラム。#4
--- Turtlebot3 Navigation の move_base で base_footprint の代わりに、base_link(base_link0) を使う方法。 ---
自作 Turtlebot3 自律走行に向けたプログラム。#3 の続きです。
前回の方針転換前のプログラムの解決方法が判ったので掲載します。
前回は、下記エラーが出て navigation がまるきり反応しなかった点への対策です。
最終的な問題が、turtlebot3/turtlebot3_description/urdf/turtlebot3_waffle.urdf に、TF Tree のすべての親(root)を、
base_footprint から定義している点に在りました。ここを変更しないといけないです。
今回は、ちょっと、修正箇所が多くなります。
1. turtlebot3_waffle.urdf の変更。
turtlebot3_waffle.urdf を turtlebot3_waffle_nishi.urdf にコピーして変更します。
注) base_footprint を base_link0 に変えます。
2. turtlebot3/turtlebot3_bringup/launch/turtlebot3_remote.launch の変更。
turtlebot3_remote.launch を turtlebot3_remote_nishi.launch にコピーして変更します。
3. turtlebot3/turtlebot3_bringup/launch/includes/ description.launch.xmlの変更。
description.launch.xml を description_nishi.launch.xml にコピーして変更します。
4. 前回の修正の変更。
turtelbot3/turtlebot3_navigation/param/global_costmap_params.yaml
turtelbot3/turtlebot3_navigation/param/local_costmap_params.yaml
5. mode1.yaml の変更。
6. launch ファイル。
rtabmap-nishi_stereo_outdoor2_bx_navi.launch
注) turtlebot3_bringup/launch/turtlebot3_remote_nishi.launch を include する。
7. 以上です。
これで、動くと思います。
早速、rosrun ros_start multi_goals1.py を試してみました。
Stereo Camera の PointCloud2 は、それなりに更新されますが、2[M] 行って帰って来た場所が、ちょっと横に 20[cm] ほどずれます。
なんか、動きが、方針転換版とおなじ様な気がします。
最後の停止位置の確認の為の、rtabmap_ros の、Map の補正動作がされない気がします。
stereo_image_proc の出力と、ekf_localizatioon_node の出力の fusion/odom の時間のズレが大きくて、
rtabmap_ros の 3D Map が結構ずれているのが原因かも知れません。
やはり、専用 Stereo Camera 、Depth Camera を使って、stereo_image_proc を使わないようにすべきだと、おんちゃんは思う。
ココら辺が、自作 Stereo Camera の限界か!!
その後、対策が見つかりました。
原因は、Lanの伝送遅れに伴う時間のズレにあるようです。
試しに、Wi-Fi Lan(180Mbps?) と、有線LAN(1Gbps) とで、ホストPC のシステムモニターで、CPUの負荷、LANの負荷を比較していた所、
LANの負荷は、どちらも、 6.9-7.3 MiB/秒 で同じでした。なので、Wi-Fi Lan(180Mbps?) がボトルネックになっている点は確認されませんでした。
但し、有線LAN(1Gbps) にすると、rviz の画面で、Stereo Camera/Point Cloud2 の表示が、良いように感じられました。
この事から、Lan の伝送遅れによる時間のちょっとしたズレが原因のような気がします。
対策としたは、Wi-Fi ドングルを 480Mbps に変える。が良いのでしょうが、
おんちゃんは、試しに transform_time_offset=0.1 にしてみました。
mode1.yaml の変更。
これで、Stereo Camera/Point Cloud2 の表示の切れがなくなって、rtabmap_ros の 3D Map の表示の大きなズレが改善されました。
3D Map の表示の大きなズレが改善されたので、rtabmap_ros の最後の位置補正が働くようになったと思います。
注) その後、本当の原因がわかりました。by nishi 2021.10.26
上述の、transform_time_offset: 0.1 の問題ではありませんでした。
本当の原因は、自作StereoCameraの取り付け方法にありました。
そのことは、ROS rtabmap_ros 自作 Stereo Camera の 注1) から、注4) へ書き加えておきました。
上記対応で、Stereo Camera/Point Cloud2 がきれいに出力されて、なおかつ、焦点距離が 70[cm] から、57[cm] と短くなった事で、うまく動作するようになりました。
焦点距離が 70[cm] だと、部屋の中で動かすと、近くの物がまるっきり判定できなくて、rtabmap_ros がまともに動きません。
欲を言えば、焦点距離 35[cm] になれば、部屋の中での、rtabmap_ros の性能が、もっと上がると思います。
8. 現状で、Python Script で、ロボットを動かす勉強は、
はじめに、Active Mapping を使って、ボットを動かして、3D-2D Map を作ります。
次回は、ratbamap_ros を、local_bundle:=true で起動すれば、作成済み、3D-2D Map を使って、Python Script でNavigation を動かせるので、
これを使って勉強します。
此処でも、 焦点距離が 70[cm] だと、おんちゃんの部屋では、まともに動きません。
現在地を把握するために、ロボットがクルクル回って周囲を撮影するのですが、ロボットのスタート位置の回りに、
40[cm]の所に障害物があって、それが把握出来なくてクルクル回り続けるだけです。
焦点距離 57[cm] 版にすると、やはりロボットがクルクル回って周囲を把握するのですが、
なんとか現在位置を把握できる様で、2[M] 行って、帰ってきて、定位位置 (5[cm]以内) に停止するようになりました。
焦点距離 57[cm] では、おんちゃんの部屋では、もっと近くに障害物が出てくるので、現在位置の把握に四苦八苦します。
焦点距離 35[cm] くらいになれば、おんちゃんの部屋でも、OK だと思います。
結論。
1) rtabmap_ros & Stereo Camera では、動かす環境に応じた、焦点距離 の Stereo Camera が必要と言う事です。
2) 唐突ですが、rtabmap_ros / rtabmap に渡す、frame_id の tf (あるいは、 odom) は、できるだけ正確な値が必要です。
正確と言うのは、もちろん、tf や、odometry の値が正確(ロボットの位置と向きが) なのと、入力の Stereo Camera( Depth Camera)
のフレームと時間軸が一致していると言う事です。
これがずれていると、rviz で表示される、3D Map がうまく重なりません。
ちょっとずつずれて、きたなくなります。
ここを、本当に正確に一致させると、見違えるほど、3D Map の映像がきれいになります。
また、ロボットを走らすところが、屋外の様に起伏、段差がある場合は、rtabmap_ros / rtabmap に IMU を与えます。
wait_imu_to_init = true
imu = 実際のIMUトピック名
3) IMU は、Arduino Turtlebot3.ino の IMUライブラリーに、ICM-20948 SPI 接続で使っています。
IMU ライブラリーの中で、MadgwickAHRS.cpp を使って、6軸あるいは、9軸のセンサーフュージョンが出来ますが、
6軸 のセンサーフュージョンを使います。9軸は、使い物になりません。
4) 上記、6軸フュージョンの補足。
6軸フュージョン の計算結果は、現在、/imu トピックとして出しているだけですが、
これを、 foxbot_core3 (自作Turtlebot3) が出力している /odom の odom.pose.pose.orientation や、
TF の /base_footprint の odom_tf.transform.rotation に直接利用すれば、
robot_localization/ekf_localization_node が必要なくなるのでは、と考えちょります。
現在は、どちらの値も、モーターの回転から算出した値を使っているので、誤差が出てきて、それを補正するために、
robot_localization/ekf_localization_node を使って、/odom と /imu をフィルターを通して補正しています。
この部分を止めれば、よりシンプルになるのではと思います。この方法で rtabmap_ros の3D Map がきれいに出来れば良いので、後で試してみたいぞね!!
5) 上記を試してみました。
大丈夫みたいです。robot_localization/ekf_localization_node を使わないで動きました。
但し、Rviz の画面で見ていると、ロボットは停止しているのに、地面が常に前後に上下しているようです。
どうやら、IMU の 6軸ヒュージョンの 値がつねに、ゆらいでいるみたいです。
MadgwickAHRS.cpp の性能か、それに渡す、IMUのデータの所為かわかりません。
結局、SparkFun_ICM-20948_ArduinoLibrary を使って、DMP の 6軸 ヒュージョン をためしたら、小さな揺らぎは、殆ど無くなりました。
長い周期のゆらぎは残りますが、こちらの、DMP の 6軸 ヒュージョンは、なかなか優秀みたいです。
但し、foxbot_core3.ino で使用すると、時々不正データが上がってくるので、これの対応をしなといけない。 ICM-20948 & DMP の問題では無いみたいだが。
6) 上記、ICM-20948 & DMP での不具合の件ですが、どうやら、 ESP32 - Jetson Nano 2G 間の uart の通信速度が遅いのが原因みたいです。
MadgwickAHRS.cpp & 6軸フュージョンの時も、時々、Check Sum エラー が出ていたようですが、この場合は、
MadgwickAHRS.cpp の処理で時間がかかっていた様で、実際に、uart で出ていくフレーム数が少なくて、Check Sum エラー が気にならなかったようです。
ところが、ICM-20948 & DMP にしたおかげで、uart で出ていくフレームが増加して、トラフィック負荷が増えた為、
Check Sum エラー が頻発した。ようです。
今まで、230400 bps でしたが、これでは、全く遅すぎて、 Check Sum エラーが頻発します。
500000 bps、1000000 bps では、時々、Check Sum エラーがでて、ロボットが暴走します。
2000000 bps で、やっと、OK になりました。 ESP32 - Jetson Nano 2G では、大丈夫みたいです。
それでも、2 [Mbps] なので、ここが、将来はボトルネックになりそうです。できれば、 Ether Net にしたいぞね。
7) move_base でナビゲーションを使って、python Script でロボットを動かしていると、rviz 画面上で、
突然ロボットの位置がワープ(50cm 程瞬間移動)してしまう現象がでて困ってしまう。
どうやら、下記、ワーニングが出ているみたいぞね!!
Google で調べたら、https://answers.ros.org/question/241969/clearing-costmap-to-unstuck-robot-3000000m/ が見つかった。
どうやら、move_base の costmap 指定で、ロボットが壁に衝突となっているみたいです。
早速、turtlebot3/turtlebot3_navigation/param/costmap_common_params_waffle.yaml を修正してみます。
turtlebot3/turtlebot3_navigation/param/costmap_common_params_waffle.yaml
footprint と、inflation_radius を見直します。footprint は、実際のロボットCar のサイズにします。
やっぱり、突然のワープは、起るようです。Warning は同じように出ていまが、それと突然のワープは、連動していない気がします。
結論としては、自作 Stereo Camera による、stereo_image_proc の出力が途切れている所為のような気がします。
やはり、専用 Stereo Camera、Depth Camera で試さないと。うまくいかないのかもしれません。
どうやら、メジャーで測った実測の距離と、Rviz での距離がかなりずれているようです。
実測、2.3 [M] が、
Rviz で、2.9 [M] になっています。2[M] を超える測距離が、結構ずれています。
rtabmap_ros は、結構くるくる回って地図の補正をするので、その時に、ロボットの位置がずれていると、
odometry をいきなり移動して調整しているように見えます。
今回も、2[M] 行った先で、くるくる回って地図の補正をしたら、50[CM] ほどずれているので、ロボットの位置を瞬間移動させている気がします。
stereo_image_proc で、距離の補正が、パラメータでできる様なので試してみます。
ROSで始めるロボティクス（9） ー ROSを使ったステレオカメラキャリブレーション
注) 昨今のブラウザーの所為か画像が、残念ながら見えません。
stereo_image_proc Tutorials ChoosingGoodStereoParameters 本家。
注1) 残念ながら、上記のパラメータ調整では、距離の補正は出来ないみたいです。やはりキャリブレーションをしっかりやります。
Skew は、100% 、Size は、60[%] 以上にします。
8) odometry と Stereo Camera のパブリッシュ間隔を同じにします。出来れば、タイミングもできるだけ同期させます。
odometry と Stereo Camera のパブリッシュ時刻がずれていると、rtabmap_ros の 3D Map の描画が崩れるみたいです。
特に、ロボットを回転させてる時に、両者が、ずれていると、在りえない所に障害物の絵が出てきます。
なので、おんちゃんは、foxbot_core3(自作 Turtelebot3) で、odometry のパブリッシュタイミングが、uvc_camera と同期するように改造してみました。
foxbot_core3 の odometry のパブリッシュ rate を、uvc_camera/stereo_node.launch の frame rate=15 と同じにします。
最初、foxbot_core3の odom のパブリッシュは、自身の タイミングで行いますが、/left/camera_info をサブクライブして、
そのトピックの timestamp から、次の stereo camera のパブリッシュ時刻を計算して、自身の次の odom の パブリッシュ
時刻をそれに合う様に少しずつずらして行きます。
注) /left/camera_info は、きっちりサブクライブできていません。時々とれます。
これだけで、ロボットの回転時の、3D Map の描画が、随分改善されました。
将来的には、完全に同期させたいです。
9) 自作 Stereo Camera の測距離が、2[M] 以上は、かなりずれているのが、ロボットの突然のワープの原因のような気がしたので、
rtabmap_ros が、2[M] 以内の障害物の判定に制限出来ないか、いろいろ試した結果、
Stereo/MinDisparity=70
Stereo/MaxDisparity=800 こちらは、ついでに大きくしました。
で、試しています。
あと、
Kp/DetectorStrategy=8 (GFTT/ORB) にしました。
当初、6(GFTT/BRIEF) にしていたので、よく暴走しました。
でも、8 (GFTT/ORB) だと、性能がいまいちみたいです。6(GFTT/BRIEF) を使うのであれば、OpenCV の contribを
組み込んで自分でビルドしなおさないといけないみたです。
後、SIFT は、特許が切れて、使えるみたいです。
その後、Jetson Nano 2G で、OpenCV4.1.1 を、 contrib and CUDA で、自分でビルドしたら、6(GFTT/BRIEF) で、うまく動きました。
それまでの、暴走も無くなりました。やれやれです。
Jetson Nano 2G で、OpenCV4.1.1 を、 contrib and CUDA のビルド方法は、改めて掲載します。結構大変ぞね!! by nishi 2021.12.10
お金ができたら、IntelRealsense D435i 辺りを買いたいぞね。
おんちゃんには、厳しい!! 誰か、中古を安くゆずってくれないかな。