自作 Turtlebot3 自律走行に向けたプログラム。#12 AutoMap II

しかし、GPUが無いと、Gazebo の映像が汚くて、rtabmap_ros の性能に悪影響がでて、/map が途中で、グニュと変形してしまうので、
どうにもならんぞね。

但し、ある程度プログラムができたら、こんどは、実機(自作 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 からは、下記ログが、出て来ますが。動作が完了しません。
[ INFO] [1661745823.242557573, 497.820000000]: TrajectoryController: Goal reached with distance 0.01, -0.01 (od = -0.05); sending zero velocity

ソースをあれこれ見て、std::cout で動作を確認して、なんとか原因がわかりました。
eband_local_planner_ros.cpp の goal_reached_ の値が、setPlan() で潰されているのが、原因のようでした。
下記のように修正しました。

~/catkin_ws/src/eband_local_planner/src/eband_local_planner_ros.cpp
// set global plan to wrapper and pass it to eband bool EBandPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) { .... // set goal as not reached // changed by nishi //goal_reached_ = false; return true; } bool EBandPlannerROS::isGoalReached() { // check if plugin initialized if(!initialized_) { ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); return false; } // test by nishi //ROS_INFO("GOAL Reached!"); //std::cout << "BandPlannerROS::isGoalReached() :#9 goal_reached_="<< goal_reached_ << std::endl; if(goal_reached_==true){ goal_reached_=false; return true; } return false; //return goal_reached_; ....

後、難点を言えば、回転の時の到達角の判定処理にチョット問題がありそう。
此処の処理は、おんちゃんも、/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
void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const { wx = origin_x_ + (mx + 0.5) * resolution_; wy = origin_y_ + (my + 0.5) * resolution_; } bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const { if (wx < origin_x_ || wy < origin_y_) return false; mx = (int)((wx - origin_x_) / resolution_); my = (int)((wy - origin_y_) / resolution_); if (mx < size_x_ && my < size_y_) return true; return false; }
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を使っています。



YouTube のページで "もっと見る" の中に、早見方法を書いています。そちらをご覧ください。