ROS2 自作 Turtlebot3 による 草刈りロボット開発。#10 走行開始時のロボットの向き。
副題: ROS2 C++ 自立走行ロボットによる草刈りロボット開発
--- ros2 nav2_gps_waypoint_follower_demo autonomous は、できるか検討する。 ---
ROS2 自作 Turtlebot3 による 草刈りロボット開発。#9 LSTM で経路計画をする。
が出来たので、自分の部屋で、実機を Auto Mower で走らせている。
その中で、気になりだしたのが、ロボットの開始時の向きのズレが気になる。
注) その前のお断り。
Robot Localization に Navigation2 の AMCL with SCAN を使っていれば、Static Map を参照して自動的にロボットの向きが補正されると思う。
今回は、ekf_localization with no GNSS をつかっているので、こんな問題がでてくると思う。
おんちゃんは、ほとんど AMCL with SCAN を使ったことがなかったから、AMCL Localiaztion の知識が欠落しちょりました。
屋内であれば、AMCL with SCAN は、Roboto Localiaztion の強力な機能じゃった。
Auto Mower で走行するときは、予め作成した、Static Map をベースに走行している。
この時、Static Map 作成時の、ロボットの向きと、 Auto Mower 走行時のロボットの向きがずれていると、具合が悪い。
1度ずれていると、数十メートルそうこうすると、かなりのズレが生じてしまう。
やはり、みちびき等の GNSS でロボットの位置を補正するしかないのかも。
その前に、予備知識として、ekf-localization 、navsat_transform を使ったロボット位置推定の場合の話だが、
robot_localization/navsat_transform_node を使った場合、
navsat_transform.yaml
の、datum: [38.161491, -122.4546443, 0.0] # pre-set datum if needed, [lat, lon, yaw]
で、tf-map の原点の緯度、経度と、x 軸の向きを指定する。
yaw (X 軸の方向)は、通常 東=0 [radian] になるみたい。
通常は、X軸の+方向が、東としているので、地図の X軸の+ 方向を変えたければ、ここで指定する。
Navigating Using GPS Localization
によると、robot_localization は、WGS84(世界測地系) を使うとのこと。
与えられた、GPS の lat,logi から、地表上の平面地図上の距離に変換する処理をしてくれる。
つまり、GPS の lat,logi の現在値から、起点となる、datum からの、地表面上の距離をWGS84を使って、算出して、
それは、tf-map(0,0) からのX軸[M]、Y軸[M] の値として教えてくれる。
結構便利なツールじゃ。
地図から、Map に変換する場合は、この WGS84 の地図を使えば良い。
出力は、odometry/gps で得られる。
$ ros2 topic echo /odometry/gps
で、tf-map からの距離が得られる。だから、 GNSS の値が正確であれば、この odometry/gps だけで、ロボットの正確な位置がわかる。
だから、本来は ekf-localization、ukf-localization は必要がない。必要がなければ、無理して使うこともない。
すでに作成された、Static Mapは、作成時に使用した、datum をnavsat_transform_node に与えれば良い。
ロボットの向きも、Static Map 作成時の向きにあわせる。
手作業で、Static Map を作成した時は、そのマップの起点と X軸(+方向)方向 を、datum を使って、navsat_transform_node に与えれば良い。
このページを書いた時は、予備知識として、navsat_transform この知識がなっかたまま書いたので、書き直しが必要みたい。by nishi 2025.8.10
注1) 部屋の中で、GPS を使わない時は、開始時のロボットの向きが、+X に、必然的になる。
以上が、予備知識である。
これ以降は、前述の予備知識で解決済だと思うので、参考程度に観てください。by nishi 2025.8.13
GNSS-RTK を使った場合に、ロボットのおいた向きが微妙にずれていた場合、走行するにつれて、自動調整されていくのであれば、最高だが、
これは、実機で試してみないとわからない!!
予測としては、
1) tf-odm->tf-basefoot_print を、navsat_transform_node の odom から自分で作れば、全て、GNSS-RTK の値になるので、自動補正される。
2) ekf-localization、ukf-localization を使った場合 GNSS-RTK 以外に、IMU、モーターによる odom とのフュージョンになるので、少しは補正される。
3) GNSS-RTK を使わない場合、現状、補正されることはない。
Auto Mower でちょっと走行した時に、ロボットの向きを補正する方法は、ないものだろうか?
1.
Navigating Using GPS Localization が参考になるのか?
nav2_gps_waypoint_follower_demo を使って動作確認ができるみたい。
rolling、jazzy バージョンが必要みたい!!
仕方がないので、 iron を入れて試してみる。
2.
Imu with magnetometer, read absolute position ここも参考になるか?
ただ、IMU Magnet での方位測定は、難しい気がする。
GNSS-RTK、GPS-RTK を使った、方位を検出する方ができそう。
例えば、ロボットのスタート位置と、ロボットを前に1[M]移動させた位置の差で、ロボットの方位が検出できそうだ。
3. Navigating Using GPS Localization を試してみる。
環境:
Ubuntu Mate 22.04
ROS2 iron
当初、 humble で試していたが、うまく動かなかったので、humble -> rolling -> iron にした。やれやれぞね!!
1) iron のインストール。
$ sudo apt install ros-iron-desktop
$ sudo apt install ros-iron-bondcpp
$ sudo apt install ros-iron-test-msgs
$ sudo apt install ros-iron-geographic-msgs
$ sudo apt install ros-iron-behaviortree-cpp-v3
$ sudo apt install ros-iron-diagnostic-updater
$ sudo apt install ros-iron-ompl
$ sudo apt install ros-iron-robot-localization
$ sudo apt install ros-iron-xacro
$ sudo apt install ros-iron-turtlebot3-fake-node
$ sudo apt install ros-iron-turtlebot3-gazebo
$ sudo apt install ros-iron-turtlebot3-simulations
#$ sudo apt install ros-iron-robot-localization
$ sudo apt install ros-iron-mapviz
$ sudo apt install ros-iron-mapviz-plugins
$ sudo apt install ros-iron-tile-map
i) DynamixelSDK
$ sudo apt install ros-iron-dynamixel-sdk
$ sudo apt install ros-iron-turtlebot3-msgs
git clone
$ cd ~/colcon_ws-iron/src
ii) navigation2
$ git clone -b iron ....
iii) turtlebot3
$ git clone -b humble-devel https://github.com/ROBOTIS-GIT/turtlebot3.git
iv) navigation2_tutorials
$ git clone -b jazzy https://github.com/ros-navigation/navigation2_tutorials.git
check required ros2 package
$ cd ~/colcon_ws-iron
$ rosdep update --rosdistro=iron && rosdep install --from-path src/navigation2_tutorials -r [-y] -s
多分、今のままでOKか。
3) source build
$ cd ~/colcon_ws-iron
$ colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --parallel-workers 2
残念!!
nav2_straightline_planner でエラーになる。やはり rolling でないとNGか!!
試しに、下記 3ファイルを削除してみた。直接は、使っていないみたい。
nav2_straightline_planner
nav2_pure_pursuit_controller
nav2_sms_behavior
一応、build ok とする。
3.1 実行。
1) gazebo 起動。
$ . /usr/share/gazebo/setup.sh
$ ros2 launch nav2_gps_waypoint_follower_demo gazebo_gps_world.launch.py
注) iron にしたら、すぐに起動できたし、 turtlebot3 の姿が見えだした。
2) Localization Testing
$ ros2 launch nav2_gps_waypoint_follower_demo dual_ekf_navsat.launch.py
robot_localization / ekf_node と navsat_transform_node だけみたい。!!
3) 別のterminal で、mapviz を開く。
$ ros2 launch nav2_gps_waypoint_follower_demo mapviz.launch.py
On a different terminal launch mapviz using the pre-built config file in the repo. Get a bing maps API key and use it to display satellite pictures.
ここで、 bing maps API key が必要なのか?
navigation2_tutorials/nav2_gps_waypoint_follower_demo/config/gps_wpf_demo.mvc
bing_api_key: ""
4) 最後の、telop-key で操作する。
Finally run the teleop twist keyboard node to teleoperate the simulated Turtlebot:
$ ros2 run teleop_twist_keyboard teleop_twist_keyboard
bing_api_key の入手方法がわからないので、mapviz で見ることが出来ない。
実際に、操作が出来なくて残念じゃ。
5) navigation で操作できるみたい。
1) - 4) は、不要か?
All in one か!!
$ . /usr/share/gazebo/setup.sh
$ ros2 launch nav2_gps_waypoint_follower_demo gps_waypoint_follower.launch.py use_rviz:=True
中は、
nav2_bringup/launch/navigation_launch.py
nav2_params=nav2_gps_waypoint_follower_demo/config/nav2_no_map_params.yaml
注) iron にしたら、やっとnavigation で動くようになった。
注2) /map が出てこないのは、正解か。そのかわり、global_costmap 、 local_costmap が出ているので、これが使えるみたい。
注3) 起動したら、Rviz2 上のロボットの向きを、Gazebo 上のロボットの向きと同じにすると良いかも。
navigation2_tutorials/nav2_gps_waypoint_follower_demo/config/nav2_no_map_params.yaml
Discuss system design and integration of vision based urban robot in the nav2 framework #2174
rolling_window: True
GPS Navigation だと、大きな static costmap を使うのは実用的でないとのこと。
なので、rolling_window: True を指定して、 十分大きな rolling global costmap を使うようだ。
navigation2_tutorials/nav2_gps_waypoint_follower_demo/config/nav2_no_map_params.yaml
には、amcl は、入っていないみたいだが。
頭に、下記記述がある。
rolling costmap と言うのがあるみたいだ!!
この機能は、新しい機能みたいだ。
少し、c++ プログラムから操作して、どんなものか確かめる必要がある。by nishi 2025.1.29
6) ladir scan の向きがなんとなく変!!
rviz2 上の Scan の投影が、ロボットを回転させても、通常は同じだが、変ってしまう。
どうも、ロボットの後方に、余計に Scan の軌跡が出ているようだ。これだけが、不具合みたいじゃ。
ロボットの左側の Scan の投影が、90度 回転して、Rviz2 上で、余計に出ているみたい。
注) なんとなく、rviz_launch.py のパラメータの気がする。
下記、ログが出ているみたい。
どこが原因かわからないが、
navigation2_tutorials/nav2_gps_waypoint_follower_demo/models/turtlebot_waffle_gps/model.sdf
をいじってみた。
でなくなったみたいだが?現象は同じ。
navigation2_tutorials/nav2_gps_waypoint_follower_demo/config/dual_ekf_navsat_params.yaml
もいじってみる。
現象は、改善しないが、 global costamp では、障害物のある場所は、正常に取れているみたい。
どうやら、 Rviz2 上で、Navi2 Goal アイコンを使って、コースを1週できそうだ。
mapviz だと、やりやすそうだ。
7) turtlebot3_navi_my を 同じ、iron で build して試してみる。
$ ros2 launch turtlebot3_navi_my multi_goals4_cmd_vel.launch.py use_sim_time:=True
は、そのまま動く。
$ ros2 launch turtlebot3_navi_my multi_goals4_nav2.launch.py use_sim_time:=True
は、goallist を試したが、そのまま動くみたい。
ただし、/map は、使えないので、/global_costmap を使うように、プログラムは、変えないといけない。
4. C++ プログラム で、コースを一周する仕組みを考えてみた。by nishi 2025.1.31
--- ros2 nav2_gps_waypoint_follower_demo autonomous は、できるか検討する。 ---
4.1 機械学習を使う。
real sense の depth、image が出ているみたいなので、機械学習で走行コースを求めれば、よいのか、
処理イメージ
i) image を入力にして、コースを Object detection で検出する。
ii) Opencv で、上記で検出された box のセンター位置 の color を基準に、 2値化 する。
iii) ROS2 自作 Turtlebot3 による 草刈りロボット開発。#3 Auto Mower でした処理を参考にして、
cv::findContours() で、ロボットがいるコースの領域のブロブを求める。
上記で求めた、コースの領域のブロブ の一番遠いところを、次の目的地とする。
iv) 上記で求まった、次の目的地 は、カメラの前面の画像上の位置なので、これを、
Global costmap の地図上に変換する。
ただし、いまは、まだやり方は思案中ぞね!!
多分、前方 image pi(x,y) が、 同時撮影の depth image pd(x,y) と対応していれば、向きと距離が求まりそうだが?
v) Global costmap 上の次の目的地へ、 Navigation を使ってロボットを移動させる。
4.2 Real Sense の、Depth と PointCloud2 が出ているので、Global costmap の、obstacle_layer を、
LaserScan から、PointCloud2 に変えて障害物を避けて走行させる。
ちょっと、実現性は、とぼしいかも。
5. 結論。
注) ここの部分は、書き直しが必要!! by nishi 2025.8.10
ekf-localization 、navsat_transform を使ったロボット位置推定の場合の話だが、
navsat_transform を使った場合、
パラメータの中で、下記値を渡せるみたい。
datum: [38.161491, -122.4546443, 0.0] # pre-set datum if needed, [lat, lon, yaw]
5.1 方位検出は、GNSS-RTK、GPS-RTK を使った、方位の検出をする。
スタート時の方位は、 ロボットのスタート位置と、ロボットを前に1[M]移動させた位置の差で、ロボットの方位を検出する。
5.2 Active SLAM の Map 作成 時は、
GZSS で、スタート時の位置と方位を記録。
これが、tf-map(x,y,orient z) となる。
Static Map による、Auto Mower 走行時は、同様に、
ロボットのスタート位置を、GZSS で、位置と方位を測定して、
前記、tf-map(x,y,orient z) との差を計算して、補正する。
この場合の補正は、
tf-map -> tf-odom を、rtabmap-ros がやるように、補正すれば良いと思う。
または、navsat_transform の param を、動的に書き換えられれば、できると思うが?
あるいは、この測定処理(cmd_vel を使って測定) の lanuch を先に動かして、ロボットの開始位置、向きを得てから、
Auto Mower の、navsat_transform のパラメータを補正して、起動する。
5.3 GNSS 測定器を使って、Auto Mower で走行させる土地を、実測する場合。
測定結果から、Static Map を手作業を作るので、
その場合の、tf-map(x,y,orient z) を記録する。
ロボットのスタート位置を、GZSS で、位置と方位を測定して、
ロボットのスタート位置、方位を、navsat_transform に知らせるか?
この部分は、まだ、具体的には、未考察です。
実行段階で、navsat_transform の param を、動的に書き換えられれば、できると思うが?
5.4 GNSS を使わない、屋内の場合のスタート時の向き補正。
残念ながら、今は、良い案が浮かばない。
単純に、ドッキングステーションから、いつも始めるしか無い。
しかし、走行中に、もし、動く動物や人のいたずらで、向きが突然変わったら、補正の方法が無いのは、すこし問題かも。
BNO086 のキャリブレーションされたマグネットで、絶対方向が正確に補足できれば、走行中に向きが変わっても補正できそうだが、
楽観的希望に過ぎないのかも。
6. 参照。
1) google map緯度経度の座標取得と地図表示を極める方法｜API活用から誤差対策まで完全網羅
▼5,000を超えるオリジナルコンテンツ
オリジナルコンテンツ数No1！地上波では見られない！オリジナルバラエティ・ドラマ・音楽番組が5000以上
あなたが見たい番組がきっと見つかる。
(広告)