ROS2 自作 Turtlebot3 による 草刈りロボット開発。#10 走行開始時のロボットの向き。
--- ros2 nav2_gps_waypoint_follower_demo autonomous は、できるか検討する。 ---
ROS2 自作 Turtlebot3 による 草刈りロボット開発。#9 LSTM で経路計画をする。
が出来たので、自分の部屋で、実機を Auto Mower で走らせている。
その中で、気になりだしたのが、ロボットの開始時の向きのズレが気になる。
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 GZSS 測定器を使って、Auto Mower で走行させる土地を、実測する場合。
測定結果から、Static Map を手作業を作るので、
その場合の、tf-map(x,y,orient z) を記録する。
ロボットのスタート位置を、GZSS で、位置と方位を測定して、
ロボットのスタート位置、方位を、navsat_transform に知らせるか?
この部分は、まだ、具体的には、未考察です。
実行段階で、navsat_transform の param を、動的に書き換えられれば、できると思うが?
6. 参照。
1) google map緯度経度の座標取得と地図表示を極める方法｜API活用から誤差対策まで完全網羅
▼5,000を超えるオリジナルコンテンツ
オリジナルコンテンツ数No1！地上波では見られない！オリジナルバラエティ・ドラマ・音楽番組が5000以上
あなたが見たい番組がきっと見つかる。
(広告)