コンテンツにスキップ
drawtonomy drawtonomy

モーションプランナー デバッグ用の Planning trace (上級)

CommonRoad の solution が持つのは 1 本の軌跡、ego が実際に走った経路だけです。 周期的な planner は再計画サイクルごとに新しい軌跡を数秒先まで計算し、そのうち コンマ数秒だけを実行して、また計画し直します。

planning trace はそのサイクルを残し、答えのほうも残します。1 つのファイルが driven (ego が実際に走った状態列) と plans (各サイクルで planner が意図していた もの) の両方を持つので自己完結し、このファイル 1 本を落とすだけで 1 回の走行が まるごと再現されます。

これを書けるのは planner の作者だけです。サイクルごとの軌跡は planner の内部ループの 中にしか存在しないからです。だからコマンドではなくライブラリになっています。

書く

Section titled “書く”
from drawtonomy_cr.trace import TraceWriter


w = TraceWriter(dt=0.1, vehicle=dict(length=4.5, width=1.8, refToCenter=1.4,
                                     type="BMW_320i"))
for cycle in my_planner_loop():
    w.plan(t=cycle.t, states=cycle.trajectory)   # 再計画サイクルごとに 1 件
w.driven(executed_states)                        # ego が実際に走った状態列
w.write("solution.planning-trace.json", solution="solution.xml")

states は commonroad-io の State オブジェクトでも、素の {"x":, "y":, "orientation":, "v":, "time_step":} dict でも受け付けます。

write() は書き出す前に 2 つの同一性を検査します。driven が solution の軌跡と 1e-6 m 以内で一致すること、各 plan の実行済み先頭が同じタイムステップの driven と 一致すること。合わなければ例外を投げます (「自分が説明していると称する solution とは 別の再生になる trace」を黙って書かないためです)。

TraceWriter の引数

Section titled “TraceWriter の引数”
引数必須意味
dtはいシナリオのタイムステップ [秒]。有限の正数。状態の time_step をファイルの秒に直すのに使います。
vehicleいいえplanner が計画に使った車体。{"length":, "width":, "refToCenter":, "type":}。CommonRoad の planning problem は ego の形を持たないので、これが無いと drawtonomy は公式 checker が判定した車体ではなく、作図時の箱で描いて判定します。
scenarioいいえシナリオ識別子 (str(scenario.scenario_id) など)。シーンとの対応付けに使います。
producerいいえ{"name":, "version":}。情報のみです。
role既定 "ego"この track がどの actor のものかを role で示します。
nameいいえこの track がどの actor のものかをエンティティ名で示します。role / name はちょうど一方だけ渡してください (name を使うときは role=None にします)。
frame既定 "center"座標が何を指すか。"center" (車体中心。CommonRoad の position の意味) か "ref" (参照点 = 後輪軸中心)。

write(path, solution=None, replanning_frequency=1, verbose=True): solution (パス、Solution オブジェクト、状態列のいずれか) を渡すと 「driven が solution と一致するか」の検査が有効になります。 replanning_frequency は各 plan の先頭何状態が実際に実行されたか = driven と突き合わせる個数です。verbose は検査ごとの PASS / FAIL を 1 行ずつ表示します。

refToCenter

Section titled “refToCenter”

参照点 (後輪軸中心) から車体中心までの、進行方向に沿った距離 [m] です。 これは frame ではなく車体の性質です。frame が「ファイルの座標が何を指すか」を 言うのに対し、refToCenter は drawtonomy がその 2 つを変換するのに使うオフセットで、 描かれる車体中心がファイルの言う位置にぴたりと乗るようにします。

値は 0 以上でなければならず、負値は拒否されます。例の 1.4 は実在の車の値です (BMW_320i は 1.4227170936)。この数字を写すのではなく、自分の planner の車両 パラメータから取ってください (commonroad-vehicle-models では wb_rear_axle という 名前です)。省略すると drawtonomy は作図した車両自身のオフセットに倒します。

書き出されるファイル

Section titled “書き出されるファイル”
{
  "schema": "drawtonomy-planning-trace-v1",
  "scenario": "ZAM_Untitled202609011139-1_1_T-1",
  "producer": { "name": "my_planner", "version": "0.1" },
  "frame": "center",
  "tracks": [
    {
      "role": "ego",
      "vehicle": { "length": 4.508, "width": 1.61, "refToCenter": 1.4227170936, "type": "BMW_320i" },
      "driven": [
        { "t": 0.0, "x": 8.18, "y": 51.37, "h": 1.567, "v": 30.0 },
        { "t": 0.1, "x": 8.19, "y": 54.37, "h": 1.567, "v": 30.0 }
      ],
      "plans": [
        {
          "t": 0.0,
          "states": [
            { "t": 0.0, "x": 8.18, "y": 51.37, "h": 1.567, "v": 30.0 },
            { "t": 0.1, "x": 8.19, "y": 54.37, "h": 1.567, "v": 30.0 }
          ]
        }
      ]
    }
  ]
}
フィールド必須意味
schemaはいちょうど "drawtonomy-planning-trace-v1"。他の値は拒否されます。
frameはい上記のとおり "center" か "ref"。
tracksはいactor 1 つにつき 1 件。最低 1 件。
scenarioいいえこの trace が計算されたシナリオの識別子。
producerいいえ自由形式。情報のみです。
tracks[].role / tracks[].name一方のみこの track がどの actor のものか。
tracks[].drivenはいactor が実際に走った状態列。t の昇順。
tracks[].plansはいplanner が出した計画。最低 1 件。
tracks[].vehicleいいえlength / width [m] (vehicle があるときは両方必須、いずれも 0 より大)、任意の refToCenter と type。
plans[].tはいその計画を発行した時刻 [秒]。states[0].t と 1e-6 以内で一致する必要があります。
plans[].states[]はいt [秒]、x / y [m、ENU で Y が上]、任意の h [rad、反時計回り、0 = +x] と v [m/s]。

この受け渡しのもう一方のファイルである verdict サイドカーと比べて、2 点だけ 注意してください。trace の時刻は秒であって、サイドカーの整数タイムステップでは ありません。向きのキーは orientation ではなく h です。未知のキーは無視されます。

読ませる

Section titled “読ませる”

solution と同じです。キャンバスにドロップする、Import… で選ぶ、URL の &trace= に指す。drawtonomy-cr open は、solution の隣に <solution 名>.planning-trace.json という名前で置いてあれば自動で拾います。

solution と trace の両方が渡されたときは trace を採ります。 どちらも同じ走行の replay なので ego を動かせるのは片方だけで、trace は solution に足したもの (その driven 状態列が solution の軌跡そのもの) だからです。落としたほうは 1 行で 名指しするので、黙って選ばれることはありません。

何が見えるか

Section titled “何が見えるか”

再生中、ego は driven に沿って動きます。solution から再生したときとまったく 同じです。trace が足すのは道路上の計画軌跡で、各時点で planner が保持していた計画 (再生ヘッド以前で最新の plan) を、再生ヘッドから先へ切り出して描きます。

planner が数秒先にどこに居るつもりだったか、そしてそれをどの瞬間に考え直したかが 見えます。最初の plan の時刻より前では、その actor には何も描かれません。

verdict は scenario id で対応付くので、solution の replay と同じように trace の replay にも載ります。

計画軌跡は画面の一部なので、書き出しにもそのまま入ります。Export video… は キャンバスと同じ見た目で各フレームに書き出します (車の下に、そのフレームの時刻から先だけ)。 画像の書き出し (PNG / JPEG / SVG / EPS / PDF) には、いまの再生時刻で画面に見えている計画軌跡が入ります。 計画軌跡の無い画像や動画にしたいときは、書き出す前に Trajectory トグルを OFF にしてください。

次へ

Section titled “次へ”