コンテンツにスキップ

Raspberry Pi 側のノード構成と内部バス

要点

  • Raspberry Pi の中は、役割ごとに別のプロセス(ノード)に分かれています。1 つが落ちても、ほかは巻き込まれません。
  • ノードは ZeroMQ のバスでつながり、カメラ画像のような大きなデータは共有メモリで渡します。
  • 記録したデータを再生するときも、実機と同じコードが動きます。

【設計の説明】

確認した版: 上位 7c7054e(2026-11-15)。コードは surge_mk2/raspi/。

Pi 側はノードごとに別プロセスで、ZeroMQ の PUB/SUB と共有メモリでつないでいます。 1 つのプロセスが落ちても、ほかが巻き込まれないようにするためです。

ノード

ノード 入力 出力 役割
io_node UART、バスの cmd vehicle_state(100 Hz)、scan(10 Hz)、diag/link STM32 との送受信、時刻同期、ARM の門番、ハートビート
telemetry_node バス、WebSocket cmd、auto/ctrl ほか ブラウザ GUI との橋渡し、操縦権、デッドマン、自律走行の中継
planning_node scan ほか、vehicle_state、auto/ctrl auto/cmd、auto/state、auto/map 走行モード(planner)を動かして指令を作る
camera_node CSI カメラ 2 台(前方のステレオ) image/front(右=主)・image/left(左)(共有メモリへの参照) 画像の取り込み。ステレオが要る間だけ左カメラを回し、左右を同期させる
stereo_node image/front・image/left scan/stereo、stereo/depth、stereo/info 左右の画像を照合して、LiDAR より低い障害物の点を出す(下)
cam_perception_node 画像 scan/cam、path/cam、cam/mask 走行可能領域のセグメンテーション(ONNX)→ 擬似 LiDAR
cam_e2e_node 画像 cam_e2e/cmd 画像から操舵を直接回帰(模倣学習)
e2e_fusion_node scan・画像・vehicle_state e2e_fusion/plan 点群・画像・車両状態から前方の経路点を推論(E2E 融合)
line_perception_node 画像 line/cam 白線検出(古典的な色しきい値)
cam_track_node 画像 + scan track/target 対象追従用のトラッキング
arrow_signal_node 画像 route/select 矢印信号の認識(HSV しきい値)
logger_node バス全体 mcap ファイル 記録
video_node 画像(共有メモリ) mp4 ファイル(Pi の logs/) GUI の録画ボタンで telemetry_node が起動する。左右カメラの映像を 30 fps の mp4 に録る(_front.mp4・_left.mp4)
replay_node .sfl 記録 io_node と同じバスの出力 記録の再生(実機なしの開発用)

planning_node のアルゴリズム自体は raspi/auto/ にあり、ノードは配線だけを持ちます(走行モード)。

ステレオで低い障害物を見る(stereo_node)

LiDAR は路面から 0.095 m の高さの面しか測らないので、それより低い物は見えません。 2026-11-13 に前方のカメラを左右 2 台のステレオにして(センサ)、その照合で低い障害物を拾うノードを足しました。

項目 内容
照合 OpenCV の StereoSGBM(ブロック 5、視差 96)。640 幅のまま、平行化した画像の 150〜260 行の帯だけを 10 Hz で照合する
出力 scan/stereo 床より上・高さ 20 cm 以下の点を、1° ごとの距離にしたもの(型は LiDAR と同じ Scan)。0 は「見つからない」で、「空いている」ではない
出力 stereo/depth 視差を色にした JPEG。GUI の深度映像(/ws/camera/depth)に出す
いつ照合するか ARM 中(自動停止の入力にするため、モードに関わらず)か、深度映像を誰かが見ている間だけ。それ以外は左カメラも止める
遅れ 撮影から scan/stereo まで中央値 47 ms(Pi 5、1.5 GHz)

設計の判断:

  • StereoBM ではなく SGBM。 BM は模様の無い面で取り違え、偽の近い障害物を出しました。
  • 行の帯を 260 行で切る。 それより下には左右とも自分の車体が写り、0.3 m 先の偽の障害物になっていました。代わりに、床が見えるのは車の中央で約 1.1 m より先になります。
  • 左右がそろった組だけを使う。 左右の撮影時刻の差が 0.5 ms 以内のとき(synced)だけ照合します。そろっていない間は「何も見ていない」空の Scan を出します。
  • 照合した距離は学習の入力に混ぜない。 障害物の点として、古典的な処理で使います。原因を切り分けやすくするためです。模倣学習や E2E 融合には、左右の生の画像 2 枚をそのまま入れます。

io_node は scan/stereo の点を STM32 へ 10 Hz で送り(STEREO_OBS)、STM32 が自動停止の判定に使います(安全層)。 自動運転のモードの中でステレオを使う(段階 5)のは、方針を決めただけでまだ作っていません(検証状況)。

バスの設計

ZeroMQ PUB/SUB(raspi/bus/zbus.py)

ワイヤ形式:  b"vehicle_state" + b"\x00" + msgpack(payload)     ← 1 フレーム

素直には multipart([topic, payload])にしたいところですが、ZMQ の CONFLATE は multipart に対応していません。 制御系に CONFLATE(最新 1 件だけ残す)は必須なので、トピックとペイロードを 1 フレームに詰めています。 区切りに NUL を入れているのは、scan の購読が scan_debug にまで前方一致してしまうのを防ぐためです。

設計判断 理由
購読は「トピック 1 本につきソケット 1 本」 CONFLATE はソケット単位でありトピック単位ではない。1 本の SUB で vehicle_state と scan を購読して CONFLATE を掛けると、片方が来るたびにもう片方が捨てられる。LiDAR が来るたびに車両状態が消えるのに、見た目は動いているので気づきにくい
配送ポリシーは購読側が決める LATEST(CONFLATE、制御・GUI 表示)と RELIABLE(RCVHWM 大きめ、記録)。制御系が「2 秒前の点群で舵を切る」のを防ぐのが LATEST の目的
ノードごとに 1 本の endpoint を bind PUB を 1 プロセス 1 本にして、購読側の接続先を固定する
endpoint の持ち主は 1 つ ZeroMQ の ipc は、既に誰かが bind している経路にもう一度 bind してもエラーにならない(ソケットファイルを消して作り直す)。2 つ目が黙って endpoint を奪い、「動いているのにデータが来ない」になる。Publisher は bind の前に .lock を排他ロックし、取れなければ起動を拒否する

メッセージ型は msgspec の Struct(raspi/msgs/types.py)で、内部の msgpack と WebSocket が同じ形です。 GUI 用の TypeScript の型は、ここから config/gen_msgs.py で生成します(CI の --check が再生成し忘れを検出)。

画像は共有メモリ(raspi/bus/shm_ring.py)

640×480×3 を 30 Hz で msgpack 経由にすると 28 MB/s のメモリコピーが発生します。そこで:

camera_node     → 共有メモリの N 枚リングに書く
ZMQ に流すのは  → {name, slot, seq, t_capture_ns, ...} の数十バイトだけ
perception_node → その参照から numpy の view を作る(コピーゼロ)

読んでいる最中の上書きは seqlock で検出します(書き手は seq を奇数にして書き、偶数に戻す。読み手は前後の seq を比べる)。 リングを 8 枚取れば、書き手が同じスロットに戻るまで 8 フレーム(30 fps なら 266 ms)あります。

弱いメモリモデルの注意

Pi 5 の Cortex-A76 は弱いメモリモデルで、プロセス間には GIL の保護もありません。理屈の上では、安定を示す seq の更新が データ書き込みより先に他コアから見えることがあり、読み手が壊れたフレームを掴みえます。Python には明示的なメモリバリアが無いので、 threading.Lock の acquire/release(CPython 内部で atomic 命令になり ARM64 では dmb が出る)で代用しています。 ソースのコメント自身が「言語仕様の保証ではなく実装の性質に頼っている。『証明された』とは言わない」と書いています。 実効的な守りは、リングの段数を十分取ることです。

再生と実機を同じコードにする

io_node(実機)と replay_node(ログ再生)は、LinkTracker(受信フレームからリンク状態を組む部分)と bus_bridge (受信フレーム → バスへの publish)を共有しています。2 つ書けば必ずズレて「再生した結果が実機と違う」ことになるためです。

「今」の時刻は呼び出し側が渡します。実機は time.monotonic_ns()、再生はログ時刻のカーソルを渡します。 こうすると再生が記録時と同じ座標系で動き、リンクの健全性(INIT/OK/DEGRADED/FAULT)の遷移まで再現できます。

シミュレータも同じ考え方で、偽装するのは STM32 だけ(SerialLink の位置に SimLink を差す)です。 io_node より上流は 1 行も変えずに動きます(検証の仕組み)。

記録

形式 書き手 特徴
.sfl io_node が直接 SD へ 送受信フレームを生のまま。Wi-Fi が切れても Pi 単体で記録が続く。replay_node で再生できる
.mcap logger_node バス上のトピックを記録。GUI からは標準出力をそのまま WebSocket へ中継し、Pi のディスクには書かない(ブラウザ側が保持し、停止時にダウンロード)

依存と配備

  • 依存は Python 3.12/3.13 の pyserial、pyzmq、msgspec、websockets、numpy など。バージョンは範囲で留め、 実機に入れるときは requirements.lock.txt(pip freeze の結果)を使う方針です。
  • 画像の前処理は OpenCV や Pillow に依存せず numpy だけで書かれ、学習側(Mac)と推論側(Pi)が同じ関数を importします (raspi/core/cam_e2e_preproc.py)。前処理を変えたら PREPROC_VERSION を上げ、古い前処理で学習したモデルは読み込みを拒否します。
  • systemd の RemoveIPC=yes(既定)は、そのユーザーの最後のセッションが終わると共有メモリを全消しします。 実際に起きたため、Pi 側は RemoveIPC=no にしてあります(設計上の教訓)。