ROS2 自作 Turtlebot3 による 草刈りロボット開発。#3 - Auto Mower
自作 Turtlebot3 自律走行に向けたプログラム。#12 AutoMap II で、Gazebo House での自動 Mapping ができるので、
今回は、その作成されたマップを使って、Auto Mower を作ってみました。
Auto Mower を試す前に、Auto Map II で、マップを作っておいとうせ。
手動操作で、teleop-keyboard で動かして作っても、OK です。
1. 開発環境
PC
Ubuntu Mate 22.04
ROS2:humble
Gazebo Turtlebot3 with Depth Camera、Rtabmap_ros
GPU: Gazebo で試すなら、GPU が必要みたい。無いと、rtabmap-ros が良く狂って、TF-Map - TF-Odom がぶっ飛ぶみたい。
2. Auto Mower コース計画のアルゴリズム。
1) Static Map を取り込んで、ロボットがいる自由領域(White) の領域を最初に求めます。
2) その自由領域を、ロボットの走行サイズに合わせて、スライスして、そのスライス線に従って、ロボットを走行させる。
以上です。
考えかたは、ずっと以前、ROS and Python で試した方法のままです。
ただし今回は、場所が、 Gazebo House と、 C++ で作成します。
3. 技術的検証。
じっさいに、Auto Mower のプログラムを作る前に、クリアしなといけない技術的問題点を検討します。
3.1 Map 上の ロボットが今いる、自由領域の範囲の特定の仕方。
プログラムやっとできた所で、プログラムの作成にも少し倦んできたので、今日の書き込みは、
ここまでじゃ。続きは、少し気晴らししてから。
今日は、続きじゃ。
1) ロボットのスタート位置の自由領域の範囲を特定する。
やり方は、 OpenCV の cv::findContours() で、出来る。
i) ROS Map の map.pgm をOpenCV で取り込んで、gray 画像を、cv::blur() で少し平滑にして、
cv::threshold() で、threshold を、Map の自由領域 white:250 で ２値画像に変換して、
その２値画像に対して、cv::findContours() で、領域のブロブリストを得る。
ii) 領域のブロブリスト の中で、ロボットがいる場所のブロブを特定する。
これを、"ロボット走行リージョン" とする。
2) ロボット走行リージョン に、ロボット走行幅の間隔で線を引く。これが、Auto Mower の走行場所になる。
i) ロボット走行リージョン のboundBoxを求めて、その y-top、 y-bottomを求める。
これは、 cv::boundingRect() で先に、cv::findContours() で求めた、ロボット走行リージョン を渡せば出てくる。
その y-top、 y-bottom の row間に、ロボット走行間隔の白線を引いたマスク画像(line_mask)を作る。
ii) 一方、ロボット走行リージョン だけ白(1) にした２値画像を用意して、この画像とマスク画像(line_mask)の AND をとれば、走行ラインだけが抽出できる。
cv::bitwise_and() でできる。
tmp_lines とする。
iii) tmp_lines に対して、cv::findContours() をかけると、簡単に走行ライン毎の start(x,y) と end(x,y) が出てくる。
ただし、このstart(x,y) - end(x,y) は、ロボットの radius+α だけ内側にずらす。
さらに、ずらした start(x,y) - end(x,y) が、ロボットの直径+α より短い線は、除外する。
一連の処理をしたものを、
std::vector<Robo_Slice> robo_slice_ とする。
これを、start(x,y) を用いて、lef-top から、ソートすれば、走行順位が出てくる。
ソート値の計算は、
dist=y+x*max_rows;
ただし、おんちゃんは、cv::findContours() が便利なので、味をしめてもっとこれを使う方法を考えてみる。
3) 隣接する走行線をクラスター化する。
i) robo_slice_ にある、各走行線の開始点か中央点を使って、半径: robot radius + α の円(white:1)を、二値画像に描く。
隣接する線の円は、重なり合うので、これを使って、cv::findContours() でブロブ化する。
走行線クラスタブロブ とする。
ii) 走行線クラスタブロブの各メンバー にブロブ番号を与えて、このブロブに属する robo_slice_ のメンバーにそれぞれ、ブロブ番号 を付与する。
ようは、マッチングをする。
方法は、1) ロボットのスタート位置の自由領域の範囲を特定する。 で実践済じゃ。
4) 走行線クラスタブロブ の順序をつける。
走行線クラスタブロブのメンバーの走行順序を決める。
走行線クラスタブロブ の各メンバーの重心点をを求めて、それを、top - left から近い順に並べて、 Auto Mower の走行計画を完成させる。
ソート順位の値: dist
cource_blob[n].g<x,y> とすると
dist = y + x*max_rows;
x をブロック単位にすれば、もう少し整列できる。
x=x/10
注) ただし、これがおんちゃんは、うまくできないまま。
多分の、OpenCV の画像原点(left-top)と、 ROS Map の原点(left-bottom) だから、やこしい
では、なくて、走行線クラスタブロブの各メンバーで示される実際の走行線の順番が
逆なだけみたい。
途中から、ほとんどプログラムの詳細になってしまった。
この Auto Mower 向け、走行プラン作成 プログラムを、github に公開しました。
このまま実用できるし、検証用に、cv::imshow() を埋め込んでいるので、#if defined(xxx) を有効にすれば、目で確認できます。
tosa-no-onchan/opencv_contours
4. あとはコース計画に従って、Robot を走行させれば、OK ぞね。
tosa-no-onchan/turtlebot3_navi_my の中に組み込んでためしてみました。
tosa-no-onchan/turtlebot3_navi_my/src/pro_control_mower.cpp
に組み込みました、細かいところは、ソースを参照して下さい。
これを、実際動かすプログラムは、
tosa-no-onchan/turtlebot3_navi_my/src/go_auto_mower.cpp
ランチファイルは、
tosa-no-onchan/turtlebot3_navi_my/launch/go_auto_mower.launch.py
1) すべての関連ランチを起動する手順は、下記ファイルの上部に記載しています。
i) tosa-no-onchan/turtlebot3_navi_my/launch/turtlebbot3_amcl_scan.launch.py
Turtlebot3 Original slam、navigation and localization ランチ。
ii) tosa-no-onchan/rtabmap_ros_my/launch/turtlebot3_rgbd_sync.launch.py
一応動く様です。
turtlebbot3_amcl_scan.launch.py は、GPU がなくても、大丈夫だと思います。
rtabmap_ros に関しては、おんちゃんの Ubuntu Mate 22.04 は、GPU が故障して外しているので、
完走できません。
途中で、 ratbmap_ros の所為だと思うが、 tf-map - tf-odom が、ぐにゅうっとずれて、テストを継続できません。
多分、GPU を入れれば問題ないと思います。
あとは、 foxbot_core3_r2 for Humble(申し訳ない、まだ Humble 版は、アップしちょりません) で実機で試してみるか?
Gazebo で完走できれば、映像を撮って YouTube に上げるのだが!!
Gazebo House には、Scan のみの場合、鬼門があるぞね。
3段棚の一番したの段が、少しだけ高くなっていて、センサーが、Scan の場合は、これを検出できましぇん。
turtlebbot3_amcl_scan.launch.py の場合、local_costmap に、scan だけでなく、rgbd camera の point_cloud_sensor を与えれば良いのかも?
なので、今は、ロボットが後ろを、ここに乗り上げて、動けなくなる時がある。
いまは、Gimp で、 Map をいじって、3段棚 の開口部を塞いでください。
2) nav2_map_server/map_server のパブリッシュする、/map を受信するには、注意が必要みたい。
localiaztion 起動の時、/map がパブリッシュされるのは、起動時の1回のみです。
rviz2 で /map の情報をみたら。
Topic /map
Depth 1
History Policy Keep Last <--- QoS の指定みたい。
どうやら、rclcpp::Node::create_subscription() の、QoS を 一致させなといけないみたい。
どうすればよいのか分からなくて、困ったが、navigation2/nav2_amcl/src/amcl_node.cpp 1359 にサンプルがありました。
qos=rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()
で、OKでした。
slam や、rtabmap_ros の slam モードでは、定期的に更新されるが、その場合の受信も、上記、qos でOKです。
ここで、もう一つ挙げれば、rtabmap_ros の localiaztion モードのときも、ロボットを動かすと、map に情報が追加されて、
書き換わって行くこと。これは、ちょっと困った事じゃ。
5. github に上げてあります。
/tosa-no-onchan/turtlebot3_navi_my
/tosa-no-onchan/rtabmap_ros_my
YouTube に公開しました。
最低限の機能は入っているので、あとは、自分でブラッシュアップしとうせ!!