ROS2 草刈りロボットの最近のブログ記事

ROS2 Jazzy で、50x50[M] の static map を作って、使う。#2

ROS2 Jazzy で、50x50[M] の static map を作って、使う。の続きです。

こちらが、本題なので、先こちらを記述します。
前回は、50x50[M] の static map を作成したので、今回は、それを、実際 ROS2 で使ってみる話です。

50x50[M] の static map の 巨大サイズゆえの問題が顕在化しました。
従来の手順で、SBC で、Map をロードして、リモート PC の Rviz2 で表示すると、Map が受信できない状態になりました。
WiFi Lan のトラフィックが、対応していないのが原因みたいです。
対策は、幾つかあるみたいですが、今回は、SBC と リモート PC で、それぞれがローカルの、 pgm、yaml をロードして使う方法を試してみました。

方法 1.
今使っている、Static Map のロード launch rtabmap_ros_my localization.launch.py を修正して、topic を、/map 、/local_map の指定ができる様にする。
つまり、SBC と リモート PC で、 map server を、2 本立てて、それぞれ別の topic 名 で、publish させる。
一応、この方法も、OK でした。

ROS2 Jazzy で、50x50[M] の static map を作って、使う。

最初は、50x50[M] の static map を作るから。
QGIS で、自宅を中心に、周辺の道路を、50x50[M] の Static Map に落としてみた。
今回も、Google Ai に聞きながらの作成ぞね!!

Google Ai に教わった手順ですが、
Step 1: プロジェクト作成と「メートル単位」の設定
まずはQGISを起動し、長さをメートルで扱えるように座標系を設定します。

QGISを開き、「新規プロジェクト」を作成します。
画面右下の 「EPSG:XXXX」（デフォルトはEPSG:3857など）をクリックします。
フィルター欄に 6677 と入力し、JGD2011 / Japan Plane Rectangular CS IX（平面直角座標第Ⅸ系：高知県などをカバー）を選択して「OK」を押します。
※もしお住まいが九州なら 6676、近畿なら 6674、関東なら 6681 など、お住まいの地域に合った平面直角座標系を選択してください。
おんちゃんは、四国なので、6672 を指定しました。
これでQGIS内の単位が完全に「メートル」になります。

DINOv2 を使って、ROS2 SLAM(自己位置推定)に役立てる。

前回、フィジカルAi で ROS2 自立走行ロボットを動かす。#2
で、Diffusion policy の input に、DINOv2 を使った model を試したが、

"DINOv2 の input 画像に何か入れると、特徴量を返してくる" 特性を活かすと、
一度通った場所のランドマークを記憶させられる様じゃ。
これを使えば、SLAM(自己位置推定)が、行える。もう、Static Map を予め作る必要がない。
Google Ai に色々訪ねていたら、教えてくれた。

なんとも便利な Model を、Meta(旧 Facebook)が作ってくれたもんじゃ!!

1. 概略は、
1.1 SLAM データ作成。
1) 屋外で、ロボットの走行コース上で、10[M] ごとに、RGB Camera の画像と、その時の、gnss-rtk から求めた、緯度、経度と、ロボットの向きのデータを集めて回る。
この時、no を順に割り当てて行く。

2) 上記データの画像をDINOv2 の feed させて、その特徴データを、Faiss( ベクトルデータが扱える DB) に、順に登録する。
以上で、 今回の、SLAM コース情報の出来上がり。

1.2 ROS2 diff car ロボットでの利用。
3) ROS2 diff car ロボットで、RGB カメラの画像を、DINOv2 の feedさせて、 出てきた特徴量で、Faiss の検索をかける。
特徴量が見つかると、Faiss は、登録時の no を返してくるので、
先に集めた、data の no と一致する、緯度、経度から、今の 位置を得る。
しかし、gnss-rtk があれば、必要が無いが!!
gnss-rtk が使えない、室内だと使えるかも。

検証コードは、python pytorch で、簡単に書ける。
Googl Ai に聞けば、コードを教えてくれる。
このページの 1.概略 の部分を Google Ai に聞けば教えてくれる。
コード自体は、簡単だから、試してみては?

ただし、Inference 時のハードのリソースが、結構要求される。
ROS2 実機 Diff Drive Car で実装、テストするには、ここが難問か!!

以前、SLAM でランドマークを使う方法を模索していたが、
ROS2 自作 Turtlebot3 による 草刈りロボット開発。#15 Robot Localization
これが、上記の、回答になりそう。

フィジカルAi で ROS2 自立走行ロボットを動かす。#2

ACT の sub goal が、芳しくないので、業を煮やして、diffusion の sub goal を試してみた。
まだ、作っている途中だが、簡単に動いた。
こちらは、優秀みたいじゃ。
なんじゃこりゃ、こんなに、簡単にうごくの?

LeRobot ACT sub goal で、使った 同じ ROS2 bag ファイルを使って、 train させました。
ROS2 Gazebo WearHouse Tugbot で、 sub goal node を作って、走行させたら、うまく完走できるようです。
当初、TF 補正ナシで、走行させていましたが、TF 補正有りが必要だとわかって、TF 補正して、走行させたら、うまく完走できるようになりました。
しかし、あまりに、障害物の近くを走行するので、見ていて冷や冷やします。

ROS2 広域 Planner の走行パスを使って、複数のチェックポイントを、 model に入力して、走行させますが、
model は、それを完全になぞることはせずに、自分の学習脳の判断で、あくまで走行します。
ここが、ROS2 Nav2 の走行とちょっと違うかな?

train 用の、ROS2 bag データを採る時は、障害物から少しゆとりをもって離れた場所を走行させた方がよいぞね!!

フィジカルAi で ROS2 自立走行ロボットを動かす。

Google Ai に、質問していたら、ROS2 自作 Turtlebot3 で色々走行させて、データを取れば、
それを、フィジカルAI の 適当な model(ここは、Lerobot ACT、diffusion etc) に学習させれば、自立走行させられそう!!
フィジカルAI の学習は、エピソード単位でさせる!!
ので、基本、目的地は、1箇所。
しかし、これをベースにして、目的地を告げて、複数ヶ所へ到達させることも出来るとの事じゃ!!

ここにきて、やっと、フィジカルAi と ROS2 自立走行ロボットの勉強がつながったぞね!!
ちょっと、ROS2 Jazzy Tugbot の Gazebo で、学習data を作ってためしてみます。
開発環境:
Ubuntu Mate 24.04
ROS2: Jazzy
Gazebo
Tugbot wearhouse
Navigation2 with AMCL front rgb camera, front scan
Model:
LeRobot ACT
注) ROS2 の環境と、virtual_env で作った、torch、lerobot の環境の2 を、使い分けます。
lerobot_env: virtual_env で作ります。
lerobot_env で、
$ python -m pip install mcap mcap-ros2-support opencv-python numpy [torch] [lerobot]

1. Gazebo Tugbot wearhouse を、Navi2 で動かして、bag data を保存します。

ROS2 環境が必要なので、下記をターミナルからたたく
$ source /opt/ros/jazzy/setup.bash
$ source ~/colcon_ws-jazzy/install/local_setup.bash
$ export ROS_DOMAIN_ID=30 #TURTLEBOT3
$ export TURTLEBOT3_MODEL=waffle
$ export GZ_SIM_SYSTEM_PLUGIN_PATH=/opt/ros/jazzy/lib

下記、launch で、 Gazebo Weahouse Tugbot が Nav2 で、操作出来ると思う。
最新版を、up しているか、自信がないが!!
tosa-no-onchan/turtlebot3_navi_my/launch/tugbot_amcl_scan.launch.py
すでに、SLAM で、map が作成されていると思うので、その Map を使います。
上記、 launch.py の上部の起動手順に従って、Rviz2 で、Nav2 操作が出来る状態にします。
#  2.2 navigation
の部分。

ROS2 自作 Turtlebot3 による 草刈りロボット開発。#17 Transformer で経路計画をする。No.2

ROS2 自作 Turtlebot3 による 草刈りロボット開発。#8 Transformer で経路計画をする。 の続きです。

同じ Transfomer model を、Pytorch で、作ってみました。
Pytorch のほうが、コードがシンプルで分かりやすいぞね!!

今回の Pytorch Transformer のモデルについて、ちょっと説明します。
ベースは、Transformer の Speech to Text の処理をベースに作っています。
詳細は、オリジナルのページを参照してください。

今回、特筆したいのは、model の outputs の取り出し方法です。
どうやら、大きく言って、２種類あるみたい。
1 は、Keras 版でも使っていた、predict 結果を、一度( 150 の位置 class) に取り出す方法と 。
2 は、最近 google ai の話だと、1個ずつ、ループして、必要分だけ取り出すほうほうです。 ここで、結構はまりました。
最終的には、onnx 変換で、1 の方しかできないので、1 の方式をメインにします。

ROS2 自作 Turtlebot3 による 草刈りロボット開発。#16 ソーラ発電所の草刈りロボット案
------ 草刈りロボットへの 3D Map の利用について ----

ROS2 Tugbot Gazebo で 3D LiDAR の SLAM を試してみた。 で、@fishros/LeGO-LOAM-ROS2 を、
Tugbot Gazebo で試してみて、出てくる 3D Map(ただし、現状 3D Map として保存する方は、判らない) が素晴らしい。

これを、Localization として使うのではなく、周囲把握に使えれば面白い気がしてきた。

1. おんちゃんの想定する、ソーラ発電所の草刈りロボットに於ける3D Map の利用。

1) ソーラー発電所で、草が生えていない時に、Rtabamap_ros、LeGO-LOAM-ROS2 等を使って、2D Map と、3D Map を作成する。
3D Map は、草の生え具合の標準データとする。
2) 草刈りのときは、2D Map や GNSS-RTK を使った、Localization を利用する。
3) ロボットは草刈りコースに則って移動する。
この時、走行コースが、草でふさがっている時、そこの場所の 3D Map と草の生え具合の標準データとを比較して、
本来であれば草がない所であれば、その草を避けずに草を刈る。
注) 2D Map でも予め作ったものなら、同じように使えそう。
敢えて、3D Map を使う価値はあまり無いのかも!!
4) また、走行コースは、おおまかなコースとして、ロボットの周囲の 3D Map が、草の生え具合の標準データと比較して、
草が生えている場合は、その周囲の草刈りを行う。
5) ケーブル、支柱などの障害物は、Object Detection で、判定して避ける。

ROS2 自作 Turtlebot3 による 草刈りロボット開発。#15 Robot Localization
---- 機械学習 Object Detection を使った Robot Localization 案 ----

なんとなく PC のブラウザーを見て暇を潰していたら、浮かんできたので、書いてみた。
ロボット Localization に、ロボットに取り付けた RGB カメラの前方画像を 使ってランドマークになる物を検出する
Object Detection をつかてみてはどうか?

検出する Object は、ランドマークになるものだけにする。
道路、家、塔、川、アンテナ、窓、ドア、橋、欄干...
家は、平屋、2階屋、ビルなどに分類する。

おんちゃんの結論は、Object Detection では、ロボットの正確な位置を算出する Robot Localization には、現状不向きか、
やはり、障害物の判定とそれからの退避利用が合っている。

1. 案 I
検出されたObject部分の画像だけを使って、それ以外の部分は、消去する。
ランドマークのみが抽出された画像を、rtabmap_ros に通す。

2. 案 II
SLAM の時は、rtabmap_ros で Mapping しながら、自分でも Localization 情報を構築する。 Navigation モードの時は、自分で Localization した情報を使う。

処理概要は、検出されたランドマークの class id と向きか、depth camera の画像も使って距離情報と、ロボットの位置、向き情報を保存する。

注1) 検出されたランドマークは、ロボットの位置、向きから、Map上の位置に変換して、単純に、保存するだけ。Map(x0,y0)にプロットするイメージ。
実際は、Roboto の 現在の位置(Map)とランドマークの位置(Map) を加えて、SQLite 等で情報として蓄積するか。

注2) Map上の位置に変換するには、距離が必要になる。距離が測定できる範囲のランドマークに限られる。

注3) ランドマークも見る向きによって、形状が変わるので難しいかも知れない。
家であれば、距離は、ロボットの方に向いている壁や屋根までの距離となる。ロボットの位置で、同じランドマークでも形状が異なる。

注4) Map 上の情報も、実際参照されるのは、ロボットの現在位置と向きによってカメラの画角に入る周辺の情報に限定される。

注5)
a. Navigationモード(非SLAM モード) の時は、もしかしたら、その時の各ランドマークとロボットを頂点とする多角形の形状のマッチングになるのかも知れない。
あるいは、多角形の領域の重なり具合を求めるのかも知れない。
しかし、ランドマークが多いと、多角形化が困難になるので、これは、無理か?
SLAM モードの時は、頻繁に ロボットを 360度回転させて、周囲の情報を収集する必要がある。

b. やはり、距離情報を必要としない、Object Detction によるランドマークのマッチングが出来ないものか?
この点だが、単純にObject Detction でランドマークを計測した時毎に、その時のロボットの位置と向き及び、
見つかったランドマークの Class id と RGB 画面上のboxの情報を SQLite で保存するのはどうだろうか?
これなら、マッチング処理の時、ロボットの位置 Map(Xc,Yc,Zc) が、近いレコードを SQL で 抽出して使える気がする?
これを、Map近辺レコード と呼ぶことにする。

Map近辺レコード が select できれば、後は、
『SLAM入門 ロボットの自己位置推定と地図構築の技術 友納正裕(著)』(オーム社)
でも参考に自己位置の補正ができそうじゃ。
注) ただこちらは、基本 2Dのレーザースキャンを使った入門書じゃ。
スキャンマッチング、センサ融合による退化への対処、ループ閉じ込み の３つが主要処理として出ている。
ランドマークとの距離が必須だが、初心者のおんちゃんには、大いに勉強にはなる。
3D LiDar and 3D SLAM は、LOAM があるらしい。

後は、『インターフェイス 2020 3』第3部 自律走行ロボ制御プログラミング実験室 あたりが参考になるか?
以前ぱっと読んで、 あまり理解せずにしていた。もう一度読んでみようと思う。

もし、Depth Camera で、距離が測定できるランドマークであれば、同時に Map 上の位置を計算していっしょに、SQLite で保存しておけば良いかも知れない。

注6) ロボットの位置と向きの参照情報についての整理。
ROS2 においてロボットの位置は、TF システムによって管理される。
ロボットの現在位置と向きは、下記関連で示される、
tf-map -> tf-odom -> tf-basefoot_print
tf-odom -> tf-basefoot_print は通常、ロボットのMoter の単位時間内の直進値や向きの回転値を積分した値で示される。当然、タイアの回転なので誤差が蓄積されていく。
ただし、ロボットの向きは、IMUのBNO086 の 6軸、9軸 DMP のQuatnion を使えば誤差は少ない。
上記誤差を補正するために、正しい tf-map-> tf-basefoot_print をタイア以外の情報も参考に求めて、
tf-map-> tf-odom を補正するのが、localization の目的ぞね。

注7) ロボットの走行と Map について。
ROS2 で、ロボットを走行をさせるには、予め作成した　Static Map が必要ぞね。
通常 SLAM で、Static Map を作成するタスクを先に実施する。
Static Map が作成されたら、それをもとに ロボットを走行させることになる。
なので、Localization は、普通は両方で必要になるが、....

ROS2 自作 Turtlebot3 による 草刈りロボット開発。#14 屋外走行ロボット。
副題: ROS2 C++ 自立走行ロボットによる草刈りロボット開発

--- そろそろ、屋外で走行させられる自作ロボットがほしい。-----
自作ロボットの ROS2 草刈りロボット で屋外を想定した走行と、
Quectel LG290P は、みちびき L6 受信ができるのか。#2 で、GNSS を使った屋外での走行の確認がほぼできた。

これからは、実際に屋外用ロボットを使って、屋外での自立走行を行って、草刈りロボットの実用化に向けた開発をしたいぞね!!

一応、ロボットの要件定義をしてみた。
RCカー 1/10 か 1/12 相当のサイズで、タイヤを大きなものにする。
電子部品は、クローズされた箱に収める。
サイズは、30 X 30 [cm] で、図面を手書きしてみた。
これだと、机の PC の隣に置ける。
あとは、これに従って、DCモーター、タイア、アンプ、ロボット本体のケース 等を集めるだけじゃ。
だが、肝心の Mony が足りない?.....
神様、どうか恵みのマニーを...

今までのおんちゃんの ROS2 自立走行ロボット技術を活かしたスポット開発の依頼がありましたら、是非、ご連絡ください。
自立走行ロボットの屋内走行であれば、十分対応できると思います。

Android スマホを、インターネットのGateWay にする。

ROS2 自作 Turtlebot3 による 草刈りロボット開発。#13 robot_localization ukf を試してみる の続きです。

屋外で、GNSS-RTK をするには、インターネットに接続する必要があるが、そのときに、Android スマホ をインーネットのGateWay できればかんたんじゃ。

早速 google で、"Android を gateway にできるのか" で検索してみた。
結果は、
デザリング、他できるそうだ。
再度 google で "android スマホ テザリングのやり方" を検索する。
ふむふむ!


最終的になにがしたいのかと言うと、以前も下記ページで試したが、
自作 Turtlebot3 自律走行に向けたプログラム。#15
要は、自作Trutlebot3(foxbot_cor3_r2) を、屋外で走行させたいのじゃ。
しかも、今度は、GNSS-RTK を使うから、インターネットにつながった環境にしないといかん。
なので、今回は、Android のスマホで、自作ロボットの Start、Stop のみを操作するだけ。
本当は、部屋の中で、LG290 で、みちびきのL6 が受信できればわざわざ屋外に持ち出す必要はないが、
L6 受信は、屋外でないと無理なきがするので、屋外での方法を考えてみただけじゃ。
また、もし L6 受信ができて、7[cm] 級の精度がでれば、GNSS-RTK も使わないのだが。

Quectel LG290P は、みちびき L6 受信ができるのか。#2

Quectel LG290P は、みちびき L6 受信ができるのか。 の続きです。

今回は、実際に Quectel LG290P を ROS2 で使ってみてのレビュー報告じゃ。

注文して、2週間以上して、やっとQuectel LG290P が届いたので、さっそく使ってみる。
実物を手にして、初めて、気がついたのだが、Dip Switch x 2 の反対側に、Rest SW らしきものがあるみたいじゃ。
これで、Firmware の upload がかんたんにできるのかも。!!

1. 取り敢えず、Windows11 で、QGNSS.exe を使って、device: LC29HEA のままで、テストしてみました。
やり方は、下記をみとうせ!!
QUECTEL LC29H GNSS L1 L5 Receiver 使えるのか検討してみた。

USBで繋げて、直ぐに、赤LED が点滅し始めた。すごい。
アンテナは、32[db] 版だったが、みちびきの L1,L5 の Signal Level の旗がくっきり表示されました。

同じアンテナで、LC29HEA の時は、残念ながら、みちびきの旗は、半透明のままだったので、LG290P が性能は断然いいみたいぞね。!!

一度、LG290P の性能を経験したら、もう、LC29HEA を使う気がしなくなる。おんちゃんだけか!!

残念ながら、L6 のシグナルは、捕れていないみたい。
Firmware をアップグレードするまで、お預けみたいぞね。
アンテナも、38[db] 版を用意する迄だめか。

ROS2 自作 Turtlebot3 による 草刈りロボット開発。#13 robot_localization ukf を試してみる。
副題: ROS2 C++ 自立走行ロボットによる草刈りロボット開発

-------- robot_localization ukf を試してみる。-------

robot_localization の ukf_localization_node を使って、
foxbot_core3 for ros2 を走行させてみる。
ロボット走行には、UKF: Unscented Kalman Filter が向いているそうなので、試してみた。
Ekf 版の launch.py があるので、その中の、ekf_localization_node 向け を ukf_localization_node 向けに変えるだけで、OK だった。

とおもったが、勘違いで、ukf用のパラメータの設定が必要じゃ。
今は、まだ動作してないぞね!!
Quectel LG290P を試す方が先だから、この記事は、少し保留じゃ。by nishi 2025.7.14

環境:
実機ロボット:foxbot_core3 for ros2
OrangePi 5 and Armbian 24.04
OAK-D Lite depth Camera
ROS2 Jazzy Navigation2
Launch: OAK-D Lite Depth Camera , GPS navsat_transform_node and ukf_node
foxbot_nav2_oak-d_depth_gps_ukf.launch.py
C++ 自動走行プログラム:multi_goals4_nav2.launch.py

ROS2 自作 Turtlebot3 による 草刈りロボット開発。#12 床が高低差がある場所での走行。
副題: ROS2 C++ 自立走行ロボットによる草刈りロボット開発

----- 床が高低差がある場所での走行。------
環境:
実機ロボット:foxbot_core3 for ros2
OrangePi 5 and Armbian 24.04
OAK-D Lite depth Camera
ROS2 Jazzy Navigation2
Launch: OAK-D Lite Depth Camera and Rtabmap_ros による、Active SLAM
rtabmap_oak-d_rgb_depth.launch.py
C++ 自動走行プログラム:multi_goals4_nav2.launch.py

BNO086 DMP でなんとか、走行できるようになったので、
Rtabmap_ros の 2D で、床が高低差がある場所でロボットを走行させて、2D Mapping が精度よく取れるかためしてみた。

おんちゃんの部屋は、畳で、床の高さが一定でない。以前は、それでも、そのままRtabmap_ros の 2D Mapping を行っていたが、
なんとなく、高低差が気になりだした。

ずっと以前も試したが、BNO086 DMP で安定して IMU が取れだしたので、再度、IMU の Pitch を使って、床の高さを使ってみようと思う。

2D Rtabmap_ros でも、ロボットの tf-basefoot_print の pose.pose.position.z に、高低値を入れてみると、一応加味して、動作するみたい。
ここで、出てくるのは、あくまでも 2D Mapがでてくる。
3D Map は必要ないので、これで十分じゃ。

ただし、どこまで正確に、pose.pose.position.z を出せるかがもんだいじゃが。

注) RTK-GNSS(GPS) を使う場合、RTK-GNSS(GPS) を使って、予め Map 作成すれば良いので、Rtabmap_ros 自体出番がないのだが。

ROS2 自作 Turtlebot3 による 草刈りロボット開発。#11 ロボットの走行方向がずれる。
副題: ROS2 C++ 自立走行ロボットによる草刈りロボット開発

----- ロボットの走行方向がずれる。------

SBC: Orange pi 5 Armbian Ubuntu 24.04
ROS2: Jazzy

Jazzy 版にして、rtabmap_ros_my/foxbot_nav2_oak-d_depth_gps.launch.py で、自作 Turtlebot3 を動かしたら、最近どうも、ロボットの向きがずれる気がする。

Rviz2 の画面上は、ロボットは、目的地にちゃんと到達しているが、実機のロボットは、結構ずれた位置に行ってしまう。

1. EKF の設定が変わったのかと、いろいりためしていたが、
どうやら、原因は、 IMU ボードの取り付け向きが、ロボットの進行方向にから少しずれているみたいじゃ。
今まで、あまり気にしていなかったが、もしかしたら、DMP 9 の場合の、IMU のキャリブレーションを取っ払ってしまっていたかも知れない。

もとい、これは、単純に、IMU ボードの向きを正すしかない。

あと、ICM-20948 の接続ケーブルの接触が、時々不良になるみたい。そのせいで、急に Device ID が取れなくて、初期化処理でずっこける。
以前は、電源電圧が低下している所為と思っていたが、単純に、接触不良みたいじゃ。

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 に、必然的になる。
以上が、予備知識である。

ROS2 自作 Turtlebot3 による 草刈りロボット開発。#9 LSTM で経路計画をする。
副題: ROS2 C++ 自立走行ロボットによる草刈りロボット開発

--- ROS2 ロボット自律走行の経路計画の補助に、機械学習(Transformer、LSTM)を使う。 ----

ROS2 自作 Turtlebot3 による 草刈りロボット開発。#8 Transformer で経路計画をする。 の続きです。

Transformer で、学習データ 1000個作って、学習させてみたが、1000個では、まるっきり足りないようで、 train loss = 0.7 を切らない。

業を煮やして、それならと、 Speech to text の LSTM を同じ様に学習させてみました。

こちらは。1000件ほどのデータで、うまく学習できました。
これは、凄い!!
こんなにうまく行くとは、予想できませんでした。
本当に、うまく予測できているのか、今も、半信半疑です。
余りの感激に、涙がちょちょでるぞね!!

github に公開しました。
tosa-no-onchan/opp_with_lstm

1. 開発環境
PC: Ubuntu Mate 22.04
Python 3.10.12
virtual_env
tensorflow 2.16.2
keras 3.6.0

ROS2 自作 Turtlebot3 による 草刈りロボット開発。#8 Transformer で経路計画をする。
副題: ROS2 C++ 自立走行ロボットによる草刈りロボット開発

--- ROS2 ロボット自律走行の経路計画の補助に、機械学習(Transformer、LSTM)を使う。 ----

ROS2 自作 Turtlebot3 による 草刈りロボット開発。#6 Nav2 Planner を替えてみる で、ThetaStarPlanner を使って、Auto Mower 時の走行の直進性の改善がみられた。

しかし、まだおんちゃんが望む直進性能には、足りない。
そこで、ThetaStarPlanner の経路プランの一部補正として、機械学習が使えないか、無い知恵を絞って、考えてみた。

1. 概要
結論を先に言ってしまえば、経路プランに、Transformer、Lstm による、Speech To Text モデルのロジックを使えないだろうか? と言う話じゃ。

下の画像だと、赤線の経路を、青線の様にしたい。
障害物の手前まで真っ直ぐ進んで、障害物をなぞるように進んで、再度、スタートとストップの直線上に戻る、走行が欲しい。

ROS2 自作 Turtlebot3 による 草刈りロボット開発。#7 障害物からの脱出に機械学習を使う。
副題: ROS2 C++ 自立走行ロボットによる草刈りロボット開発

---- Auto Mower で、障害物に囲まれた際の脱出方向の算出に、機械学習の画像分類 (Image Classify) を使ってみる。-----

Auto Mower で、ロボットを動かして、目的地に到達させた場合に、障害物に近づき過ぎる場合がある。
そうすると、navigation2 で、その場所から抜け出せずに、エラー終了してしまう。

これを、防ぐ為に、static map 上で、障害物に囲まれた際に、脱出方向を計算して、cmd_vel モードで、ロボットを少し遠ざけている。

ROS2 自作 Turtlebot3 による 草刈りロボット開発。#6 Nav2 Planner を替えてみる。
副題: ROS2 C++ 自立走行ロボットによる草刈りロボット開発

---- Ros2 Navigation2 Planner を、NavfnPlanner から、ThetaStarPlanner に替えてみた。 ----

nav2.org / tuning を見ていたら、

Planner Plugin Selection
NavFn will typically make broad, sweeping curves; Theta* prefers straight lines and supports them at any angle;
and Smac 2D is essentially a classical A* algorithm with cost-aware penalties.

と記述があったので、
Theta を使えば、少しは、ロボットを直線的に走らせられるのではないか?
と思ったので、早速試してみた。

具体的には、xxx.yaml の中の、
planner_server:
で指定するので、ここを変更すれば良い。

ROS2 自作 Turtlebot3 による 草刈りロボット開発。#5
副題: ROS2 C++ 自立走行ロボットによる草刈りロボット開発
----- C++ cmd_vel 走行で障害物をよける方法 -----

今回は、ROS2 自作 Turtlebot3 や、Gazebo Turtlebot3 で、C++ cmd_vel でコントロールしているときに、障害物を検知する機能を組み込んでみました。

方法としては、Stero Camera、Lidar、Depth Camera を入力として、Local Costmap に 障害物が、Topic として出てくるので、
それを、定期的(1 [sec]) に取りこんで、ロボットの前方に障害物が投影されたら、ロボットの方で、なんらかの対応処理をする。

Navigation2 を使えば、かんたんですが、 rpp 等を使うと、まずロボットがまっすぐ走行しません。

草刈りロボットとしては、障害物が無い間は、常に直線上を走行させたいので、cmd_vel で走行させています。
この時に、障害物が現れたら、Nav2 走行に切り替わって、障害物を避ける動きを組み込んでみました。

cmd_vel 走行のみの場合は、そこで、障害物がなくなるのを待つか、走行処理を終了させる か、どちらかになります。