自作 motion planner を CommonRoad シナリオにつなぐ

このページを終えると、自作の motion planner が drawtonomy の書き出した任意のシナリオを解き、その答え・判定・どこで失敗したかをブラウザで確認できます。

drawtonomy は planner を実行しません。planner は既に動いている場所に、書かれている言語のまま置いておき、両者はファイルでやりとりします。

1 周を通しで: 追従シーンを描いて CommonRoad XML を書き出し、naive planner の FAIL を最後まで再生し、IDM に切り替えて回し直し、同じタブが新しい solution を読み直して PASS まで再生するところまで。

契約

scenario.xml  (CommonRoad 2020a, drawtonomy が書き出す)
      │
      ▼
[自分の planner]  ── 必須 ──▶ solution.xml   (CommonRoadSolution, commonroad-io)
      │
      ├── 任意 ──▶ solution.planning-trace.json   (drawtonomy_cr.trace.TraceWriter)
      │
      ▼ (drawtonomy-cr が後段で作る)
solution.verdict.json   (drawtonomy-verdict/1, 公式 checker の 4 判定)

必須なのは solution XML だけです。 再生と drawtonomy 自身の衝突判定は他の 2 つが無くても動きます。verdict は公式 checker の答えを足し、 planning trace は各再計画サイクルで planner が何をするつもりだったかを足します。

3 つの名前は 1 つの規則に従い、CLI もそれを前提にしています: <solution の stem>.verdict.json と <solution の stem>.planning-trace.json。

0. インストール

pip install "drawtonomy-commonroad[checker]"

Python 3.11 以上。ハードな依存は commonroad-io だけです。[checker] extra は commonroad-drivability-checker を入れますが、これは Linux x86_64 が必要です。無い場合でも verdict 以外はすべて動き、verdict は exit code 3 と 1 行の説明を返して終わります。

4 つの検査のうち 1 つは別のパッケージを使います。boundary_collision は triangle (Shewchuk’s Triangle) で道路を三角形分割しますが、これは商用では無償でないため既定の依存には入っていません。ライセンスを読んだうえで [boundary] extra を足してください:

pip install "drawtonomy-commonroad[checker,boundary]"

無くても壊れません。boundary_collision は FAIL ではなく SKIP になり、残り 3 検査は走り、exit code も 0 のままです。

[PASS] obstacle_collision
[SKIP] boundary_collision (pip install triangle)
[PASS] goal_reached
[PASS] solution_feasible

サイドカーには "status": "SKIP" と、その検査を飛ばした旨のメッセージが記録されます。drawtonomy はバッジの母数からも外すので、赤い 3/4 ではなく Checker PASS 3/3 になります。 SKIP: 判定できなかった検査を参照してください。

1. シナリオを用意する

3 つの出発点のどれからでも、planner が受け取るファイルの形は同じです。

	やること
自分で描く	レーンを引き、車両を置き、初速を設定し、1 台を Ego (external control) に印を付け、ゴールレーンを選ぶ
手元の OpenSCENARIO ファイルを開く	`.xosc` と `.xodr` をキャンバスにドロップする、または `?open=` で GitHub から開き、ego に印を付けてゴールレーンを選ぶ
CommonRoad ベンチマークシナリオを開く	`.xml` をキャンバスにドロップする。ego とゴールレーンはファイルに入っている

2 つの前提はスクリーンショット付きでシナリオを書き出すにあります。

2. CommonRoad XML を書き出す

Export ▸ .xml (CommonRoad)。専用のフォルダに置いてください。そのフォルダを CLI が配信します。

3. planner を書く

commonroad-io でシナリオを読み、解き、その隣に CommonRoadSolution を書き出します:

from commonroad.common.file_reader import CommonRoadFileReader
from commonroad.common.solution import CommonRoadSolutionWriter

scenario, planning_problem_set = CommonRoadFileReader("results/scenario.xml").open()
solution = my_planner(scenario, planning_problem_set)   # 自分のコード
CommonRoadSolutionWriter(solution).write_to_file(
    output_path="results/", filename="planner_solution.xml", overwrite=True
)

filename は自分で渡してください。渡さないと commonroad-io が solution の識別子から名前を作ります (solution_KS2:JB1:ZAM_…:2020a.xml)。CLI は名前ではなく内容で判別するのでそれでも動きますが、サイドカーの名前もその stem に引きずられます。

コピー元にするサンプル

examples/idm_planner/idm_planner.py から始めてください。commonroad-io と numpy 以外に依存しない 1 ファイルです。 ego は出発した lanelet の中心線をたどり、速度は IDM の追従モデルが決めます。 --mode naive も持っていて、こちらは計画せず初速を維持します。同梱の cut-in シナリオでは naive が衝突し、idm は 4 検査すべてを通ります。

ファイルは 2 つに分かれています。PLANNER-SPECIFIC が計画の中身、末尾の DRAWTONOMY HAND-OFF が契約のすべてです。前半を自分のものに置き換え、後半をそのまま残してください。

examples/reactive_planner/ は同じ形で commonroad-reactive-planner を端から端まで繋いだもので、再計画ループから planning trace を書くところまで含みます。

drawtonomy のシナリオが planner にどう見えるか

drawtonomy が書き出したシナリオは正しい CommonRoad ですが、実走の記録ではなく「描いたもの」の形をしています。ベンチマークコーパスと違う点が 4 つあり、どれも planner を静かに壊し得ます。

	どうするか
`successor` / `predecessor` は基本的に空です。片方のレーンの終端がもう片方の始端に実際に接している場合にだけ張られ、並走するレーンは `adjacentLeft` / `adjacentRight` だけを持ちます。	経路は初期 lanelet の中心線から作ってください。これは既に描いたレーン全長にわたっています。`successor` は「あれば延長に使う」程度に留めます。`successor` を辿って作る経路は lanelet 1 本ぶんで終わります。
直線レーンは 2 点の polyline です。境界は描いたときの点数のままで、120 m の直線は 120 m 離れた 2 頂点になります。	位置は頂点ではなく線分に射影してください。この形の lanelet で最近傍頂点に丸めると lanelet 長の半分ほどずれることがあり、症状は「前方車が静かに見えなくなる」形で出ます。`Lanelet.interpolate_position` と公式 route planner は既に線分で扱っています。
ego の初期状態が planning problem です。印を付けた車両と設定した初速から作られます。CommonRoad の planning problem は ego の寸法を持ちません。	planner が計画に使った車体を solution の `vehicle_type` に宣言し、書くなら planning trace にも宣言してください。
ゴールは lanelet です。任意でその一部区間、任意で速度区間も持てます。姿勢 (pose) ではありません。	waypoint ではなく planning problem の `goal` から読んでください。

dt は 0.1 秒です。シナリオ・solution・verdict サイドカーの time step はすべて 0 から数える整数なので、秒は step × dt です。

4. 判定を得る

drawtonomy-cr verdict scenario.xml solution.xml
# solution の隣に solution.verdict.json を書く

公式 checker の 4 検査を走らせ、1 行ずつ表示し、drawtonomy-verdict/1 サイドカーを書きます:

[PASS] obstacle_collision
[PASS] boundary_collision
[PASS] goal_reached
[PASS] solution_feasible
wrote results/planner_solution.verdict.json

FAIL した検査は該当する time step を出します (衝突なら障害物も):

[FAIL] solution_feasible t=42..61

判定できなかった検査は FAIL ではなく [SKIP] で、括弧内に次にやることが出ます:

[SKIP] boundary_collision (pip install triangle)

失敗した理由はサイドカーの message にあり、アプリはそれをバッジに出します。

exit code: サイドカーを書けたら 0 (FAIL 判定は JSON の中にあるのでエラーではありません)、checker が入っていなければ 3。

4 つの検査の意味とバッジの読み方は Verdict リファレンスにあります。

5. 開いて回す

drawtonomy-cr open ./results

フォルダの中の 4 種類のファイルを 内容で 判別し (<commonRoad、 <CommonRoadSolution、drawtonomy-verdict/1、 drawtonomy-planning-trace-v1)、判定が無く checker が入っていれば計算し、 127.0.0.1 でフォルダを配信して、URL を表示してブラウザで開きます:

solution: planner_solution.xml
verdict: planner_solution.verdict.json
trace: planner_solution.planning-trace.json
serving /path/to/results at http://127.0.0.1:53101
https://drawtonomy.com/?open=http%3A%2F%2F127.0.0.1%3A53101%2Fscenario.xml&solution=planner_solution.xml&verdict=planner_solution.verdict.json&trace=planner_solution.planning-trace.json
Open the URL in Chrome or Firefox (Safari blocks http://127.0.0.1 from an https page).
Watching for changes. Press Ctrl+C to stop.

見つけた相棒ファイルを 1 行ずつ名前で出し、URL には持っているものだけを載せます。solution しか無いフォルダなら solution: の行だけが出て、URL にも &solution= だけが載ります。serving の行にはそのフォルダの絶対パスが出て、ポートは --port を渡さない限り空いているものが選ばれます。

checker が入っていない場合は、その旨を 1 度だけ言ってシナリオは開きます:

No verdict: the official checker is not installed, so the scenario opens without it (install with: pip install "drawtonomy-commonroad[checker]", Linux x86_64 only).

タブにはシーンが開き、自分の solution が ego として再生され、checker の判定が RUN バッジに載ります。

planner を回し直すとタブが追従する

CLI を動かしたまま、タブも開いたままにしてください。planner を回し直すと、タブは新しい solution・判定・trace を 自分で 拾います。リロードも、ドロップし直しも、ダイアログもありません。

これを安全にしている規則が 2 つあります:

シナリオは再読み込みされません。 取り直すのは planner が書くファイルだけです。シーンを編集したら書き出し直してください。ループはステップ 2 からやり直しになります。
solution だけが変わったときは、古い checker の結果を降ろします。 CLI 自身が計算した判定は solution が変わるたびに自動で再計算されます。自分で書いた判定ファイルには手を触れないので、更新するまでバッジに checker の行が出ないだけです。

2 つめの規則で自分の判定が降ろされたときは、変更の次の行で CLI がそれを言い、戻すためのコマンドも添えます:

changed: solution
verdict: planner_solution.verdict.json is now older than the solution; checker results are cleared in the tab until you recompute: drawtonomy-cr verdict scenario.xml planner_solution.xml

CLI はファイルのサイズと mtime が 500 ms 静止するまで待ってから配信するので、書きかけの solution が読まれることはありません。

naive は FAIL、IDM は PASS

サンプル planner の 2 モードが、このループが何を教えてくれるかを示します。同じシーン、同じ ego、違うのは「前方車を見るかどうか」だけです。

	`--mode naive`	`--mode idm`
速度	初速を維持する	先行車に合わせて減速する
`obstacle_collision`	FAIL、時刻範囲と障害物 id 付き	PASS
バッジ	`RUN FAIL`、tooltip とトーストは `Checker FAIL 1/4`	`RUN PASS`、tooltip とトーストは `Checker PASS 4/4`
タイムライン	衝突した秒に赤い ✗ マーカー	マーカーなし
再生	最後まで走る。ヘッドが衝突の時刻を過ぎるとバッジが FAIL になる	最後まで走る。バッジは PASS のまま
計画軌跡	先行車を突き抜けていく	手前で曲がり、減速する

語の後ろの数は、その語のものを数えます。FAIL 1/4 は「判定した 4 件のうち 1 件が FAIL」、PASS 4/4 は「4 件すべて PASS」です。通らなかった検査は tooltip に 1 行ずつ出ます。

フォルダに solution が複数あるとき

CLI は黙って選びません。種類ごとに 名前順の先頭 を採り、無視したものを solution: の行の前に 1 行ずつ言います。

2 solution files found; using idm.xml (naive.xml ignored)
2 verdict files found; using idm.verdict.json (naive.verdict.json ignored)
2 trace files found; using idm.planning-trace.json (naive.planning-trace.json ignored)

更新時刻ではなく名前順なので、選択は毎回同じになります。3 つのサイドカーは solution と stem を共有するので、通常は同じ変種で揃います。別のものを選ぶときは名前で指定してください: --solution naive.xml (--verdict / --trace も同様)。

solution より古い判定は配信されない

判定は、それが計算された対象の solution に属します。フォルダの中の判定が隣の solution より古い場合、CLI はそれを配信せず、再計算の方法を言います:

verdict: naive.verdict.json is older than the solution and is not shown. Recompute: drawtonomy-cr verdict scenario.xml naive.xml

これは 自分で 書いた判定に対する挙動です。CLI 自身が計算したものは単に再計算されます。

どちらの場合も <solution の stem>.verdict.json は監視され続けます。 まだそのファイルが存在しない場合も同様です。 open を起動した後に書かれた判定 (たとえば CI ジョブが終わったとき) は、リロード無しでタブに反映されます。

変更の通知ストリーム

タブは CLI のサーバの GET /events (text/event-stream) を購読しています。 CI に組み込む前に疎通を確かめたいときに使えます:

curl -N http://127.0.0.1:53101/events

: connected

event: changed
data: {"files":["solution","verdict"],"names":{"solution":"planner_solution.xml","verdict":"planner_solution.verdict.json"}}

files は変わった種類 (solution, verdict, trace) を、names はそれぞれの配信フォルダ内のパスを表します。実際に配信できる種類だけが載るので、存在しない判定や solution より古い判定はどちらにも出ません。コメント行 (:) は最初のハンドシェイクと 30 秒ごとの keepalive です。

ブラウザ

Chrome と Firefox のみです。Safari は https:// ページからの http://127.0.0.1 へのリクエストを mixed content として遮断するので、タブがフォルダを読めません。Safari ではフォールバックを使ってください:

drawtonomy-cr open ./results --copy

配信する代わりにファイルの絶対パスを表示するので、 drawtonomy.com に手でドロップしてください。

フラグ

フラグ
`--solution` / `--verdict` / `--trace`	自動判別の結果を上書きする
`--port N`	ポートを固定する (既定: 空いているもの)
`--no-browser`	ブラウザを開かず URL だけ表示する
`--copy`	配信する代わりにファイルパスを表示する (Safari 用)
`--app-origin URL`	ファイルの読み取りを許可する origin (既定 `https://drawtonomy.com`)

サーバは 127.0.0.1 にのみバインドし、GET / HEAD / OPTIONS にのみ応答し、フォルダの外は配信せず (.. と絶対パスは 404)、 Access-Control-Allow-Origin にはちょうど 1 つの origin を書きます (* は書きません)。

次に読むもの

Verdict リファレンス: 公式の 4 検査、サイドカーの形式、バッジ。
Planning trace: 走った軌跡だけでなく、planner が何をするつもりだったか。
シナリオを書き出す: ステップ 1 の前提。

Appendix: checker を入れられない環境で判定を出す

commonroad-drivability-checker は Linux x86_64 の wheel だけを配布しています。判定のステップだけコンテナで実行してください。この Dockerfile で完結します:

FROM --platform=linux/amd64 python:3.11-slim
RUN apt-get update && apt-get install -y --no-install-recommends gcc libc6-dev \
    && rm -rf /var/lib/apt/lists/*
RUN pip install --no-cache-dir \
        --only-binary=commonroad-drivability-checker,commonroad-clcs \
        "drawtonomy-commonroad[checker,boundary]"
WORKDIR /work

docker build --platform linux/amd64 -t cr-verdict .
docker run --rm --platform linux/amd64 -v "$PWD:/work" cr-verdict \
    drawtonomy-cr verdict /work/scenario.xml /work/solution.xml

判定サイドカーはマウント越しに solution の隣に出力され、動いている drawtonomy-cr open がそれを拾います。pip の行から ,boundary を外すと boundary_collision は SKIP になります。