はじめに
こんにちは！はしもとかずさです！！
ROS2アドカレ16日目はロボコンです！！
近年高専ロボコンや学生ロボコンをはじめとするロボットコンテストでROS 2の採用率が向上しています
しかし実際に自動運転などを始めるとアルゴリズム選定やパラメーター調整、アルゴリズム実装などたくさんの壁があると思います
茨城高専では香川高専高松キャンパスさんのUdonLibraryとTIER IV社のAutowareを参考にロボコン向けROS 2ライブラリであるnatto_libraryを開発しています
この記事ではプロジェクト開始から1ヶ月半で実装、テストが終わった機能をnav2などと比較して紹介します
もくじ
納豆の経緯
まず最初になんでロボコンでOSSを開発しているかを説明します
僕は2年のNHK高専ロボコンから自動運転をフルスクラッチで作成しています
アルゴリズム実装をまとめてロボコンで使いやすくしようということで開発を始めました
OSSにしようと思った理由としてはアルゴリズム実装は誰でもできるのでわざわざ隠す必要がないと感じたのと、ロボコンでだいじなのはあくまでアイディアだと思うからです
またOSSあるあるの専用Orgを作成せず茨城高専としてやっているのには以下の理由があります
- 部内の資産にすることで管理、継承を絶やしにくくする
- 身近な人が作っているから誰でも参加してもよいという雰囲気を作る
主に茨城高専がロボコン用に作っているものを公開し、外部の意見を参考に改良をしていくという方針です
また納豆という名前は 香川がUdonなら茨城は？と言われたら納豆が一番しっくり来たからです
(茨城には色んな魅力があります！！！)
さらに継承のコスト軽減やどんなデバイスでも動くことを目標とし以下のルールを設けて開発しています
- 外部ライブラリ禁止
- tf2,pointcloud iteratorはOK
- Eigen, PCL, OpenCVなどは禁止
- 依存はROS 2だけ
raspberry pi 5 ros2 humbleを経験した身としてはapt install ros-humble-**が使えないと非常につらいので、ROS 2ソースビルド勢にも優しいライブラリにしています
依存関係が増えるとバージョン違いによる問題やGithub Actionsの速度低下などがあるからというのも理由の一つです
継承、管理コストを極限まで減らすとたぶんこうなります
実装されている機能について
現在納豆では以下の機能が実装されています
- 線分と円でマップを表現、OccupancyGridへの変換
- emcl2とamclを参考にロボコン用に改造されたmcl位置推定
- N輪独立ステアリングやN輪オムニの運動学、逆運動学計算(メカナムは年内対応予定)
- A＊を用いたx,y,yawのプランナーと全方位移動用に改造された HDWPP(HolonomicDynamicWindowPurePursuit) ←これらは別記事で解説します
- autoware風のlaunchシステム
この用に短期間で爆発的進捗を出しています！
マップデータの作成、管理について
納豆では以下の理由でOccupancyGridによるマップデータ管理をやめ、独自の形式で管理しています
- 円などグリッドで表現しにくいフィールドがよくある
- フィールド寸法のmm単位の情報量を落としたくない
- 1D配列でデータが送られてくるので非常に使いづらい
データの作成はWebアプリかSpreadSheetで直接入力することでできます
データはcsvで保存されます
jsonやyamlにしようと思いましたがwebアプリができる前、スプシで更新できると便利だと思いcsvにしました
またOccupancyGrid変換Nodeもあるので既存のプログラムを使用することもできます
自己位置推定について
自己位置推定は現在ホイールオドメトリと2D LiDARによるMCLのみの実装となっています
MCLでは以下機能が実装済みです
- odomTFを使用するかの選択
- 移動速度に応じたノイズと静止時にも発生するノイズによるモーションアップデート
- パーティクル数を固定にすることで最悪計算量等が事前に分かるだけでなく、静止時などに高精度に
odomフレームについてはNav2やAMCL,emcl2を使用する際に必要となるので互換性のために提供可能としています
デフォルトではodomを使用せず、map-base_linkを提供します
odomフレームという概念は理解が難しく、高周波でmap-base_linkが更新できるので必要ないと考えました
またジャンプが発生したらどうするのか、連続性がーというお話ですが、自己位置飛んだらロボコンでは手動に切り替えるべきなのでそこに時間やリソースをかけるべきでないと判断しました
emcl2を参考にsin,cosテーブル作成などを行うことで3000点群/scan,300パーティクルで400Hzの位置推定を確認しています
またRANSACによる直線、線分、コーナー検出が可能なのでそれらを用いた位置推定や初期位置推定も実装中です
IMUやオドメトリセンサーなどホイールオドメトリ以外のデータも使えるようEKFの実装も検討していますがまだ決まっていないのでもしよい案があれば教えて欲しいです
経路生成、追従について
現在はA*によるプランニングとPurePursuitの追従が実装済みです
詳細は次の記事で書きますがx,y,yawの3DのA*よりも計算量を抑えつつ、複雑なフットプリントでも壁に当たらない経路生成や、加速度、速度制約などから経路を外れにくく、回転も間に合う全方位移動用PurePursuitを作成しました。
今後速度情報などを含む経路生成や追従、また必要に応じて経路を動的に計画し直すものも作成中です
launchシステムについて
autowareを参考にlaunchファイルのcompinent分散により責任を分散させるようにしました
各componentは決められた入力トピックを受け取り決められた出力トピックを出すということを守りつつ、argで動かすアルゴリズムを選べるようにすることで柔軟に対応できるようにしています
また怒られ案件ですがchassis_typeを変えると独ステからオムニにシステムを変更できますが文字チェックが入っていないのでそれ以外の文字があると構文(node見つからない)エラーで起動できません
各トピックの説明を丁寧に書き自動運転におけるデータの流れを理解しやすくすることで初心者でも簡単に自動運転ができるようになります
launchは基本的にxmlで書かれていますが、LiDARの個数で起動するNode数を変更するSensingやSimulationは例外的にpythonを使用していますがxmlから呼び出せるようにしています
爆速ビルドチェック
mainブランチ更新時にdocker buildが実行されるようになってます
build時にcolcon buildも実行され、build,install dirが残ります
これによりPR時に変更がない部分のビルドがスキップされるので1分以内にチェックが終了しストレスフリーに開発できます
実質mainブランチとの差分となるのでPRチェックとしてそのまま使えます
今後やりたいこと
今後はmermaidによる行動管理や簡易的な自動運転もできるようにしたいと思っています
また自己位置推定の充実を最優先に進めていこうと思います
こんな機能欲しい！こんなアルゴリズム使ってるけど便利だよ！というのがあればIssue,コメント、DM等お願いします！