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

1.1 bag ファイルを作成状態にする。
別のターミナルで、
$ ros2 topic list
トピックが出ているか、念のため確認
# 1回目の走行データを録る時
ros2 bag record -b 0 -o ~/ros2-bags/tugbot_bag_ep0 /scan /odom /cmd_vel /camera_front/color/image
この状態を、維持する。
終了は、CTL+c

# 2回目の走行データを録る時
ros2 bag record -b 0 -o ~/ros2-bags/tugbot_bag_ep1 /scan /odom /cmd_vel /camera_front/color/image
# 3回目の走行データを録る時
ros2 bag record -b 0 -o ~/ros2-bags/tugbot_bag_ep2 /scan /odom /cmd_vel /camera_front/color/image

google ai の後出し回答で、またやられた!! by nishi 2026.6.5
次の、自由に動き回れる ACT model の Dataset には、/tf /tf-static も取らないといかん
其のために、少し工夫が必要!!
上記、コマンドを実行する、ディレクトリーに、下記ファイルを作成します。
qos_override.yaml
/tf_static: history: keep_last depth: 1 reliability: reliable durability: transient_local
その後、上記 bag コマンドを実行します。
ros2 bag record -b 0 -o ~/ros2-bags/tugbot_bag_ep0 /scan /odom /cmd_vel /camera_front/color/image /tf \
/tf_static --qos-profile-overrides-path qos_override.yaml
とさ。
手入力だと面倒なので、python script を作った。
$ python ros2_bag_start.py
自動でナンバリングされます。

1.2 Rviz2 の Nav2 で、 tugbot を適当に目的地へ向かわせる。
長くて、1 [分] 前後みたい。
目的地についたら、上記の、 bag_record を終了させる、

2. bag ファイルを、Lerobot Datasets に変換する。
1. の環境は、閉じて、(ターミナルを閉じる)
新たに、
$ source lerobot_env/bin/activate

$ python convert_mcap_to_lerobot.py
注1) 複数エピソード対応版にしました!!
注2) "observation.environment_state" に、なんの情報を入れるかによって、
model で動くロボットの動作が変わるみたい。
i) /odom の値を入れるだけ
同じ目的地を目出す、model になる。
ii) /odom + 目的地の向き、距離 、到達時のロボットの向き を入れる。
bag ファイルの中の、一番最後の /odom 、/tf から、目的地の向き、距離、ロボットの到着時の向き を計算して入れる。
Tugbot の ACT による走行で、model へ入力の /odom + 目的地の向き、距離 で、あちこっち動き回る、model になるそう!!

おんちゃんとは違った、"observation.environment_state" もいろいろ出来ると思うから、 おんちゃんの、コードの URL を、
google ai 、Chat GTP にわたして、どうしたいか、問い合わせすれば、簡単に、改造出来ると思う!!
注) どちらも、1回の回答で、全てを出してこない。小出しにする。
回答に疑問をぶつけると、さらに、少し本当の回答を出してくる。
なので、自分がある程度の知識を持って接しないと、不完全な回答のところで止まってしまう。くれぐれも、
自分も予め勉強をしておいたほうが良い!!

2.1 lerobot-dataset-viz で、上記 ローカル datasetを 確認する。
$ export PYTHONPATH=$PYTHONPATH:/home/your-id/local/git-download/lerobot/src
注) git clone した場所の src
$ lerobot-dataset-viz --repo-id motion1 --root outputs/tugbot_nav2_imitation --mode local --episode-index 0
多分、見えると思う!!

3. train
jupyter notebook で、
train.ipynb で、 train させます。

後で、わかったが、 scan データが、model で、入力として、使われていないみたい。
今は、 fornt rgb camera と、 /odm データが、使われているみたい。
今後、/scan_front を、入力にするかは、要検討しないといかん。by nishi 2026.6.3

4. 記事を書き進めるまえに、もう bag ファイルから、 LeRobot Datasets への変換と、 ACT train jupyter notebook が出来たので、
github に上げておきます。
tosa-no-onchan/lerobot-my/ACT-ROS2-Tugbot

注1) train.ipynb は、いま、ブラウザーから表示しようとするとエラーになるみたい。
サイズ容量は、削ってあるので、そのうち、表示できると思います。
git clone すれば、 local では、問題ないと思う!!
train.py も上げたので、コード確認は、こちらをご覧ください!!

5. torch での、model inference 単体チェック
$ python Act_check.py

ROS2 ノードで、実行する前に、torch 環境で、predict 動作をチェックします。

6. ROS2 ノードにして、実際に、Gazebo Wearhose Tugbot を、この ACT model で走行させてみる。
環境:
Ubuntu Mate 24.04
ROS2 Jazzy
torch
LeRobot

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

i) download
$ cd ~/colcon_ws-jazzy/src
$ git clone https://github.com/tosa-no-onchan/tugbot_my.git

ii) build
$ cd ..
$ colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --parallel-workers 1 --packages-select tugbot_ai_my

iii) run
前記の、Gazebo Wearhouse Tugbot の AMCL RGB-Camera SCAN navigation を起動して
tosa-no-onchan/turtlebot3_navi_my/launch/tugbot_amcl_scan.launch.py
今回は、Nav2 を操作しないで、bag ファイルも取得しないで、

$ ros2 run tugbot_ai_my ai_policy_node

あとは、Gazebo 、 Rviz2 で、見ているだけ。
自動で、学習時に向かった目的地へ、移動します。
すごい!! ACT できるな、お前!!

ところが、どうも、うまく行かない時が、多いみたい。
train させた、コースは、中央の通路を、通っていって、突き当りの壁の手前で、左に曲がって、創庫の奥、左の隅で止まる。
しかし、突き当たって、なぜか、右に曲がる事が多いみたい。
どうやら、過去のデータ重視の設定がひつようみたい。
EMA_K = 0.01
これで、改善されたみたいじゃ。

詳しくは、ai_policy_node.py をご覧ください。

build 時、run 時の、ROS2 Jazzy と、torch + LeRobot の共存方法が、結構手間がかかるけど、Google Ai に聞いて、トライして見てください。

7. 今後!!
次は、model の学習時の status に、 odom + 目的地の位置、角度、到着時のロボットの向き
を使って、学習させて、
目的地の位置、角度、到着時のロボットの向きを渡せば、あとは勝手に、動く ACT model にしてみます。

注) 到着時のロボットの向き は、無いほうが良い気がします。
到着時のロボットの向き は、 ロボット が到着した時点で、自分で、向かせれば良いので、
学習させなくても良いとおもう。
あとで出てくる、sub goal model の途中の 目的point の向きの算出が出来ないし、不要な気がする。by nishi 2026.6.9

これができれば、次は、実機の 自作 Turtlebot3 草刈りロボットで、
屋外で走行データを採取して、ACT Model を学習させて、走らせてみたいです。

その時は、雑草の箇所は、そのまま突進して、人、木 etc 実際の障害物は、回避するように学習させます。
あるいは、雑草の場所を選んで、進むように学習させます。
そうすれば、堤防の草刈りで、今、人が操作している機械を、自動で動かせるぞね!!
人は、傍らで見ていて、危ない時だけ、止めるだけ!

8. 目的地を示せば、自由に動き回れる ACT Model
一応、 train までは、できました。
1) bag to LeRobot Dataset 変換。
$ python convert_mcap_to_lerobot_3.py

~/ros2-bags/tugbot_bag_ep?
から、変換します。
1 bag ファイルが、1 エピソードになって、その目的地、ロボットの向きは、
各 bag ファイルの中の最後の /odom トピックから採取します。

2) train
jupyter notebook
train.ipynb
use_run_about=True
で、学習させてください。

3) ROS2 node を走らせる。
ROS2 node を作ったので、走行させます。
ai_policy_run_about_node.py

走行手順は、前の iii) run と同じ。
tugbot_amcl_scan.launch.py の一連の起動をして、
その後、
$ ros2 run tugbot_ai_my ai_policy_run_about_node --ros-args -p use_sim_time:=true

注1) 今回は、roboto の現在位置は、tf-map -> tf-base_footprint から採取しています。
convert_mcap_to_lerobot_2.py は、まだ、 /odom からのデータを使っています。
なので、train 時と、 run 時では、完全に一致していませんが、一応動くみたいです。
だれか、ファイトがある人は、convert_mcap_to_lerobot_2.py を、tf を使うように改造してください。
注) convert_mcap_to_lerobot_3.py で、/tf から、robot 位置を取得するようにしました。

一応、完走できました。

実際の運用イメージ。
i) 一気に、遠い場所を指定せずに、3 - 5 [M] 先を指定して、それを繰り返して、遠い場所まで フィジカル Ai で 走行させます。
ii) 障害物を回避するデータを、学習させてください。
そうすれば、障害物を避けて走行するようになると思います
つまり、model には、公道走行時の基本マナーを、学習させるだけ。
遠い目的地への長いコース学習はさせない。長いコース学習は、ナンセンスじゃ!!
ロボットの脳には、何 [M] 先へ進むための学習は、出来ている。
注) ACT Model に学習させているのは、Front Camera 画像と、
ロボットの今の前進速度、旋回速度(右、左への回転)、目的地の距離、角度差、最後に到着時のロボットの向きの差
なので、地図上の位置を具体的に示す必要は無い。
ただ、何[M] 動いたかは、正確性が要求されるから、GNSS-RTK でも使う。
iii) 公道の自立走行。
イメージ的には、多分、地図を開いて、
ロボットの現在位置から目的地までのコースを一定間隔でプロットして、
そのプロット点を目指すようにロボットに指示する。
将来的には、この部分もプログラムコード化できれば、全自動で、道の公道も走行させられると思う。

iv) 早速 Google ai さんに聞いて、
Map を渡して、planner に目的地を渡せば、広域 通路 plann が得られる launch ファイルと、問い合わせの python code を教えてもらった。
後は、実機で、 model を学習させて試すだけじゃ!!
簡単に言えば、ROS2 広域プランナーを、最小構成で立ち上げて、直接えば、今回の目的に適う。

注) こう言った目的の model の学習距離は、2.0m 〜 5.0m（最大でも7.0m程度）が良いとの事。

以上ぞね!!
やれやれ。by nishi 2026.6.6

9. Sub Goal Model.
上記の、運用イメージで書いた、モデルは、どうやら、 Sub Goal Model と言うらしい。
Gazebo Wearhose Tugbot で、ちょっと、検証用に作ってみました。
1) 広域プランナーを、直接使う。
launch/nav2_min_planner.launch.py

上記を動かして、目的地への経路計画を作ってもらいます。
事前に作った、Wearhose の、yaml、pgm ファイルを読み込ませます。

2) 上記、プランナーをコールして、経路計画をもらって、それを、単位M で、分割して目的地の案内をさせます。

tugbot_ai_my/subgoal_extractor.py

3) 実際の、subgoal 形式の、ACT model による、Diff ロボットナビゲーション。

tugbot_ai_my/ai_policy_run_about_node_subgoal.py
注) 必要な パケージの起動手順は、上記コードの上部に記載しています。
それに従って、順に起動すれば、OK です。

github に上げてあります。
tosa-no-onchan/tugbot_my/tugbot_ai_my

おんちゃんは、試してみたけど、まだ、 model の学習が、ぜんぜん足りていなので、
まだまだ、これからじゃ!!
学習させるデータ取りにも、コツが必要みたい。コツは、 google ai にでも聞いてみとうせ!!

短い距離のエピソードを、200 個位は、学習させないといかん、そうじゃ!!
とりあえず、
Gazebo WaerHouse Tugbot で、ros2_bag_start.py を使って、27 エピソードを作って、試す。
これで、25,000 steps じゃ。
200エピソードだと、本当に 2[M] -3[M] を多くしなと、steps が増えて大変そう!!
27 エピソードの評価は、まだNG!! フルコース学習した、エピソード にかなり、影響を受ける。
次は、99 エピソード じゃ。
48,217 steps じゃ。
これが、却って、性能が落ちた。どういう事。
model inputs の observation.environment_state:
[現在並進, 現在旋回, 距離, 目標点への角度差, ゴール時の指定向きへの角度差] の5要素
から、
[現在並進, 現在旋回, 距離, 目標点への角度差] の4要素 に減らした。
だいぶ良くなったが、途中で、つぎの sub goal へ向かわず、停止する。
一度停止すると、速度:0 から、次の sub goalへ向かう学習脳が無いのだろ。うんともすんとも動かなくなる。
なので、
[現在旋回, 距離, 目標点への角度差] の3要素 にした。
最後の目的地での向きは、model ではなく、上位プログラムで行うことにした。
[現在旋回] もいらないかも知れない。
[現在並進] を取ったので、急加速、急停止をするかもしれない。この時も、上位プログラムで、調整するしかない。

[現在旋回, 距離, 目標点への角度差] の3要素は、うまく行かなかった。
フルコース学習のエピソードの影響が大きい。
[距離, 目標点への角度差] の2要素は、3要素 よりは、進んだが、途中で止まった。
フルコース学習のエピソードの影響があるのと、多分複数選択肢があったら、どちらも選択出来なくて、止まってしまうみたい。

学習データは、全て 1 、... 5[M] の細切れのエピソードにすべきだ。フルコース学習のエピソードがあってはいけない。これかな!!

今度は、全て 1 、2 [M] の細切れのエピソードで、フルコース学習のエピソードナシで試すぞね!!
結果!!
Planner のコースから、けっこう外れたけど、最終的には、目的地に到達した!!
やっほー!! やった!!
なんだ、ACT での sub goal できるじゃん!!
今回は、[距離, 目標点への角度差] の2要素 のままで、試した!!
あとは、Googl Ai のアドバイスの通りに、 model が画像を頼らずに、roboto の[距離, 目標点への角度差] をメインにするように、
いろんな場所での短距離データを追加で覚えさせる。これに尽きる!!

ACT モデルの特徴が見えてきた。
i) ステータス学習より画像学習の 方が強いので、フルコース学習が得意だが、sub goal の細切れの学習は、不得意かも!!
学習した周囲の画像の影響が、けっこう尾をひくので、ステータス情報の 目的地への距離、方角の情報が、意味をなさない場合が、おおい。
sub goal 目的なら、別の Model も検討すべきかも。
Googl Ai が言うように、「景色がどうあれ、インプットの角度が右を向いていたら右に旋回するしかない」という汎用的な追従ルール を学習させられれば、解決するだろうが、
いまは、そのような train が出来ないので、仕方がない。

業を煮やしたので、diffusion の sub goal の mdel を試していたが、(こちらは、最高、簡単に動く)
こちらは、state(goal-x,goal-y) : goal-x,goal-y はゴール地点の、roboto からの x,y を使っている。
ACT の今は、state(distance,goal-angle) を使っている。これも、diffusion と同じに、
state(goal-x,goal-y) をつかった方が良いのかも?

10. いま、おんちゃんが気になっているページ。
NoMaD: Goal Masking Diffusion Policies for Navigation and Exploration

目標地点の画像を示すだけで、到達できるのだろうか?
なんか、種もしかけもありそう。
なんとなく、途中の画像も何枚か見せている気がするけど!!
もしかして、全コース学習なのかな?

11. Sub Goal ACT の学習方法のまとめ。
普通に、(道路)走行の基本を学習させよう!!
1) フルコース学習は、させない。
2) 走行の基本を、普通に学習させる。
ロボットの前方を基準に、1[M]、2[M]、(3[M]) 前進、後進、左横、右横、右斜め前、左斜め前、右斜め後ろ、左斜め後ろ へ走行させる。
角を曲がる。
3) 例外走行。
障害物の手前で止まる。
障害物を避ける。
障害物の向こう側へ、迂回して進む。
注) 草刈りロボットの場合は、雑草は、無視する。
4) 上記走行を、いろんな所、気象状況(晴れ、曇、雨、雪、朝方、夕方、昼間)で、おこなう。
5) 前のエピソードの終点から、次のエピソードのデータ収集をしないこと。かならず、移動してから、次のエピソードを始める。

注)ACT の場合、10,000 ケースは、学習できるそうじゃ。これより多い場合は、ACT の学習能力を上げるか、別の ポリシーモデルを使う。

上記データで学習させれば、あとは、広域プランナーを使って、いろんな場所を走行させよう。

12. Googl Ai に、聞いた、今回の、WearHouse での学習のコツ
問題の根本はご指摘の通り「ACTモデルが『1.0m先の数値目標に従って旋回・修正する』というマルチモーダルな関係性をまだ十分に学習（Train）できていないこと」にあります。
🧠 なぜ今のデータではNav2に従えないのか？
今回の用途における「短距離コースデータ」の不足は、模倣学習（Imitation Learning）の特性上、極めて致命的になります。

「環境（景色）の記憶」の呪縛:
現在のモデルが「特定のフルコース（長距離）」だけをなぞるデータで学習されている場合、モデルにとっては「1.0m先の目標が右にあるか左にあるか」という数値情報よりも、「今カメラに見えているWarehouse（倉庫）の壁や棚の景色」の方が圧倒的に情報量が多く、強烈なトリガーになってしまいます。そのため、数値データを無視して、景色の記憶だけでハンドルを切ってしまいます。
「微修正」の経験不足:
1.0m先にNav2の点があるとき、ロボットがその点から少しズレた際に「グッと進路を戻すための制御（負のフィードバック）」の挙動を、現在のモデルは教わっていません。

🎯 次のステップ：性能を100%引き出すための「データ収集（Train）戦略」
「2.0〜5.0[m]の短距離コースをいっぱい学習させてみる」という作戦は完全に大正解です。 モデルに「景色の記憶」を忘れさせ、「1.0m先の目標数値を何よりも最優先にトレースする脳みそ」に育てるために、データ収集時に以下の3つの実戦テクニックを意識して教師データを集めてみてください。

① 【超重要】目的地（目標値）の座標を「バラバラ」にする
倉庫の中の「特定の場所」だけでデータを集めるのではなく、マップ上のあらゆる場所（直線通路、交差点、行き止まりなど）で、「1m〜5m先」のランダムな目的地をプロポ（テレオペ）で目指して走るデータを100本以上集めます。

効果: これにより、モデルは「この景色のときは左」という覚え方ができなくなり、「景色がどうあれ、インプットの角度が右を向いていたら右に旋回するしかない」という汎用的な追従ルールを強制的に学習します。

② 意図的に「失敗させてから復帰する」データを混ぜる
綺麗な走行データ（Nav2のライン上をぴったりトレースするデータ）だけでなく、人間があえて「ラインから1メートル右に脱線させ、そこから目標に向かってグッと左に曲がって復帰する」というリカバリーの操作データを意図的に10〜20%ほど含めてください。

効果: ロボットがNav2のルートから少しはみ出したときに、パニックにならずに自力でルートへ戻ってくるタフなモデルになります。

③ 目的地での「ピタ留まり」を叩き込む
1.0mや2.0m先の目的地に近づいた際、「速度を滑らかに落として、目的地のジャストの位置で綺麗に減速・停止する」瞬間のデータを多めに覚えさせてください。