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 でロボットの位置を補正するしかないのかも。
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. 結論。
ekf-localization 、navsat_transform を使ったロボット位置推定の場合の話だが、
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 を、動的に書き換えられれば、できると思うが?