自作 Turtlebot3 自律走行に向けたプログラム。#11 -- Ros2 C++ AutoMap
--- Ros2 Gazebo Turtlebot3 waffle で、C++プログラムによる自律走行でMap を自動作成する。 ---
自作 Turtlebot3 自律走行に向けたプログラム。#10 で、Gazebo & Turtlebot3 burger with Depth Camera and Rtabmap_ros だと、
床が自由領域だと判定できるのが判ったので roslaunch turtlebot3_gazebo turtlebot3_house.launch で、
Burger を起動して、全て C++プログラムで、Houseの中の、自由領域と未知領域の境界を選んで自由に走らせて、
Rtabmap_ros で、Map を完成させられるか試してみます。
注1) オペレータは、C++ プログラを起動したあとは、人の操作は必要ありません。すべて、ロボットの自律走行で、Map が作成されます。
最後に、Map の保存だけ操作してください。
注2) Stereo Camera だと、Rtabmap_ros で、床が自由領域として、 /map に出てこないのでこちらは、この方法ではできません。
あくまでも、Depth Camera の場合です。
注3) 今は、Ros2 Humble 版で、waffle になっています。by nishi 2024.3.12
開発環境
Ubuntu Mate 22.04
Ros2 Humble
Navigation2
Gazebo & Turtlebot3 Waffle
1. アルゴリズム。
i) C++ プログラムで、Burger を起動地点で、360度回転させます。
ii) rtabmap_ros が出す、 /map (nav_msgs/Occupancy Grid int8) を、C++プログラムで取り込んで、
そのなかで、自由領域(Free Area:0) で、４近傍(上、下、左、右)のいずれかが、未知領域(Unkonow Area:-1) に接するセルをピックアップします。
そして、/map を、ロボットの回転円の直径のサイズで、ブロック分割します。分割する際に、該当ブロックの中に1個でも、
上記、未知領域と隣接する自由領域セルがあれば、そのブロックに未知領域マーク(白)を付与します。
未知領域接続ブロック(白) と其れ以外(黒)の、2次元配列(Mat でもOK)を作成します。
iii) さて、ここからが、OpenCV の登場です。
おんちゃんは、ずっと以前に買っておいた、『OpenCVによる画像処理入門』(小枝正直/上田悦子/中村恭之著)を引張だして、それと、にらめっこです。
a) 上記、ブロックでは、未知領域と隣接する自由領域セルが一個でも含まれるブロックを白として、その他は全て黒の2値データにします。
これを、上記著の P165 ラベリング処理を行って、自由領域(白)を、ブロブ分けします。
自由領域(白) が纏まってあるところは、未だ、ロボットが Depth Camera で十分捉えていない場所と判断できるので、
ラベリング処理で分割したブロブの中で一番大きなものを選んで、そのブロブのみの重心 (同著 P161) を算出します。
その重心を、次のロボットの目的地として、C++ で、 Burger を移動させます。
目的地に、Burger が到着したら、その場所で、360度回転させて、/map を更新させます。
これで、ii) に戻って、同じ処理を繰り返します。
自由領域(白) が一定以上、纏まってあるブロブが無くなれば、完了(2D-Map完成 )だと思います。
こっほ!!
アルゴリズムが出来たので、あとは、えいやで、試してみるのみじゃ!!
iv) ここは、実際試してみて、改善点を列挙します。
a) 最初の1回目は、全方向を走査します。2回め以降は、x=A の軸で、前後に分割して、2ブロックに分けて、
ブロック単位で走査します。
後ろブロック->前ブロックの順で走査します。
後ろブロック 走査中に出てきた、前ブロックの地点は、前ブロックの<vector> テーブルに登録します。但し、重複は、排除します。
b) 走査動作中に、ロボットが、到達出来なかった地点は、ブラックポイントとして登録して、以後、走査の対象から除外します。
c) ロボット可動範囲を、Map 上の To-Left、Bottom-Right で制限して、その中の地点のみ走査対象にします。
d) ロボットの移動先地点が、障害物と近い時は、少し離れた地点を、移動先地点 とする。
注) この機能は、未だ、これからぞね。
注1) ロボットの移動は、global_planner と、local_planner を C++ から直接コールして、Navigate させれば簡単ぞね!! 但し、local_planner(base_local_planner::TrajectoryPlannerROS) の精度がちょっと問題かも。
自作 Turtlebot3 自立走行に向けたプログラム。#9
2. 検証プログラム。
roslaunch turtlebot3_gazebo turtlebot3_house.launch で、Gazebo & Turtlebot3 burger with Depth Camera and Rtabmap_ros を動かして、
起動地点で、360度回転させた時の、/map を PGM ファイル(map_builder.pgm) に落としたものを入力にして、
対象ブログの重心を求める検証プログラムを作成してみます。
注) map_server/map_saver のファイルでは、NG です。自分で出力した PGM ファイルを使います。
map_server/map_saver のソースを見たけれで、処理がちょっと、おかしくないかな?
一応、検証プログラムが出来たので、後で、github に公開します。
tosa-no-onchan/opencv-test1/blob/main/main-8.cpp
実行方法。
$ git clone https://github.com/tosa-no-onchan/opencv-test1.git
$ cd opencv-test1
$ mkdir build
$ cd build
$ cmake ..
$ make
$ ./main-8
3. 組み込み。
いよいよ、実際の C++プログラムに組み込んでみます。
上記、main-8.cpp をほとんどそのまま、Class BlobFinder として作成しました。
一応組み込んで、早速試してみました。
Turtlebot3 House で試しましたが、予想通り、/map の未知領域(Unknown) を自分で探して、動き回ります。
これは、ちょっと感動しました。但し、local_planner がやはり問題で、途中で動かなくなります。
全ての部屋のMap をさくせするには、まだ改良が必要です。
一方、Gazebo Gas Station では、問題がでました。起動地点で、360度回転させた時点で、未知領域のブロブの重心が、
ロボットの位置近くになるので、これも改良が必要です。
しかし、ロボットが、自分で動き回るのは、感動です。いままでは、目的地を、予めテーブルに記述して、それに従って動いていたのが、いまは、自分で動きます。
後日、github に公開します。
しかし、base_local_planner::TrajectoryPlannerROS をどうしたものか? 難儀じゃ。
少し、立ち返って、move_base and eband_local_planner で目的地へ移動させるのもありか?
自作 Turtlebot3 自立走行に向けたプログラム。#8 の下記参照
--> 2. move_baseのローカルプランナーを変更する があるが、別の local planner を選べるのか?
注) base_local_planner::TrajectoryPlannerROS 使用上の注意。
どうも、global_planner が、障害物に近いパスを返してくるようです。
下記に、同じ様な問題の記述がありました。
Path is made very close to obstacles #922
どうやら、inflation_radius:xx の値に問題があるようです。
結論を言えば、footprint: で、Burger のサイズを正しく指定して、その回転円の半径以上を、inflation_radius:xx に指定すれば良いようです。
Burger のサイズは、Burger のサイズ
を参考にして、
# burger size R=132
footprint: [[-0.098, -0.089], [-0.098, 0.089], [0.041, 0.089], [0.041, -0.089]]
inflation_layer:
inflation_radius: 0.18
0.14 以上。
にしました。
でも、やっぱり、global_planner は、障害物に近い経路を返してきます。
下記にも記述があります。
cost_scaling_factorの計算とinflation_radius
4. move_base and dwa_local_planner 使用に切り替えて。
base_local_planner::TrajectoryPlannerROS が芳しくないので、結局、Turtlebot3 move_base and dwa_local_planner に切り替えました。
ロボットの走査順も、起動した時、一番近いブロブの重心に移動して、その後は、そこからまた一番近いブロブの重心に移動して、
毎回、Mapした時の全てのブロブを処理するアルゴリズムにしました。
これで、大部、House のMap が取れるようになりましたが、処理が進むにつれて、対象のブロブが、端ばかりになって、ロボットが端から、反対側の端まで移動するようになりました。
ここは、ちょっと改善しないといけないぞね。
後、ロボットは、移動先々で、あまりクルクルさせないほうが良いようです。クルクルさせすぎると、Map の TFの場所がずれて、Map の図が歪んできます。
取り敢えず、現状のものを、git-hub にアップロードしました。
tosa-no-onchan/turtlebot3_navi_my
1) ビルド.
$ cd ~/catkin_ws/src
$ git clone https://github.com/tosa-no-onchan/turtlebot3_navi_my.git
$ cd ..
$ catkin build turtlebot3_navi_my
2) Gazebo and Turtlebot3 Burger ( rtabmap_ros with Depth Camera) での走行
走行させるには、
tosa-no-onchan/rtabmap_ros_my/blob/main/launch/demo_turtlebot3_navigation_nishi2.launch を使います。
$ cd ~/catkin_ws/src
$ git clone https://github.com/tosa-no-onchan/rtabmap_ros_my.git
$ cd ..
$ catkin build rtabmap_ros_my
実際の launch 手順は、上記ファイルの上部に記載しています。
今回の追加機能(Auto map) のプログラムの主要部分は、
multi_goals.cpp, .h の中の
class BlobFinder と、MultiGoals::auto_map() です。
起動は、multi_goals4_move_base.cpp の main() の中から行っています。
GoalList turtlebot3_auto_map[] の中に、
{30,0.0,0.0, 0.0}, // Auto map builder
を記述して、
mg_ex.mloop_ex(turtlebot3_auto_map);
するだけです。
注1) ロボットを向かわせる位置が、障害物に近すぎるのが問題としてあります。
ここで、DWA local_planner がすんなり行かないので、
これを解消しないといけんぞね。
注2) Segmentation Fault が出るみたい。
multi_goals.cpp
下記ラインを、コメントにしとうせ。
//grid_.init(map->info,_line_w,map->data);
5. 障害物 との距離を取るアルゴリズムを考えてみたぞね。
1) /map から、障害物を白とする、2値化画像、Mat mapr を用意する。
2) ロボットの到達ポイントを、Pr(xr,yr) として、そこから、ロボットの回転円の半径+α で、円を描く (circlePr とする)。
3) mapr に、circlePr との AND を取る(OpenCV のマスク処理で可能)。
AND の結果、白の部分が無ければ、
ロボットは、障害物にはぶつからない。
4) もし、白の部分 が残れば、ロボットは、障害物にぶつかるので、白の部分の重心:Gp を求めて、Gp - Pr(xr,yr) の直線上を、
Pr を、Gp から遠い方に、5[cm] から 10[cm] ずらす。
これだけで、後は、local_planner に任せる。でなんか、出来そうぞね。
同じように、OpenCV で検証プログラムを作ってみました。
main-10.cpp
しかし、/map のresolution は、5[cm]/1[dot] なので、ロボットの回転円半径を、35[cm] にしても、7 pixel しか無い。
取り敢えず、ためしてみるしか無いか。
multi_goals.cpp
GetMap::check_collision() を、今回組み込みました。
一応、動作するみたいでっす。
今までよりも、壁から少し距離を取った所を目指すようです。
git-hub にアップしっちょきました。 by nishi 2022.8.14
tosa-no-onchan/turtlebot3_navi_my
残念な事に、おんちゃんの geforce GTX 1050 が壊れたみたです。Kernel が認識しなくなりました。
Gazebo で、GPU が無いと、カメラの映像の質が悪くなるので、そのせいで、rtabmap_ros の2D Map がかなり崩れるようになりました。
これでは、Gazebo での動作確認は、今のところ、できまへん。
なんとか、GPU を買わないといきまへん。まっこと残念!!
6. ROS2 Humble で試す。
最近、ROS2 Humble 版でも動作するように、プログラムを修正しました。by nishi 2023.4.25
ランチファイル: rtabmap_ros_my/launch/turtlebot3_scan.launch.py
Active SLAM: rtabmap_ros
入力: Ladir scan
Local Planner: teb_local_planner
Gazebo turtlebot3 house & waffle
C++プログラム: turtlebot3_navi_my/launch/multi_goals4_nav2.launch.py
turtlebot3_navi_my/src/multi_goals4_nav2.cpp
注1) mg_ex.mloop_ex(turtlebot3_auto_map); を実行します。
GPU 無しでも、動作します。以前より、上手く動くみたいぞね。
実機でも、ここまで上手く動けば、Map 作成用の商品になりそうぞね。
後で、github にアップしておきます。
注2) rtabmap_ros ですが、rgbd だけだと常に周囲を写してやらないと精度が落ちるみたい。
現状、ひどい場合は、tf-map と tf-odom が、ぐにゅっとずれてしまいます。
rgbd & Scan(Lidar) の組み合わせだと、それを気にせずに、精度がかなり良くなります。
rtabmap_ros_my/launch/turtlebot3_rgbd_sync.launch.py
だと、かなり正確に Map が出来ます。
rtabmap_ros_my/launch/turtlebot3_rgbd.launch.py
だと、現状、かなり精度が落ちます。また、たまに、tf-map と tf-odom が、ぐにゅっとずれてしまいます。
Gazebo House の場合、外への通路があるので、AutoMap だと、ロボットが外に出てしましまい、外周を先に Map すると
そこで処理が終わってしまいます。
Gazebo House のばあい、AutoMap II の方が向いているみたい。
AutoMap の場合、完全に室内の場合は、向いているかもしれません。
どちらの場合も、先に外周が完成してしまうと、そのなかの未到達エリアには、スケジュールされなくなります。
これは、今後の課題じゃ!!
7. 関連GitHub
最初の掲載からかなり経過して、プログラム関連が変わっているので、最新は下記を参照っしてください。
tosa-no-onchan/turtlebot3_navi_my
tosa-no-onchan/rtabmap_ros_my
Auto Map II のほうですが、Youtube に、動画をアップしました。
turtlebot3_rgbd_sync.launch.py & rpp local plannerを使っています。
YouTube のページで "もっと見る" の中に、早見方法を書いています。そちらをご覧ください。