Gazebo turtlebot3 Burger Depth Camera Navigation 草刈りロボット

1. グリッド情報テーブル 作成。
畑(部屋)の Map ファイルから、移動可能なグリッド情報テーブルを作成します。
map.yaml の情報は、下記。
 image: map.pgm
 resolution: 0.05 # 解像度 [m/pixel] -- 5 [cm/pixel]
 origin: [-10.525000, -10.525000, 0.000000] # 原点
 negate: 0
 occupied_thresh: 0.65
 free_thresh: 0.196 # この値以下だと、移動可能

Turtolebot3 Burger のサイズを、1 グリッドとして、
3 x 3 [pixel] 又は 4 x 4 [pixel] --- 今回は、4 x 4 [pixel] とします。

/map (nav_msgs/OccupancyGrid) を取り込んで、 4x4[pixel] で分割して、グリッド情報テーブルを作成します。
各グリッドは、階調値の平均をセットして、free_thresh で判定して、移動可否を判定します。
グリッド情報テーブルの中の、移動可能なグリッドだけを、草刈りの対象とします。
注1) free_thresh の判定は、 0.196 x 255 = 50 で判定すれば良いと思うので、グリッドの階調をそのまま使います。
注2) グリッドの作成は、/map トピックを取り込んで、Numpy に変換して、Opencv で 1/4 に縮小すれば、簡単か?
注3) /map トピック のデータで、 -1 は、 255 に変換します。

2. ライン情報テーブル 作成。
 草刈りの移動は、グリッド上の行または、列毎 (1ライン毎) に行います。
今回は、行単位で行うものとして、1行(1ライン) の草刈りが終わったら、次の行(1ライン) へと進みます。
なので、それに沿った、ライン情報テーブル を作成します。
前述の、グリッド情報テーブルの中の移動可能なグリッドのみピックアップして、
行単位で、1ラインとして、ライン情報テーブル へ登録します。
1ライン情報は、start グリッド[x,y]、 end グリッド[x,y] をもたせます。
ただし、同一行の中で、移動不可のグリッドが在れば、別のライン情報として、作成します。
同一行 が、2 つに別れていたら、2 ラインとして、3 つに別れていたら、3 ラインとして作成します。

情報テーブルとしては、以上2 つです。
次は、いよいよ処理の概要です。

3. 移動 (草刈り) 処理。
 処理の考え方は、シンプルです。
1) プログラムが開始したら、現在位置から現在のグリッドを求めます。
2) ここで、開始するラインを検索します。
求め方は、ライン情報テーブルのstartグリッド、endグリッド で一番近いグリッドを持つラインを開始ライン、
開始グリッドとします。

開始ラインの開始グリッドへ移動します。
これで、該当のラインの草刈りを行います。
1ライン上の移動は、from startグリッド to endグリッド 又は、from endグリッド to startグリッド とします。
その時の草刈りロボットから近いグリッドを、from グリッドとします。

3) 1ライン 刈り終わったら、次の、未処理のラインへと進みますが、
次の、未処理のライン の検出方法は、2) の、"開始するラインを検索" と同じです。

刈り終わった、ラインは、処理済とします。

上記処理を、未処理のラインがなくなるまで、草刈りロボット を移動させます。

注) ロボットの向きは、そのラインが、start グリッド to end グリッド の時は、右向き、
end グリッド to start グリッド の時は、左向き にします。

以上ぞね!!