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にしてあります(設計上の教訓)。