自作 Turtlebot3 自律走行に向けたプログラム。#12 Ros2 C++ AutoMap II
--- Ros2 Gazebo Turtlebot3 waffle で、C++プログラムによる自律走行でMap を自動作成する 2。 ---
前回、自作 Turtlebot3 自律走行に向けたプログラム。#11 で、rtabmap_ros with Depth Camera の /map の
、
未知領域 と自由領域の境界を目標にロボットを 自律走行させて、2D Map を自動作成しましたが、今回は、障害物の領域のみを使ってみます。
注1) オペレータは、C++ プログラを起動したあとは、人の操作は必要ありません。すべて、ロボットの自律走行で、Map が作成されます。
最後に、Map の保存だけ操作してください。
1. 開発環境
PC
Ubuntu Mate 22.04
ROS2:humble
Gazebo Turtlebot3 with Depth Camera、Rtabmap_ros
注1) 今は、Ros2 Humble 版で、waffle になっています。by nishi 2024.3.12
2. アルゴリズム。
1) /map から、障害物のみを2値画像に変換して、それを、ブロブ化します。
2) 上記ブロブの外周を辿って、一定間隔でアンカーを付けて、そこへロボットを行かせて、/map を更新します。
注1) 『OpenCVによる画像処理入門』(講談社) P157 周囲長 を求める処理を参考にします。
3) ロボットを向かわせる地点は、上記アンカーから、半径Rのマスク処理を、今度は、非障害物に対して行って、その重心の方向へ、ロボットを動かした点とします。
4) ロボットを向かわせている途中、到着点が、障害物に含まれるか近接した場合は、その場所は、止めて、ロボットを次の候補地に向かわせます。
5) ロボットを向かわせる候補地は、ロボットから近い順に選びます。
6) 少し大きめのブロックで、/map を分割して、一度行った所はチェックして、次からは行かないようにします。
アルゴリズムは、簡単じゃ、後は、プログラムで試すのみじゃ。
当初のアルゴリズムは、ちょっと無理だったので、変更したぞね。by nishi 2022.8.16
但し、オンちゃんの GPU が壊れたのでちょっと、意気消沈しちょります。
GPUが買える迄、CPU オンリーで試すしかないぞね!!
その後、Humble 版になったら、非GPU でも、問題なくなりました。
しかし、GPUが無いと、Gazebo の映像が汚くて、rtabmap_ros の性能に悪影響がでて、/map が途中で、グニュと変形してしまうので、
どうにもならんぞね。
注) 今は、改善しています。問題は、qos=1 で統一できていなかった所為みたい。by nishi 2024.9.20
github の最新版を使ってください。
但し、ある程度プログラムができたら、こんどは、実機(自作 Turtlebot3 / foxbot_core3) with Stereo Camera で試せそうぞね。
しかし、これが出来ても、実際の草刈りロボットには役立ちそうもないか。
3. 検証プログラム作成。
例に拠って、OpenCV で検証プログラムを作成してみました。上述のアルゴリズムで、なんとか実現できそうです。
後で、git-hub にアップしときます。
main-11.cpp
4. 組み込み。
いよいよ、実際の C++プログラムに組み込んでみます。
一応組み込んで、House と Gas Station で試してみました。
House で、判った事は、壁で部屋が全て覆われると、その中の走査は完全に出来なくなってしまう事。
House の配置は、全て壁になっては、いないが rtabmap_ros の誤差の所為で、障害物として、全周が覆われることがある。
隣の部屋に通じる狭い通路が、rtabmap_ros の判定で、障害物として埋まってしまうと、もう隣の部屋の走査ができなくなる事。
rtabmap_ros with Depth Camera で、3-4[M] 先の判定性能が悪くて、すぐ障害物と判定される点が一番問題だ。
これさえ無ければ、大部違うと思う。
まあ、今のアルゴリズムだと、障害物の外周上の幾つかの点をすべて走査するので、無駄が多い。
最初から、行けない場所も結構あるので、そこが難点ぞね。
House の様にクローズした所より、Gas Station のようにオープンの場所が向いているのかも。
GitHub にアップしました。
tosa-no-onchan/turtlebot3_navi_my
今回の追加機能(Auto Map builder of Anchor) のプログラムの主要部分は、
multi_goals.cpp, .h の中の
class AnchorFinder と、MultiGoals::auto_map_anchor() です。
起動は、multi_goals4_move_base.cpp の main() の中から行っています。
GoalList turtlebot3_auto_map_achor[] の中に、
{31,0.0,0.0, 0.0}, // Auto map builder of anchor
を記述して、
mg_ex.mloop_ex(turtlebot3_auto_map_achor);
するだけです。
実際の手順は、前ページの 自作 Turtlebot3 自律走行に向けたプログラム。#11
4. move_base and dwa_local_planner 使用に切り替えて。
と同じです。
4. local planner の変更。
1) eband_local_planner に変更。
noetic では、eband_local_planner の Packege 版が無いので、試しに、ソース Build 版を試してみました。
$ cd ~/catkin_ws/src
$ git clone https://github.com/utexas-bwi/eband_local_planner.git
$ cd ..
$ catkin build eband_local_planner
もんだい無く、build できました。
i) Rviz で動作確認。
問題が出ました。ロボットが目的地に到達しても、ロボットが止まりません。
move_base のトピックの move_base/status 、move_base/result に完了報告が出てきません。
eband_local_planner_ros からは、下記ログが、出て来ますが。動作が完了しません。
ソースをあれこれ見て、std::cout で動作を確認して、なんとか原因がわかりました。
eband_local_planner_ros.cpp の goal_reached_ の値が、setPlan() で潰されているのが、原因のようでした。
下記のように修正しました。
~/catkin_ws/src/eband_local_planner/src/eband_local_planner_ros.cpp
後、難点を言えば、回転の時の到達角の判定処理にチョット問題がありそう。
此処の処理は、おんちゃんも、/cmd_vel を直接操作して、ロボットが目的の角度になった時に、回転を止める判定に結構苦労しました。
この作者も、此処の処理に関しては、まだまだ配慮が足りない。
要は、TFでロボットの角度を読み取るのだが、TFは、そんなに、スムーズには回転を上げてこない。前回のTFが、目的角の前だったのが、
今回のTFでは、目的角を通り過ぎて、オーバーしていたりする。
⊿θ(目的角とロボットの向きの差)は、大きな値から段々小さくなって、目的角をすぎると、また大きくなって行く。
なので、⊿θ が最小になったところでやめれば良い。但し、実際には最小から、増加に転じた時に判断できるので、その時少し、回転を戻せば良い。コッホ!!
ii) multi_goals4_move_base での実行
何はともあれ、これで、move_base 経由で eband_local_planner を使っての実行ができるようになりました。
DWA_local_planner より、断然良いです。
目的地に向かって、どんどん進んでくれます。また、障害物に接近しても、なんとか回復して動作を継続してくれます。
teb_local_planner も試してみましたが、こちらも 目的地にむかって、とことん進んでくれます。
但し、こちらの方は、障害物に接近し過ぎたら、動けなくなる事があるので、実際の利用はどうか...
GitHub には、後でアップロードしておきます。
幾つかバグが出て修正が入っているので、そろそろ最新版をアップロードしないといけないぞね。 by nishi 2022.8.29
tosa-no-onchan/turtlebot3_navi_my
tosa-no-onchan/rtabmap_ros_my
アップロードしました。by nishi 2022.8.29
5. おんちゃんのメモ。
1) /map のデータから、map_saver のサンプルで保管した場合の Mat map など色々あって、混乱しちょります。
参考になるページがあったので、残しちょきます。
Robot coordinates in map
nav_msgs/MapMetaData Message
ロボットの基本座標(World)の原点 O(x=0,y=0) は、map の真ん中に配置する、 O(0,0) = cell(x=width/2,y=height/2) との事のようです。
注) MAP情報の定義で自由に決まるので、必ずしも真ん中であることも無い。
これで、map の原点(左、下 cell(0,0)) と、基本座標の意味が少しは、判った気がします。
/map をSubscribe で取り込んだ場合、OpenCV の Mat や、C++ プログラムの配列では、左上が原点のほうが扱いやすいので、
map_saver で、Y軸を逆転している意味が理解できました。
Y軸を逆転したからと言って、y の値に違いは無いので、別に特別な配慮は、必要無い。
y=2 だと、2行目を指すが、map だと、下から 2行目、Mat だと、上から 2行目で、どちらも同じ行を指す。
xxx.pgm で画像ファイルに落として、Viewer で表示した時、上下が逆に見えるだけ。
但し、Y軸を逆転した、Mat を /map にPublish する時は、再度、Y軸を逆転する必要がある。
2) World 座標と Map 座標の変換ルーチンの例が、navigation に、あったので、のっけておきます。
navigation/costmap_2d/src/costmap_2d.cpp
origin_x_ , origin_y_, resolution_ は、
nav_msgs/MapMetaData Message
の、resolution, origin of the map [m, m, rad] の値がそのまま使えます。
注) 今の c++ コンパイラーは、勝手に Cast してくれるので、便利なのか、危ないのか?
3) 2値画像の細線化をつかってみる。
今回は、障害物のブロブの外周にアンカーを配置して、そこを目当てにロボットを移動させたが、外周の代わりに、
2値画像の細線化を使って、障害物を線にして、そこに一定間隔でアンカーを配置しても面白いかも。
但し、この場合は、線のどちら側に実際の到達点を出すか、迷うところだ。
例は、
OpenCVによる細線化.(スケルトン)
2値画像の細線化
また、線が枝分かれしていると、そのノードを、Push して、一方を処理したあとで、再度戻ってきて、残りの一方を処理する。これを繰り返す処理が必要だ。
参考資料:『基礎と実践 画像処理入門』(杉山賢二著 コロナ社) P61 [1] 細線化
4) move_base を使わずに、C++ から、直接 eband_local_planner を使う方法も試してみました。
自作 Turtlebot3 自律走行に向けたプログラム。#9 の 5. global_planner、local_palnner の実装。
で、base_local_planner::TrajectoryPlannerROS を、eband_local_planner に変えるだけで、同じようにできました。
注) この構成は、Ros2 Humble & Navigation2 になってからは、なくなりました。
また、move_base ではなくなって、navigation2 になりました。
Rtabmap_ros による、SLAM は、これくらいにして、これからは、実際の草刈りロボットにつながる、GPS による Navigation の方を試すことにします。
6. 関連GitHub
最初の掲載からかなり経過して、プログラム関連が変わっているので、最新は下記を参照してください。Ros2 Humble and navigation2 対応になっています。
tosa-no-onchan/turtlebot3_navi_my
tosa-no-onchan/rtabmap_ros_my
Youtube に、動画をアップしました。
turtlebot3_rgbd_sync.launch.py & rpp local plannerを使っています。
Ros2 humble c++ auto mapping デモ
YouTube のページで "もっと見る" の中に、早見方法を書いています。そちらをご覧ください。