自作 Turtlebot3 自律走行に向けたプログラム。#13
--- Gazebo Turtlebot3 burger で、GPS を使ってみる。 ---
ROS IMUとGPSのGazeboプラグインを使う に、Gazebo で、GPS を使う例があったので、試してみました。
1. GPS プラグインの組み込み。
パッケージ版があったので、おんちゃんは、こちらををつかってみました。
$ sudo apt install ros-noetic-hector-gazebo-plugins
2. URDF の作成。
URDF は、上記ページにも例がありますが、おんちゃんは、以前、IMU、Camera を組み込んだ例に習って、今回も作りました。
catkin_ws/src/turtlebot3/turtlebot3_description/urdf/gps_macro.xacro
注) 使えるパラメータは、hector_gazebo_plugins に、記載があります。
注2) frameId
<frameID>${prefix}_link</frameID> --- Bad
<frameId>${prefix}_link</frameId> --- OK
catkin_ws/src/turtlebot3/turtlebot3_description/urdf/turtlebot3_burger.urdf.xacro へ追記します。
3. テスト起動
$ export TURTLEBOT3_MODEL=burger
$ roslaunch turtlebot3_gazebo turtlebot3_house.launch
/gps トピックの確認。
$ rostopic list
$ rostopic hz /gps
注) House だと、Burger のStart 位置が、World (0,0,0) ではないので、GPS の起点をずらさないといけないのでは?
4. Gazebo Turtlebot3 Buger with rtabmap_ros with Depth Camera の Launch で試してみる。
robot_localizationでGPS自己位置認識をする を参考に、して、navsat_transform_node だけを使って、
プラグインの GPS 情報を元にした、 odom/gps を使って、試してみました。
demo_turtlebot3_navigation_nishi2_navsat.launch の抜粋。
注1) publish_filtered_gps=true
注2) yaw_offset=1.5707963
当初、下記ワーニングがでました。
/odom_gps を見てみると、
$ rostopic echo /odom_gps | more
...
frame_id:"odom"
chiled_frame_id:''
...
ー方、/odom の方は、
$ rostopic echo /odom | more
...
frame_id:"odom"
chiled_frame_id:"base_footprint"
...
どうやら、此処が問題の様です。
Google で検索したら、 navsat_transform_node does not publish GPS odometry when using manual datum #225 があったので、参考にして
...
<param name="wait_for_datum" value="true"/>
<rosparam param="datum">[49.2757225, -122.895907, 134.4, map, base_link]
</rosparam>
...
を、使ってみたが、ワーニングは、同じようです。おまけに、position-x,y 値がめちゃくちゃ大きい。
結局、
publish_filtered_gps=true
で、OK になりました。
注) publish_filtered_gps=true も不要みたいだが、true にしときます。
5. 使ってみて、気がついた所。
/odom_gps と、/odom の、psoe:pose:positon.x,.y が入れ替わっているみたい。
更に、/odom_gps psoe:pose:positon.x は、符号が逆のようだ。
なんだ、こりぃは?
Rviz 上の ロボットは、TF 情報が使われるから、此処がずれなければ、問題ないのだが。それにしても、合点が行かない。
yaw_offset=1.5707963
で、解決しました。
IMU のデータの向きを、90度回転させる意味だそうだ。
おんちゃんは、imu_macro.xacro で、90度回転させてみたが、そちらでは、うまくいかなかった。
関連ドキュメント。
1) doc/navsat_transform_node.rst
2) Integrating GPS Data
6. 次は、ekf_localization_node で、Fusion となるのだが、これが難問だ。以前もチャレンジして、あまり感触は、良くないままだ。
以前、失敗したのは、TF情報で、
TF : map -> odom -> base_footprint -> base_link
の階層になっているのだが、各階層のLink情報を出すのは、ただ1箇所でないといけない。
ところが、実機だと、foxbot_core3 が出しているところに、ekf_localization_node でも、tf を出力する設定にすると、おかしくなる。
ekf_localization_node で tf odom->base_footprint を出す場合は、従来、tf odom->base_footprint を 出しているところを止めないといけないみたい。
Gazebo Turtlebot3 Burger でも、tf odom->base_footprint を出していれば、止めないと、うまく行かないのでは。
今回も試してみたが、案の定、2箇所で出しているみたいで、Rviz上で、ロボットの位置がぴくぴく常に動く。
tf map->odom は、通常、同一地点を示す。但し、rtabmap_ros を使うと、tf map->odom を操作して、odom の位置を変えて、
ロボットの位置 (tf map->base_footprint) を補正する。
なので、Roboto からは、通常は、 tf odom->base_footprint を Publish する。
1) launch ファイル。
demo_turtlebot3_navigation_nishi2_navsat.launch の抜粋。
2) mode1.yaml ファイル
注1) publish_tf: false です。
publish_tf: true にする時は、tf odom->base_footprint の Publish が競合するので、
Gazebo Turetlebot3 Burger の tf odom->base_footprint の Publish を停止しないといけない。
注2) frequency=4 にしたが、大きくすると、Rviz 上で、表示がかなりフッリカリングする。結構微妙に、
tf odom->base_footprint が変化しているみたいだ。rtabmap_ros、move_base に影響が出ない範囲で、遅くしました。
3) Gazebo Turetlebot3 Burger の tf odom->base_footprint の Publish を停止するには。
おんちゃんもよくわからないが、たまたま試したら、うまく行ったので、書いておきます。
turtlebot3/turtlebot3_bringup/launch/turtlebot3_remote.launch を修正します。
use_tf_static=false にしました。
これで、 mode1.yaml ファイルで
publish_tf: true
に出来ます。
4) turtlebot3_remote.launch は、他でも使うので、最終的に、下記の様にしました。
turtlebot3/turtlebot3_bringup/launch/turtlebot3_remote.launch
実際のコールは、
5) おんちゃんの感想。
今回は、なんとかうまく行きました。
ekf_localization_node で、wheel odometry, GPS odometory and IMU の３つを、Fusion した、odometry を、
rtabmap_ros と move_base の入力に与えて、tf odom->base_footprint もPublish しています。
やはり、 Turtlebot3 Burger からの tf odom->base_footprint の Publish を抑止したおかげみたい。
自作 Turtlebot3(foooxbot_core3) も、 tf odom->base_footprint の Publish を止めれば、OKか。
6. 今後は、
みちびきの L6 が受信できるボードで安いのが、もうすぐ発売になりそうなので、其れが手に入ったら、
自作 Turtlebot3(Foxbot_core3) に取り付けて、実機で試してみたいぞね。
やはり、最終的には、実機で試してみないと、シュミレーションでは、限界があります。
なにー、もう販売になっちょりますと?
みちびきCLAS L6 受信用IC NEO-D9C 開発ボードD9CX1の試作
Yahoo Shop で、18,700円じゃとー
あとは、おんちゃんのふところ具合だけの問題か。
注) このボード+ZED-F9Pモジュールボードが必要みたいでした。
spresense だと、QZSS(L1 C/A, L1 S) で、 L1S だと、1mから2m みたいだ。
こちらだと、7,000円 程度だから、どうだろうか?