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

2. プログラムの概要。
local_costmap は、costmap_2d::Costmap2DROS のコンストラクターに、Subscribe したいトピック名 "local_costmap" と、tf::TransformListener tf_; を与えると、
取得できるようです。
costmap_2d::Costmap2DROS local_costmap_("local_costmap", tf_);
その後、
costmap_2d::Costmap2D* costmap2D_ = local_costmap_.getCostmap(); で、costmap_2d::Costmap2D の Pointer を貰えば、後は、costmap2D_-> で直接アクセスできます。
local_costmap_.getRobotFootprint(); で、ロボットのfootprint がえられます。

ロボットの今の　map 上の位置(map ->base_footprint) を、TF 情報から取得して、x,y を求めれば、後は、
costmap2D_->worldToMap(x,y,mx,my) で、x,y を Costmap 上のCell point mx,my に変換して、
costmap2D_->getCost(mx,my); をコールすれば、cost が取得出来ます。

注1) local_costmap、global_costmap の、本体の実行部分は、plugin なので、xxx.yaml でロードしないといけないぞね。
move_navigation_point_cloud2.yaml
注2) 定期的に、ros::spinOnce(); を行って、local_costmap_ インスタンスを更新しないといけないぞね。
注3) 一応、demo_turtlebot3_navigation_stereo2.launch で、roslaunch turtlebot3_gazebo turtlebot3_house.launch を起動して、
scan/LaserScan の方で試したら、OK となりました。
rtabmap_ros/point_cloud_xyz の方は、Gazebo だと、Stereo Camera の近場がうまく捉えられないので、local_costmap に入って来ません。
これは、今後の対応にします。
実機の 自作 Turtlebot3(foxbot_core3.ino) だと、OK のような気がします。まだ試していませんが。

その後、Gazebo の場合の、rtabmap_ros/point_cloud_xyz の方は、トピックの frame_id: stereo_camera_optical_frame だったので、
これに合わせれば、local_costmap に障害物が出てきました。

一応、Gazebo Turtlebot3 Burger and rtabmap_ros with Stereo Camera で、自分の c++ プログラムから、robot の4隅の
local_costmap の cost を読み取って、停止できるようになりました。
コッホ!!
注4) costmap_2d の設定は、costmap_2d を参考におんちゃんの環境に合わせます。

  inflation_layer:
    inflation_radius: 0.14
    cost_scaling_factor: 10.0

inflation_radius は、ロボットを回転させてできる円の半径以上を指定します。
cost_scaling_factor は、値が大きい方が、障害物のcost値が少ないみたい。

3. github にアップロードしました。
github.com/tosa-no-onchan/turtlebot3_navi_my/blob/main/src/robot_navi.cpp
注) 昨日の公開分は、バグがありました。"update 2022.7.11" 版を使って下さい。by nishi 2022.7.11

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

実行方法は、下記 launch ファイルの上部に記載しています。
demo_turtlebot3_navigation_stereo2.launch
Gazebo Trutelbot3 Burger の Lanuch ファイルです。

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

$ roslaunch turtlebot3_gazebo turtlebot3_house.launch
で、 turtlebot3_house.launch を試せます。

4. rtabmap_ros のパラメータの見直し。
Gazebo で、roslaunch turtlebot3_gazebo turtlebot3_house.launch を、rtabmap_ros with Stereo Camera で動かしていると、
隣の部屋への通路が、障害物判定で、通れなくなる。または、 畑のフィールドで、並べた木の横を通ろうとすると、
木の上部の枝部分が、障害物判定されて、通れる範囲が狭くなってしまう。これの対策は、
部屋の上部や、木の上部を、障害物として判定しないよう設定します。

Grid/MaxObstacleHeight=1.0

で、障害物判定の高さを制限すると解決します。

5. PC に、GPU を付けてみました。
Gazebo Turtlebot3 で、rtabmap_ros with Stereo Camera or Depth Camera を利用していますが、どうも遅くてやる気が出ませんでした。
試しに、GTX-1050 が在ったので、だめもとで、付けて試してみました。
結果、Gazebo 上の Turtlebot3 の動きが早くなって、マウスの応答も問題なくなりました。 ものは試してみるものぞね!

CPU: Intel Core i5 haswell 4570(3.2 GHz boost 3.6 GHz) 4 core/4 threads
Memory: DDR3/1600 12GB
CPU 単体では、Stereo Camera で、CPU1-4 90% だったのが、GPU 導入後 80% になって、
メモリー使用量も、3.5 - 4 G ほどになりました。ぶん回っていたCPU ファンも普通になりました。これは、正解でした。

GPU を使って、Gazebo Turtlebot3 Rtabmap_ros with Depth Camera or Stereo Camera を 試してみて、CPUの負荷が一番軽いのは、
Burger + Depth Camera で、CPU 1-4 が、40 - 45[%] 程度の負荷でした。
上記以外は、CPU 1-4 が、80 - 90[%] 負荷 になります。

Burger + Stereo Camera
Waffle + Stereo Camera or Depth Camera
いずれも、高負荷です。
GPU 導入で、Gazebo Turtlebot3 and Rtabmap_ros でなんとか、シミュレーション上でテストができる環境になりました。

6. 今、ちょっと困っている事。
Gazebo Turtleboto3 burger and rtabmap_ros with Stereo Camera で、roslaunch turtlebot3_gazebo turtlebot3_house.launch を起動して、
C++ プログラムで360 度回転させて Map を作成してみますが、床の障害物の無い所が、 白(=0)になりません。
Depth Camera だと、普通に白(=0) になります。
C++ プログラムで、Burger を隈無く動かして、部屋の輪郭を捉えたいと考えていますが、Stereo Camera だと、
殆ど、白(=0) のグリッドが出来ません。なので、実際にグレー(=-1) なのか、白(=0)　なのかが、rtabmap_ros の Map だけでは、判別出来ないので困っちょります
実機の自作Turtlebot3(foxbot_core3) が、rtabmap_ros with Stereo Camera なので、最終的には、 こちらで動作させたいので、困ったもんです。

7. rtabmap_ros のパラメータの見直し。#2
GPU で描画性能が上がった所為で、照明がはっきり判るようになりました。
Gazebo で、house や、gass station で光が強くなった所為で、壁の影や木(Gass station に自分で植えた)の影がはっきり出てきて、
rtabmap_ros with Stereo Camera だと、その影を障害物と判定してしまいます。但しその障害物は、高さが 0 なので、

Grid/MinGroundHeight=0.05

で回避できました。

関連ページ
Gazebo Turtlebot3 burger への、Stereo Camera、Depth Camera の組み込みは、下記を参照しとおせ
Gazebo turtlebot3 burger stereo camera plugin