本專案是一條離線路徑推論（trajectory / path planning）流程： 把原始 rosbag 轉成 224×224 的 RGB 影像、語意分割與里程計，切成 0.5 秒的序列， 最後用 ST-P3 模型推論車輛未來路徑，並輸出 L2 誤差統計與視覺化圖。
整條流程共四個步驟，執行順序如下：
原始 bag ─(Step1: bag_to_data.py)→ img/ + seg/ + odom.csv
─(Step2: resample.py)→ 0.5 秒序列 resample/
─(Step3: convert_cls4png_to_npy.py)→ seg 轉 .npy
─(Step4: park_L2_ASAP.py)→ 推論路徑
| 步驟 | 腳本 | 用途 |
|---|---|---|
| Step 1 | bag_to_data.py |
直接讀原始 bag，做語意分割 + resize，輸出 img/、seg/ PNG 與 odom.csv |
| Step 2 | dataset/0624bkgd/resample.py |
以 0.5 秒為間隔重採樣，產生 resample/ |
| Step 3 | convert_cls4png_to_npy.py |
把彩色語意 PNG 轉成 class-id .npy |
| Step 4 | park_L2_ASAP.py |
載入模型推論路徑，輸出軌跡與 L2 誤差 |
Step 1 的
bag_to_data.py取代了舊的「rosbag play→seg_real_time.py節點 →rosbag record→extract_bag_data.py」四步即時流程。 若仍需即時處理，舊做法保留於 附錄：備選（即時）資料轉出流程。
本流程用到兩套不同的環境：
source /opt/ros/noetic/setup.bash- 此環境的
python3（3.8）已同時具備rosbag、torch、transformers、cv2、numpy， 是執行bag_to_data.py所需的一切（也是seg_real_time.py的執行環境）。 - Step 1 首次執行會自 HuggingFace 下載模型
nvidia/segformer-b2-finetuned-cityscapes-1024-1024。 - Step 2（
resample.py，純標準函式庫）與 Step 3（convert_cls4png_to_npy.py，需numpy+PIL） 也可在此環境執行。
conda activate stp3_ros # 每次使用前啟用- PyTorch 2.2.2+cu121 / torchvision / transformers / opencv / pandas / pytorch-lightning / yacs / fvcore 均已具備，實測可完成 Step 4 推論。
- 推論步驟務必在
stp3_ros內執行，勿用系統 python。 - environment.yml 是另一台機器上
stp3_env（py3.9）的環境定義，本機不使用； 其prefix:指向不存在的/home/cyc/miniconda3，若要據此另建環境請先移除該行。
bag_to_data.py（Step 1）只需要原始 bag 內的兩個 topic：
| 用途 | Topic 名稱 | 說明 / 參數 |
|---|---|---|
| 相機 RGB 輸入 | /zed2i/zed_node/rgb_raw/image_raw_color |
bag_to_data.py 的 --rgb-topic 預設值；亦為 seg_real_time.py 的預設訂閱 topic（seg_real_time.py:216） |
| 里程計 | /odom |
bag_to_data.py 的 --odom-topic 預設值（extract_bag_data.py:16） |
原始 ZED bag 通常沒有深度 topic，因此整條流程一律不使用深度（見 注意事項）。 若你的相機用的是
right_raw而非rgb_raw，執行時加--rgb-topic /zed2i/zed_node/right_raw/image_raw_color覆寫即可。
| class id | 類別 | RGB |
|---|---|---|
| 0 | road（道路） | (128, 64, 128) |
| 1 | person（行人） | (220, 20, 60) |
| 2 | movable（可移動物） | (0, 0, 142) |
| 3 | static（靜態物） | (70, 70, 70) |
調色盤定義於 seg_real_time.py:35-40 與 convert_cls4png_to_npy.py:10-15。
bag_to_data.py 重用 seg_real_time.py 的 colorize_cls4_rgb，並在存檔時做 RGB→BGR 轉換，
確保磁碟 PNG 的顏色值與 PALETTE4 完全一致，Step 3 才不會因未知顏色而報錯。
以下以資料根目錄 <root> 為例（例如 dataset/0624bkgd），各 video 依 video1 ... videoN 命名，
每個 videoN/ 內放恰好一個原始 .bag。
直接離線讀取原始 bag（不需 roscore / play / record），對每張 RGB 影像做 SegFormer 分割 + resize，
輸出結構與舊 extract_bag_data.py 完全相同。
- 輸入：
<root>/video1/*.bag ... <root>/videoN/*.bag，每個 video 資料夾恰好一個.bag。 - 輸出（寫在每個
videoN/內）：img/：resize 後 224×224 RGB PNG，檔名為時間戳{timestamp_ns}.pngseg/：4 類語意分割 224×224 彩色 PNG，檔名與對應的img/同一時間戳odom.csv：里程計（欄位timestep, position_x/y/z, orientation_x/y/z/w）
source /opt/ros/noetic/setup.bash
python3 bag_to_data.py --root <root> --start 1 --end N [--overwrite]- 參數：
--root（預設.）、--start（1）、--end（9）、--overwrite、--rgb-topic（預設/zed2i/zed_node/rgb_raw/image_raw_color）、--odom-topic（預設/odom）。 - 預設不覆寫已存在的檔案，加
--overwrite才會重寫。
⚠️ 請使用dataset/0624bkgd/resample.py（新版），不要用頂層的resample.py（舊版，會強制要求poseimu_zero.csv與depth/）。詳見 注意事項。
- 以
HALF_SECOND_NS = 500_000_000（0.5 秒）為步長，對時間軸逐點取最近鄰樣本。 - 輸入：
<root>/videoN的img/、seg/、odom.csv（無深度，用--no-depth）。 - 輸出：
<root>/resample/videoN/內含img/、seg/（複製選中的 PNG，沿用原始時間戳檔名）odom.csv（重採樣後）resample_index.csv（對照表：target/img/seg/depth/odom時間戳）
python3 dataset/0624bkgd/resample.py \
--root <root> --output resample --start 1 --end N --no-depth [--overwrite]- 輸出根資料夾預設名為
resample；相對路徑會接在<root>底下（即<root>/resample）。
把 4 類彩色語意 PNG 依調色盤轉成 uint8 的 class-id .npy，就地輸出在與 PNG 相同的資料夾（convert_cls4png_to_npy.py:72）。此步是 Step 4 推論的必要前置（載入器只讀 seg/{ts}.npy）。
# 注意：--root 必須指向 seg 資料夾。一次處理所有 video 的 seg：
for d in <root>/resample/video*/seg; do
python3 convert_cls4png_to_npy.py --root "$d" --force
done- convert 以
rglob("*.png")遞迴尋找 PNG 且無檔名過濾（convert_cls4png_to_npy.py:65），因此--root必須指到只含語意圖的seg/，不可指向同時含img/相片的resample/videoN（strict 模式會因相片顏色不在PALETTE4而報錯）。 - 未知顏色預設會報錯（strict）；若確定顏色略有偏差，可加
--non_strict改用最近鄰對應。 - 已存在的
.npy預設會跳過，加--force可覆蓋。
- 環境：
conda activate stp3_ros。 - 輸入
--dataroot指向某個resample/videoN；載入器會讀取（NuscenesData_0624_ASAP.py:52-53、:501-505）：resample_index.csv、odom.csvimg/{img_timestep}.png、seg/{seg_timestep}.npy- 深度必須自己用
--no-depth關掉：use_depth預設是True（park_L2_ASAP.py:565、:1082）， 不加旗標載入器就會去找depth_infer/{ts}.npy而中斷（NuscenesData_0624_ASAP.py:577）。 這條流程一路不產生深度，故 Step 4 一律要加--no-depth。
- checkpoint：
--checkpoint（預設last.ckpt）；本機請用model/best-box-col-*.ckpt。⚠️ checkpoint/last.ckpt不是完整模型：只有 10 個 tensor（純 AD-MLP planner head，3.4MB）， 不含model.vlm.*視覺權重，無法推論。完整模型是model/best-box-col-*.ckpt（646 個 tensor）。
conda activate stp3_ros
python park_L2_ASAP.py \
--checkpoint "model/best-box-col-epoch=24-epoch_val_plan_obj_box_col=0.0054.ckpt" \
--dataroot dataset/0624bkgd/resample/video1 \
--no-depth- 輸出：每次執行都集中在
inference/imgs/<MMDDHHMMSS>/（每次執行一個時間戳資料夾，不覆蓋前次，park_L2_ASAP.py:40、:564），內含：trajectories.csv、l2_errors.csv、l2_error_summary.csv（park_L2_ASAP.py:581-587）inference_plots/內的推論視覺化 PNG（park_L2_ASAP.py:565-566）
senpai/
├── bag_to_data.py # Step 1：原始 bag → img/ seg/ + odom.csv（推薦）
├── seg_real_time.py # 備選（即時）：語意分割 ROS 節點，被 bag_to_data 重用其函式
├── extract_bag_data.py # 備選（即時）：bag → PNG + odom.csv，被 bag_to_data 重用其函式
├── resample.py # （舊版，勿用；請改用 dataset/0624bkgd/resample.py）
├── convert_cls4png_to_npy.py # Step 3：seg PNG → class-id .npy
├── park_L2_ASAP.py # Step 4：路徑推論主程式
├── environment.yml # 另一台機器的 conda 環境定義（name: stp3_env，本機不用）
├── checkpoint/
│ └── last.ckpt # ⚠️ 非主權重：純 AD-MLP baseline（10 tensor）
│ # 被完整模型當凍結 coarse baseline 載入，勿刪
├── model/
│ └── best-box-col-*.ckpt # ★ 主推論權重（646 tensor，含 model.vlm.*）
├── stp3/ # ST-P3 模型與資料集程式
│ ├── config.py # 設定入口 get_cfg
│ └── data_0512_graduate/
│ └── NuscenesData_0624_ASAP.py # 推論資料集載入器
├── dataset/
│ └── 0624bkgd/
│ ├── resample.py # Step 2：0.5 秒重採樣（★新版，請用這支）
│ └── resample/video1/ # 範例：已重採樣好的一段資料
└── inference/ # Step 4 輸出根目錄
└── imgs/<時間戳>/ # 每次執行一個資料夾，內含：
├── trajectories.csv # 輸入/GT/預測軌跡
├── l2_errors.csv # 逐點 L2 誤差
├── l2_error_summary.csv # L2 誤差統計摘要
└── inference_plots/ # 推論視覺化 PNG
以 dataset/0624bkgd 為資料根目錄，處理 video1（每個 videoN/ 內先放好一個原始 .bag）：
# Step 1（noetic python3）：原始 bag → img/ seg/ + odom.csv
source /opt/ros/noetic/setup.bash
python3 bag_to_data.py --root dataset/0624bkgd --start 1 --end 1
# Step 2（新版 resample）：0.5 秒重採樣
python3 dataset/0624bkgd/resample.py \
--root dataset/0624bkgd --output resample --start 1 --end 1 --no-depth
# Step 3：seg PNG → .npy（--root 指向 seg 資料夾，勿指整個 resample）
for d in dataset/0624bkgd/resample/video*/seg; do
python3 convert_cls4png_to_npy.py --root "$d" --force
done
# Step 4（stp3_ros）：推論路徑
conda activate stp3_ros
python park_L2_ASAP.py \
--checkpoint "model/best-box-col-epoch=24-epoch_val_plan_obj_box_col=0.0054.ckpt" \
--dataroot dataset/0624bkgd/resample/video1 \
--no-depth若要即時跑完整條鏈路（相機 → 分割 → 路徑），不需要上面的四個步驟：
realtime/realtime_planner_node.py 是單一 ROS 節點，
訂閱相機與 /odom，內部跑 SegFormer + ST-P3，直接把預測路徑以 nav_msgs/Path 發到 /senpai/path。
source /opt/ros/noetic/setup.bash # 提供 ROS 環境變數
conda activate stp3_ros # 即時節點同樣用 stp3_ros（系統 python3 缺 pyquaternion 等套件）
python3 realtime/realtime_planner_node.py- 外部預設路徑必須覆寫：
convert_cls4png_to_npy.py的--root預設（:53）與park_L2_ASAP.py的 dataroot 預設（:629）、範例 checkpoint（:34）都是外部路徑/home/cyc/...，在本機不存在，請一律用參數指定自己的路徑。 checkpoint/last.ckpt不可當主權重：它是純 AD-MLP baseline（只吃 21 維狀態、不看影像）的存檔，只有 10 個 tensor，缺全部 633 個model.vlm.*視覺權重 → strict 載入會直接RuntimeError: Missing key(s)。請一律用model/best-box-col-*.ckpt。但不要刪除它 —— 完整模型會把它當凍結的 AD-MLP coarse baseline 載入（codex_pure_ASAP.py:16寫死路徑），缺檔會無法建構模型。- convert 的
--root只能指向seg/：convert 遞迴抓所有*.png且無檔名過濾，指向整個resample/videoN（含img/相片）會在 strict 模式報未知色錯誤。 - resample 用新版：請用
dataset/0624bkgd/resample.py（無 poseimu、支援--no-depth）。頂層resample.py為舊版，會強制要求每個 video 內有poseimu_zero.csv與depth/，缺檔會直接中斷。 - 深度全程停用，且兩處都要自己加旗標：原始 ZED bag 通常無深度 topic；
bag_to_data.py不輸出深度， Step 2 與 Step 4 都必須加--no-depth（兩者的預設都是「用深度」，忘記加就會找不到depth_infer/而中斷）。 - 環境分離：Step 1–3 用 ROS noetic 的
python3；Step 4 用 condastp3_ros。兩者是不同環境（無法互相 importrosbag），勿混用。 - 調色盤一致性：分割輸出顏色必須與 Step 3 的
PALETTE4完全一致；bag_to_data.py存檔時做 RGB→BGR 轉換即是為此，否則 Step 3 在 strict 模式下會因未知顏色報錯。
若需要以即時方式產生資料（例如邊跑相機邊處理），可用舊的四步流程取代 Step 1。
seg_real_time.py 是 ROS 節點，訂閱即時相機 topic，發佈 /seg_cls4_224、/image_224、/depth_224，本身不寫檔；再用 extract_bag_data.py 從錄下的 bag 抽出 PNG。
# 終端 1：啟動 ROS master
roscore
# 終端 2：啟動語意分割節點
python3 seg_real_time.py
# 終端 3：錄製節點輸出（含里程計）成新 bag
rosbag record -O <root>/video1/processed.bag /image_224 /seg_cls4_224 /depth_224 /odom
# 終端 4：播放原始 bag（提供 /zed2i/... 與 /odom）
rosbag play 2026-06-23-18-23-14.bag
# 錄完後：從新 bag 抽出 PNG 與 odom.csv
python3 extract_bag_data.py --root <root> --start 1 --end N [--overwrite]seg_real_time.py的輸入／輸出 topic 可用 ROS private param 覆寫：~in_topic、~depth_topic、~out_topic、~depth_out_topic（seg_real_time.py:216-220）。extract_bag_data.py讀取的 topic 預設為/image_224、/seg_cls4_224、/depth_224、/odom（extract_bag_data.py:12-17），可用--img-topic等覆寫。- 此流程之後接 Step 2 → Step 3 → Step 4，與主流程相同。