コンテンツにスキップ
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 の軌跡そのもの) だからです。solution は捨てられません。 trace の solutionFingerprint が solution と一致すれば、drawtonomy は solution ファイルを 保持し、Export solution はそれをバイト単位で同一のまま返します (KS のまま、 ファイル名もそのまま)。solution だけを読み込んだときと同じです。指紋が一致しないか trace が指紋を持たないときは 1 行でそう言い、Export solution は trace から質点モデルの 解を組み直します。

何が見えるか

Section titled “何が見えるか”

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

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

緑のリボンの読み方

Section titled “緑のリボンの読み方”

リボンは形のほかに 2 つの情報を運んでいます。

  • 濃淡は既定で加速度、速度にも切り替えられます。 選んだ量の「遅い側」が濃い緑 (加速度なら減速、速度なら低速)、「速い側」が淡い緑です。加速度モードでは一定速が もとの緑のまま残ります。Trajectory トグルを押すとパネルが開き、Shade の Accel / Speed で選べます。
  • 不透明度は時刻です。 リボンは先へ行くほど薄くなるので、車体に近い側が近い未来、 薄い端が予測の先端です。

自動スケールと固定スケール

Section titled “自動スケールと固定スケール”

同じパネルの Scale で「どこを満色にするか」を決めます。

  • Auto (既定) はいま見ている軌跡の実測レンジを使うので、色域を常に使い切ります。 濃く見える区間は「その走行の中で最も強く減速した所 / 最も遅い所」であって、決まった 値ではありません。別の走行どうしで濃さを見比べても値の比較にはなりません。
  • Fixed はスケールの端を指定した数値に固定します (速度は km/h、加速度は m/s²)。 どのスクリーンショットでも同じ色が同じ値を意味するので、複数の走行を並べて比べるときに 使います。代償として、市街地の走行を 180 km/h のスケールで見ると色域の濃い側しか 使わず、ほぼ一定色に見えます。

Trajectory トグルの説明にはいま効いている符号化が出るので、キャンバスを離れずに読み方を 確認できます。選んだ設定はセッションをまたいで保たれ、キャンバス / 3D 表示 / 書き出しの すべてが同じ設定を使います。

verdict は trace の solutionFingerprint で対応付きます。solution の replay と同じ 規則です。drawtonomy-commonroad 0.3 以降が書いた trace はこの指紋を持つので、trace と その verdict は照合済みとして読み込まれ、公式検査が RUN の判定に合流します。 solution 指紋を持たない trace も読み込めますが、判定は未照合の CHECK ? に留まります。

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

次へ

Section titled “次へ”