Turtlebot3 の自作 #11
-- ROS rtabmap_ros 自作 Stereo Camera で MappingAndNavigationOnTurtlebot を試す --
今回は、自作 Turtlebot3 に搭載した、自作 Stereo Camera を使って、
rtabmap_ros/Tutorials/MappingAndNavigationOnTurtlebot を試してみます。
上記ページの 6.1 Turtlebot3 on Melodic and Noetic を試してみます。
これが、上手く動けば、rtabmap_ros で、自作 Stereo Camera を使って、Mapping しながら、ナビゲーションができるようになります。
自作 Turtlebot3 foxbot_core3
1. 環境
1)リモートPC
Ubuntu Mate 18.04
ROS: Melodic
IP:192.168.1.170
2)TurtleBot3 SBC
Jetson Nano 2G
Wi-Fi Lan
IP:192.168.1.37
ROS:Melodic
USB Camera: Logi Cool C270 x 2
3. リモートPC での準備
オリジナル launch ファイルは、catkin_ws/src/rtabmap_ros/launch/demo/demo_turtlebot3_navigation.launch ですが、
多分 Gazebo Trutlebot3 対応で、入力が、Lidar、rgb-d(Kinect?) カメラ になっているようです。
当初、上記ファイルをいじっていましたが、かえって面倒なので、
同ファイル上の Navigation に必要なコードを、rtabmap_ros_my/rtabmap-nishi-stereoB.launch へ取り組む事にしました。
rtabmap_ros_my/rtabmap-nishi-stereoB.launch を rtabmap-nishi-stereoB_navigation.launch にコピーします。
取り込む主要な部分は、2個の様です。
<include file="$(find turtlebot3_bringup)/launch/turtlebot3_remote.launch">
と、
<include file="$(find turtlebot3_navigation)/launch/move_base.launch">
注) 一応テストが完了しました。2021.9.20
odom が、/odom と /stereo_camera/odom の２つ有って、Rviz で両方表示されているのでどちらか一方にしないといけないです。
結局、rtabmap_ros/stereo_odometry をやめました。
rtabmap_ros_my/launch/rtabmap-nishi-stereoB_navigation2.launch
4. 実行手順
ここからが、実行の手順です。
1) リモートPC
$ roscore
2) SBC(Jetson Nano 2G)
$ roslaunch uvc_camera stereo_node.launch
$ sudo chmod 666 /dev/ttyTHS1
$ roslaunch turtlebot3_bringup turtlebot3_robot.launch
Topic の一覧が出てくることを確認します。エラーになったら、一度 Ctl+Cで中止して、再度起動します。
注1) stereo_node.launch と turtlebot3_robot.launch は、別々に起動します。
turtlebot3_robot.launch のなかで、 stereo_node.launch を起動すると、Stereo Cameraのキャリブレーションが未だの様な状況になるので。
3) リモートPC
$ export TURTLEBOT3_MODEL=waffle
$ roslaunch rtabmap_ros_my rtabmap-nishi-stereoB_navigation2.launch
注) waffle だと実際の自作 Turtlebot3 より大きいかも。bugger だと逆に小さすぎか。
4) 操作。
Rviz 画面上で、上部の 2D Navi Goal アイコンをクリックして、画面の目的地をクリックしながら、方向をドラッグで指定します。
そうすれば、Active Mapping (Active SLAM) をしながら、目的地へのルート計算をして、移動します。
目的地へ行く途中でクルクル回って、周囲のマップを作成させて行きます。
注1) これは、stereo_odometry を使ったとき、ただし、stereo_odometry は、すぐ Mapping が進むが、止まりやすい。
stereo_odometry を切ると、Mapping の進みが遅い(フレームの数が少ない)が、丈夫になる。
注2) stereo_odometry を使わない時は、 2D Navi Goal アイコンを使って、自分で少しずつ、Turetlebot3 を回転させて、
周りのMapping を進めないといけない。
これは、フレームの速度が、1 fps になるから、Mappinng に時間がかかる所為と思われます。
注3) フレームの fps が一番上がるのは、stereo_odometry を有効にして、
rtabmap_ros/stereo_odometry のパラメータを
approx_sync=false
rtabmap_ros/rtabmap のパラメータも
approx_sync=false
すると、フレーム速度が上がります。
しかし、この場合は、かなり CPU パワーが必要みたいです。
5. 今回、学んだ事。
1) ２つのカメラの取り付けは、本当に正しく行う事。
いい加減だと、カメラの同期が取れなくて、rtabmap がワーンニングばかりだして、Mapping が止まる。
２つのカメラは、鉛直で、真直ぐ前方を向かせる事。
下を向いていると、部屋のMapping をさせていると、床を障害物と判定して、部屋が障害物だらけになって、行き場所がなくなる。
2) /odom (foxbot_core3 が出力) を使って、Mapping をずっと続けていると、/odom 自体が誤差を蓄積していくので、
その影響が、障害物の方角の判定の誤差に出てくる。
なので、壁が最初は正しい場所に出ていたものが、だんだんずれてきて部屋の中に描画されるようになる。
odom からの、障害物の方向&距離は、合っているのに、その odom 自体がずれているので、Map上、ありえない場所に、
障害物が現れるのでどうしようも無い。
Gazebo 上は、絶対位置が得られるので、Mapping をいくら続けても、こんな現象には出会えない。実機ゆえの現象だと思う。
odom の精度をできるだけ上げるのはもちろんだが、これも限界がある。
自作 Stereo Camera & rtabmap_ros で障害物の距離、方角はそれなりに判定できる。odom さえ正確であれば、
2D Map にも正確に描画できる。
odom が正確でないと、正しくない 2D Map となり。それ故、2D Map を参照する、Navigation も正しく出来なくなってしまう。
結局は、Odometry の出来、次第か!!
試しに、foxbot_core3 の odometry を今回は入念に詰めてみました。
Turtlebot3 の自作 #4 のSLAMの起動方法で、直進と回転を実測とRviz の計測値とが一致するように詰めてみました。
i) 直進の補正。
実測2M の直線を、実際に走らせて、Rviz の Measure 値との差がなくなる様に、WEHEEL_RADIUS を詰めます。
今回、実際 2M 走行させて、Rviz の Measure で、 2.0554[M] まで詰めました。欲を言えば、2.01[M] までにしたいですが。
ii) 回転の補正。
Rviz で、360度1回転させて、その時の回転の実測との差がなくなるように、BASE_LENGTH を詰めまてみます。
今回、殆ど差が無い所まで詰めました。
結果は、かなり GOOD!! です。
部屋の中を何回か、行ったり来たりさせて、Mapping させてみましたが、以前の様に壁の障害物が中の方に浸食する事が無くなりました。
屋外だと、GPS(みちびき) や、コンパスを使ってなんとか domerty を補正できないか?
3) stereo_odometry は、Stereo 画像マッチングが停滞すると、停止してしまう。
Navigation で移動している際に、stereo_odometry が止まると、
ロボットは動いているのに、odometry が止まるので、2D Map がぐちゃぐちゃになる。
これを回避するには、3 Ghz 4 core ほどの CPU が必要か?。又は、専用 Stereo Camera が必要か?
Jetson TX2 だと、SBC 上で、rtabmap_ros を走らせられるみたいだ。
Turtlebot3 3D-SLAM using RTAB-Map with Jetson TX2
4) 2D Mapping and 3D Mapping のRviz を作成しました。
catkin_ws/src/rtabmap_ros_my/launch/config/nishi-stereoB-navi.rviz
今回の確認は、これくらいで終でしょうか?。
今後は、草刈りロボットへの応用を考えて行きたいです。
草刈りロボットと言えば、屋外での運用で、明確な境界物がない環境を想定しています。rtabmap_ros の 3D Mapping と、
2D Mapping を併用して問題解決を探りたいぞね。
もちろん、Jetson Nano を使って、機械学習の利用も取り入れたい。
コホッ!!