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

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

その前に、取り敢えず、0.8 x 0.8 の面積を、縦5列に分割して走破させてみました。
やはり、 rtabmap_ros で、一部だけ、Active Slam でマッピングしてから、その後、ナビゲーションモードだけで、自己位置推定をしながらの走破は、かなり難しい様ぞね。

Navigation モードに切り替えせずに走破させるとどうか?
これは、問題なく完走したぞね。
また、一度、マッピングして、次に、最初からNavigation モードで完走するのは、問題なくできるぞね!!
注) eband_local_planner を使用の場合です。DWA では、難しい。

2. move_base を使わずに、turtlebot3_teleop_key.launch で操縦するとどうなのか?
2M 行って帰ってくる走行を、 turtlebot3_teleop_key で行ってみたぞね。
2M 先から帰って来る時に、Rviz の 3D-Map の tf-map を目標にしました。
無事、元の場所に帰ってこれました。
草刈りロボットは、move_base を使わずに、rtabmap_ros の Slam情報の現在位置を頼りに、プログラムで直接操縦したほうが良いような気がします。
但し、草刈り前と草刈り後では、風景が変わるので、rtabmap_ros ではやはり限界があると思う。
ただ、ちょっと前方の草刈りをしては、後ろの風景を、rtabmap_ros に記憶させる方法はどうなのだろうか?
ドローンの空撮で、草刈り範囲のMap を作ってから、草刈りをさせる方法になるのか?
LiDAR で 3D Map を作る製品の紹介がちょっと前にあった気がします。

GPS + モータ+ IMUの姿勢 のフュージョンでの、自己位置推定になるのか?

3. move_base を使わずに、自分で cmd_vel を操作して、multi_goals4.py の cmd_vel 版を作ってみました。
同じ様に、部屋の中を、2M 行って帰ってくるプログラムですが、帰って来る場所が、tf-map の原点よりずれた、tf-odom の原点に帰ってきます。
rtabmap_ros を実行すると、3D-Mapping の closeuer 処理の影響で、tf-map と tf-odom がずれて来ます。
Python Scripts で、ロボット位置の確認には、tf の odom - base_footprint の情報を使っています。
なので、その位置から、map - odom のズレを計算に組み込んだ ロボット位置 の算出が必要みたいぞね。
tf-map と tf-odom の位置は、rtabmap_ros の処理が進むと、かなり頻繁に動き出すので、それからロボット位置、向きを算出するには、フュージョンが必要になりそう。

その前に、TF のライブラリーに、 TF チェインから、目的ノードの位置と向きを一発で求める関数はないのだろうか?
オーマイゴッチ! もっと単純だった
#t = self.tfBuffer.lookup_transform('odom', 'base_footprint', rospy.Time())
t= self.tfBuffer.lookup_transform('map','base_footprint', rospy.Time())
親を、 'odom' から、'map' にするだけだった。

後、tf-map と tf-odom のズレの暴れは、位置決めのプログラムを柔軟にすれば、OK みたいぞね!
一応、精度を度外視すれば、なんとか出来たぞね。!!
スタートの、tf-map の原点目指して帰ってくるようになりました。by nishi 2022.6.7
後日、github に上げます。

github にアップロードしました。by nishi 2022.6.9
Python Scripts は、/tosa-no-onchan/turtlebot3_navi_my/blob/main/scripts/multi_goals4_cmd_vel.py です。

Gazebo Turtlebot3 or 自作 Turtlebot3(foxbot_core3) で実行出来ます。
1) Gazebo Turtlebot3 は、
/tosa-no-onchan/rtabmap_ros_my/blob/main/launch/demo_turtlebot3_navigation_nishi2.launch で出来ます。
こちらは、rtabmap_ros with Depth Camera(kinect) 版です。
注1) turtlebot3_gazebo/turtlebot3_world_nav_nishi2.launch は、
turtlebot3_gazebo/turtlebot3_world.launch をベースに、World を Gass Station にしたものです。おんちゃんは、Gass Station で試しています。
オリジナルは、/rtabmap_ros/Tutorials/MappingAndNavigationOnTurtlebot の
6.1 Turtlebot3 on Melodic and Noetic
に記述があります。

2) 自作 Turtlebot3(foxbot_core3) では、
/tosa-no-onchan/rtabmap_ros_my/blob/main/launch/rtabmap-nishi_stereo_outdoor2_bz3_navi.launch で出来ます。
こちらは、rtabmap_ros with Single USB Stereo Camera 版です。
実行手順は、どちらも、launch ファイルの上部に記述しています。

4. multi_goals4_cmd_vel.py 版を、C++ 版にコンバートしてみました。by nishi 2022.6.11
tosa-no-onchan/turtlebot3_navi_my/blob/main/src/multi_goals4_cmd_vel.cpp

ビルドしてから、試しとうせ。
$ cd ~/catkin_ws
$ catkin_make --pkg turtlebot3_navi_my

実行は、下記ぞね!!
$ rosrun turtlebot3_navi_my multi_goals4_cmd_vel

5. global_planner、local_palnner の実装。
ROS講座116 navigationを実装する に、global_costmap、local_costmap、global_planner、local_planner を自分で使う方法の記述があります。
これを参考にして、eband_local_planner を直接自分で使えないか検討してみるぞね!!
もとい、おんちゃんが真に必要なのは、global_planner の方でした。

1) 取り敢えず、上のサンプルを試してみます。
$ roslaunch nav_lecture move_navigation.launch
注) plugin のロードでエラーが出ているみたいぞね。
nav_lecture/config/move_base/move_navigation.yaml
sonar_layer が、まだ入ってい無いので、関連箇所を全て、コメントにします。
これで、うまく動きだしました。
最初に、Global Planner が、最終目的地まで計算してくれて、
後は、Local Planner がチョットずつコースを選んで最終目的地まで、到着できる様です。
おんちゃんは、Global Planner の経路情報だけ得られれば、後は、自分で目的地まで行けそうです。
これは、使えそうです。

2) Gazebo Turtlebot3 だと、Scan が使えるので、早速、
demo_turtlebot3_navigation_nishi2.launch に組み込んで、Gazebo で試して見ました。
トピック名、TF farme 名を Turtlebot3 に合わせて試してみました。

うまく動きました。

3) 次は、rtabmap-nishi_stereo_outdoor2_bz3_navi.launch にも組み込んで試してみました。
この場合は、LocalCostMap の obstacle layers の入力に、scan に変えて、point_cloud2 を指定します。
これは、以前にもやったので、要領は同じです。

その前に、wiki.ros.org/costmap_2d
8.2 Layer Specifications
 8.2.2 Obstacle Map Layer
を見ておくと、何処を変えればよいか、わかりやすいぞね!
Global Cost Map も Local Cost Map も、この Map Layer (Plug in なので、自由にロードできる) を組み合わせて、自由に欲しいコストマップを作れるみたいぞね。
例えば、おんちゃんが、SteroCamera から取り込んだ PointCloud2 を使って、障害物を避けたい場合は、Local Cost Map に、Obstacle Layer を指定して、その入ロに、 <point_cloud2_topic> を指定すれば、
後は、Local Cost Map を Subscribe するだけで、障害物を避けたコースが判定できる。
これは、便利ぞね!!

一応、Rviz から、目的地を示すとその方へ移動を初めます。しかしかなり精度に難があります。
しつこく操作していると、LocalPlanner がハングしてしまいました。
トータルで見直ししないといけない様です。
作成済 Map で試すと、うまく行くかも知れません。但し、オリジナルも、Active SLAM で動いているようですが。

再度、実機の 自作 Turtlebot3(foxbot_cor3) で試してみました。
最初はなかなか目的地の方に行こうとしません。どうやら周囲を見回して、Map を作っている様です。
操作を繰り返して、部屋全体のMap が完成すると、目的地へすぐ行くようになりました。
なかなか良いです。DWAよりも良い気がします。

4) local_planner について。
オリジナルは、base_local_planner::TrajectoryPlannerROS を使っていますが、
この代わりに、DWA も使えるようです。
wiki.ros.org/dwa_local_planner
dwa_local_planner::DWAPlannerROS
これなら、Turtlbot3 Navigation の設定ファイルがそのまま利用できそうです。
Rtabmap_ros with Single USB Stereo Camera で、move_base and DWA を使っていて性能が悪いのは、目的地の近くに行って、
到着の判定がシビアすぎるのでは、と感じています。そこの所を自分で C++ で作れば良いような気がします。

eband_local_planner だと、下記みたいぞね。
/eband_local_planner/eband_local_planner_ros.h

一旦Mapが完成すると、TrajectoryPlannerROS の方が良い気がします。このまま、暫くは、TrajectoryPlannerROS を使うことにします。

5) いよいよ、multi_goals4_cmd_vel.cpp への取り込み。
従来の、GoalList、GoalList2 での走行の中に、今回の、global_planner、local_planner での移動機能を組み込んでみます。

これは、例えば、草刈りで、畑を縦走して草を刈終わった後、スタート地点に戻る時に、この planner での走行に切り替えて、
あとはまっしぐらに帰るだけ。の時に使えそうです。帰り道は、Auto Pilot 走行です。

6) 一応、うまく動いたので、git にアップロードしました。by nishi 2022.6.17
/tosa-no-onchan/turtlebot3_navi_my
/tosa-no-onchan/turtlebot3_navi_my/blob/main/src/robot_navi.cpp

i) ビルドは、
$ cd ~/catkin_ws/src
$ git clone https://github.com/tosa-no-onchan/turtlebot3_navi_my.git
$ cd ..
$ catkin build turtlebot3_navi_my

$ cd ~/catkin_ws/src
$ git clone https://github.com/tosa-no-onchan/rtabmap_ros_my.git
$ cd ..
$ catkin build rtabmap_ros_my

ii) 起動は、launch ファイルから、multi_goals4_cmd_vel を起動します。
注) global_costmap、local_costmap を Plugin で起動しないといけないぞね。

launch ファイルは、/tosa-no-onchan/rtabmap_ros_my の中の

a) 自作 Turtlebot3 (foxbot_core3) rtabmap_ros with Single USB Stereo Camera 版
rtabmap-nishi_stereo_outdoor2_bz3_navi.launch
と
b) Gazebo Turtlebot3(Burger) rtabmap_ros with Stereo Camera 版
demo_turtlebot3_navigation_stereo2.launch
です。

注1) Gazebo Turtlebot3 Burger には、Stereo Camera が必要です。
Gazebo turtlebot3 burger stereo camera plugin
注2) 実行方法は、それぞれの　launch ファイルの上部に記載しています。