Gazebo turtlebot3 waffle Depth Camera Slam and real time Mapping.

1. パッケージの導入。
https://github.com/introlab/rtabmap_ros に記述があります。
  Installation > ROS distribution > Melodic
  $ sudo apt install ros-melodic-rtabmap-ros

2. Gazebo/Turtlebot3 の実行は、同じく wiki の、
  2. Tutorials > 7. Mapping and Navigation with Turtlebot に記述があります。

  上記ページの
  6.Simulation (Gazebo)
  ...
   6.1 Turtlebot3 On Melodic を行えば起動できます。

     $ export TURTLEBOT3_MODEL=waffle
     $ roslaunch turtlebot3_gazebo turtlebot3_world.launch

     $ export TURTLEBOT3_MODEL=waffle
     $ roslaunch rtabmap_ros demo_turtlebot3_navigation.launch

3. Active Mapping and Navigation の確認。
ここで、従来であれば、turtlebot3_teleop_key.launch で、 waffle を 動かして、
マップ作成、保存するのですが、今回は、そのまま、Navigation が実行できます。

Rviz の アイコン: 2D Nav Goal をクリックして、地図上の目的地をクリックします。
これで、 waffle が自動で目的地まで、移動します。

4. ある程度、地図が完成したら、MAP を保存。
$ rosrun map_server map_saver -f ~/map
で、~/map.yaml, ~/map.pgm が保存できて、
次回からは、ナビゲーションが即実行できると思います。

5. Navigation の実行。
 注) ここでは、レーザーセンサーでの実行です。
 2. の起動プログラムをすべて終了させる。
 $ export TURTLEBOT3_MODEL=waffle
 $ roslaunch turtlebot3_gazebo turtlebot3_world.launch

 $ export TURTLEBOT3_MODEL=waffle
 $ roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/map.yaml

これで、Rviz の アイコン: 2D Nav Goal をクリックして、地図上の目的地をクリックすれば、waffle が自動で動くと思います。

ただし、こちらだと、新たな障害物が在っても、避ける事ができないので、どうなのか?

注1) この部分は、誤りなので、訂正します。
turtlebot3_navigation.launch でも、レーザーによる障害物の判定と、回避が行われる。とのことです。
障害物の判定の設定は、costmap の設定で、
burger であれば、
turtlebot3_navigation/param/costmap_common_params_burger.yaml
で、できるみたいです。

注2) Rviz で アイコン: 2D Nav Goal をクリックして、地図上の目的地をクリックすると、
トピック /move_base_simple/goal (geometry_msgs/PoseStamped) がパブリッシュされて、ロボットがそこへ移動する。
との事なので、プログラムから同じようにパブリッシュすれば、ロボットを動かせる気がします。

$ rostopic pub /move_base_simple/goal geometry_msgPoseStamped '{header: {stamp: now, frame_id: "map"}, pose: {position: {x: -1.36, y: 2.75, z: 0.0}, orientation: {w: 1.0}}}'

move_base に関しては、下記ページに記述がありました。
wiki.ros.org/move_base
ROS講座92 move_baseで移動する
Navigation スタックの中心パッケージとの事。

6.感想。
Turtlebot3 の 機能を、自作の車(Raspberry Pi + 3輪車)に組み込めば、かんたんに、ここまでの機能が、
実現できると言うことか?

おんちゃんの最終目標は、自動走行 の 草刈りロボットを作りたいのだが?
やはり入力センサーは、Depth カメラになると思うけど。

あと、実機では、Gazeboのシュミレーション と違って、Odometry の算出がうまく行かないと思う。
なので、マップ使用自体が、期待できないかも!!

7. Navigation のセンサー入力に関して。
http://wiki.ros.org/navigation/Tutorials/RobotSetup
の 1.3 Sensor Information (sensor sources) に、
The navigation stack uses information from sensors to avoid obstacles in the world, it assumes that
these sensors are publishing either sensor_msgs/LaserScan or sensor_msgs/PointCloud messages over ROS.
とありました。
Depth Camera(openni kinect) の sensor_msgs/PointCloud2 でも OK なのだろうか?

8. Depth Camera による、 costmap の設定。
Laser Scan Sensor に替えて Point Cloud Sensor (Depth Camera) の設定方法がありました。
wiki.ros.org/ja/navigation/Tutorials/RobotSetup
 2.3.1 Common Configuration (local_costmap) & (global_costmap)

早速、 Gazebo/Turtlebot3 Burger に取り込んでみます。

turtlebot3_navigation/param/costmap_common_params_burger.yaml
#obstacle_range: 3.0 # changed by nishi obstacle_range: 0.35 raytrace_range: 3.5 footprint: [[-0.105, -0.105], [-0.105, 0.105], [0.041, 0.105], [0.041, -0.105]] #robot_radius: 0.105 inflation_radius: 1.0 cost_scaling_factor: 3.0 map_type: costmap #observation_sources: scan #scan: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true} # test by nishi observation_sources: point_cloud_snsor point_cloud_snsor: {sensor_frame: camera_link, data_type: PointCloud, topic: camera/depth/points, marking: true, clearing: true}

注)
sensor_frame:frame_name に何を設定するかは、camera_macro.xacro と一致させます。
  ↓
turtlebot3/turtlebot3_description/urdf/camera_macro.xacro
.... <xacro:macro name="rgbd_camera_macro2" params="parent prefix xyz"> .... <plugin name="${prefix}_controller" filename="libgazebo_ros_openni_kinect.so"> ... <frameName>camera_link</frameName> # sensor_frame ....

9. Depth Camera(Rgb-d Camera) による、Navigation の実行。
1) 起動
 $ export TURTLEBOT3_MODEL=burger
 $ roslaunch turtlebot3_gazebo turtlebot3_world_nav_nishi.launch

 $ export TURTLEBOT3_MODEL=burger
 $ roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/map.yaml

2) Gazebo の画面で、通り道に、Cinder Block を置く。
Insert > Cinder Block
3) Rviz で、
 Add > By Topic > camera > depth > points > PoinCloud2
アイコン: 2D Nav Goal で目的地をクリック。

何回かぶつかるけど、なんとか、避けて動くみたい。
障害物を検知すると、青い領域が表示されるぞね!!
OK じゃ!!

注) 今日、再度試してみました。
障害物に近づいても、青の警告領域が表示されずに、障害物を回避できません。
Laser に戻して、試すと、こちらは、ちゃんと青の警告領域が表示されて、回避できます。
これは、なぜか!!

10. 再度、costmap_common_params_burger.yaml を見直しました。
turtlebot3_navigation/param/costmap_common_params_burger.yaml
#obstacle_range: 3.0 # changed by nishi obstacle_range: 0.3 raytrace_range: 3.5 footprint: [[-0.105, -0.105], [-0.105, 0.105], [0.041, 0.105], [0.041, -0.105]] #robot_radius: 0.105 inflation_radius: 1.0 cost_scaling_factor: 3.0 map_type: costmap #observation_sources: scan #scan: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true} # test by nishi observation_sources: point_cloud_snsor point_cloud_snsor: {sensor_frame: camera_link, data_type: PointCloud2, topic: camera/depth/points, marking: true, clearing: true}

 data_type: PointCloud2 にしたら、
障害物検知の青の領域がでるようになりました。
しかし、障害物を避ける動作には、もう少し改善の必要がある気がします。

注1) もう一つ、収穫がありました。
Turtlebot3 が、障害物にぶつかって、新たなルートを検出する際に、車体をくるくる、その場所で回る動作をします。
なので、実際の車体も、角のない丸い車体にしないと、この動作を妨げると思います。

注2) 更にもう一つ、発見しました。
コースに拠っては、バーガーが、バックで目的地まで進むときがありました。
これでは、進行方向が、Depth カメラに映らないので、問題か?
もっと、ナビゲーションをうごかして、このプログラムの癖を把握しないと、いけないぞね!!

ナビゲーションプログラム改善が、将来は必要になるのか?
やはり、レーザーセンサーが必要なのか!!
これは、後ろへ向かうときは、 /move_base_simple/goal (或いは、/cmd_vel) に先に向きを変えるメッセージを送って、
その後、目的地の位置データを送れば良い。のかも!!

11. Navigation を使えば、後は、 /move_base_simple/goal へ目的地の位置情報を送れば、良い。だけなので、
後は、この目的地情報をどのように決めればかの問題になりそうです。
後、実機では、位置情報の算出の問題が出てきそうです。