完善....

2026-06-05 12:53:48 +08:00
parent a713b5d9ab
commit f2e5a4bb7d
64 changed files with 3294 additions and 445 deletions
@@ -652,28 +652,11 @@
  },
  "chassis_calibration": {
    "chassis_type": "ackermann",
    "straight_track_length_m": 5.0,
    "straight_track_width_m": 0.7,
    "arc_track_radius_m": 1.6,
    "wheel_radius_m": 0.1,
    "wheel_track_m": 0.52,
    "wheel_base_m": 0.8
  },
  "control_calibration": {
    "reference_path": [
      {
        "x_m": -2.5,
        "y_m": 0.0,
        "yaw_rad": 0.0,
        "target_speed_ms": 0.3
      },
      {
        "x_m": 2.5,
        "y_m": 0.0,
        "yaw_rad": 0.0,
        "target_speed_ms": 0.3
      }
    ],
    "parameter_version": "isaac_control_baseline_v1"
  },
  "external_truth": {
@@ -682,7 +665,36 @@
    "workcell_zone_id": "isaac_workcell_zone_a",
    "expected_position_stddev_m": 0.01,
    "expected_yaw_stddev_rad": 0.01,
-    "expected_time_sync_offset_ms": 2.0
+    "expected_time_sync_offset_ms": 2.0,
    "visualization_enabled": false,
    "visualization_root_prim": "/World/ExternalTruthVisualization",
    "target_ball_radius_m": 0.075,
    "target_balls": [
      {
        "ball_id": "front_left",
        "center_in_base_link": {
          "x_m": 0.58,
          "y_m": 0.32,
          "z_m": 1.28
        }
      },
      {
        "ball_id": "front_right",
        "center_in_base_link": {
          "x_m": 0.58,
          "y_m": -0.32,
          "z_m": 1.28
        }
      },
      {
        "ball_id": "rear_center",
        "center_in_base_link": {
          "x_m": -0.5,
          "y_m": 0.0,
          "z_m": 1.22
        }
      }
    ]
  },
  "sensors": [
    {
@@ -61,28 +61,11 @@
  },
  "chassis_calibration": {
    "chassis_type": "ackermann",
    "straight_track_length_m": 5.0,
    "straight_track_width_m": 0.7,
    "arc_track_radius_m": 1.6,
    "wheel_radius_m": 0.1,
    "wheel_track_m": 0.52,
    "wheel_base_m": 0.8
  },
  "control_calibration": {
    "reference_path": [
      {
        "x_m": -2.5,
        "y_m": 0.0,
        "yaw_rad": 0.0,
        "target_speed_ms": 0.3
      },
      {
        "x_m": 2.5,
        "y_m": 0.0,
        "yaw_rad": 0.0,
        "target_speed_ms": 0.3
      }
    ],
    "parameter_version": "isaac_control_baseline_v1"
  },
  "external_truth": {
@@ -91,7 +74,8 @@
    "workcell_zone_id": "isaac_workcell_zone_a",
    "expected_position_stddev_m": 0.01,
    "expected_yaw_stddev_rad": 0.01,
-    "expected_time_sync_offset_ms": 2.0
+    "expected_time_sync_offset_ms": 2.0,
    "visualization_enabled": false
  },
  "sensors": [
    {
@@ -61,28 +61,11 @@
  },
  "chassis_calibration": {
    "chassis_type": "ackermann",
    "straight_track_length_m": 5.0,
    "straight_track_width_m": 0.7,
    "arc_track_radius_m": 1.6,
    "wheel_radius_m": 0.1,
    "wheel_track_m": 0.52,
    "wheel_base_m": 0.8
  },
  "control_calibration": {
    "reference_path": [
      {
        "x_m": -2.5,
        "y_m": 0.0,
        "yaw_rad": 0.0,
        "target_speed_ms": 0.3
      },
      {
        "x_m": 2.5,
        "y_m": 0.0,
        "yaw_rad": 0.0,
        "target_speed_ms": 0.3
      }
    ],
    "parameter_version": "isaac_control_baseline_v1"
  },
  "external_truth": {
@@ -91,7 +74,8 @@
    "workcell_zone_id": "isaac_workcell_zone_a",
    "expected_position_stddev_m": 0.01,
    "expected_yaw_stddev_rad": 0.01,
-    "expected_time_sync_offset_ms": 2.0
+    "expected_time_sync_offset_ms": 2.0,
    "visualization_enabled": false
  },
  "sensors": [
    {
@@ -61,28 +61,11 @@
  },
  "chassis_calibration": {
    "chassis_type": "ackermann",
    "straight_track_length_m": 5.0,
    "straight_track_width_m": 0.7,
    "arc_track_radius_m": 1.6,
    "wheel_radius_m": 0.1,
    "wheel_track_m": 0.52,
    "wheel_base_m": 0.8
  },
  "control_calibration": {
    "reference_path": [
      {
        "x_m": -2.5,
        "y_m": 0.0,
        "yaw_rad": 0.0,
        "target_speed_ms": 0.3
      },
      {
        "x_m": 2.5,
        "y_m": 0.0,
        "yaw_rad": 0.0,
        "target_speed_ms": 0.3
      }
    ],
    "parameter_version": "isaac_control_baseline_v1"
  },
  "external_truth": {
@@ -91,7 +74,8 @@
    "workcell_zone_id": "isaac_workcell_zone_a",
    "expected_position_stddev_m": 0.01,
    "expected_yaw_stddev_rad": 0.01,
-    "expected_time_sync_offset_ms": 2.0
+    "expected_time_sync_offset_ms": 2.0,
    "visualization_enabled": false
  },
  "sensors": [
    {
@@ -61,28 +61,11 @@
  },
  "chassis_calibration": {
    "chassis_type": "ackermann",
    "straight_track_length_m": 5.0,
    "straight_track_width_m": 0.7,
    "arc_track_radius_m": 1.6,
    "wheel_radius_m": 0.1,
    "wheel_track_m": 0.52,
    "wheel_base_m": 0.8
  },
  "control_calibration": {
    "reference_path": [
      {
        "x_m": -2.5,
        "y_m": 0.0,
        "yaw_rad": 0.0,
        "target_speed_ms": 0.3
      },
      {
        "x_m": 2.5,
        "y_m": 0.0,
        "yaw_rad": 0.0,
        "target_speed_ms": 0.3
      }
    ],
    "parameter_version": "isaac_control_baseline_v1"
  },
  "external_truth": {
@@ -91,7 +74,8 @@
    "workcell_zone_id": "isaac_workcell_zone_a",
    "expected_position_stddev_m": 0.01,
    "expected_yaw_stddev_rad": 0.01,
-    "expected_time_sync_offset_ms": 2.0
+    "expected_time_sync_offset_ms": 2.0,
    "visualization_enabled": false
  },
  "sensors": [
    {
@@ -61,28 +61,11 @@
  },
  "chassis_calibration": {
    "chassis_type": "ackermann",
    "straight_track_length_m": 5.0,
    "straight_track_width_m": 0.7,
    "arc_track_radius_m": 1.6,
    "wheel_radius_m": 0.1,
    "wheel_track_m": 0.52,
    "wheel_base_m": 0.8
  },
  "control_calibration": {
    "reference_path": [
      {
        "x_m": -2.5,
        "y_m": 0.0,
        "yaw_rad": 0.0,
        "target_speed_ms": 0.3
      },
      {
        "x_m": 2.5,
        "y_m": 0.0,
        "yaw_rad": 0.0,
        "target_speed_ms": 0.3
      }
    ],
    "parameter_version": "isaac_control_baseline_v1"
  },
  "external_truth": {
@@ -91,7 +74,8 @@
    "workcell_zone_id": "isaac_workcell_zone_a",
    "expected_position_stddev_m": 0.01,
    "expected_yaw_stddev_rad": 0.01,
-    "expected_time_sync_offset_ms": 2.0
+    "expected_time_sync_offset_ms": 2.0,
    "visualization_enabled": false
  },
  "sensors": [
    {
@@ -15,7 +15,7 @@ CONDA_ENV_NAME="${CONDA_ENV_NAME:-AutoCalib_Workshop}"
 ROS_LOG_DIR="${ROS_LOG_DIR:-/tmp/roslog}"
 RUN_LOG_ROOT="${RUN_LOG_ROOT:-${WORKSPACE_DIR}/log/isaac_real_sim_$(date +%Y%m%d_%H%M%S)}"
-HEADLESS=1
+HEADLESS=0
 DO_BUILD=0
 RUN_SMOKE=1
 KEEP_RUNNING=0
@@ -499,6 +499,7 @@ log "工作空间: ${WORKSPACE_DIR}"
 log "ROS_LOG_DIR: ${ROS_LOG_DIR}"
 log "进程日志目录: ${RUN_LOG_ROOT}"
 log "Isaac conda 环境: ${CONDA_ENV_NAME}"
 log "Isaac headless: ${HEADLESS}"
 log "总控底盘/运控模式 use_gateway=${WORKSHOP_USE_GATEWAY_VALUE}"
 if [[ -n "${DATA_INPUT_PARAMS_FILE}" ]]; then
  log "数据输入参数文件: ${DATA_INPUT_PARAMS_FILE}"
@@ -589,13 +590,19 @@ start_bg "workshop-demo" bash -lc "
  source '${WORKSPACE_DIR}/install/setup.bash'
  set -u
  export ROS_LOG_DIR='${ROS_LOG_DIR}'
-  exec ros2 launch win_ubuntu_bridge minimal_workshop_demo.launch.py \
+  launch_args=(
-    use_gateway:=${WORKSHOP_USE_GATEWAY_VALUE} \
+    'use_gateway:=${WORKSHOP_USE_GATEWAY_VALUE}'
-    chassis_host:=127.0.0.1 \
+    'chassis_host:=127.0.0.1'
-    control_host:=127.0.0.1 \
+    'control_host:=127.0.0.1'
-    sensor_registry:=demo_front_camera,demo_down_camera,demo_lidar_3d,demo_lidar_2d,demo_imu \
+    'sensor_registry:=demo_front_camera,demo_down_camera,demo_lidar_3d,demo_lidar_2d,demo_imu'
-    data_input_params_file:='${DATA_INPUT_PARAMS_FILE}' \
+  )
-    dataset_index_file:='${DATASET_INDEX_FILE}'
+  if [[ -n '${DATA_INPUT_PARAMS_FILE}' ]]; then
    launch_args+=('data_input_params_file:=${DATA_INPUT_PARAMS_FILE}')
  fi
  if [[ -n '${DATASET_INDEX_FILE}' ]]; then
    launch_args+=('dataset_index_file:=${DATASET_INDEX_FILE}')
  fi
  exec ros2 launch win_ubuntu_bridge minimal_workshop_demo.launch.py \"\${launch_args[@]}\"
 "
 wait_for_ros_service "/workshop_v2/create_session" 60 || exit 1
@@ -19,6 +19,12 @@ source install/setup.bash
 python3 src/apps/operator_ui/main.py
 ```
 仿真联调时可以让界面直接加载仿真 profile：
 ```bash
 AGV_OPERATOR_SITE_PROFILE=src/deployment/profiles/sim_workshop.yaml python3 src/apps/operator_ui/main.py
 ```
 如果环境里还没有界面依赖：
 ```bash
@@ -50,6 +56,29 @@ pip install -r src/apps/operator_ui/requirements.txt
 8. 点击“开始执行本轮标定”，在“报告”页查看最终结果。
 9. 验证结束后点击“停止现场服务”。
 ## Isaac 中测试底盘选路采集
 如果只是验证“底盘标定流程里能不能按选定路径让车动起来、采集底盘遥测和外部真值”，先启动 Isaac 仿真链路：
 ```bash
 ./run_isaac_real_sim_test.sh --no-smoke --keep-running
 ```
 再启动 UI：
 ```bash
 AGV_OPERATOR_SITE_PROFILE=src/deployment/profiles/sim_workshop.yaml python3 src/apps/operator_ui/main.py
 ```
 在“底盘标定”页选择底盘类型和“参考路径”，然后点击“开始底盘路径采集测试”。该按钮会调用
 `run_chassis_profile_capture.py`，自动传入当前选中的 `reference_path_id`，并把采集结果写到 `/tmp/agv_calib_chassis_sim/ui_*`。
 ## 在 UI 中录制参考路径
 底盘标定、运控参数和传感器标定页的“参考路径”下拉框旁都有“录制”和“停止录制”按钮。点击“录制”后填写中文名称、路径 ID、录制来源、话题、路径类型和目标速度；录制来源可选外部真值定位或底盘遥测。录制时长填 `0` 表示一直录，点击“停止录制”后会收尾并把单条路径 YAML 写入对应模块的 `reference_paths/` 目录。
 底盘路径会按当前底盘类型写入 `allowed_chassis_types`，因此录制完成后只会出现在对应底盘类型下。需要注意的是，“开始底盘路径采集测试”仍然走 `chassis_action_profile.yaml` 的动作原语执行链路；如果一条新录制底盘路径还没有绑定到动作 profile，UI 会阻止直接执行，只把它作为参考路径文件用于路径管理和任务预览。
 ## 注意事项
 - “跳过网络质量检查（仅调试）”只适合本地联调，真实部署默认不要勾选。
@@ -65,6 +94,7 @@ pip install -r src/apps/operator_ui/requirements.txt
 - “车间定位”窗口只显示 3D 位置、本轮计划轨迹和定位数据，不允许修改固定 topic 或定位系统名称。
 - 3D 车间图支持鼠标左键拖动旋转视角，左键双击恢复默认视角。
 - 本轮计划轨迹用绿色点显示，来自当前勾选的底盘动作和运控轨迹参数，不要求操作员额外输入。
 - 底盘标定、运控参数标定和传感器标定页都有“参考路径”下拉框，选项来自对应模块的 `reference_paths/*.yaml`；运控轨迹跟踪会把选中的路径展开为实际下发的轨迹点。
 - 标定流程运行时，界面订阅 `/workshop_v2/events`，上一项任务完成后会自动切到下一项需要动车采集的轨迹。
 - 界面会把明细选择写入本轮任务文件，并让标定流程读取这些任务。
 - 界面通过现有 CLI 和 ROS launch 运行流程，没有绕过总控。
@@ -2,13 +2,26 @@
 from __future__ import annotations
 import os
 from pathlib import Path
 REPO_ROOT = Path(__file__).resolve().parents[3]
-DEFAULT_SITE_PROFILE = REPO_ROOT / "src/deployment/profiles/site_template.yaml"
+_DEFAULT_SITE_PROFILE_RAW = Path(
    os.environ.get("AGV_OPERATOR_SITE_PROFILE", "src/deployment/profiles/site_template.yaml")
 ).expanduser()
 DEFAULT_SITE_PROFILE = (
    _DEFAULT_SITE_PROFILE_RAW.resolve(strict=False)
    if _DEFAULT_SITE_PROFILE_RAW.is_absolute()
    else (REPO_ROOT / _DEFAULT_SITE_PROFILE_RAW).resolve(strict=False)
 )
 DEFAULT_VEHICLE_PROFILE_DIR = REPO_ROOT / "src/deployment/profiles/vehicle_profiles"
 DEFAULT_VEHICLE_PROFILE = DEFAULT_VEHICLE_PROFILE_DIR / "ackermann_default.yaml"
 DEFAULT_REFERENCE_PATH_DIRS = {
    "chassis": REPO_ROOT / "src/site_deployment/workshop_chassis_calibration_real/reference_paths",
    "control": REPO_ROOT / "src/site_deployment/workshop_control_calibration_real/reference_paths",
    "sensor": REPO_ROOT / "src/site_deployment/workshop_sensor_calibration_real/reference_paths",
 }
 VEHICLE_PROFILE_SAVE_EXTENSION = ".ymal"
 DEFAULT_SESSION_CONFIG = Path("/tmp/agv_calib_operator_ui/workshop_session_config.yaml")
 PLACEHOLDER_MARKERS = ("replace_with", "measured_on_site", "session_xxx")
@@ -373,5 +386,3 @@ for sensor_key, sensor_label, subtype, _label, stage_type in SENSOR_TASK_OPTIONS
        SENSOR_STAGE_DISPLAY_NAMES[task_code] = f"{sensor_label}外参标定"
    elif stage_type == STAGE_HAND_EYE:
        SENSOR_STAGE_DISPLAY_NAMES[task_code] = f"{sensor_label}手眼标定"
@@ -80,6 +80,9 @@ class OperatorMainWindow(
        self.generated_session_config: dict[str, Any] = {}
        self.launch_process = QProcess(self)
        self.smoke_process = QProcess(self)
        self.chassis_capture_process = QProcess(self)
        self.reference_path_record_process = QProcess(self)
        self.reference_path_record_module = ""
        self.report_metadata: dict[str, str] = {}
        self.report_stages: list[dict[str, str]] = []
        self.last_unresolved_signature = ""
@@ -97,6 +100,8 @@ class OperatorMainWindow(
        self._setup_process(self.launch_process, "现场服务")
        self._setup_process(self.smoke_process, "标定流程")
        self._setup_process(self.chassis_capture_process, "底盘路径采集")
        self._setup_process(self.reference_path_record_process, "参考路径录制")
        self._build_ui()
        self._connect_live_refresh()
        self.load_profile()
@@ -302,16 +307,24 @@ class OperatorMainWindow(
        self.stop_launch_btn = QPushButton("停止现场服务")
        self.smoke_btn = QPushButton("开始执行本轮标定")
        self.stop_smoke_btn = QPushButton("停止本轮标定")
        self.chassis_capture_btn = QPushButton("开始底盘路径采集测试")
        self.stop_chassis_capture_btn = QPushButton("停止底盘路径采集")
        self.build_config_btn.clicked.connect(self.build_session_config)
        self.launch_btn.clicked.connect(self.start_launch_stack)
        self.stop_launch_btn.clicked.connect(lambda: self.stop_process(self.launch_process, "现场服务"))
        self.smoke_btn.clicked.connect(self.start_smoke_test)
        self.stop_smoke_btn.clicked.connect(lambda: self.stop_process(self.smoke_process, "标定流程"))
        self.chassis_capture_btn.clicked.connect(self.start_chassis_path_capture)
        self.stop_chassis_capture_btn.clicked.connect(
            lambda: self.stop_process(self.chassis_capture_process, "底盘路径采集")
        )
        action_layout.addWidget(self.build_config_btn, 0, 0)
        action_layout.addWidget(self.launch_btn, 0, 1)
        action_layout.addWidget(self.stop_launch_btn, 1, 1)
        action_layout.addWidget(self.smoke_btn, 1, 0)
        action_layout.addWidget(self.stop_smoke_btn, 2, 0, 1, 2)
        action_layout.addWidget(self.chassis_capture_btn, 3, 0)
        action_layout.addWidget(self.stop_chassis_capture_btn, 3, 1)
        layout.addWidget(action_box)
        layout.addStretch(1)
        return panel
@@ -325,10 +338,26 @@ class OperatorMainWindow(
        for value, label in CHASSIS_TYPES:
            self.chassis_type_combo.addItem(label, value)
        self.chassis_type_combo.currentIndexChanged.connect(self.rebuild_chassis_parameter_checks)
        self.chassis_type_combo.currentIndexChanged.connect(self.refresh_reference_path_combos)
        type_row.addWidget(self.chassis_type_combo)
        type_row.addStretch(1)
        layout.addLayout(type_row)
        path_row = QHBoxLayout()
        path_row.addWidget(QLabel("参考路径"))
        self.chassis_reference_path_combo = QComboBox()
        self.chassis_reference_path_combo.currentIndexChanged.connect(self.refresh_command_preview)
        path_row.addWidget(self.chassis_reference_path_combo, 1)
        self.record_chassis_path_btn = QPushButton("录制")
        self.stop_chassis_path_record_btn = QPushButton("停止录制")
        self.record_chassis_path_btn.clicked.connect(lambda: self.start_reference_path_recording("chassis"))
        self.stop_chassis_path_record_btn.clicked.connect(
            lambda: self.stop_process(self.reference_path_record_process, "参考路径录制")
        )
        path_row.addWidget(self.record_chassis_path_btn)
        path_row.addWidget(self.stop_chassis_path_record_btn)
        layout.addLayout(path_row)
        self.chassis_param_area = QWidget()
        self.chassis_param_layout = QVBoxLayout(self.chassis_param_area)
        self.chassis_param_layout.setContentsMargins(0, 0, 0, 0)
@@ -341,6 +370,21 @@ class OperatorMainWindow(
    def _build_control_task_tab(self) -> QWidget:
        tab = QWidget()
        layout = QVBoxLayout(tab)
        path_row = QHBoxLayout()
        path_row.addWidget(QLabel("参考路径"))
        self.control_reference_path_combo = QComboBox()
        self.control_reference_path_combo.currentIndexChanged.connect(self.refresh_command_preview)
        path_row.addWidget(self.control_reference_path_combo, 1)
        self.record_control_path_btn = QPushButton("录制")
        self.stop_control_path_record_btn = QPushButton("停止录制")
        self.record_control_path_btn.clicked.connect(lambda: self.start_reference_path_recording("control"))
        self.stop_control_path_record_btn.clicked.connect(
            lambda: self.stop_process(self.reference_path_record_process, "参考路径录制")
        )
        path_row.addWidget(self.record_control_path_btn)
        path_row.addWidget(self.stop_control_path_record_btn)
        layout.addLayout(path_row)
        self.control_param_checks: dict[str, QCheckBox] = {}
        for option in CONTROL_PARAMETER_OPTIONS:
            check = QCheckBox(f"{option['label']} · {option['axis']} · {option['algorithm']}")
@@ -354,6 +398,21 @@ class OperatorMainWindow(
    def _build_sensor_task_tab(self) -> QWidget:
        tab = QWidget()
        layout = QVBoxLayout(tab)
        path_row = QHBoxLayout()
        path_row.addWidget(QLabel("参考路径"))
        self.sensor_reference_path_combo = QComboBox()
        self.sensor_reference_path_combo.currentIndexChanged.connect(self.refresh_command_preview)
        path_row.addWidget(self.sensor_reference_path_combo, 1)
        self.record_sensor_path_btn = QPushButton("录制")
        self.stop_sensor_path_record_btn = QPushButton("停止录制")
        self.record_sensor_path_btn.clicked.connect(lambda: self.start_reference_path_recording("sensor"))
        self.stop_sensor_path_record_btn.clicked.connect(
            lambda: self.stop_process(self.reference_path_record_process, "参考路径录制")
        )
        path_row.addWidget(self.record_sensor_path_btn)
        path_row.addWidget(self.stop_sensor_path_record_btn)
        layout.addLayout(path_row)
        self.sensor_id_edits: dict[str, QLineEdit] = {
            "front_camera": QLineEdit("demo_front_camera"),
            "down_camera": QLineEdit("demo_down_camera"),
@@ -391,7 +450,7 @@ class OperatorMainWindow(
        chassis_type = self.chassis_type_combo.currentData() or "ackermann"
        for option in CHASSIS_PARAMETER_OPTIONS[str(chassis_type)]:
            check = QCheckBox(f"{option['label']} · {option['target']}")
-            check.setChecked(True)
+            check.setChecked(False)
            check.stateChanged.connect(self.refresh_command_preview)
            self.chassis_param_checks[str(option["code"])] = check
            self.chassis_param_layout.addWidget(check)
@@ -476,6 +535,8 @@ class OperatorMainWindow(
        return panel
    def closeEvent(self, event) -> None:
        self.stop_process(self.reference_path_record_process, "参考路径录制")
        self.stop_process(self.chassis_capture_process, "底盘路径采集")
        self.stop_process(self.smoke_process, "标定流程")
        self.stop_process(self.launch_process, "现场服务")
        if self.localization_spin_timer.isActive():
@@ -4,19 +4,328 @@ from __future__ import annotations
 import re
 import sys
 from datetime import datetime
 from pathlib import Path
 from PySide6.QtCore import QProcess
-from PySide6.QtWidgets import QMessageBox
+from PySide6.QtWidgets import (
    QCheckBox,
    QComboBox,
    QDialog,
    QDialogButtonBox,
    QFormLayout,
    QLineEdit,
    QMessageBox,
    QVBoxLayout,
 )
 try:
-    from .profile_io import launch_arg, shell_join
+    from .constants import REPO_ROOT
    from .profile_io import launch_arg, read_nested, shell_join
    from .ui_helpers import table_item
 except ImportError:
-    from profile_io import launch_arg, shell_join
+    from constants import REPO_ROOT
    from profile_io import launch_arg, read_nested, shell_join
    from ui_helpers import table_item
 REFERENCE_PATH_RECORDER = "src/site_deployment/workshop_reference_paths/record_calibration_reference_path.py"
 RECORD_MODULE_LABELS = {
    "chassis": "底盘标定",
    "control": "运控参数",
    "sensor": "传感器标定",
 }
 RECORD_DEFAULT_PATH_TYPES = {
    "chassis": "recorded",
    "control": "recorded",
    "sensor": "recorded",
 }
 RECORD_DEFAULT_RECOMMENDED_TASKS = {
    "chassis": "recorded",
    "control": "trajectory_tracking",
    "sensor": "sensor_extrinsic",
 }
 class ReferencePathRecordDialog(QDialog):
    def __init__(self, module: str, defaults: dict[str, str], parent=None) -> None:
        super().__init__(parent)
        self.setWindowTitle(f"录制{RECORD_MODULE_LABELS.get(module, module)}参考路径")
        self.module = module
        self.default_topics = {
            "external_pose": defaults.get("external_pose_topic", ""),
            "chassis_telemetry": defaults.get("chassis_telemetry_topic", ""),
        }
        layout = QVBoxLayout(self)
        form = QFormLayout()
        layout.addLayout(form)
        self.display_name_edit = QLineEdit(defaults.get("display_name", ""))
        self.path_id_edit = QLineEdit(defaults.get("path_id", ""))
        self.source_combo = QComboBox()
        self.source_combo.addItem("外部真值定位", "external_pose")
        self.source_combo.addItem("底盘遥测里程计", "chassis_telemetry")
        self.topic_edit = QLineEdit(defaults.get("topic", ""))
        self.path_type_combo = QComboBox()
        for item in defaults.get("path_type_options", "").split(","):
            item = item.strip()
            if item:
                self.path_type_combo.addItem(item)
        self.path_type_combo.setEditable(True)
        current_path_type = defaults.get("path_type", "")
        if current_path_type:
            index = self.path_type_combo.findText(current_path_type)
            if index >= 0:
                self.path_type_combo.setCurrentIndex(index)
            else:
                self.path_type_combo.setEditText(current_path_type)
        self.recommended_task_types_edit = QLineEdit(defaults.get("recommended_task_types", ""))
        self.target_speed_edit = QLineEdit(defaults.get("target_speed_ms", "0.05"))
        self.duration_edit = QLineEdit(defaults.get("duration_sec", "0"))
        self.min_distance_step_edit = QLineEdit(defaults.get("min_distance_step_m", "0.03"))
        self.min_yaw_step_edit = QLineEdit(defaults.get("min_yaw_step_rad", "0.03"))
        self.description_edit = QLineEdit(defaults.get("description", ""))
        self.overwrite_check = QCheckBox("允许覆盖同名路径文件")
        source = defaults.get("source", "external_pose")
        source_index = self.source_combo.findData(source)
        if source_index >= 0:
            self.source_combo.setCurrentIndex(source_index)
        self.source_combo.currentIndexChanged.connect(self._apply_default_topic_for_source)
        form.addRow("中文名称", self.display_name_edit)
        form.addRow("路径 ID", self.path_id_edit)
        form.addRow("录制来源", self.source_combo)
        form.addRow("话题", self.topic_edit)
        form.addRow("路径类型", self.path_type_combo)
        form.addRow("推荐任务类型", self.recommended_task_types_edit)
        form.addRow("目标速度 m/s", self.target_speed_edit)
        form.addRow("录制时长 s", self.duration_edit)
        form.addRow("最小点间距 m", self.min_distance_step_edit)
        form.addRow("最小 yaw 间隔 rad", self.min_yaw_step_edit)
        form.addRow("说明", self.description_edit)
        form.addRow("", self.overwrite_check)
        buttons = QDialogButtonBox(QDialogButtonBox.Ok | QDialogButtonBox.Cancel)
        buttons.accepted.connect(self.accept)
        buttons.rejected.connect(self.reject)
        layout.addWidget(buttons)
    def _apply_default_topic_for_source(self, *_args) -> None:
        source = str(self.source_combo.currentData() or "external_pose")
        self.topic_edit.setText(self.default_topics.get(source, ""))
    def values(self) -> dict[str, object]:
        return {
            "module": self.module,
            "display_name": self.display_name_edit.text().strip(),
            "path_id": self.path_id_edit.text().strip(),
            "source": str(self.source_combo.currentData() or "external_pose"),
            "topic": self.topic_edit.text().strip(),
            "path_type": self.path_type_combo.currentText().strip() or "recorded",
            "recommended_task_types": [
                item.strip()
                for item in re.split(r"[,;，；\s]+", self.recommended_task_types_edit.text().strip())
                if item.strip()
            ],
            "target_speed_ms": self.target_speed_edit.text().strip() or "0.0",
            "duration_sec": self.duration_edit.text().strip() or "0",
            "min_distance_step_m": self.min_distance_step_edit.text().strip() or "0.03",
            "min_yaw_step_rad": self.min_yaw_step_edit.text().strip() or "0.03",
            "description": self.description_edit.text().strip(),
            "overwrite": self.overwrite_check.isChecked(),
        }
 class ProcessReportMixin:
    def _resolved_repo_path(self, raw_path: str) -> str:
        path = Path(raw_path).expanduser()
        if path.is_absolute():
            return str(path.resolve(strict=False))
        return str((REPO_ROOT / path).resolve(strict=False))
    def _resolved_bridge_script(self, raw_path: str) -> str:
        return self._resolved_repo_path(raw_path)
    def _configured_path_or_default(self, keys: tuple[str, ...], default_path: str) -> str:
        raw_path = read_nested(getattr(self, "site_profile", {}), keys, "")
        return self._resolved_repo_path(raw_path or default_path)
    def default_record_topic(self, source: str) -> str:
        if source == "chassis_telemetry":
            return (
                read_nested(self.site_profile, ("vehicle_agent", "chassis_telemetry_topic"))
                or read_nested(self.site_profile, ("chassis_telemetry_bridge", "output_topic"))
                or "/chassis/telemetry"
            )
        return self.external_topic_edit.text().strip() or "/workshop/external_localization/vehicle/pose"
    def reference_path_record_defaults(self, module: str) -> dict[str, str]:
        timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
        selected = self.selected_reference_path(module) if hasattr(self, "selected_reference_path") else None
        recommended = selected.get("recommended_task_types", []) if isinstance(selected, dict) else []
        recommended_text = ",".join(str(item) for item in recommended) if recommended else RECORD_DEFAULT_RECOMMENDED_TASKS[module]
        path_type = str(selected.get("path_type", "")) if isinstance(selected, dict) else ""
        path_type_options = {
            "chassis": "recorded,straight_line,arc,s_curve,in_place_rotation,lateral_translation,diagonal_motion",
            "control": "recorded,straight_line,arc,s_curve,lateral_offset,stop_accuracy",
            "sensor": "recorded,static_station,sampling_line,imu_motion,pose_sweep",
        }[module]
        return {
            "display_name": f"{RECORD_MODULE_LABELS[module]}录制路径 {timestamp}",
            "path_id": f"{module}_recorded_{timestamp}",
            "source": "external_pose",
            "topic": self.default_record_topic("external_pose"),
            "external_pose_topic": self.default_record_topic("external_pose"),
            "chassis_telemetry_topic": self.default_record_topic("chassis_telemetry"),
            "path_type": path_type or RECORD_DEFAULT_PATH_TYPES[module],
            "path_type_options": path_type_options,
            "recommended_task_types": recommended_text,
            "target_speed_ms": "0.05" if module != "chassis" else "0.08",
            "duration_sec": "0",
            "min_distance_step_m": "0.03",
            "min_yaw_step_rad": "0.03",
            "description": "操作台录制参考路径",
        }
    def validate_reference_path_record_values(self, values: dict[str, object]) -> bool:
        path_id = str(values.get("path_id", "")).strip()
        display_name = str(values.get("display_name", "")).strip()
        if not display_name:
            QMessageBox.warning(self, "路径名称缺失", "请填写中文名称。")
            return False
        if not re.fullmatch(r"[A-Za-z0-9][A-Za-z0-9_-]*", path_id):
            QMessageBox.warning(self, "路径 ID 不合法", "路径 ID 只能使用英文、数字、下划线和短横线，且不能以符号开头。")
            return False
        for key, label in [
            ("target_speed_ms", "目标速度"),
            ("duration_sec", "录制时长"),
            ("min_distance_step_m", "最小点间距"),
            ("min_yaw_step_rad", "最小 yaw 间隔"),
        ]:
            try:
                value = float(str(values.get(key, "0")))
            except ValueError:
                QMessageBox.warning(self, "数值格式错误", f"{label} 必须是数字。")
                return False
            if key != "target_speed_ms" and value < 0.0:
                QMessageBox.warning(self, "数值格式错误", f"{label} 不能小于 0。")
                return False
            if key == "target_speed_ms" and value < 0.0:
                QMessageBox.warning(self, "数值格式错误", f"{label} 不能小于 0。")
                return False
        return True
    def build_reference_path_record_command(self, values: dict[str, object]) -> str:
        module = str(values["module"])
        recorder = self._configured_path_or_default(
            ("reference_paths", "recorder"),
            REFERENCE_PATH_RECORDER,
        )
        args = [
            sys.executable,
            recorder,
            "--output-dir",
            str(self.reference_path_dir(module)),
            "--path-id",
            str(values["path_id"]),
            "--display-name",
            str(values["display_name"]),
            "--module-type",
            module,
            "--path-type",
            str(values["path_type"]),
            "--frame-id",
            "workshop",
            "--target-speed-ms",
            str(values["target_speed_ms"]),
            "--duration-sec",
            str(values["duration_sec"]),
            "--min-distance-step-m",
            str(values["min_distance_step_m"]),
            "--min-yaw-step-rad",
            str(values["min_yaw_step_rad"]),
            "--source",
            str(values["source"]),
        ]
        topic = str(values.get("topic", "")).strip()
        if topic:
            args.extend(["--topic", topic])
        description = str(values.get("description", "")).strip()
        if description:
            args.extend(["--description", description])
        for task_type in values.get("recommended_task_types", []) or []:
            args.extend(["--recommended-task-type", str(task_type)])
        if module == "chassis":
            args.extend(["--allowed-chassis-type", str(self.chassis_type_combo.currentData() or "ackermann")])
        if bool(values.get("overwrite", False)):
            args.append("--overwrite")
        return "source install/setup.bash && exec " + shell_join(args)
    def build_chassis_path_capture_command(self) -> str:
        reference_path = self.selected_reference_path("chassis")
        reference_path_id = str(reference_path.get("path_id", "")).strip() if reference_path else ""
        timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
        session_dir = Path("/tmp/agv_calib_chassis_sim") / f"ui_{timestamp}"
        vehicle_id = self.vehicle_id_edit.text().strip() or "demo_agv_001"
        chassis_type = str(self.chassis_type_combo.currentData() or "ackermann")
        chassis_topic = (
            read_nested(self.site_profile, ("vehicle_agent", "chassis_telemetry_topic"))
            or read_nested(self.site_profile, ("chassis_telemetry_bridge", "output_topic"))
            or "/chassis/telemetry"
        )
        ackermann_topic = read_nested(
            self.site_profile,
            ("vehicle_agent", "internal_ackermann_command_topic"),
            f"/vehicle/{vehicle_id}/internal/ackermann_cmd",
        )
        runner = self._configured_path_or_default(
            ("chassis_calibration", "action_profile_capture_runner"),
            "src/site_deployment/workshop_chassis_calibration_real/run_chassis_profile_capture.py",
        )
        config_file = self._configured_path_or_default(
            ("chassis_calibration", "data_config_file"),
            "src/site_deployment/workshop_chassis_calibration_real/config/chassis_data_sim.yaml",
        )
        action_profile = self._configured_path_or_default(
            ("chassis_calibration", "action_profile_file"),
            "src/site_deployment/workshop_chassis_calibration_real/config/chassis_action_profile.yaml",
        )
        args = [
            sys.executable,
            runner,
            "--config",
            config_file,
            "--action-profile",
            action_profile,
            "--chassis-type",
            chassis_type,
            "--session-id",
            f"ui_chassis_path_{timestamp}",
            "--site-id",
            self.workcell_zone_edit.text().strip() or "isaac_workcell_zone_a",
            "--vehicle-id",
            vehicle_id,
            "--session-dir",
            str(session_dir),
            "--dataset-index-path",
            str(session_dir / "dataset_index.yaml"),
            "--chassis-telemetry-topic",
            chassis_topic,
            "--external-pose-topic",
            self.external_topic_edit.text().strip() or "/isaac/external_localization/vehicle/pose",
            "--ackermann-command-topic",
            ackermann_topic,
            "--command-source",
            "auto",
            "--action-server",
            "/chassis/execute_motion_primitive",
        ]
        if reference_path_id:
            args.extend(["--reference-path-id", reference_path_id])
        return "source install/setup.bash && " + shell_join(args)
    def build_launch_command(self) -> str:
        args = [
            "ros2",
@@ -36,6 +345,45 @@ class ProcessReportMixin:
            args.append(launch_arg("data_input_params_file", self.data_input_edit.text().strip()))
        if self.dataset_index_edit.text().strip():
            args.append(launch_arg("dataset_index_file", self.dataset_index_edit.text().strip()))
        bridge_config = getattr(self, "site_profile", {}).get("chassis_telemetry_bridge", {})
        if isinstance(bridge_config, dict) and bridge_config:
            bridge_script = read_nested(
                self.site_profile,
                ("chassis_telemetry_bridge", "bridge_tool"),
                "src/site_deployment/workshop_chassis_calibration_real/workshop_chassis_telemetry_bridge.py",
            )
            args.extend([
                launch_arg("enable_chassis_telemetry_bridge", "true"),
                launch_arg("chassis_telemetry_bridge_script", self._resolved_bridge_script(bridge_script)),
                launch_arg(
                    "chassis_telemetry_bind_host",
                    read_nested(self.site_profile, ("chassis_telemetry_bridge", "bind_host"), "0.0.0.0"),
                ),
                launch_arg(
                    "chassis_telemetry_bind_port",
                    read_nested(self.site_profile, ("chassis_telemetry_bridge", "bind_port"), "9010"),
                ),
                launch_arg(
                    "chassis_telemetry_protocol",
                    read_nested(self.site_profile, ("chassis_telemetry_bridge", "protocol"), "frame"),
                ),
                launch_arg(
                    "chassis_telemetry_expected_msg_type",
                    read_nested(self.site_profile, ("chassis_telemetry_bridge", "expected_msg_type"), "0"),
                ),
                launch_arg(
                    "chassis_telemetry_topic",
                    read_nested(self.site_profile, ("chassis_telemetry_bridge", "output_topic"), "/chassis/telemetry"),
                ),
                launch_arg(
                    "chassis_telemetry_default_chassis_type",
                    read_nested(self.site_profile, ("chassis_telemetry_bridge", "default_chassis_type"), "ackermann"),
                ),
                launch_arg(
                    "chassis_telemetry_max_payload_bytes",
                    read_nested(self.site_profile, ("chassis_telemetry_bridge", "max_payload_bytes"), "262144"),
                ),
            ])
        return "source install/setup.bash && " + shell_join(args)
    def build_smoke_command(self) -> str:
@@ -61,6 +409,7 @@ class ProcessReportMixin:
            "240",
            "--service-timeout-sec",
            "10",
            "--verbose-feedback",
        ]
        if self.vehicle_profile_path_edit.text().strip():
            args.extend(["--vehicle-profile-file", self.vehicle_profile_path_edit.text().strip()])
@@ -115,6 +464,61 @@ class ProcessReportMixin:
        self.smoke_state.setText("标定流程：运行中")
        self.smoke_process.start("bash", ["-lc", command])
    def start_chassis_path_capture(self) -> None:
        if self.chassis_capture_process.state() != QProcess.NotRunning:
            QMessageBox.warning(self, "底盘路径采集已运行", "底盘路径采集进程已经在运行。")
            return
        if not self.selected_reference_path("chassis"):
            QMessageBox.warning(self, "未选择路径", "请先在底盘标定页选择参考路径。")
            return
        reference_path = self.selected_reference_path("chassis")
        reference_path_id = str(reference_path.get("path_id", "")).strip() if reference_path else ""
        chassis_type = str(self.chassis_type_combo.currentData() or "ackermann")
        if reference_path_id not in self.allowed_chassis_reference_path_ids(chassis_type):
            QMessageBox.warning(
                self,
                "路径未绑定动作",
                "这条路径是录制参考路径，但还没有绑定到底盘动作 profile，不能直接用于“底盘路径采集测试”。"
                "请先在 chassis_action_profile.yaml 中为当前底盘类型添加对应动作，或仅用于任务预览/路径管理。",
            )
            return
        if self.launch_process.state() == QProcess.NotRunning:
            reply = QMessageBox.question(
                self,
                "现场服务未运行",
                "当前界面没有检测到由本界面启动的现场服务。若你已经用脚本启动 Isaac 仿真链路，可以继续执行底盘路径采集测试。",
            )
            if reply != QMessageBox.Yes:
                return
        command = self.build_chassis_path_capture_command()
        self.append_log("[底盘路径采集] 启动：" + command)
        self.smoke_state.setText("底盘路径采集：运行中")
        self.chassis_capture_process.start("bash", ["-lc", command])
    def start_reference_path_recording(self, module: str) -> None:
        if self.reference_path_record_process.state() != QProcess.NotRunning:
            QMessageBox.warning(self, "参考路径录制已运行", "参考路径录制进程已经在运行。")
            return
        if self.launch_process.state() == QProcess.NotRunning:
            reply = QMessageBox.question(
                self,
                "现场服务未运行",
                "当前界面没有检测到由本界面启动的现场服务。若你已经用脚本启动 Isaac 或 ROS 现场链路，可以继续录制参考路径。",
            )
            if reply != QMessageBox.Yes:
                return
        dialog = ReferencePathRecordDialog(module, self.reference_path_record_defaults(module), self)
        if dialog.exec() != QDialog.Accepted:
            return
        values = dialog.values()
        if not self.validate_reference_path_record_values(values):
            return
        command = self.build_reference_path_record_command(values)
        self.reference_path_record_module = module
        self.append_log("[参考路径录制] 启动：" + command)
        self.smoke_state.setText("参考路径录制：运行中")
        self.reference_path_record_process.start("bash", ["-lc", command])
    def stop_process(self, process: QProcess, name: str) -> None:
        if process.state() == QProcess.NotRunning:
            self.append_log(f"[{name}] 当前没有运行中的进程。")
@@ -142,11 +546,16 @@ class ProcessReportMixin:
        self.append_log(f"[{name}] {status_text}")
        if name == "现场服务":
            self.stack_state.setText("现场服务：未启动")
-        else:
+        elif name == "标定流程":
            self.trajectory_runtime_active = False
            self.trajectory_last_completed_stage_id = ""
            self.refresh_planned_trajectory()
            self.smoke_state.setText("标定流程：空闲")
        elif name == "底盘路径采集":
            self.smoke_state.setText("底盘路径采集：空闲")
        elif name == "参考路径录制":
            self.smoke_state.setText("参考路径录制：空闲")
            self.refresh_reference_path_combos()
    def append_log(self, text: str) -> None:
        self.log_view.appendPlainText(text)
@@ -380,3 +380,5 @@ class ProfileHandlersMixin:
        self.vehicle_profile_path_edit.setText(str(resolve_vehicle_profile_path(vehicle_profile_file)))
        if Path(self.vehicle_profile_path_edit.text()).exists():
            self.load_vehicle_profile_from_current_path()
        if hasattr(self, "refresh_reference_path_combos"):
            self.refresh_reference_path_combos()
@@ -13,6 +13,7 @@ try:
    from .constants import (
        CHASSIS_PARAMETER_OPTIONS,
        CONTROL_PARAMETER_OPTIONS,
        DEFAULT_REFERENCE_PATH_DIRS,
        DEFAULT_VEHICLE_PROFILE,
        POLICY_DISPLAY_NAMES,
        REPO_ROOT,
@@ -41,6 +42,7 @@ except ImportError:
    from constants import (
        CHASSIS_PARAMETER_OPTIONS,
        CONTROL_PARAMETER_OPTIONS,
        DEFAULT_REFERENCE_PATH_DIRS,
        DEFAULT_VEHICLE_PROFILE,
        POLICY_DISPLAY_NAMES,
        REPO_ROOT,
@@ -89,6 +91,219 @@ class SessionBuilderMixin:
        tasks = self.selected_tasks()
        return ",".join(tasks) if tasks else "external"
    def reference_path_dir(self, module: str) -> Path:
        config_keys = {
            "chassis": ("chassis_calibration", "reference_path_dir"),
            "control": ("control_calibration", "reference_path_dir"),
            "sensor": ("sensor_calibration", "reference_path_dir"),
        }
        raw_path = read_nested(self.site_profile, config_keys[module])
        if raw_path:
            path = Path(raw_path).expanduser()
            if path.is_absolute():
                return path.resolve(strict=False)
            profile_path = resolve_profile_path(self.profile_path_edit.text().strip())
            return (profile_path.parent / path).resolve(strict=False)
        return DEFAULT_REFERENCE_PATH_DIRS[module]
    def resolve_site_or_repo_path(self, raw_path: str) -> Path:
        path = Path(raw_path).expanduser()
        if path.is_absolute():
            return path.resolve(strict=False)
        profile_path = resolve_profile_path(self.profile_path_edit.text().strip())
        profile_relative = (profile_path.parent / path).resolve(strict=False)
        if profile_relative.exists():
            return profile_relative
        return (REPO_ROOT / path).resolve(strict=False)
    def chassis_action_profile_path(self) -> Path:
        raw_path = read_nested(
            self.site_profile,
            ("chassis_calibration", "action_profile_file"),
            "src/site_deployment/workshop_chassis_calibration_real/config/chassis_action_profile.yaml",
        )
        return self.resolve_site_or_repo_path(raw_path)
    def allowed_chassis_reference_path_ids(self, chassis_type: str) -> set[str]:
        profile_path = self.chassis_action_profile_path()
        if not profile_path.exists():
            return set()
        try:
            profile = load_yaml(profile_path)
        except Exception as exc:
            self.append_log(f"[参考路径] 底盘动作 profile 加载失败 {profile_path}: {exc}")
            return set()
        section = profile.get("chassis_profiles", {}).get(chassis_type, {})
        actions = section.get("actions", []) if isinstance(section, dict) else []
        if not isinstance(actions, list):
            return set()
        return {
            str(action.get("reference_path_id", "")).strip()
            for action in actions
            if isinstance(action, dict) and str(action.get("reference_path_id", "")).strip()
        }
    def load_reference_path_options(self, module: str) -> tuple[dict[str, Any], list[dict[str, Any]]]:
        path_dir = self.reference_path_dir(module)
        index_path = path_dir / "index.yaml"
        index = load_yaml(index_path) if index_path.exists() else {}
        paths: list[dict[str, Any]] = []
        if not path_dir.exists():
            return index, paths
        for path_file in sorted(path_dir.glob("*.yaml")):
            if path_file.name in {"index.yaml", "_index.yaml"}:
                continue
            try:
                payload = load_yaml(path_file)
            except Exception as exc:
                self.append_log(f"[参考路径] 跳过 {path_file}: {exc}")
                continue
            path_id = str(payload.get("path_id", "")).strip()
            display_name = str(payload.get("display_name", "")).strip()
            points = payload.get("points", [])
            if not path_id or not display_name or not isinstance(points, list) or not points:
                self.append_log(f"[参考路径] 跳过无效路径文件: {path_file}")
                continue
            payload["_source_file"] = str(path_file.resolve(strict=False))
            payload["_path_dir"] = str(path_dir.resolve(strict=False))
            paths.append(payload)
        if module == "chassis":
            chassis_type = (
                str(self.chassis_type_combo.currentData() or "ackermann")
                if hasattr(self, "chassis_type_combo")
                else "ackermann"
            )
            allowed_ids = self.allowed_chassis_reference_path_ids(chassis_type)
            if allowed_ids:
                def recorded_path_chassis_types(path: dict[str, Any]) -> set[str]:
                    raw_types = path.get("allowed_chassis_types", []) or []
                    if isinstance(raw_types, str):
                        return {raw_types}
                    if isinstance(raw_types, list):
                        return {str(item) for item in raw_types}
                    return set()
                paths = [
                    path
                    for path in paths
                    if str(path.get("path_id", "")).strip() in allowed_ids
                    or chassis_type in recorded_path_chassis_types(path)
                ]
        return index, paths
    def default_reference_path_id(self, module: str, index: dict[str, Any], paths: list[dict[str, Any]]) -> str:
        chassis_type = str(self.chassis_type_combo.currentData() or "ackermann") if hasattr(self, "chassis_type_combo") else "ackermann"
        by_chassis = index.get("default_path_id_by_chassis", {}) if isinstance(index, dict) else {}
        by_task = index.get("default_path_id_by_task_type", {}) if isinstance(index, dict) else {}
        if module in {"chassis", "control"} and isinstance(by_chassis, dict):
            default_id = str(by_chassis.get(chassis_type, "")).strip()
            if default_id:
                return default_id
        if isinstance(by_task, dict):
            task_key = {
                "chassis": "straight_line",
                "control": "trajectory_tracking",
                "sensor": "camera_intrinsic",
            }[module]
            default_id = str(by_task.get(task_key, "")).strip()
            if default_id:
                return default_id
        return str(paths[0].get("path_id", "")) if paths else ""
    def refresh_reference_path_combos(self) -> None:
        combo_specs = {
            "chassis": "chassis_reference_path_combo",
            "control": "control_reference_path_combo",
            "sensor": "sensor_reference_path_combo",
        }
        for module, attr_name in combo_specs.items():
            if not hasattr(self, attr_name):
                continue
            combo = getattr(self, attr_name)
            previous = combo.currentData()
            previous_id = str(previous.get("path_id", "")) if isinstance(previous, dict) else ""
            index, paths = self.load_reference_path_options(module)
            ids = {str(path.get("path_id", "")) for path in paths}
            selected_id = previous_id if previous_id in ids else self.default_reference_path_id(module, index, paths)
            if selected_id not in ids and paths:
                selected_id = str(paths[0].get("path_id", ""))
            combo.blockSignals(True)
            combo.clear()
            if not paths:
                combo.addItem("未找到参考路径", None)
                combo.setEnabled(False)
            else:
                combo.setEnabled(True)
                for path in paths:
                    label = f"{path.get('display_name', path.get('path_id'))} ({path.get('path_id')})"
                    combo.addItem(label, path)
                    if str(path.get("path_id", "")) == selected_id:
                        combo.setCurrentIndex(combo.count() - 1)
            combo.blockSignals(False)
        if hasattr(self, "launch_command_preview"):
            self.refresh_command_preview()
    def selected_reference_path(self, module: str) -> dict[str, Any] | None:
        combo = getattr(self, f"{module}_reference_path_combo", None)
        if combo is None:
            return None
        path = combo.currentData()
        return path if isinstance(path, dict) else None
    @staticmethod
    def stringify_reference_value(value: Any) -> str:
        if isinstance(value, bool):
            return "true" if value else "false"
        if isinstance(value, float):
            return f"{value:.9g}"
        return str(value)
    def add_reference_path_metadata(self, params: dict[str, Any], path: dict[str, Any] | None) -> None:
        if not path:
            return
        points = path.get("points", [])
        if not isinstance(points, list):
            return
        params["reference_path.id"] = path.get("path_id", "")
        params["reference_path.display_name"] = path.get("display_name", "")
        params["reference_path.frame_id"] = path.get("frame_id", "workshop")
        params["reference_path.path_type"] = path.get("path_type", "polyline")
        params["reference_path.point_count"] = len(points)
        if path.get("_path_dir"):
            params["reference_path.path_dir"] = path["_path_dir"]
        if path.get("_source_file"):
            params["reference_path.file"] = path["_source_file"]
        if path.get("module_type"):
            params["reference_path.module_type"] = path["module_type"]
        if path.get("description"):
            params["reference_path.description"] = path["description"]
        for index, point in enumerate(points):
            if not isinstance(point, dict):
                continue
            prefix = f"reference_path.pt_{index}"
            params[f"{prefix}_x_m"] = self.stringify_reference_value(point.get("x_m", 0.0))
            params[f"{prefix}_y_m"] = self.stringify_reference_value(point.get("y_m", 0.0))
            params[f"{prefix}_z_m"] = self.stringify_reference_value(point.get("z_m", 0.0))
            params[f"{prefix}_yaw_rad"] = self.stringify_reference_value(point.get("yaw_rad", 0.0))
            params[f"{prefix}_speed_ms"] = self.stringify_reference_value(point.get("target_speed_ms", 0.0))
    def apply_reference_path_as_trajectory(self, params: dict[str, Any], path: dict[str, Any] | None) -> None:
        if not path:
            return
        points = path.get("points", [])
        if not isinstance(points, list) or len(points) < 2:
            return
        for key in list(params):
            if key.startswith("traj_pt_"):
                del params[key]
        for index, point in enumerate(points):
            if not isinstance(point, dict):
                continue
            params[f"traj_pt_{index}_x_m"] = self.stringify_reference_value(point.get("x_m", 0.0))
            params[f"traj_pt_{index}_y_m"] = self.stringify_reference_value(point.get("y_m", 0.0))
            params[f"traj_pt_{index}_yaw_rad"] = self.stringify_reference_value(point.get("yaw_rad", 0.0))
            params[f"traj_pt_{index}_speed_ms"] = self.stringify_reference_value(point.get("target_speed_ms", 0.0))
    def refresh_all(self) -> None:
        self.refresh_command_preview()
        self.refresh_task_preview_from_config()
@@ -158,6 +373,10 @@ class SessionBuilderMixin:
    def trajectory_segments_from_task_params(self, params: dict[str, str]) -> list[list[tuple[float, float, float]]]:
        segments: list[list[tuple[float, float, float]]] = []
        segment = self.trajectory_points_from_reference_path(params)
        if segment:
            segments.append(segment)
            return segments
        segment = self.trajectory_points_from_explicit_params(params)
        if segment:
            segments.append(segment)
@@ -212,6 +431,21 @@ class SessionBuilderMixin:
                indexed_points[index] = (x, y)
        return [(x, y, 0.04) for _, (x, y) in sorted(indexed_points.items())]
    def trajectory_points_from_reference_path(self, params: dict[str, str]) -> list[tuple[float, float, float]]:
        indexes = sorted({
            int(match.group(1))
            for key in params
            if (match := re.fullmatch(r"reference_path\.pt_(\d+)_x_m", key))
        })
        points: list[tuple[float, float, float]] = []
        for index in indexes:
            x = float_or_none(params.get(f"reference_path.pt_{index}_x_m"))
            y = float_or_none(params.get(f"reference_path.pt_{index}_y_m"))
            z = float_or_none(params.get(f"reference_path.pt_{index}_z_m"))
            if x is not None and y is not None:
                points.append((x, y, z if z is not None else 0.04))
        return points
    def trajectory_points_from_motion_primitive(self, params: dict[str, str]) -> list[tuple[float, float, float]]:
        primitive_type = params.get("primitive_type", "")
        if primitive_type == "straight_line":
@@ -268,6 +502,7 @@ class SessionBuilderMixin:
        if not hasattr(self, "chassis_type_combo"):
            return []
        chassis_type = str(self.chassis_type_combo.currentData() or "ackermann")
        reference_path = self.selected_reference_path("chassis")
        tasks: list[dict[str, Any]] = []
        for option in CHASSIS_PARAMETER_OPTIONS[chassis_type]:
            check = self.chassis_param_checks.get(str(option["code"]))
@@ -280,6 +515,7 @@ class SessionBuilderMixin:
            }
            params.setdefault("brake_when_finished", "true")
            params.setdefault("timeout_sec", "20.0")
            self.add_reference_path_metadata(params, reference_path)
            tasks.append(self.make_task(
                STAGE_CHASSIS,
                str(option["code"]),
@@ -292,6 +528,7 @@ class SessionBuilderMixin:
    def selected_control_tasks(self) -> list[dict[str, Any]]:
        if not hasattr(self, "control_param_checks"):
            return []
        reference_path = self.selected_reference_path("control")
        tasks: list[dict[str, Any]] = []
        for option in CONTROL_PARAMETER_OPTIONS:
            check = self.control_param_checks.get(str(option["code"]))
@@ -308,6 +545,8 @@ class SessionBuilderMixin:
                    "trajectory_tracking.required_external_pose_source_id",
                    self.reference_source_edit.text().strip(),
                )
                self.apply_reference_path_as_trajectory(params, reference_path)
            self.add_reference_path_metadata(params, reference_path)
            tasks.append(self.make_task(
                STAGE_CONTROL,
                str(option["code"]),
@@ -320,6 +559,7 @@ class SessionBuilderMixin:
    def selected_sensor_tasks(self) -> list[dict[str, Any]]:
        if not hasattr(self, "sensor_task_checks"):
            return []
        reference_path = self.selected_reference_path("sensor")
        tasks: list[dict[str, Any]] = []
        for sensor_key, sensor_label, subtype, label, stage_type in SENSOR_TASK_OPTIONS:
            check = self.sensor_task_checks.get((sensor_key, subtype))
@@ -358,6 +598,7 @@ class SessionBuilderMixin:
                    "hand_eye.required_pose_count": "1",
                    "hand_eye.timeout_sec": "5.0",
                })
            self.add_reference_path_metadata(params, reference_path)
            tasks.append(self.make_task(
                stage_type,
                f"sensor.{sensor_key}.{subtype}",
@@ -1,5 +1,7 @@
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction
+import sys
 from launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
@@ -13,6 +15,8 @@ from launch_ros.actions import Node
 #       传感器 readiness 的最小真实配置入口。
 #   external_telemetry_topic / expected_reference_source_name / expected_workcell_zone_id：
 #       外部真值定位源的现场配置入口。
 #   enable_chassis_telemetry_bridge / chassis_telemetry_*：
 #       车间电脑侧 WiFi/TCP 底盘遥测接收桥，发布 /chassis/telemetry。
 #   data_input_params_file：可选 ROS 参数文件，用于把真实采集/ingest 产物路径注入算法输入。
 #   dataset_index_file：可选数据集索引文件；为空时主控会按 data_input_params_file 同目录自动查找。
 def generate_launch_description():
@@ -29,6 +33,16 @@ def generate_launch_description():
        "control_host", default_value="192.168.1.100",
        description="Windows 车端 IP（use_gateway=true 时生效）",
    )
    chassis_timeout_ms_arg = DeclareLaunchArgument(
        "chassis_timeout_ms",
        default_value="120000",
        description="底盘网关 TCP 请求超时，动作原语需要覆盖完整运动时长",
    )
    control_timeout_ms_arg = DeclareLaunchArgument(
        "control_timeout_ms",
        default_value="120000",
        description="运控网关 TCP 请求超时，轨迹评估需要覆盖完整运动时长",
    )
    profile_storage_path_arg = DeclareLaunchArgument(
        "profile_storage_path",
        default_value="/tmp/agv_calib_vehicle_profiles.db",
@@ -89,11 +103,63 @@ def generate_launch_description():
        default_value="",
        description="external_localization_service 期望的工位区域 ID，留空表示不校验",
    )
    enable_chassis_telemetry_bridge_arg = DeclareLaunchArgument(
        "enable_chassis_telemetry_bridge",
        default_value="false",
        description="true: 启动车间电脑侧 WiFi/TCP 底盘遥测 bridge，发布 chassis_telemetry_topic",
    )
    chassis_telemetry_bridge_script_arg = DeclareLaunchArgument(
        "chassis_telemetry_bridge_script",
        default_value="src/site_deployment/workshop_chassis_calibration_real/workshop_chassis_telemetry_bridge.py",
        description="车间电脑侧底盘遥测 bridge Python 脚本路径",
    )
    chassis_telemetry_bind_host_arg = DeclareLaunchArgument(
        "chassis_telemetry_bind_host",
        default_value="0.0.0.0",
        description="底盘遥测 TCP 监听地址",
    )
    chassis_telemetry_bind_port_arg = DeclareLaunchArgument(
        "chassis_telemetry_bind_port",
        default_value="9010",
        description="底盘遥测 TCP 监听端口",
    )
    chassis_telemetry_protocol_arg = DeclareLaunchArgument(
        "chassis_telemetry_protocol",
        default_value="frame",
        description="底盘遥测包协议：frame/json_lines/raw_json/auto",
    )
    chassis_telemetry_expected_msg_type_arg = DeclareLaunchArgument(
        "chassis_telemetry_expected_msg_type",
        default_value="0",
        description="frame 协议期望 msg_type；0 表示接受任意类型",
    )
    chassis_telemetry_topic_arg = DeclareLaunchArgument(
        "chassis_telemetry_topic",
        default_value="/chassis/telemetry",
        description="底盘遥测 bridge 输出 topic",
    )
    chassis_telemetry_default_chassis_type_arg = DeclareLaunchArgument(
        "chassis_telemetry_default_chassis_type",
        default_value="ackermann",
        description="底盘遥测包未携带 chassis_type 时使用的默认车型",
    )
    chassis_telemetry_max_payload_bytes_arg = DeclareLaunchArgument(
        "chassis_telemetry_max_payload_bytes",
        default_value="262144",
        description="底盘遥测单包最大 payload 字节数",
    )
    chassis_telemetry_send_ack_arg = DeclareLaunchArgument(
        "chassis_telemetry_send_ack",
        default_value="true",
        description="true: bridge 收到遥测后向 TCP 连接回 ACK",
    )
    def make_nodes(context):
        use_gateway = LaunchConfiguration("use_gateway").perform(context).lower() == "true"
        chassis_host = LaunchConfiguration("chassis_host").perform(context)
        control_host = LaunchConfiguration("control_host").perform(context)
        chassis_timeout_ms = int(LaunchConfiguration("chassis_timeout_ms").perform(context))
        control_timeout_ms = int(LaunchConfiguration("control_timeout_ms").perform(context))
        profile_storage_path = LaunchConfiguration("profile_storage_path").perform(context)
        sensor_storage_root = LaunchConfiguration("sensor_storage_root").perform(context)
        sensor_registry = LaunchConfiguration("sensor_registry").perform(context)
@@ -106,6 +172,26 @@ def generate_launch_description():
        external_telemetry_topic = LaunchConfiguration("external_telemetry_topic").perform(context).strip()
        expected_reference_source_name = LaunchConfiguration("expected_reference_source_name").perform(context).strip()
        expected_workcell_zone_id = LaunchConfiguration("expected_workcell_zone_id").perform(context).strip()
        enable_chassis_telemetry_bridge = (
            LaunchConfiguration("enable_chassis_telemetry_bridge").perform(context).lower() == "true"
        )
        chassis_telemetry_bridge_script = LaunchConfiguration("chassis_telemetry_bridge_script").perform(context).strip()
        chassis_telemetry_bind_host = LaunchConfiguration("chassis_telemetry_bind_host").perform(context).strip()
        chassis_telemetry_bind_port = LaunchConfiguration("chassis_telemetry_bind_port").perform(context).strip()
        chassis_telemetry_protocol = LaunchConfiguration("chassis_telemetry_protocol").perform(context).strip()
        chassis_telemetry_expected_msg_type = (
            LaunchConfiguration("chassis_telemetry_expected_msg_type").perform(context).strip()
        )
        chassis_telemetry_topic = LaunchConfiguration("chassis_telemetry_topic").perform(context).strip()
        chassis_telemetry_default_chassis_type = (
            LaunchConfiguration("chassis_telemetry_default_chassis_type").perform(context).strip()
        )
        chassis_telemetry_max_payload_bytes = (
            LaunchConfiguration("chassis_telemetry_max_payload_bytes").perform(context).strip()
        )
        chassis_telemetry_send_ack = (
            LaunchConfiguration("chassis_telemetry_send_ack").perform(context).lower() == "true"
        )
        def with_data_input_params(params=None):
            merged = []
@@ -172,10 +258,10 @@ def generate_launch_description():
                    parameters=[{
                        "chassis_host": chassis_host,
                        "chassis_port": 9000,
-                        "chassis_timeout_ms": 5000,
+                        "chassis_timeout_ms": chassis_timeout_ms,
                        "control_host": control_host,
                        "control_port": 9000,
-                        "control_timeout_ms": 5000,
+                        "control_timeout_ms": control_timeout_ms,
                    }],
                ),
            ]
@@ -198,12 +284,44 @@ def generate_launch_description():
                ),
            ]
-        return common_nodes + chassis_control_nodes
+        telemetry_bridge_nodes = []
        if enable_chassis_telemetry_bridge:
            ack_arg = "--send-ack" if chassis_telemetry_send_ack else "--no-send-ack"
            telemetry_bridge_nodes.append(
                ExecuteProcess(
                    cmd=[
                        sys.executable,
                        chassis_telemetry_bridge_script,
                        "--bind-host",
                        chassis_telemetry_bind_host,
                        "--bind-port",
                        chassis_telemetry_bind_port,
                        "--protocol",
                        chassis_telemetry_protocol,
                        "--expected-msg-type",
                        chassis_telemetry_expected_msg_type,
                        "--output-topic",
                        chassis_telemetry_topic,
                        "--default-chassis-type",
                        chassis_telemetry_default_chassis_type,
                        "--max-payload-bytes",
                        chassis_telemetry_max_payload_bytes,
                        ack_arg,
                    ],
                    name="workshop_chassis_telemetry_bridge",
                    output="screen",
                    emulate_tty=True,
                )
            )
        return common_nodes + chassis_control_nodes + telemetry_bridge_nodes
    return LaunchDescription([
        use_gateway_arg,
        chassis_host_arg,
        control_host_arg,
        chassis_timeout_ms_arg,
        control_timeout_ms_arg,
        profile_storage_path_arg,
        sensor_storage_root_arg,
        sensor_registry_arg,
@@ -216,5 +334,15 @@ def generate_launch_description():
        external_telemetry_topic_arg,
        expected_reference_source_name_arg,
        expected_workcell_zone_id_arg,
        enable_chassis_telemetry_bridge_arg,
        chassis_telemetry_bridge_script_arg,
        chassis_telemetry_bind_host_arg,
        chassis_telemetry_bind_port_arg,
        chassis_telemetry_protocol_arg,
        chassis_telemetry_expected_msg_type_arg,
        chassis_telemetry_topic_arg,
        chassis_telemetry_default_chassis_type_arg,
        chassis_telemetry_max_payload_bytes_arg,
        chassis_telemetry_send_ack_arg,
        OpaqueFunction(function=make_nodes),
    ])
@@ -50,9 +50,14 @@ vehicle_sensor_agent:
 external_pose_bridge:
  type: external_pose_wifi6_bridge
  source_topic: /isaac/external_localization/vehicle/pose
-  reference_source_name: isaac_external_truth
+  reference_source_name: isaac_sim_truth_source
  max_hz: 30.0
 external_localization:
  output_topic: /isaac/external_localization/vehicle/pose
  reference_source_name: isaac_sim_truth_source
  workcell_zone_id: isaac_workcell_zone_a
 workshop_sensor_ingest:
  type: workshop_sensor_ingest_sim
  publish_prefix: /workshop/vehicle_sensor
@@ -29,6 +29,17 @@ vehicle_agent:
  wheel_base_m: measured_on_site
  max_steering_angle_rad: measured_on_site
 chassis_telemetry_bridge:
  type: wifi6_tcp_chassis_telemetry_bridge
  bridge_tool: src/site_deployment/workshop_chassis_calibration_real/workshop_chassis_telemetry_bridge.py
  bind_host: 0.0.0.0
  bind_port: 9010
  protocol: frame
  expected_msg_type: 0
  output_topic: /chassis/telemetry
  default_chassis_type: ackermann
  max_payload_bytes: 262144
 vehicle_sensor_agent:
  type: real_vehicle_sensor_agent
  front_camera_sensor_id: replace_with_front_camera_id
@@ -64,6 +75,7 @@ chassis_calibration:
  chassis_type: ackermann
  data_config_file: /data/agv_calib/replace_with_site_or_line_id/chassis_data.yaml
  action_profile_file: /data/agv_calib/replace_with_site_or_line_id/chassis_action_profile.yaml
  reference_path_dir: /data/agv_calib/replace_with_site_or_line_id/chassis_reference_paths
  action_profile_tool: src/site_deployment/workshop_chassis_calibration_real/chassis_action_profile.py
  action_profile_capture_runner: src/site_deployment/workshop_chassis_calibration_real/run_chassis_profile_capture.py
  capture_tool: src/site_deployment/workshop_chassis_calibration_real/capture_chassis_session.py
@@ -89,6 +101,7 @@ control_calibration:
  data_config_file: /data/agv_calib/replace_with_site_or_line_id/control_data.yaml
  capture_tool: src/site_deployment/workshop_control_calibration_real/capture_control_session.py
  evaluation_profile_file: src/site_deployment/workshop_control_calibration_real/config/control_evaluation_profile.yaml
  reference_path_dir: /data/agv_calib/replace_with_site_or_line_id/control_reference_paths
  evaluation_profile_tool: src/site_deployment/workshop_control_calibration_real/control_evaluation_profile.py
  evaluation_profile_capture_runner: src/site_deployment/workshop_control_calibration_real/run_control_profile_capture.py
  evaluation_profile_capture_smoke_tool: src/site_deployment/workshop_control_calibration_real/smoke_test_control_profile_capture.py
@@ -115,6 +128,7 @@ sensor_calibration:
  # 这里只定义传感器标定算法的数据输入、任务 profile 和参数交接产物。
  type: workshop_sensor_data
  profile_file: src/site_deployment/workshop_sensor_calibration_real/config/sensor_calibration_profile.yaml
  reference_path_dir: /data/agv_calib/replace_with_site_or_line_id/sensor_reference_paths
  profile_tool: src/site_deployment/workshop_sensor_calibration_real/sensor_calibration_profile.py
  dataset_manifest_tool: src/site_deployment/workshop_sensor_calibration_real/sensor_dataset_manifest.py
  preprocess_tool: src/site_deployment/workshop_sensor_calibration_real/sensor_preprocess_pipeline.py
@@ -194,27 +194,45 @@ def normalize_profile_task(raw_task: dict[str, Any]) -> dict[str, Any]:
    return task
-def export_chassis_profile_tasks(profile_path: Path, chassis_type: str) -> list[dict[str, Any]]:
+def export_chassis_profile_tasks(
    profile_path: Path,
    chassis_type: str,
    reference_path_dir: Path | None = None,
 ) -> list[dict[str, Any]]:
    tool_path = REPO_ROOT / "src/site_deployment/workshop_chassis_calibration_real/chassis_action_profile.py"
    tool = load_module("chassis_action_profile_tool", tool_path)
    profile = tool.validate_profile(tool.load_yaml(profile_path))
-    exported = tool.export_requested_tasks(profile, chassis_type)
+    if reference_path_dir is not None:
        profile["reference_path_dir"] = str(reference_path_dir)
    exported = tool.export_requested_tasks(profile, chassis_type, profile_path)
    return [normalize_profile_task(raw_task) for raw_task in exported["requested_tasks"]]
-def export_control_profile_tasks(profile_path: Path, chassis_type: str) -> list[dict[str, Any]]:
+def export_control_profile_tasks(
    profile_path: Path,
    chassis_type: str,
    reference_path_dir: Path | None = None,
 ) -> list[dict[str, Any]]:
    tool_path = REPO_ROOT / "src/site_deployment/workshop_control_calibration_real/control_evaluation_profile.py"
    tool = load_module("control_evaluation_profile_tool", tool_path)
    profile = tool.validate_profile(tool.load_yaml(profile_path))
-    exported = tool.export_requested_tasks(profile, chassis_type)
+    if reference_path_dir is not None:
        profile["reference_path_dir"] = str(reference_path_dir)
    exported = tool.export_requested_tasks(profile, chassis_type, profile_path)
    return [normalize_profile_task(raw_task) for raw_task in exported["requested_tasks"]]
-def export_sensor_profile_tasks(profile_path: Path, task_name: str) -> list[dict[str, Any]]:
+def export_sensor_profile_tasks(
    profile_path: Path,
    task_name: str,
    reference_path_dir: Path | None = None,
 ) -> list[dict[str, Any]]:
    tool_path = REPO_ROOT / "src/site_deployment/workshop_sensor_calibration_real/sensor_calibration_profile.py"
    tool = load_module("sensor_calibration_profile_tool", tool_path)
    profile = tool.validate_profile(tool.load_yaml(profile_path))
-    exported = tool.export_requested_tasks(profile, {task_name})
+    if reference_path_dir is not None:
        profile["reference_path_dir"] = str(reference_path_dir)
    exported = tool.export_requested_tasks(profile, {task_name}, profile_path)
    return [normalize_profile_task(raw_task) for raw_task in exported["requested_tasks"]]
@@ -327,32 +345,62 @@ def build_requested_tasks(
        site_profile_path,
        ("chassis_calibration", "action_profile_file"),
    )
    chassis_reference_path_dir = resolve_configured_path(
        "",
        site_profile,
        site_profile_path,
        ("chassis_calibration", "reference_path_dir"),
    )
    control_profile_path = resolve_configured_path(
        args.control_evaluation_profile,
        site_profile,
        site_profile_path,
        ("control_calibration", "evaluation_profile_file"),
    )
    control_reference_path_dir = resolve_configured_path(
        "",
        site_profile,
        site_profile_path,
        ("control_calibration", "reference_path_dir"),
    )
    sensor_profile_path = resolve_configured_path(
        args.sensor_calibration_profile,
        site_profile,
        site_profile_path,
        ("sensor_calibration", "profile_file"),
    )
    sensor_reference_path_dir = resolve_configured_path(
        "",
        site_profile,
        site_profile_path,
        ("sensor_calibration", "reference_path_dir"),
    )
    requested_tasks: list[dict[str, Any]] = []
    for task_name in tasks:
        if task_name == "chassis" and profile_tasks_available(chassis_profile_path, args.profile_mode, "底盘动作"):
-            requested_tasks.extend(export_chassis_profile_tasks(chassis_profile_path, chassis_type))
+            requested_tasks.extend(export_chassis_profile_tasks(
                chassis_profile_path,
                chassis_type,
                chassis_reference_path_dir,
            ))
            continue
        if task_name == "control" and profile_tasks_available(control_profile_path, args.profile_mode, "运控评估"):
-            requested_tasks.extend(export_control_profile_tasks(control_profile_path, chassis_type))
+            requested_tasks.extend(export_control_profile_tasks(
                control_profile_path,
                chassis_type,
                control_reference_path_dir,
            ))
            continue
        if (
            task_name in {"sensor_intrinsic", "sensor_extrinsic", "hand_eye"} and
            profile_tasks_available(sensor_profile_path, args.profile_mode, "传感器标定")
        ):
-            requested_tasks.extend(export_sensor_profile_tasks(sensor_profile_path, task_name))
+            requested_tasks.extend(export_sensor_profile_tasks(
                sensor_profile_path,
                task_name,
                sensor_reference_path_dir,
            ))
            continue
        requested_tasks.append(minimal_task(task_name, args, site_profile, chassis_type))
    return requested_tasks
@@ -10,7 +10,8 @@
 - 墙面棋盘格阵列。
 - 2D LiDAR 外参标定角落。
 - 下视相机 3D ChArUco 内参标定台。
- 底盘标定路线和停车标记。
+- 标定路径不在 Isaac 场景中固化，由各标定模块的 `reference_paths/*.yaml` 选择和录制。
 - 外部真值遥测发布；Isaac 默认不显示黄球/观测连线这类定位调试可视化。
 - 场景 manifest 生成。
 默认启动：
@@ -34,6 +35,13 @@ headless 模式：
 python3 src/simulation/tools/launch_sim_stack.py --component isaac --headless
 ```
 外部真值定位调试可视化默认关闭。需要临时查看四角 LiDAR、车端标定球和观测连线时：
 ```bash
 python3 src/simulation/tools/launch_sim_stack.py --component isaac \
  --isaac-arg=--enable-external-truth-visualization
 ```
 边界规则：
 - Isaac API 只能留在这里。
@@ -48,6 +48,11 @@ def parse_args():
    parser.add_argument("--camera-focal-length", type=float, default=5.0, help="相机焦距")
    parser.add_argument("--ros-topic-prefix", type=str, default="/AutoCalib_Workshop", help="相机与激光雷达话题前缀")
    parser.add_argument("--cmd-vel-topic", type=str, default="/cmd_vel", help="车辆速度控制话题")
    parser.add_argument(
        "--disable-kinematic-command-follow",
        action="store_true",
        help="禁用仿真车辆按 cmd_vel 直接积分更新位姿；默认启用，便于在 Isaac 中可视化路径执行。",
    )
    parser.add_argument("--lidar-config", type=str, default="Example_Rotary", help="Isaac LiDAR 配置名")
    parser.add_argument("--lidar-3d-config", type=str, default="Hesai_XT32_SD10", help="车载3D LiDAR 配置名（默认32线）")
    parser.add_argument("--disable-camera", action="store_true", help="不创建顶置相机")
@@ -105,6 +110,9 @@ def parse_args():
    parser.add_argument("--external-quality-score", type=float, default=0.98, help="上报的 external 质量分数")
    parser.add_argument("--external-observed-target-count", type=int, default=4, help="上报的目标观测数量")
    parser.add_argument("--external-pose-drop-rate", type=float, default=0.0, help="位姿失效率，取值 [0,1]")
    parser.add_argument("--enable-external-truth-visualization", action="store_true", help="在 Isaac 场景中显示外部真值定位调试可视化")
    parser.add_argument("--disable-external-truth-visualization", action="store_true", help=argparse.SUPPRESS)
    parser.add_argument("--external-target-ball-radius", type=float, default=0.075, help="车端外部定位标定球可视化半径（米）")
    parser.add_argument("--disable-chassis-telemetry", action="store_true", help="不发布底盘标定遥测")
    parser.add_argument("--chassis-telemetry-topic", type=str, default="/chassis/telemetry", help="底盘标定遥测话题")
    parser.add_argument("--chassis-telemetry-hz", type=float, default=20.0, help="底盘标定遥测发布频率（Hz）")
@@ -122,8 +130,8 @@ def parse_args():
    parser.add_argument("--disable-control-telemetry", action="store_true", help="不发布运控评估遥测")
    parser.add_argument("--control-telemetry-topic", type=str, default="/control/telemetry", help="运控评估遥测话题")
    parser.add_argument("--control-telemetry-hz", type=float, default=20.0, help="运控评估遥测发布频率（Hz）")
-    parser.add_argument("--control-reference-speed-ms", type=float, default=0.3, help="运控遥测参考速度（m/s）")
+    parser.add_argument("--control-reference-speed-ms", type=float, default=0.3, help="兼容旧启动参数；Isaac 不再生成运控参考速度")
-    parser.add_argument("--control-reference-y-m", type=float, default=0.0, help="运控直线参考轨迹的 Y 坐标（米）")
+    parser.add_argument("--control-reference-y-m", type=float, default=0.0, help="兼容旧启动参数；Isaac 不再生成运控参考轨迹")
    parser.add_argument("--control-parameter-version", type=str, default="isaac_control_baseline_v1", help="当前控制参数版本")
    parser.add_argument("--disable-vehicle-tf", action="store_true", help="不发布车辆 TF 变换")
    parser.add_argument("--vehicle-tf-publish-hz", type=float, default=50.0, help="车辆 TF 发布频率（Hz）")
@@ -135,10 +143,10 @@ def parse_args():
    parser.add_argument("--sensor-quality-score", type=float, default=0.95, help="传感器观测质量评分，取值 [0,1]")
    parser.add_argument("--sensor-target-missing", action="store_true", help="发布 target_detected=false 的传感器遥测")
    parser.add_argument("--disable-calibration-boards", action="store_true", help="不创建多姿态棋盘格标定目标阵列")
-    parser.add_argument("--disable-calibration-fixtures", action="store_true", help="不创建地面标定路线和停车目标标识")
+    parser.add_argument("--disable-calibration-fixtures", action="store_true", help="兼容旧启动参数；Isaac 不再创建地面标定路线")
-    parser.add_argument("--straight-track-length", type=float, default=5.0, help="底盘直线标定路线长度（米）")
+    parser.add_argument("--straight-track-length", type=float, default=5.0, help="兼容旧启动参数；标定路径由 reference_paths/*.yaml 提供")
-    parser.add_argument("--straight-track-width", type=float, default=0.7, help="底盘直线标定路线宽度（米）")
+    parser.add_argument("--straight-track-width", type=float, default=0.7, help="兼容旧启动参数；标定路径由 reference_paths/*.yaml 提供")
-    parser.add_argument("--arc-track-radius", type=float, default=1.6, help="圆弧/转向标定路线半径（米）")
+    parser.add_argument("--arc-track-radius", type=float, default=1.6, help="兼容旧启动参数；标定路径由 reference_paths/*.yaml 提供")
    parser.add_argument("--random-seed", type=int, default=42, help="随机种子，保证实验可重复")
    parser.add_argument("--vehicle-id", type=str, default="demo_agv_001", help="车辆 ID，用于 manifest 和后端会话配置对齐")
    parser.add_argument("--vehicle-initial-x", type=float, default=0.0, help="车辆初始 X 坐标")
@@ -257,12 +265,12 @@ def validate_args(args):
        raise ValueError("sensor_quality_score 必须在 [0,1] 范围内。")
    if args.external_observed_target_count < 0 or args.sensor_target_sample_count < 0:
        raise ValueError("目标数量/样本数不能为负数。")
    if args.external_target_ball_radius <= 0:
        raise ValueError("external_target_ball_radius 必须大于 0。")
    if args.wheel_radius <= 0 or args.wheel_track <= 0 or args.wheel_base <= 0:
        raise ValueError("wheel_radius、wheel_track 和 wheel_base 必须大于 0。")
    if args.odom_noise_stddev_m < 0 or args.yaw_noise_stddev_rad < 0:
        raise ValueError("里程计噪声不能为负数。")
    if args.straight_track_length <= 0 or args.straight_track_width <= 0 or args.arc_track_radius <= 0:
        raise ValueError("标定路线尺寸必须大于 0。")
    if args.lidar_2d_target_width <= 0 or args.lidar_2d_target_height <= 0 or args.lidar_2d_target_thickness <= 0:
        raise ValueError("2D LiDAR 靶标尺寸必须大于 0。")
    if args.lidar_2d_target_bottom_z < 0:
@@ -318,7 +326,7 @@ import omni.graph.core as og
 import omni.kit.commands
 import omni.usd
 import omni.replicator.core as rep
-from pxr import Gf, PhysxSchema, Sdf, UsdGeom, UsdShade, Vt
+from pxr import Gf, PhysxSchema, Sdf, UsdGeom, UsdShade
 from omni.isaac.core import World
 from omni.isaac.core.objects import FixedCuboid
@@ -331,7 +339,6 @@ from omni.isaac.core.utils.viewports import set_camera_view
 from usd_utils import create_raw_usd_material, create_textured_board, create_textured_top_strip
 from calibration_targets import (
    add_calibration_boards,
    add_calibration_floor_fixtures,
    add_down_camera_intrinsic_target,
    add_2d_lidar_calibration_targets,
    lidar_2d_checkerboard_reserved_zones,
@@ -381,6 +388,45 @@ except ImportError:
    Imu = None
    LaserScan = None
 try:
    from geometry_msgs.msg import Twist
 except ImportError:
    Twist = None
 def external_lidar_mount_specs(room_length, room_width, height):
    """Return the four fixed workshop LiDAR poses used by external truth visualization."""
    offset = 0.3
    x_pos = (room_length / 2.0) - offset
    y_pos = (room_width / 2.0) - offset
    return [
        {"name": "FL", "pos": [x_pos, y_pos, height], "yaw": np.degrees(np.arctan2(-y_pos, -x_pos))},
        {"name": "FR", "pos": [x_pos, -y_pos, height], "yaw": np.degrees(np.arctan2(y_pos, -x_pos))},
        {"name": "BL", "pos": [-x_pos, y_pos, height], "yaw": np.degrees(np.arctan2(-y_pos, x_pos))},
        {"name": "BR", "pos": [-x_pos, -y_pos, height], "yaw": np.degrees(np.arctan2(y_pos, x_pos))},
    ]
 def create_preview_material(stage, mat_path, color, opacity=1.0, emissive_scale=0.0):
    material = UsdShade.Material.Define(stage, mat_path)
    shader = UsdShade.Shader.Define(stage, f"{mat_path}/PreviewSurface")
    shader.CreateIdAttr("UsdPreviewSurface")
    shader.CreateInput("diffuseColor", Sdf.ValueTypeNames.Color3f).Set(Gf.Vec3f(*color))
    shader.CreateInput("roughness", Sdf.ValueTypeNames.Float).Set(0.55)
    shader.CreateInput("metallic", Sdf.ValueTypeNames.Float).Set(0.0)
    if emissive_scale > 0.0:
        shader.CreateInput("emissiveColor", Sdf.ValueTypeNames.Color3f).Set(
            Gf.Vec3f(*(min(1.0, channel * emissive_scale) for channel in color))
        )
    if opacity < 1.0:
        shader.CreateInput("opacity", Sdf.ValueTypeNames.Float).Set(float(opacity))
    material.CreateSurfaceOutput().ConnectToSource(shader.ConnectableAPI(), "surface")
    return material
 def bind_material(prim, material):
    UsdShade.MaterialBindingAPI.Apply(prim).Bind(material)
 def add_corner_rotary_lidars(room_length, room_width, height, lidar_config, topic_prefix):
    """添加四角旋转 LiDAR"""
@@ -390,16 +436,7 @@ def add_corner_rotary_lidars(room_length, room_width, height, lidar_config, topi
    import omni.replicator.core as rep
    from pxr import Gf
-    offset = 0.3
+    lidar_configs = external_lidar_mount_specs(room_length, room_width, height)
    x_pos = (room_length / 2.0) - offset
    y_pos = (room_width / 2.0) - offset
    lidar_configs = [
        {"name": "FL", "pos": [x_pos, y_pos, height], "yaw": np.degrees(np.arctan2(-y_pos, -x_pos))},
        {"name": "FR", "pos": [x_pos, -y_pos, height], "yaw": np.degrees(np.arctan2(y_pos, -x_pos))},
        {"name": "BL", "pos": [-x_pos, y_pos, height], "yaw": np.degrees(np.arctan2(-y_pos, x_pos))},
        {"name": "BR", "pos": [-x_pos, -y_pos, height], "yaw": np.degrees(np.arctan2(y_pos, x_pos))},
    ]
    keys = og.Controller.Keys
    graph_path = "/World/ROS2_Lidar_Graph"
@@ -447,6 +484,194 @@ def add_corner_rotary_lidars(room_length, room_width, height, lidar_config, topi
    )
 class ExternalTruthSceneVisualization:
    """Isaac viewport visualization for the external truth localization flow."""
    TARGET_BALLS = [
        {"name": "front_left", "pos": np.array([0.58, 0.32, 1.28], dtype=float)},
        {"name": "front_right", "pos": np.array([0.58, -0.32, 1.28], dtype=float)},
        {"name": "rear_center", "pos": np.array([-0.50, 0.0, 1.22], dtype=float)},
    ]
    def __init__(self, args, world, vehicle_mount_parent_path):
        self.enabled = args.enable_external_truth_visualization and not args.disable_external_truth_visualization
        self.args = args
        self.stage = omni.usd.get_context().get_stage()
        self.root_path = "/World/ExternalTruthVisualization"
        self.vehicle_mount_parent_path = vehicle_mount_parent_path
        self.lidar_specs = external_lidar_mount_specs(
            args.room_length,
            args.room_width,
            args.room_height - 0.2,
        )
        self.observation_segments = []
        self.heading_segments = []
        self.uncertainty_segments = []
        self.last_update_wall_time = 0.0
        if not self.enabled:
            print("[*] Isaac 外部真值定位调试可视化未启用。")
            return
        UsdGeom.Xform.Define(self.stage, self.root_path)
        UsdGeom.Xform.Define(self.stage, f"{self.root_path}/Materials")
        UsdGeom.Xform.Define(self.stage, f"{self.root_path}/LidarStations")
        UsdGeom.Xform.Define(self.stage, f"{self.root_path}/ObservationLines")
        UsdGeom.Xform.Define(self.stage, f"{self.root_path}/PoseOverlay")
        self.materials = {
            "tower": create_preview_material(self.stage, f"{self.root_path}/Materials/TowerCyan", (0.10, 0.72, 0.84), emissive_scale=0.35),
            "beam": create_preview_material(self.stage, f"{self.root_path}/Materials/ObservationBeam", (0.20, 0.85, 1.0), opacity=0.72, emissive_scale=0.25),
            "ball": create_preview_material(self.stage, f"{self.root_path}/Materials/TargetBall", (1.0, 0.72, 0.12), emissive_scale=0.2),
            "pose": create_preview_material(self.stage, f"{self.root_path}/Materials/PoseArrow", (0.24, 0.90, 0.45), emissive_scale=0.35),
            "uncertainty": create_preview_material(self.stage, f"{self.root_path}/Materials/PoseUncertainty", (0.98, 0.84, 0.25), opacity=0.85, emissive_scale=0.15),
        }
        self._create_lidar_station_visuals(world)
        self._create_vehicle_target_balls()
        self._create_dynamic_segments()
        print("[*] Isaac 外部真值定位可视化已启用：四角 LiDAR、车端标定球、观测连线、真值姿态箭头。")
    def _create_lidar_station_visuals(self, world):
        for spec in self.lidar_specs:
            name = spec["name"]
            x, y, z = spec["pos"]
            stand_height = max(z, 0.4)
            world.scene.add(FixedCuboid(
                prim_path=f"{self.root_path}/LidarStations/{name}_Stand",
                name=f"external_truth_lidar_{name.lower()}_stand",
                position=np.array([x, y, stand_height / 2.0]),
                scale=np.array([0.08, 0.08, stand_height]),
                color=np.array([0.08, 0.52, 0.62]),
            ))
            world.scene.add(FixedCuboid(
                prim_path=f"{self.root_path}/LidarStations/{name}_Head",
                name=f"external_truth_lidar_{name.lower()}_head",
                position=np.array([x, y, z]),
                scale=np.array([0.22, 0.22, 0.16]),
                color=np.array([0.12, 0.78, 0.90]),
            ))
    def _create_vehicle_target_balls(self):
        target_root = f"{self.vehicle_mount_parent_path}/ExternalTruthTargets"
        UsdGeom.Xform.Define(self.stage, target_root)
        for target in self.TARGET_BALLS:
            sphere_path = f"{target_root}/Ball_{target['name']}"
            sphere = UsdGeom.Sphere.Define(self.stage, sphere_path)
            sphere.GetRadiusAttr().Set(float(self.args.external_target_ball_radius))
            xform = UsdGeom.Xformable(sphere.GetPrim())
            xform.AddTranslateOp().Set(Gf.Vec3d(*target["pos"]))
            bind_material(sphere.GetPrim(), self.materials["ball"])
    def _make_segment(self, path, width, material):
        cube = UsdGeom.Cube.Define(self.stage, path)
        cube.CreateSizeAttr(1.0)
        xform = UsdGeom.Xformable(cube.GetPrim())
        segment = {
            "translate": xform.AddTranslateOp(),
            "rotate": xform.AddRotateXYZOp(),
            "scale": xform.AddScaleOp(),
            "width": float(width),
        }
        segment["translate"].Set(Gf.Vec3d(0.0, 0.0, 0.0))
        segment["rotate"].Set(Gf.Vec3f(0.0, 0.0, 0.0))
        segment["scale"].Set(Gf.Vec3f(0.001, float(width), float(width)))
        bind_material(cube.GetPrim(), material)
        return segment
    def _create_dynamic_segments(self):
        for lidar in self.lidar_specs:
            for target in self.TARGET_BALLS:
                segment = self._make_segment(
                    f"{self.root_path}/ObservationLines/{lidar['name']}_to_{target['name']}",
                    0.014,
                    self.materials["beam"],
                )
                self.observation_segments.append((segment, lidar, target))
        for name in ("Shaft", "ArrowLeft", "ArrowRight"):
            self.heading_segments.append(self._make_segment(
                f"{self.root_path}/PoseOverlay/ExternalPoseHeading{name}",
                0.045,
                self.materials["pose"],
            ))
        for index in range(32):
            self.uncertainty_segments.append(self._make_segment(
                f"{self.root_path}/PoseOverlay/ExternalPoseUncertainty_{index:02d}",
                0.025,
                self.materials["uncertainty"],
            ))
    @staticmethod
    def _rotate_local_by_yaw(local_pos, yaw_rad):
        cos_yaw = math.cos(yaw_rad)
        sin_yaw = math.sin(yaw_rad)
        return np.array([
            cos_yaw * local_pos[0] - sin_yaw * local_pos[1],
            sin_yaw * local_pos[0] + cos_yaw * local_pos[1],
            local_pos[2],
        ], dtype=float)
    def _target_world_position(self, vehicle_position, yaw_rad, target):
        return np.array(vehicle_position, dtype=float) + self._rotate_local_by_yaw(target["pos"], yaw_rad)
    @staticmethod
    def _update_segment(segment, start, end):
        start = np.array(start, dtype=float)
        end = np.array(end, dtype=float)
        delta = end - start
        length = float(np.linalg.norm(delta))
        width = float(segment["width"])
        if length < 1e-4:
            midpoint = start
            yaw_deg = 0.0
            pitch_deg = 0.0
            length = 1e-4
        else:
            midpoint = (start + end) * 0.5
            horizontal = math.hypot(float(delta[0]), float(delta[1]))
            yaw_deg = math.degrees(math.atan2(float(delta[1]), float(delta[0])))
            pitch_deg = math.degrees(math.atan2(float(delta[2]), horizontal))
        segment["translate"].Set(Gf.Vec3d(float(midpoint[0]), float(midpoint[1]), float(midpoint[2])))
        segment["rotate"].Set(Gf.Vec3f(0.0, float(-pitch_deg), float(yaw_deg)))
        segment["scale"].Set(Gf.Vec3f(float(length), width, width))
    def update(self, agv):
        if not self.enabled or agv is None:
            return
        now = time.time()
        if now - self.last_update_wall_time < 0.02:
            return
        self.last_update_wall_time = now
        vehicle_position, quat = agv.get_world_pose()
        yaw_rad = quat_wxyz_to_yaw(quat)
        target_positions = {
            target["name"]: self._target_world_position(vehicle_position, yaw_rad, target)
            for target in self.TARGET_BALLS
        }
        for segment, lidar, target in self.observation_segments:
            self._update_segment(segment, np.array(lidar["pos"], dtype=float), target_positions[target["name"]])
        pose_anchor = np.array(vehicle_position, dtype=float) + np.array([0.0, 0.0, 1.55])
        heading_length = 0.80
        heading = np.array([math.cos(yaw_rad), math.sin(yaw_rad), 0.0], dtype=float)
        side = np.array([-math.sin(yaw_rad), math.cos(yaw_rad), 0.0], dtype=float)
        tip = pose_anchor + heading * heading_length
        self._update_segment(self.heading_segments[0], pose_anchor, tip)
        self._update_segment(self.heading_segments[1], tip, tip - heading * 0.22 + side * 0.13)
        self._update_segment(self.heading_segments[2], tip, tip - heading * 0.22 - side * 0.13)
        radius = max(0.18, min(0.65, self.args.external_position_stddev_m * 18.0))
        ring_center = np.array(vehicle_position, dtype=float) + np.array([0.0, 0.0, 0.045])
        ring_points = []
        for index in range(len(self.uncertainty_segments) + 1):
            angle = 2.0 * math.pi * index / len(self.uncertainty_segments)
            ring_points.append(ring_center + np.array([math.cos(angle) * radius, math.sin(angle) * radius, 0.0]))
        for index, segment in enumerate(self.uncertainty_segments):
            self._update_segment(segment, ring_points[index], ring_points[index + 1])
 class ExternalTruthTelemetryPublisher:
    def __init__(self, args):
@@ -1068,8 +1293,6 @@ class ControlCalibrationTelemetryPublisher:
        self.enabled = not args.disable_control_telemetry
        self.topic = args.control_telemetry_topic
        self.publish_period_sec = 0.0 if args.control_telemetry_hz <= 0.0 else 1.0 / args.control_telemetry_hz
        self.reference_speed_ms = args.control_reference_speed_ms
        self.reference_y_m = args.control_reference_y_m
        self.parameter_version = args.control_parameter_version
        self.last_publish_wall_time = 0.0
        self.node = None
@@ -1102,12 +1325,13 @@ class ControlCalibrationTelemetryPublisher:
        position, yaw_rad, linear_velocity, angular_velocity = vehicle_state_from_isaac(agv)
        forward_axis = np.array([math.cos(yaw_rad), math.sin(yaw_rad)])
        forward_velocity = float(np.dot(np.array([linear_velocity[0], linear_velocity[1]]), forward_axis))
-        lateral_error = float(position[1] - self.reference_y_m)
+        # Isaac 不再持有标定参考路径；路径误差由标定任务按所选 YAML 路径计算。
-        heading_error = float(normalize_angle(yaw_rad))
+        lateral_error = 0.0
-        speed_error = float(forward_velocity - self.reference_speed_ms)
+        heading_error = 0.0
-        steering_output = float(clamp(-0.8 * lateral_error - 1.2 * heading_error, -1.0, 1.0))
+        speed_error = 0.0
-        throttle_output = float(clamp(-2.0 * speed_error, 0.0, 1.0))
+        steering_output = 0.0
-        brake_output = float(clamp(2.0 * speed_error, 0.0, 1.0))
+        throttle_output = 0.0
        brake_output = 0.0
        msg = ControlTelemetry()
        msg.hardware_timestamp_us = time.time_ns() // 1000
@@ -1274,6 +1498,87 @@ class AckermannUrdfJointDriver:
                self.warned = True
 class VehicleCmdVelSubscriber:
    """Direct ROS subscriber used by Isaac's Python loop to avoid OmniGraph command lag."""
    def __init__(self, args):
        self.topic = args.cmd_vel_topic
        self.node = None
        self.latest_linear_x = 0.0
        self.latest_angular_z = 0.0
        self.last_message_wall_time = 0.0
        if rclpy is None or Twist is None:
            print("[WARN] 未找到 rclpy 或 geometry_msgs/Twist，Isaac 直接 cmd_vel 订阅已禁用。")
            return
        if not rclpy.ok():
            rclpy.init(args=None)
        self.node = rclpy.create_node("isaac_vehicle_cmd_vel_subscriber")
        self.node.create_subscription(Twist, self.topic, self.on_cmd_vel, 10)
        print(f"[*] Isaac 直接订阅车辆 cmd_vel: topic={self.topic}")
    def on_cmd_vel(self, msg):
        self.latest_linear_x = float(msg.linear.x)
        self.latest_angular_z = float(msg.angular.z)
        self.last_message_wall_time = time.time()
    def spin_once(self):
        if self.node is None or not rclpy.ok():
            return
        rclpy.spin_once(self.node, timeout_sec=0.0)
    def command(self, stale_timeout_sec=0.5):
        if self.node is None or self.last_message_wall_time <= 0.0:
            return None
        age_sec = time.time() - self.last_message_wall_time
        if age_sec > stale_timeout_sec:
            return 0.0, 0.0
        return self.latest_linear_x, self.latest_angular_z
    def shutdown(self):
        if self.node is not None:
            self.node.destroy_node()
            self.node = None
 class KinematicCommandFollower:
    def __init__(self, enabled=True):
        self.enabled = enabled
        self.last_wall_time = time.time()
    def apply(self, agv, linear_x, angular_z, ackermann_joint_driver):
        now = time.time()
        dt = max(0.0, min(now - self.last_wall_time, 0.1))
        self.last_wall_time = now
        position, quat = agv.get_world_pose()
        yaw = quat_wxyz_to_yaw(quat)
        current_linear_velocity = agv.get_linear_velocity()
        world_linear_velocity = np.array([
            linear_x * math.cos(yaw),
            linear_x * math.sin(yaw),
            current_linear_velocity[2],
        ])
        agv.set_linear_velocity(world_linear_velocity)
        agv.set_angular_velocity(np.array([0.0, 0.0, angular_z]))
        ackermann_joint_driver.apply(agv, linear_x, angular_z)
        if not self.enabled or dt <= 0.0:
            return
        if abs(linear_x) <= 1e-4 and abs(angular_z) <= 1e-4:
            return
        next_yaw = normalize_angle(yaw + angular_z * dt)
        travel_yaw = yaw + 0.5 * angular_z * dt
        next_position = np.array(position, dtype=float)
        next_position[0] += linear_x * math.cos(travel_yaw) * dt
        next_position[1] += linear_x * math.sin(travel_yaw) * dt
        next_quat = euler_angles_to_quat(np.array([0.0, 0.0, next_yaw]), degrees=False)
        agv.set_world_pose(position=next_position, orientation=np.array(next_quat))
 class IsaacWorkshopRuntime:
    def __init__(self, args):
        self.args = args
@@ -1304,6 +1609,7 @@ class IsaacWorkshopRuntime:
        self.calibration_board_specs = []
        self.lidar_2d_target_specs = []
        self.down_camera_target_specs = []
        self.external_truth_visualization = None
        self.scene_manifest_path = (
            Path(args.scene_manifest_path).expanduser().resolve()
            if args.scene_manifest_path
@@ -1390,18 +1696,11 @@ class IsaacWorkshopRuntime:
            },
            "chassis_calibration": {
                "chassis_type": self.args.chassis_type,
                "straight_track_length_m": self.args.straight_track_length,
                "straight_track_width_m": self.args.straight_track_width,
                "arc_track_radius_m": self.args.arc_track_radius,
                "wheel_radius_m": self.args.wheel_radius,
                "wheel_track_m": self.args.wheel_track,
                "wheel_base_m": self.args.wheel_base,
            },
            "control_calibration": {
                "reference_path": [
                    {"x_m": -self.args.straight_track_length / 2.0, "y_m": self.args.control_reference_y_m, "yaw_rad": 0.0, "target_speed_ms": self.args.control_reference_speed_ms},
                    {"x_m": self.args.straight_track_length / 2.0, "y_m": self.args.control_reference_y_m, "yaw_rad": 0.0, "target_speed_ms": self.args.control_reference_speed_ms},
                ],
                "parameter_version": self.args.control_parameter_version,
            },
            "external_truth": {
@@ -1411,6 +1710,23 @@ class IsaacWorkshopRuntime:
                "expected_position_stddev_m": self.args.external_position_stddev_m,
                "expected_yaw_stddev_rad": self.args.external_yaw_stddev_rad,
                "expected_time_sync_offset_ms": self.args.external_time_sync_offset_ms,
                "visualization_enabled": (
                    self.args.enable_external_truth_visualization
                    and not self.args.disable_external_truth_visualization
                ),
                "visualization_root_prim": "/World/ExternalTruthVisualization",
                "target_ball_radius_m": self.args.external_target_ball_radius,
                "target_balls": [
                    {
                        "ball_id": target["name"],
                        "center_in_base_link": {
                            "x_m": float(target["pos"][0]),
                            "y_m": float(target["pos"][1]),
                            "z_m": float(target["pos"][2]),
                        },
                    }
                    for target in ExternalTruthSceneVisualization.TARGET_BALLS
                ],
            },
            "sensors": [
                {
@@ -1756,6 +2072,14 @@ class IsaacWorkshopRuntime:
            {keys.CREATE_NODES: nodes, keys.CONNECT: connections, keys.SET_VALUES: set_values},
        )
    def add_external_truth_visualization(self, world):
        vehicle_mount_parent_path = self.get_vehicle_sensor_mount_parent_path()
        self.external_truth_visualization = ExternalTruthSceneVisualization(
            self.args,
            world,
            vehicle_mount_parent_path,
        )
    def build_workshop(self):
        world = World(stage_units_in_meters=1.0)
        room_length = self.args.room_length
@@ -1868,9 +2192,6 @@ class IsaacWorkshopRuntime:
            self.lidar_2d_target_specs = add_2d_lidar_calibration_targets(world, self.args)
            print(f"[*] 已创建 {len(self.lidar_2d_target_specs)} 个 2D LiDAR 角落外参标定靶标。")
        if not self.args.disable_calibration_fixtures:
            add_calibration_floor_fixtures(world, self.args)
        if self.args.disable_down_camera_intrinsic_target:
            print("[*] 已禁用下视相机 3D ChArUco 内参标定台。")
        else:
@@ -1918,6 +2239,7 @@ class IsaacWorkshopRuntime:
            )
        self.import_vehicle_to_stage(world)
        self.add_external_truth_visualization(world)
        self.hide_sensor_placeholder_visuals()
        self.add_vehicle_sensor_publishers()
@@ -1968,7 +2290,12 @@ def main():
    print(f"  - 2D LiDAR topic: {runtime.vehicle_2d_lidar_topic}")
    print(f"  - IMU topic: {runtime.vehicle_imu_topic}")
    print(f"  - /cmd_vel topic: {runtime.cmd_vel_topic}")
    print(f"  - kinematic command follow: enabled={not ARGS.disable_kinematic_command_follow}")
    print(f"  - external telemetry topic: {ARGS.external_telemetry_topic}")
    print(
        "  - external truth visualization: "
        f"enabled={ARGS.enable_external_truth_visualization and not ARGS.disable_external_truth_visualization}"
    )
    print(f"  - chassis telemetry topic: {ARGS.chassis_telemetry_topic}")
    print(f"  - vehicle TF: enabled={not ARGS.disable_vehicle_tf}, hz={ARGS.vehicle_tf_publish_hz}")
    print(f"  - control telemetry topic: {ARGS.control_telemetry_topic}")
@@ -1979,6 +2306,8 @@ def main():
    agv = runtime.vehicle or world.scene.get_object("agv_vehicle")
    ackermann_joint_driver = AckermannUrdfJointDriver(ARGS)
    ackermann_joint_driver.configure(agv)
    cmd_vel_subscriber = VehicleCmdVelSubscriber(ARGS)
    command_follower = KinematicCommandFollower(enabled=not ARGS.disable_kinematic_command_follow)
    render_sensor_outputs = (
        not ARGS.disable_camera
        or not ARGS.disable_lidars
@@ -2003,7 +2332,9 @@ def main():
        (control_telemetry_publisher, (agv,)),
        (sensor_telemetry_publisher, ()),
    )
-    ros_context_required = any(getattr(publisher, "node", None) is not None for publisher, _ in ros_publishers)
+    ros_context_required = any(getattr(publisher, "node", None) is not None for publisher, _ in ros_publishers) or (
        cmd_vel_subscriber.node is not None
    )
    try:
        while True:
            if SHUTDOWN_REQUESTED:
@@ -2017,29 +2348,29 @@ def main():
                exit_reason = "simulation_app.is_running() returned False"
                break
            try:
-                lin_vel = og.Controller.get(og.Controller.attribute("/World/ROS2_Twist_Graph/TwistSub.outputs:linearVelocity"))
+                cmd_vel_subscriber.spin_once()
-                ang_vel = og.Controller.get(og.Controller.attribute("/World/ROS2_Twist_Graph/TwistSub.outputs:angularVelocity"))
+                command = cmd_vel_subscriber.command()
-
+                if command is None:
                    lin_vel = og.Controller.get(
                        og.Controller.attribute("/World/ROS2_Twist_Graph/TwistSub.outputs:linearVelocity")
                    )
                    ang_vel = og.Controller.get(
                        og.Controller.attribute("/World/ROS2_Twist_Graph/TwistSub.outputs:angularVelocity")
                    )
                    if lin_vel is not None and ang_vel is not None and len(lin_vel) == 3:
-                    _, quat = agv.get_world_pose()
+                        command = float(lin_vel[0]), float(ang_vel[2])
                    current_linear_velocity = agv.get_linear_velocity()
-                    q = Gf.Quatd(float(quat[0]), float(quat[1]), float(quat[2]), float(quat[3]))
+                if command is not None:
-                    rot_mat = Gf.Matrix3d(Gf.Rotation(q))
+                    linear_x, angular_z = command
-                    world_linear_velocity = Gf.Vec3d(*lin_vel) * rot_mat
+                    command_follower.apply(agv, linear_x, angular_z, ackermann_joint_driver)
-                    world_angular_velocity = Gf.Vec3d(*ang_vel) * rot_mat
+                    if abs(linear_x) > 0.01 or abs(angular_z) > 0.01:
-
+                        print(f"\r[ROS2 Debug] 车辆移动中: 线速度 {linear_x:.2f}, 角速度 {angular_z:.2f}", end="")
                    target_linear_velocity = np.array([world_linear_velocity[0], world_linear_velocity[1], current_linear_velocity[2]])
                    target_angular_velocity = np.array([0.0, 0.0, world_angular_velocity[2]])
                    agv.set_linear_velocity(target_linear_velocity)
                    agv.set_angular_velocity(target_angular_velocity)
                    ackermann_joint_driver.apply(agv, lin_vel[0], ang_vel[2])
                    if abs(lin_vel[0]) > 0.01 or abs(ang_vel[2]) > 0.01:
                        print(f"\r[ROS2 Debug] 车辆移动中: 线速度 {lin_vel[0]:.2f}, 角速度 {ang_vel[2]:.2f}", end="")
            except Exception as exc:
                print(f"\n[ERROR] 运动学控制循环异常: {exc}")
            if runtime.external_truth_visualization is not None:
                runtime.external_truth_visualization.update(agv)
            world.step(render=render_frame)
            try:
                for publisher, publish_args in ros_publishers:
@@ -2079,6 +2410,7 @@ def main():
        chassis_telemetry_publisher.shutdown()
        control_telemetry_publisher.shutdown()
        sensor_telemetry_publisher.shutdown()
        cmd_vel_subscriber.shutdown()
        if rclpy is not None and rclpy.ok():
            rclpy.shutdown()
        if SHUTDOWN_REQUESTED or ros_context_shutdown_seen:
@@ -360,93 +360,3 @@ def add_2d_lidar_calibration_targets(world, args):
    )
    return specs
 def add_floor_marker(world, prim_path, name, position, scale, color):
    """添加地面标记"""
    world.scene.add(FixedCuboid(
        prim_path=prim_path,
        name=name,
        position=np.array(position),
        scale=np.array(scale),
        color=np.array(color),
    ))
 def add_calibration_floor_fixtures(world, args):
    """添加地面标定设施"""
    z = 0.012
    thickness = 0.012
    length = args.straight_track_length
    width = args.straight_track_width
    add_floor_marker(
        world,
        "/World/Workshop/CalibrationFixtures/StraightTrack",
        "straight_track",
        [0.0, 0.0, z],
        [length, width, thickness],
        [0.08, 0.12, 0.10],
    )
    add_floor_marker(
        world,
        "/World/Workshop/CalibrationFixtures/StraightCenterLine",
        "straight_center_line",
        [0.0, 0.0, z + thickness],
        [length, 0.035, thickness],
        [0.95, 0.95, 0.15],
    )
    for side, y in (("Left", width / 2.0), ("Right", -width / 2.0)):
        add_floor_marker(
            world,
            f"/World/Workshop/CalibrationFixtures/StraightBoundary{side}",
            f"straight_boundary_{side.lower()}",
            [0.0, y, z + thickness],
            [length, 0.025, thickness],
            [0.95, 0.78, 0.08],
        )
    stop_x = min(length / 2.0 - 0.35, args.room_length / 2.0 - 1.0)
    add_floor_marker(
        world,
        "/World/Workshop/CalibrationFixtures/StopAccuracyTarget",
        "stop_accuracy_target",
        [stop_x, 0.0, z + 2.0 * thickness],
        [0.6, 0.9, thickness],
        [0.88, 0.10, 0.10],
    )
    lateral_x = -min(length / 2.0 - 0.8, args.room_length / 2.0 - 1.2)
    add_floor_marker(
        world,
        "/World/Workshop/CalibrationFixtures/LateralMotionPad",
        "lateral_motion_pad",
        [lateral_x, 0.0, z + thickness],
        [0.75, 2.0, thickness],
        [0.10, 0.34, 0.88],
    )
    arc_center = np.array([0.0, -args.room_width * 0.20])
    arc_radius = min(args.arc_track_radius, args.room_width * 0.32)
    marker_count = 25
    for index in range(marker_count):
        theta = math.radians(20.0 + 140.0 * index / (marker_count - 1))
        x = arc_center[0] + arc_radius * math.cos(theta)
        y = arc_center[1] + arc_radius * math.sin(theta)
        add_floor_marker(
            world,
            f"/World/Workshop/CalibrationFixtures/ArcMarker_{index:02d}",
            f"arc_marker_{index:02d}",
            [x, y, z + 3.0 * thickness],
            [0.10, 0.10, thickness],
            [0.12, 0.75, 0.35],
        )
    add_floor_marker(
        world,
        "/World/Workshop/CalibrationFixtures/SensorCapturePad",
        "sensor_capture_pad",
        [0.0, args.room_width * 0.28, z + thickness],
        [1.2, 0.9, thickness],
        [0.42, 0.18, 0.78],
    )
@@ -42,7 +42,9 @@ def chassis_type_value(name, chassis_type_cls=None):
        "ackermann": chassis_type_cls.ACKERMANN,
        "differential": chassis_type_cls.DIFFERENTIAL,
        "single_steer": chassis_type_cls.SINGLE_STEER_WHEEL,
        "single_steer_wheel": chassis_type_cls.SINGLE_STEER_WHEEL,
        "multi_steer": chassis_type_cls.MULTI_STEER_WHEEL,
        "multi_steer_wheel": chassis_type_cls.MULTI_STEER_WHEEL,
    }
    return mapping.get(name, chassis_type_cls.CHASSIS_TYPE_UNSPECIFIED)
@@ -205,6 +205,8 @@ def build_isaac_command(profile: dict, python_executable: str, headless: bool, e
    isaac = profile["isaac"]
    targets = profile.get("targets", {})
    down_camera = targets.get("down_camera_charuco", {})
    external_localization = profile.get("external_localization", {})
    external_pose_bridge = profile.get("external_pose_bridge", {})
    command = [
        python_executable,
        str(repo_path(isaac["scene_script"])),
@@ -222,6 +224,20 @@ def build_isaac_command(profile: dict, python_executable: str, headless: bool, e
        str(get_nested(profile, "vehicle_sensor_agent.lidar_2d_topic", "/sensor/lidar_2d/scan")),
        "--vehicle-imu-topic",
        str(get_nested(profile, "vehicle_sensor_agent.imu_topic", "/sensor/imu/data")),
        "--external-telemetry-topic",
        str(
            external_localization.get("output_topic")
            or external_pose_bridge.get("source_topic")
            or get_nested(profile, "vehicle_agent.external_pose_topic", "/isaac/external_localization/vehicle/pose")
        ),
        "--external-reference-source-name",
        str(
            external_localization.get("reference_source_name")
            or external_pose_bridge.get("reference_source_name")
            or "isaac_sim_truth_source"
        ),
        "--external-workcell-zone-id",
        str(external_localization.get("workcell_zone_id", "isaac_workcell_zone_a")),
        "--scene-manifest-path",
        str(repo_path(isaac["scene_manifest_path"])),
    ]
@@ -625,7 +625,7 @@ def make_chassis_profile_tasks(args: argparse.Namespace) -> list[RequestedCalibr
    )
    profile_path = Path(args.chassis_action_profile).expanduser().resolve(strict=False)
    profile = tool.validate_profile(tool.load_yaml(profile_path))
-    exported = tool.export_requested_tasks(profile, args.chassis_profile_type)
+    exported = tool.export_requested_tasks(profile, args.chassis_profile_type, profile_path)
    return [
        make_requested_task_from_profile(
            raw_task,
@@ -643,7 +643,7 @@ def make_control_profile_tasks(args: argparse.Namespace) -> list[RequestedCalibr
    )
    profile_path = Path(args.control_evaluation_profile).expanduser().resolve(strict=False)
    profile = tool.validate_profile(tool.load_yaml(profile_path))
-    exported = tool.export_requested_tasks(profile, args.chassis_profile_type)
+    exported = tool.export_requested_tasks(profile, args.chassis_profile_type, profile_path)
    return [
        make_requested_task_from_profile(
            raw_task,
@@ -661,7 +661,7 @@ def make_sensor_profile_tasks(task_name: str, args: argparse.Namespace) -> list[
    )
    profile_path = Path(args.sensor_calibration_profile).expanduser().resolve(strict=False)
    profile = tool.validate_profile(tool.load_yaml(profile_path))
-    exported = tool.export_requested_tasks(profile, {task_name})
+    exported = tool.export_requested_tasks(profile, {task_name}, profile_path)
    return [
        make_requested_task_from_profile(
            raw_task,
@@ -56,6 +56,14 @@ src/site_deployment/workshop_chassis_calibration_real/config/chassis_action_prof
 这个 profile 固定了四类底盘在真实标定车间中建议执行的动作序列、速度、距离、超时和采集要求。现场部署时需要先按工位尺寸、安全速度、真实模块 ID 修改它，再用工具校验：
 底盘参考路径单独放在：
 ```text
 src/site_deployment/workshop_chassis_calibration_real/reference_paths/
 ```
 每条路径一个 YAML 文件，必须配置 `path_id` 和中文 `display_name`。`chassis_action_profile.yaml` 中每个动作通过 `reference_path_id` 选择要用的路径，导出任务时会自动展开为 `reference_path.*` metadata。
 ```bash
 python3 src/site_deployment/workshop_chassis_calibration_real/chassis_action_profile.py \
  --profile /data/agv_calib/site_a/chassis_action_profile.yaml
@@ -125,6 +133,73 @@ python3 src/site_deployment/workshop_chassis_calibration_real/run_chassis_profil
 - 命令话题不可用时，`command_source=auto` 会按动作 profile 写计划命令行。
 - 全部动作结束后统一写 `dataset_index.yaml`。
 ## 车间电脑侧底盘遥测 WiFi/TCP bridge
 如果真实车端通过 WiFi/TCP 把底盘遥测发到车间电脑，而不是直接发布 ROS2 topic，先启动桥接节点：
 ```bash
 source /opt/ros/humble/setup.bash
 source install/setup.bash
 python3 src/site_deployment/workshop_chassis_calibration_real/workshop_chassis_telemetry_bridge.py \
  --bind-host 0.0.0.0 \
  --bind-port 9010 \
  --protocol frame \
  --output-topic /chassis/telemetry \
  --default-chassis-type ackermann
 ```
 桥接节点接收 JSON payload，并发布 `calibration_chassis_interfaces/msg/ChassisTelemetry`。支持两种常用输入：
 - `frame`：项目统一 TCP 帧，`<uint32 msg_type><uint32 payload_len><json payload>`，建议车端使用 `msg_type=7`。
 - `json_lines`：每行一条 UTF-8 JSON。
 payload 字段示例：
 ```json
 {
  "hardware_timestamp_us": 1711234567000000,
  "chassis_type": "ackermann",
  "odom_x_m": 0.12,
  "odom_y_m": 0.0,
  "odom_yaw_rad": 0.0,
  "linear_velocity_ms": 0.1,
  "angular_velocity_rads": 0.0,
  "estop_engaged": false,
  "driver_error_code": 0,
  "active_job_id": "chassis_req_001",
  "modules": [
    {
      "module_id": "rear_left",
      "encoder_ticks": 12345,
      "wheel_speed_rpm": 18.0,
      "steer_angle_deg": 0.0,
      "motor_current_amp": 1.2
    }
  ]
 }
 ```
 确认车间电脑侧已经收到并转成 ROS topic：
 ```bash
 ros2 topic hz /chassis/telemetry
 ros2 topic echo /chassis/telemetry --once
 ```
 通过操作台 UI 启动“现场服务”时，如果现场配置里存在 `chassis_telemetry_bridge`，UI 会把监听地址、端口、协议和输出 topic 传给
 `minimal_workshop_demo.launch.py`，并自动启动这个 bridge。也可以手动通过 launch 参数启用：
 ```bash
 ros2 launch win_ubuntu_bridge minimal_workshop_demo.launch.py \
  use_gateway:=true \
  enable_chassis_telemetry_bridge:=true \
  chassis_telemetry_bind_host:=0.0.0.0 \
  chassis_telemetry_bind_port:=9010 \
  chassis_telemetry_protocol:=frame \
  chassis_telemetry_topic:=/chassis/telemetry
 ```
 本地最小 smoke：
 ```bash
@@ -176,6 +176,8 @@ class ChassisSessionCapture:
    def on_chassis_telemetry(self, msg: Any) -> None:
        chassis_type = normalize_chassis_type_name(getattr(getattr(msg, "chassis_type", None), "value", ""))
        if not chassis_type:
            chassis_type = normalize_chassis_type_name(self.config.get("chassis_type", ""))
        self.chassis_csv.write_row({
            "hardware_timestamp_us": int_value(getattr(msg, "hardware_timestamp_us", 0), now_us()),
            "chassis_type": chassis_type,
@@ -13,6 +13,13 @@ try:
 except ImportError as exc:
    raise SystemExit("缺少 PyYAML，请先在当前 Python 环境安装 yaml 模块。") from exc
 SCRIPT_DIR = Path(__file__).resolve().parent
 REFERENCE_PATH_TOOL_DIR = SCRIPT_DIR.parents[0] / "workshop_reference_paths"
 if str(REFERENCE_PATH_TOOL_DIR) not in sys.path:
    sys.path.insert(0, str(REFERENCE_PATH_TOOL_DIR))
 from calibration_reference_paths import selected_path_metadata  # noqa: E402
 SUPPORTED_CHASSIS_TYPES = {
    "ackermann",
@@ -230,6 +237,8 @@ def validate_action(
    for key, value in metadata.items():
        validate_metadata_value(key, value, task_code, max_linear_speed_ms)
    if action.get("reference_path_id") not in (None, ""):
        require_string(action.get("reference_path_id"), f"{action_name}.reference_path_id")
 def validate_profile(profile: dict[str, Any]) -> dict[str, Any]:
@@ -281,12 +290,25 @@ def make_task_param(key: str, value: Any) -> dict[str, str]:
    }
-def export_requested_tasks(profile: dict[str, Any], chassis_type: str) -> dict[str, Any]:
+def export_requested_tasks(
    profile: dict[str, Any],
    chassis_type: str,
    profile_path: Path | None = None,
 ) -> dict[str, Any]:
    section = profile["chassis_profiles"][chassis_type]
    reference_path_dir = profile.get("reference_path_dir", "reference_paths")
    tasks: list[dict[str, Any]] = []
    for action in section["actions"]:
        metadata = dict(action["metadata"])
        metadata["primitive_type"] = action["primitive_type"]
        reference_metadata, _ = selected_path_metadata(
            reference_path_dir,
            profile_path,
            str(action.get("reference_path_id", "")),
            chassis_type,
            str(action["primitive_type"]),
        )
        metadata.update(reference_metadata)
        task_params = [
            make_task_param(key, metadata[key])
            for key in sorted(metadata)
@@ -341,7 +363,7 @@ def main() -> int:
    if args.format == "requested_tasks":
        if not args.chassis_type:
            raise ValueError("--format requested_tasks 必须指定 --chassis-type。")
-        output = export_requested_tasks(profile, args.chassis_type)
+        output = export_requested_tasks(profile, args.chassis_type, profile_path)
    else:
        output = build_summary(profile)
@@ -0,0 +1,22 @@
 #!/usr/bin/env python3
 """兼容入口：底盘参考路径已迁移到通用标定参考路径工具。"""
 from __future__ import annotations
 import sys
 from pathlib import Path
 COMMON_TOOL_DIR = Path(__file__).resolve().parents[1] / "workshop_reference_paths"
 if str(COMMON_TOOL_DIR) not in sys.path:
    sys.path.insert(0, str(COMMON_TOOL_DIR))
 from calibration_reference_paths import main  # noqa: E402
 DEFAULT_PATH_DIR = Path(__file__).resolve().parent / "reference_paths"
 if __name__ == "__main__":
    if "--path-dir" not in sys.argv:
        sys.argv.extend(["--path-dir", str(DEFAULT_PATH_DIR)])
    raise SystemExit(main())
@@ -4,6 +4,7 @@
 schema_version: 1
 profile_name: workshop_chassis_calibration_action_profile
 reference_path_dir: ../reference_paths
 safety:
  max_linear_speed_ms: 0.1
@@ -33,6 +34,7 @@ chassis_profiles:
      - task_code: chassis.ackermann.straight_forward
        display_name: 阿克曼直线前进
        primitive_type: straight_line
        reference_path_id: chassis_straight_forward_1m
        metadata:
          straight_line.target_distance_m: 1.0
          straight_line.target_speed_ms: 0.1
@@ -42,6 +44,7 @@ chassis_profiles:
      - task_code: chassis.ackermann.straight_reverse
        display_name: 阿克曼直线倒车
        primitive_type: straight_line
        reference_path_id: chassis_straight_reverse_0_6m
        metadata:
          straight_line.target_distance_m: 0.6
          straight_line.target_speed_ms: 0.08
@@ -51,6 +54,7 @@ chassis_profiles:
      - task_code: chassis.ackermann.arc_left
        display_name: 阿克曼左圆弧
        primitive_type: arc
        reference_path_id: chassis_arc_left_r1_45deg
        metadata:
          arc.target_speed_ms: 0.08
          arc.radius_m: 1.0
@@ -61,6 +65,7 @@ chassis_profiles:
      - task_code: chassis.ackermann.arc_right
        display_name: 阿克曼右圆弧
        primitive_type: arc
        reference_path_id: chassis_arc_right_r1_45deg
        metadata:
          arc.target_speed_ms: 0.08
          arc.radius_m: 1.0
@@ -68,9 +73,54 @@ chassis_profiles:
          arc.clockwise: true
          brake_when_finished: true
          timeout_sec: 45.0
      - task_code: chassis.ackermann.s_curve_left_entry
        display_name: 阿克曼 S 形 1/4 左入弯
        primitive_type: arc
        reference_path_id: chassis_ackermann_s_curve_r1_5
        metadata:
          arc.target_speed_ms: 0.1
          arc.radius_m: 1.5
          arc.sweep_angle_deg: 25.0
          arc.clockwise: false
          brake_when_finished: false
          timeout_sec: 30.0
      - task_code: chassis.ackermann.s_curve_right_middle
        display_name: 阿克曼 S 形 2/4 右转
        primitive_type: arc
        reference_path_id: chassis_ackermann_s_curve_r1_5
        metadata:
          arc.target_speed_ms: 0.1
          arc.radius_m: 1.5
          arc.sweep_angle_deg: 60.0
          arc.clockwise: true
          brake_when_finished: false
          timeout_sec: 40.0
      - task_code: chassis.ackermann.s_curve_left_middle
        display_name: 阿克曼 S 形 3/4 左转
        primitive_type: arc
        reference_path_id: chassis_ackermann_s_curve_r1_5
        metadata:
          arc.target_speed_ms: 0.1
          arc.radius_m: 1.5
          arc.sweep_angle_deg: 60.0
          arc.clockwise: false
          brake_when_finished: false
          timeout_sec: 40.0
      - task_code: chassis.ackermann.s_curve_right_exit
        display_name: 阿克曼 S 形 4/4 右出弯
        primitive_type: arc
        reference_path_id: chassis_ackermann_s_curve_r1_5
        metadata:
          arc.target_speed_ms: 0.1
          arc.radius_m: 1.5
          arc.sweep_angle_deg: 25.0
          arc.clockwise: true
          brake_when_finished: true
          timeout_sec: 30.0
      - task_code: chassis.ackermann.steering_sweep
        display_name: 阿克曼舵角扫动
        primitive_type: steering_sweep
        reference_path_id: chassis_static_station
        metadata:
          steering_sweep.target_angle_deg: 0.0
          steering_sweep.sweep_amplitude_deg: 8.0
@@ -89,6 +139,7 @@ chassis_profiles:
      - task_code: chassis.differential.straight_forward
        display_name: 差速直线前进
        primitive_type: straight_line
        reference_path_id: chassis_straight_forward_1m
        metadata:
          straight_line.target_distance_m: 1.0
          straight_line.target_speed_ms: 0.1
@@ -98,6 +149,7 @@ chassis_profiles:
      - task_code: chassis.differential.straight_reverse
        display_name: 差速直线倒车
        primitive_type: straight_line
        reference_path_id: chassis_straight_reverse_0_6m
        metadata:
          straight_line.target_distance_m: 0.6
          straight_line.target_speed_ms: 0.08
@@ -107,6 +159,7 @@ chassis_profiles:
      - task_code: chassis.differential.rotate_left
        display_name: 差速左原地旋转
        primitive_type: in_place_rotation
        reference_path_id: chassis_rotate_left_90deg
        metadata:
          in_place_rotation.target_yaw_deg: 90.0
          in_place_rotation.target_angular_vel_deg_s: 10.0
@@ -115,6 +168,7 @@ chassis_profiles:
      - task_code: chassis.differential.rotate_right
        display_name: 差速右原地旋转
        primitive_type: in_place_rotation
        reference_path_id: chassis_rotate_right_90deg
        metadata:
          in_place_rotation.target_yaw_deg: -90.0
          in_place_rotation.target_angular_vel_deg_s: 10.0
@@ -123,6 +177,7 @@ chassis_profiles:
      - task_code: chassis.differential.arc_left
        display_name: 差速左转圆弧
        primitive_type: arc
        reference_path_id: chassis_arc_left_r1_45deg
        metadata:
          arc.target_speed_ms: 0.08
          arc.radius_m: 1.0
@@ -133,6 +188,7 @@ chassis_profiles:
      - task_code: chassis.differential.arc_right
        display_name: 差速右转圆弧
        primitive_type: arc
        reference_path_id: chassis_arc_right_r1_45deg
        metadata:
          arc.target_speed_ms: 0.08
          arc.radius_m: 1.0
@@ -152,6 +208,7 @@ chassis_profiles:
      - task_code: chassis.single_steer.straight_forward
        display_name: 单舵轮直线前进
        primitive_type: straight_line
        reference_path_id: chassis_straight_forward_1m
        metadata:
          straight_line.target_distance_m: 1.0
          straight_line.target_speed_ms: 0.1
@@ -161,6 +218,7 @@ chassis_profiles:
      - task_code: chassis.single_steer.steering_sweep
        display_name: 单舵轮舵角扫动
        primitive_type: steering_sweep
        reference_path_id: chassis_static_station
        metadata:
          steering_sweep.target_angle_deg: 0.0
          steering_sweep.sweep_amplitude_deg: 10.0
@@ -171,6 +229,7 @@ chassis_profiles:
      - task_code: chassis.single_steer.arc_left
        display_name: 单舵轮左圆弧
        primitive_type: arc
        reference_path_id: chassis_arc_left_r1_45deg
        metadata:
          arc.target_speed_ms: 0.08
          arc.radius_m: 1.0
@@ -181,6 +240,7 @@ chassis_profiles:
      - task_code: chassis.single_steer.arc_right
        display_name: 单舵轮右圆弧
        primitive_type: arc
        reference_path_id: chassis_arc_right_r1_45deg
        metadata:
          arc.target_speed_ms: 0.08
          arc.radius_m: 1.0
@@ -200,6 +260,7 @@ chassis_profiles:
      - task_code: chassis.multi_steer.straight_forward
        display_name: 多舵轮直线前进
        primitive_type: straight_line
        reference_path_id: chassis_straight_forward_1m
        metadata:
          straight_line.target_distance_m: 1.0
          straight_line.target_speed_ms: 0.1
@@ -209,6 +270,7 @@ chassis_profiles:
      - task_code: chassis.multi_steer.lateral_left
        display_name: 多舵轮左横移
        primitive_type: lateral_translation
        reference_path_id: chassis_lateral_left_0_5m
        metadata:
          lateral_translation.target_speed_ms: 0.06
          lateral_translation.target_distance_m: 0.5
@@ -218,6 +280,7 @@ chassis_profiles:
      - task_code: chassis.multi_steer.lateral_right
        display_name: 多舵轮右横移
        primitive_type: lateral_translation
        reference_path_id: chassis_lateral_right_0_5m
        metadata:
          lateral_translation.target_speed_ms: 0.06
          lateral_translation.target_distance_m: 0.5
@@ -227,6 +290,7 @@ chassis_profiles:
      - task_code: chassis.multi_steer.diagonal_forward_left
        display_name: 多舵轮左前斜移
        primitive_type: diagonal_motion
        reference_path_id: chassis_diagonal_forward_left_0_5m
        metadata:
          diagonal_motion.target_speed_ms: 0.06
          diagonal_motion.target_distance_m: 0.5
@@ -236,6 +300,7 @@ chassis_profiles:
      - task_code: chassis.multi_steer.diagonal_forward_right
        display_name: 多舵轮右前斜移
        primitive_type: diagonal_motion
        reference_path_id: chassis_diagonal_forward_right_0_5m
        metadata:
          diagonal_motion.target_speed_ms: 0.06
          diagonal_motion.target_distance_m: 0.5
@@ -245,6 +310,7 @@ chassis_profiles:
      - task_code: chassis.multi_steer.module_alignment
        display_name: 多舵轮模块零位检查
        primitive_type: module_alignment
        reference_path_id: chassis_static_station
        metadata:
          module_alignment.module_ids:
            - front_left
@@ -258,6 +324,7 @@ chassis_profiles:
      - task_code: chassis.multi_steer.coordinated_steering
        display_name: 多舵轮协同转向
        primitive_type: coordinated_steering
        reference_path_id: chassis_static_station
        metadata:
          coordinated_steering.module_ids:
            - front_left
@@ -0,0 +1,36 @@
 # Isaac 仿真底盘路径执行/采集配置。
 # 用于验证：选择底盘 reference path -> 执行动作 -> 采集底盘遥测和 external truth。
 csv_contract_version: 1
 chassis_type: ackermann
 action_profile_file: src/site_deployment/workshop_chassis_calibration_real/config/chassis_action_profile.yaml
 session_id: sim_chassis_path_test
 site_id: isaac_workcell_zone_a
 vehicle_id: demo_agv_001
 session_dir: /tmp/agv_calib_chassis_sim/session_latest
 dataset_index_path: /tmp/agv_calib_chassis_sim/session_latest/dataset_index.yaml
 files:
  chassis_motion_data_file: chassis/chassis_motion.csv
  actuator_command_file: chassis/actuator_commands.csv
  truth_trajectory_file: external/truth_trajectory.csv
  diagnostics_file: chassis/chassis_diagnostics.json
 capture:
  chassis_telemetry_topic: /chassis/telemetry
  external_pose_topic: /isaac/external_localization/vehicle/pose
  ackermann_command_topic: /vehicle/demo_agv_001/internal/ackermann_cmd
  command_source: auto
 validation:
  min_chassis_motion_samples: 2
  min_actuator_command_samples: 1
  min_truth_trajectory_samples: 2
  min_motion_distance_m: 0.01
  max_time_gap_ms: 300.0
 synthetic:
  enabled: true
  sample_period_ms: 100
  target_speed_ms: 0.1
  target_distance_m: 0.2
@@ -0,0 +1,47 @@
 schema_version: 1
 path_id: chassis_ackermann_s_curve_r1_5
 display_name: 阿克曼 S 形路径 R1.5
 module_type: chassis
 frame_id: workshop
 path_type: s_curve
 recommended_task_types:
  - arc
  - s_curve
 description: 由左 25deg、右 60deg、左 60deg、右 25deg 四段 R1.5 圆弧组成的低速阿克曼 S 形路径。
 points:
  - x_m: 0.000
    y_m: 0.000
    yaw_rad: 0.000
    target_speed_ms: 0.10
  - x_m: 0.325
    y_m: 0.036
    yaw_rad: 0.218
    target_speed_ms: 0.10
  - x_m: 0.634
    y_m: 0.141
    yaw_rad: 0.436
    target_speed_ms: 0.10
  - x_m: 1.399
    y_m: 0.275
    yaw_rad: -0.087
    target_speed_ms: 0.10
  - x_m: 2.128
    y_m: 0.010
    yaw_rad: -0.611
    target_speed_ms: 0.10
  - x_m: 2.858
    y_m: -0.256
    yaw_rad: -0.087
    target_speed_ms: 0.10
  - x_m: 3.623
    y_m: -0.121
    yaw_rad: 0.436
    target_speed_ms: 0.10
  - x_m: 3.932
    y_m: -0.016
    yaw_rad: 0.218
    target_speed_ms: 0.10
  - x_m: 4.256
    y_m: 0.020
    yaw_rad: 0.000
    target_speed_ms: 0.10
@@ -0,0 +1,21 @@
 schema_version: 1
 path_id: chassis_arc_left_r1_45deg
 display_name: 底盘左圆弧 R1 45deg
 module_type: chassis
 frame_id: workshop
 path_type: arc
 recommended_task_types:
  - arc
 points:
  - x_m: 0.0
    y_m: 0.0
    yaw_rad: 0.0
    target_speed_ms: 0.08
  - x_m: 0.383
    y_m: 0.076
    yaw_rad: 0.393
    target_speed_ms: 0.08
  - x_m: 0.707
    y_m: 0.293
    yaw_rad: 0.785
    target_speed_ms: 0.08
@@ -0,0 +1,21 @@
 schema_version: 1
 path_id: chassis_arc_right_r1_45deg
 display_name: 底盘右圆弧 R1 45deg
 module_type: chassis
 frame_id: workshop
 path_type: arc
 recommended_task_types:
  - arc
 points:
  - x_m: 0.0
    y_m: 0.0
    yaw_rad: 0.0
    target_speed_ms: 0.08
  - x_m: 0.383
    y_m: -0.076
    yaw_rad: -0.393
    target_speed_ms: 0.08
  - x_m: 0.707
    y_m: -0.293
    yaw_rad: -0.785
    target_speed_ms: 0.08
@@ -0,0 +1,17 @@
 schema_version: 1
 path_id: chassis_diagonal_forward_left_0_5m
 display_name: 多舵轮左前斜移 0.5m
 module_type: chassis
 frame_id: workshop
 path_type: diagonal_motion
 recommended_task_types:
  - diagonal_motion
 points:
  - x_m: 0.0
    y_m: 0.0
    yaw_rad: 0.0
    target_speed_ms: 0.06
  - x_m: 0.354
    y_m: 0.354
    yaw_rad: 0.0
    target_speed_ms: 0.06
@@ -0,0 +1,17 @@
 schema_version: 1
 path_id: chassis_diagonal_forward_right_0_5m
 display_name: 多舵轮右前斜移 0.5m
 module_type: chassis
 frame_id: workshop
 path_type: diagonal_motion
 recommended_task_types:
  - diagonal_motion
 points:
  - x_m: 0.0
    y_m: 0.0
    yaw_rad: 0.0
    target_speed_ms: 0.06
  - x_m: 0.354
    y_m: -0.354
    yaw_rad: 0.0
    target_speed_ms: 0.06
@@ -0,0 +1,17 @@
 schema_version: 1
 path_id: chassis_lateral_left_0_5m
 display_name: 多舵轮左横移 0.5m
 module_type: chassis
 frame_id: workshop
 path_type: lateral_translation
 recommended_task_types:
  - lateral_translation
 points:
  - x_m: 0.0
    y_m: 0.0
    yaw_rad: 0.0
    target_speed_ms: 0.06
  - x_m: 0.0
    y_m: 0.5
    yaw_rad: 0.0
    target_speed_ms: 0.06
@@ -0,0 +1,17 @@
 schema_version: 1
 path_id: chassis_lateral_right_0_5m
 display_name: 多舵轮右横移 0.5m
 module_type: chassis
 frame_id: workshop
 path_type: lateral_translation
 recommended_task_types:
  - lateral_translation
 points:
  - x_m: 0.0
    y_m: 0.0
    yaw_rad: 0.0
    target_speed_ms: 0.06
  - x_m: 0.0
    y_m: -0.5
    yaw_rad: 0.0
    target_speed_ms: 0.06
@@ -0,0 +1,17 @@
 schema_version: 1
 path_id: chassis_rotate_left_90deg
 display_name: 底盘原地左转 90deg
 module_type: chassis
 frame_id: workshop
 path_type: in_place_rotation
 recommended_task_types:
  - in_place_rotation
 points:
  - x_m: 0.0
    y_m: 0.0
    yaw_rad: 0.0
    target_speed_ms: 0.0
  - x_m: 0.0
    y_m: 0.0
    yaw_rad: 1.570796327
    target_speed_ms: 0.0
@@ -0,0 +1,17 @@
 schema_version: 1
 path_id: chassis_rotate_right_90deg
 display_name: 底盘原地右转 90deg
 module_type: chassis
 frame_id: workshop
 path_type: in_place_rotation
 recommended_task_types:
  - in_place_rotation
 points:
  - x_m: 0.0
    y_m: 0.0
    yaw_rad: 0.0
    target_speed_ms: 0.0
  - x_m: 0.0
    y_m: 0.0
    yaw_rad: -1.570796327
    target_speed_ms: 0.0
@@ -0,0 +1,15 @@
 schema_version: 1
 path_id: chassis_static_station
 display_name: 底盘静态检查点
 module_type: chassis
 frame_id: workshop
 path_type: static_station
 recommended_task_types:
  - steering_sweep
  - module_alignment
  - coordinated_steering
 points:
  - x_m: 0.0
    y_m: 0.0
    yaw_rad: 0.0
    target_speed_ms: 0.0
@@ -0,0 +1,18 @@
 schema_version: 1
 path_id: chassis_straight_forward_1m
 display_name: 底盘直线前进 1m
 description: 用于轮径、里程计比例和直线跑偏检查。
 module_type: chassis
 frame_id: workshop
 path_type: straight_line
 recommended_task_types:
  - straight_line
 points:
  - x_m: 0.0
    y_m: 0.0
    yaw_rad: 0.0
    target_speed_ms: 0.10
  - x_m: 1.0
    y_m: 0.0
    yaw_rad: 0.0
    target_speed_ms: 0.10
@@ -0,0 +1,17 @@
 schema_version: 1
 path_id: chassis_straight_reverse_0_6m
 display_name: 底盘直线倒车 0.6m
 module_type: chassis
 frame_id: workshop
 path_type: straight_line
 recommended_task_types:
  - straight_line
 points:
  - x_m: 0.0
    y_m: 0.0
    yaw_rad: 0.0
    target_speed_ms: 0.08
  - x_m: -0.6
    y_m: 0.0
    yaw_rad: 3.141592654
    target_speed_ms: 0.08
@@ -0,0 +1,19 @@
 profile_name: chassis_calibration_reference_paths
 frame_id: workshop
 default_path_id_by_chassis:
  ackermann: chassis_straight_forward_1m
  differential: chassis_straight_forward_1m
  single_steer_wheel: chassis_straight_forward_1m
  multi_steer_wheel: chassis_lateral_left_0_5m
 default_path_id_by_task_type:
  straight_line: chassis_straight_forward_1m
  arc: chassis_arc_left_r1_45deg
  in_place_rotation: chassis_rotate_left_90deg
  s_curve: chassis_ackermann_s_curve_r1_5
  steering_sweep: chassis_static_station
  lateral_translation: chassis_lateral_left_0_5m
  diagonal_motion: chassis_diagonal_forward_left_0_5m
  module_alignment: chassis_static_station
  coordinated_steering: chassis_static_station
@@ -51,6 +51,7 @@ def parse_args() -> argparse.Namespace:
    )
    parser.add_argument("--chassis-type", default="", help="覆盖 chassis_type，并选择对应动作序列。")
    parser.add_argument("--task-code", default="", help="只执行指定 task_code；为空时执行该底盘类型全部动作。")
    parser.add_argument("--reference-path-id", default="", help="只执行绑定到指定 reference_path_id 的动作。")
    parser.add_argument("--session-id", default="", help="覆盖 session_id。")
    parser.add_argument("--site-id", default="", help="覆盖 site_id。")
    parser.add_argument("--vehicle-id", default="", help="覆盖 vehicle_id。")
@@ -130,14 +131,28 @@ def spin_until_future(
    return future.result()
-def select_actions(profile: dict[str, Any], chassis_type: str, task_code: str) -> list[dict[str, Any]]:
+def select_actions(
    profile: dict[str, Any],
    chassis_type: str,
    task_code: str,
    reference_path_id: str,
 ) -> list[dict[str, Any]]:
    section = profile["chassis_profiles"][chassis_type]
    actions = list(section["actions"])
-    if not task_code:
+    if task_code:
        actions = [action for action in actions if action.get("task_code") == task_code]
    if reference_path_id:
        actions = [action for action in actions if action.get("reference_path_id") == reference_path_id]
    if not task_code and not reference_path_id:
        return actions
-    selected = [action for action in actions if action.get("task_code") == task_code]
+    selected = actions
    if not selected:
-        raise ValueError(f"动作 profile 中没有 task_code={task_code!r}。")
+        detail = []
        if task_code:
            detail.append(f"task_code={task_code!r}")
        if reference_path_id:
            detail.append(f"reference_path_id={reference_path_id!r}")
        raise ValueError(f"动作 profile 中没有 {' 且 '.join(detail)} 的动作。")
    return selected
@@ -349,7 +364,7 @@ def main() -> int:
    profile_path = Path(args.action_profile).expanduser().resolve(strict=False)
    action_profile = validate_action_profile(load_action_profile_yaml(profile_path))
    chassis_type = str(config["chassis_type"])
-    actions = select_actions(action_profile, chassis_type, args.task_code)
+    actions = select_actions(action_profile, chassis_type, args.task_code, args.reference_path_id)
    data_capture = action_profile.get("data_capture", {}) or {}
    start_before_sec = (
        as_float(data_capture.get("start_before_motion_sec"), 1.0)
@@ -0,0 +1,322 @@
 #!/usr/bin/env python3
 """车间电脑侧 WiFi/TCP 底盘遥测接收桥。
 车端通过 TCP 推送 JSON 遥测，本节点转换成标准 ChassisTelemetry 并发布到 ROS2。
 """
 from __future__ import annotations
 import argparse
 import json
 import socket
 import socketserver
 import struct
 import threading
 import time
 from typing import Any
 try:
    import rclpy
    from calibration_chassis_interfaces.msg import ChassisTelemetry, WheelModuleState
 except ImportError:
    rclpy = None
    ChassisTelemetry = None
    WheelModuleState = None
 CHASSIS_TELEMETRY_PUSH_REQ = 7
 CHASSIS_TELEMETRY_PUSH_RSP = 8
 CHASSIS_TYPE_BY_NAME = {
    "ackermann": 1,
    "differential": 2,
    "single_steer_wheel": 3,
    "multi_steer_wheel": 4,
 }
 def now_us() -> int:
    return int(time.time() * 1_000_000)
 def read_exactly(conn: socket.socket, size: int) -> bytes:
    chunks: list[bytes] = []
    remaining = size
    while remaining > 0:
        chunk = conn.recv(remaining)
        if not chunk:
            raise ConnectionError("连接已关闭")
        chunks.append(chunk)
        remaining -= len(chunk)
    return b"".join(chunks)
 def send_frame(conn: socket.socket, msg_type: int, payload: dict[str, Any]) -> None:
    encoded = json.dumps(payload, ensure_ascii=False, separators=(",", ":")).encode("utf-8")
    conn.sendall(struct.pack("<II", msg_type, len(encoded)) + encoded)
 def as_float(value: Any, default: float = 0.0) -> float:
    if value in (None, ""):
        return default
    return float(value)
 def as_int(value: Any, default: int = 0) -> int:
    if value in (None, ""):
        return default
    return int(float(value))
 def as_bool(value: Any, default: bool = False) -> bool:
    if value in (None, ""):
        return default
    if isinstance(value, bool):
        return value
    return str(value).strip().lower() in {"1", "true", "yes", "on"}
 def chassis_type_value(value: Any, default_name: str) -> int:
    if isinstance(value, dict):
        value = value.get("value", "")
    if value in (None, ""):
        return CHASSIS_TYPE_BY_NAME.get(default_name, 0)
    if isinstance(value, (int, float)):
        return int(value)
    text = str(value).strip().lower()
    if text.isdigit():
        return int(text)
    return CHASSIS_TYPE_BY_NAME.get(text, CHASSIS_TYPE_BY_NAME.get(default_name, 0))
 def nested_payload(payload: dict[str, Any]) -> dict[str, Any]:
    for key in ("telemetry", "chassis_telemetry", "chassis"):
        value = payload.get(key)
        if isinstance(value, dict):
            return value
    return payload
 def module_payloads(payload: dict[str, Any]) -> list[dict[str, Any]]:
    raw = payload.get("modules", payload.get("module_states", []))
    if isinstance(raw, dict):
        modules = []
        for module_id, value in raw.items():
            if isinstance(value, dict):
                modules.append({"module_id": module_id, **value})
        return modules
    if isinstance(raw, list):
        return [value for value in raw if isinstance(value, dict)]
    return []
 def build_module_msg(payload: dict[str, Any]) -> Any:
    module = WheelModuleState()
    module.module_id = str(payload.get("module_id", payload.get("id", "")))
    module.encoder_ticks = as_int(payload.get("encoder_ticks", payload.get("ticks", 0)))
    module.wheel_speed_rpm = as_float(payload.get("wheel_speed_rpm", payload.get("rpm", 0.0)))
    module.steer_angle_deg = as_float(payload.get("steer_angle_deg", payload.get("steer_deg", 0.0)))
    module.motor_current_amp = as_float(payload.get("motor_current_amp", payload.get("current_amp", 0.0)))
    return module
 def build_chassis_telemetry(payload: dict[str, Any], default_chassis_type: str) -> Any:
    data = nested_payload(payload)
    msg = ChassisTelemetry()
    raw_timestamp = data.get("hardware_timestamp_us", data.get("timestamp_us", data.get("timestamp", "")))
    msg.hardware_timestamp_us = as_int(raw_timestamp, now_us())
    msg.chassis_type.value = chassis_type_value(data.get("chassis_type", ""), default_chassis_type)
    msg.odom_x_m = as_float(data.get("odom_x_m", data.get("x_m", 0.0)))
    msg.odom_y_m = as_float(data.get("odom_y_m", data.get("y_m", 0.0)))
    msg.odom_yaw_rad = as_float(data.get("odom_yaw_rad", data.get("yaw_rad", 0.0)))
    msg.linear_velocity_ms = as_float(data.get("linear_velocity_ms", data.get("linear_speed_ms", 0.0)))
    msg.angular_velocity_rads = as_float(data.get("angular_velocity_rads", data.get("angular_speed_rads", 0.0)))
    msg.estop_engaged = as_bool(data.get("estop_engaged", data.get("estop", False)))
    msg.driver_error_code = as_int(data.get("driver_error_code", data.get("error_code", 0)))
    msg.active_job_id = str(data.get("active_job_id", data.get("job_id", "")))
    msg.lateral_slip_estimate = as_float(data.get("lateral_slip_estimate", 0.0))
    msg.curvature_estimate = as_float(data.get("curvature_estimate", 0.0))
    msg.modules = [build_module_msg(module) for module in module_payloads(data)]
    return msg
 class TelemetryTCPServer(socketserver.ThreadingMixIn, socketserver.TCPServer):
    allow_reuse_address = True
    daemon_threads = True
 class TelemetryHandler(socketserver.BaseRequestHandler):
    def handle(self) -> None:
        self.server.bridge.handle_connection(self.request, self.client_address)
 class WorkshopChassisTelemetryBridge:
    def __init__(self, args: argparse.Namespace) -> None:
        if rclpy is None or ChassisTelemetry is None or WheelModuleState is None:
            raise RuntimeError("缺少 ROS 2 Python 依赖或 calibration_chassis_interfaces。")
        self.args = args
        self.node = rclpy.create_node("workshop_chassis_telemetry_bridge")
        self.publisher = self.node.create_publisher(ChassisTelemetry, args.output_topic, 50)
        self.received_count = 0
        self.published_count = 0
        self.error_count = 0
        self.lock = threading.Lock()
        self.last_stats_log_monotonic = 0.0
        self.server = TelemetryTCPServer((args.bind_host, args.bind_port), TelemetryHandler)
        self.server.bridge = self
        self.thread = threading.Thread(target=self.server.serve_forever, name="chassis-telemetry-bridge", daemon=True)
    def start(self) -> None:
        self.thread.start()
        self.node.get_logger().info(
            "底盘遥测 WiFi/TCP bridge 已启动: "
            f"{self.args.bind_host}:{self.args.bind_port} -> {self.args.output_topic}, "
            f"protocol={self.args.protocol}"
        )
    def shutdown(self) -> None:
        self.server.shutdown()
        self.server.server_close()
    def log_stats(self) -> None:
        now = time.monotonic()
        if now - self.last_stats_log_monotonic < self.args.stats_log_interval_sec:
            return
        self.last_stats_log_monotonic = now
        self.node.get_logger().info(
            "底盘遥测 bridge 统计: "
            f"received={self.received_count}, published={self.published_count}, errors={self.error_count}"
        )
    def detect_protocol(self, conn: socket.socket) -> str:
        if self.args.protocol != "auto":
            return self.args.protocol
        peek = conn.recv(8, socket.MSG_PEEK)
        first = peek.lstrip()[:1]
        if first in (b"{", b"["):
            return "json_lines"
        return "frame"
    def handle_connection(self, conn: socket.socket, address: tuple[str, int]) -> None:
        conn.settimeout(self.args.connection_timeout_sec)
        try:
            protocol = self.detect_protocol(conn)
            if protocol == "frame":
                self.handle_frame_connection(conn)
            elif protocol == "json_lines":
                self.handle_json_lines_connection(conn)
            elif protocol == "raw_json":
                self.handle_raw_json_connection(conn)
            else:
                raise ValueError(f"未知协议: {protocol}")
        except ConnectionError:
            return
        except Exception as exc:
            with self.lock:
                self.error_count += 1
            self.node.get_logger().warning(f"底盘遥测连接处理失败 {address}: {exc}")
    def publish_payload(self, payload: dict[str, Any]) -> None:
        msg = build_chassis_telemetry(payload, self.args.default_chassis_type)
        self.publisher.publish(msg)
        with self.lock:
            self.received_count += 1
            self.published_count += 1
        self.log_stats()
    def handle_frame_connection(self, conn: socket.socket) -> None:
        while rclpy.ok():
            header = read_exactly(conn, 8)
            msg_type, payload_len = struct.unpack("<II", header)
            if payload_len > self.args.max_payload_bytes:
                raise ValueError(f"payload 过大: {payload_len} > {self.args.max_payload_bytes}")
            raw_payload = read_exactly(conn, payload_len) if payload_len else b"{}"
            payload = json.loads(raw_payload.decode("utf-8"))
            if self.args.expected_msg_type and msg_type != self.args.expected_msg_type:
                raise ValueError(f"msg_type 不匹配: expected={self.args.expected_msg_type}, actual={msg_type}")
            self.publish_payload(payload)
            if self.args.send_ack:
                send_frame(
                    conn,
                    self.args.ack_msg_type,
                    {"success": True, "message": "chassis telemetry accepted", "timestamp_us": now_us()},
                )
    def handle_json_lines_connection(self, conn: socket.socket) -> None:
        with conn.makefile("rb") as stream:
            for raw_line in stream:
                line = raw_line.strip()
                if not line:
                    continue
                if len(line) > self.args.max_payload_bytes:
                    raise ValueError(f"payload 过大: {len(line)} > {self.args.max_payload_bytes}")
                payload = json.loads(line.decode("utf-8"))
                self.publish_payload(payload)
                if self.args.send_ack:
                    conn.sendall(
                        (json.dumps({"success": True, "timestamp_us": now_us()}, separators=(",", ":")) + "\n").encode(
                            "utf-8"
                        )
                    )
    def handle_raw_json_connection(self, conn: socket.socket) -> None:
        chunks: list[bytes] = []
        total = 0
        while True:
            chunk = conn.recv(4096)
            if not chunk:
                break
            chunks.append(chunk)
            total += len(chunk)
            if total > self.args.max_payload_bytes:
                raise ValueError(f"payload 过大: {total} > {self.args.max_payload_bytes}")
        if not chunks:
            return
        payload = json.loads(b"".join(chunks).decode("utf-8"))
        self.publish_payload(payload)
        if self.args.send_ack:
            conn.sendall(json.dumps({"success": True, "timestamp_us": now_us()}).encode("utf-8"))
 def parse_args() -> argparse.Namespace:
    parser = argparse.ArgumentParser(description="车间电脑侧 WiFi/TCP 底盘遥测 -> ROS2 /chassis/telemetry bridge")
    parser.add_argument("--bind-host", default="0.0.0.0")
    parser.add_argument("--bind-port", type=int, default=9010)
    parser.add_argument("--output-topic", default="/chassis/telemetry")
    parser.add_argument("--protocol", choices=["auto", "frame", "json_lines", "raw_json"], default="frame")
    parser.add_argument("--expected-msg-type", type=int, default=0, help="0 表示接受任意帧类型；建议车端使用 7。")
    parser.add_argument("--ack-msg-type", type=int, default=CHASSIS_TELEMETRY_PUSH_RSP)
    parser.add_argument("--send-ack", action=argparse.BooleanOptionalAction, default=True)
    parser.add_argument("--default-chassis-type", choices=sorted(CHASSIS_TYPE_BY_NAME), default="ackermann")
    parser.add_argument("--max-payload-bytes", type=int, default=262144)
    parser.add_argument("--connection-timeout-sec", type=float, default=30.0)
    parser.add_argument("--stats-log-interval-sec", type=float, default=10.0)
    return parser.parse_args()
 def main() -> int:
    args = parse_args()
    if rclpy is None:
        print("[错误] 缺少 ROS 2 Python 依赖，无法启动底盘遥测 bridge。")
        return 1
    rclpy.init(args=None)
    bridge = None
    try:
        bridge = WorkshopChassisTelemetryBridge(args)
        bridge.start()
        while rclpy.ok():
            rclpy.spin_once(bridge.node, timeout_sec=0.2)
    except KeyboardInterrupt:
        pass
    finally:
        if bridge is not None:
            bridge.shutdown()
            bridge.node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()
    return 0
 if __name__ == "__main__":
    raise SystemExit(main())
@@ -95,7 +95,15 @@ python3 src/site_deployment/workshop_control_calibration_real/capture_control_se
 src/site_deployment/workshop_control_calibration_real/config/control_evaluation_profile.yaml
 ```
-该文件按 `ackermann`、`differential`、`single_steer_wheel`、`multi_steer_wheel` 拆分，固定每类底盘建议跑的轨迹跟踪、速度阶跃、加减速和停车精度任务。每个任务同时声明 `control_axis`、`controller_algorithm`、`control_role` 和必要的外部真值质量门限。
+该文件按 `ackermann`、`differential`、`single_steer_wheel`、`multi_steer_wheel` 拆分，固定每类底盘建议跑的轨迹跟踪、速度阶跃、加减速和停车精度任务。每个任务同时声明 `control_axis`、`controller_algorithm`、`control_role`、`reference_path_id` 和必要的外部真值质量门限。
 运控参考路径单独放在：
 ```bash
 src/site_deployment/workshop_control_calibration_real/reference_paths/
 ```
 每条路径一个 YAML 文件，必须配置 `path_id` 和中文 `display_name`。导出 `requested_tasks` 时，`reference_path_id` 会被展开成 `reference_path.*` metadata；轨迹跟踪任务还会同步展开为现有 `traj_pt_*` 字段，供运控执行链路读取。
 校验并查看摘要：
@@ -4,6 +4,7 @@
 schema_version: 1
 profile_name: workshop_control_evaluation_profile
 reference_path_dir: ../reference_paths
 safety:
  max_linear_speed_ms: 0.2
@@ -34,6 +35,7 @@ chassis_profiles:
      - task_code: control.ackermann.path_tracking_s_curve
        display_name: 阿克曼低速 S 形轨迹跟踪
        selected_task: trajectory_tracking
        reference_path_id: control_s_curve_low_speed
        control_axis: combined
        controller_algorithm: pure_pursuit
        control_role: path_tracking_outer_loop
@@ -47,26 +49,10 @@ chassis_profiles:
          segment_index: 0
          total_segments: 1
          is_final_segment: true
          path:
            - x_m: 0.0
              y_m: 0.0
              yaw_rad: 0.0
              target_speed_ms: 0.10
            - x_m: 0.6
              y_m: 0.10
              yaw_rad: 0.12
              target_speed_ms: 0.12
            - x_m: 1.2
              y_m: -0.10
              yaw_rad: -0.12
              target_speed_ms: 0.12
            - x_m: 1.8
              y_m: 0.0
              yaw_rad: 0.0
              target_speed_ms: 0.10
      - task_code: control.ackermann.speed_step_low
        display_name: 阿克曼低速速度阶跃
        selected_task: velocity_step
        reference_path_id: control_line_low_speed
        control_axis: longitudinal_control
        controller_algorithm: pid
        control_role: speed_loop
@@ -78,6 +64,7 @@ chassis_profiles:
      - task_code: control.ackermann.steering_response_arc
        display_name: 阿克曼转角响应轨迹
        selected_task: trajectory_tracking
        reference_path_id: control_arc_low_speed
        control_axis: lateral_control
        controller_algorithm: pid
        control_role: steering_angle_inner_loop
@@ -89,22 +76,10 @@ chassis_profiles:
          required_external_pose_source_id: workshop_external_localization
          max_external_pose_age_ms: 100.0
          min_external_pose_quality_score: 0.7
          path:
            - x_m: 0.0
              y_m: 0.0
              yaw_rad: 0.0
              target_speed_ms: 0.08
            - x_m: 0.5
              y_m: 0.08
              yaw_rad: 0.15
              target_speed_ms: 0.08
            - x_m: 1.0
              y_m: 0.28
              yaw_rad: 0.30
              target_speed_ms: 0.08
      - task_code: control.ackermann.stop_accuracy
        display_name: 阿克曼停车精度
        selected_task: stop_accuracy
        reference_path_id: control_stop_accuracy_1_2m
        control_axis: longitudinal_control
        controller_algorithm: pid
        control_role: speed_loop
@@ -126,6 +101,7 @@ chassis_profiles:
      - task_code: control.differential.path_tracking_line
        display_name: 差速低速直线轨迹跟踪
        selected_task: trajectory_tracking
        reference_path_id: control_line_low_speed
        control_axis: combined
        controller_algorithm: pure_pursuit
        control_role: path_tracking_outer_loop
@@ -136,22 +112,10 @@ chassis_profiles:
          required_external_pose_source_id: workshop_external_localization
          max_external_pose_age_ms: 100.0
          min_external_pose_quality_score: 0.7
          path:
            - x_m: 0.0
              y_m: 0.0
              yaw_rad: 0.0
              target_speed_ms: 0.10
            - x_m: 0.8
              y_m: 0.0
              yaw_rad: 0.0
              target_speed_ms: 0.12
            - x_m: 1.6
              y_m: 0.0
              yaw_rad: 0.0
              target_speed_ms: 0.10
      - task_code: control.differential.speed_step_low
        display_name: 差速低速速度阶跃
        selected_task: velocity_step
        reference_path_id: control_line_low_speed
        control_axis: longitudinal_control
        controller_algorithm: pid
        control_role: speed_loop
@@ -163,6 +127,7 @@ chassis_profiles:
      - task_code: control.differential.yaw_rate_arc
        display_name: 差速角速度响应圆弧
        selected_task: trajectory_tracking
        reference_path_id: control_arc_low_speed
        control_axis: lateral_control
        controller_algorithm: pid
        control_role: yaw_rate_loop
@@ -174,22 +139,10 @@ chassis_profiles:
          required_external_pose_source_id: workshop_external_localization
          max_external_pose_age_ms: 100.0
          min_external_pose_quality_score: 0.7
          path:
            - x_m: 0.0
              y_m: 0.0
              yaw_rad: 0.0
              target_speed_ms: 0.08
            - x_m: 0.45
              y_m: 0.08
              yaw_rad: 0.18
              target_speed_ms: 0.08
            - x_m: 0.85
              y_m: 0.30
              yaw_rad: 0.36
              target_speed_ms: 0.08
      - task_code: control.differential.accel_decel_low
        display_name: 差速低速加减速响应
        selected_task: acceleration_deceleration
        reference_path_id: control_line_low_speed
        control_axis: longitudinal_control
        controller_algorithm: pid
        control_role: acceleration_loop
@@ -211,6 +164,7 @@ chassis_profiles:
      - task_code: control.single_steer.path_tracking_arc
        display_name: 单舵轮低速圆弧轨迹跟踪
        selected_task: trajectory_tracking
        reference_path_id: control_arc_low_speed
        control_axis: combined
        controller_algorithm: pure_pursuit
        control_role: path_tracking_outer_loop
@@ -221,22 +175,10 @@ chassis_profiles:
          required_external_pose_source_id: workshop_external_localization
          max_external_pose_age_ms: 100.0
          min_external_pose_quality_score: 0.7
          path:
            - x_m: 0.0
              y_m: 0.0
              yaw_rad: 0.0
              target_speed_ms: 0.09
            - x_m: 0.55
              y_m: 0.08
              yaw_rad: 0.12
              target_speed_ms: 0.10
            - x_m: 1.10
              y_m: 0.28
              yaw_rad: 0.24
              target_speed_ms: 0.09
      - task_code: control.single_steer.speed_step_low
        display_name: 单舵轮低速速度阶跃
        selected_task: velocity_step
        reference_path_id: control_line_low_speed
        control_axis: longitudinal_control
        controller_algorithm: pid
        control_role: speed_loop
@@ -248,6 +190,7 @@ chassis_profiles:
      - task_code: control.single_steer.steering_response_s_curve
        display_name: 单舵轮舵角响应 S 形轨迹
        selected_task: trajectory_tracking
        reference_path_id: control_s_curve_low_speed
        control_axis: lateral_control
        controller_algorithm: pid
        control_role: steering_angle_inner_loop
@@ -259,26 +202,10 @@ chassis_profiles:
          required_external_pose_source_id: workshop_external_localization
          max_external_pose_age_ms: 100.0
          min_external_pose_quality_score: 0.7
          path:
            - x_m: 0.0
              y_m: 0.0
              yaw_rad: 0.0
              target_speed_ms: 0.08
            - x_m: 0.5
              y_m: 0.10
              yaw_rad: 0.12
              target_speed_ms: 0.08
            - x_m: 1.0
              y_m: -0.10
              yaw_rad: -0.12
              target_speed_ms: 0.08
            - x_m: 1.5
              y_m: 0.0
              yaw_rad: 0.0
              target_speed_ms: 0.08
      - task_code: control.single_steer.stop_accuracy
        display_name: 单舵轮停车精度
        selected_task: stop_accuracy
        reference_path_id: control_stop_accuracy_1_2m
        control_axis: longitudinal_control
        controller_algorithm: pid
        control_role: speed_loop
@@ -300,6 +227,7 @@ chassis_profiles:
      - task_code: control.multi_steer.path_tracking_lateral_offset
        display_name: 多舵轮横向偏移轨迹跟踪
        selected_task: trajectory_tracking
        reference_path_id: control_lateral_offset_low_speed
        control_axis: combined
        controller_algorithm: mpc
        control_role: path_tracking_outer_loop
@@ -310,26 +238,10 @@ chassis_profiles:
          required_external_pose_source_id: workshop_external_localization
          max_external_pose_age_ms: 100.0
          min_external_pose_quality_score: 0.7
          path:
            - x_m: 0.0
              y_m: 0.0
              yaw_rad: 0.0
              target_speed_ms: 0.08
            - x_m: 0.4
              y_m: 0.2
              yaw_rad: 0.0
              target_speed_ms: 0.08
            - x_m: 0.8
              y_m: 0.2
              yaw_rad: 0.0
              target_speed_ms: 0.08
            - x_m: 1.2
              y_m: 0.0
              yaw_rad: 0.0
              target_speed_ms: 0.08
      - task_code: control.multi_steer.wheel_speed_step_low
        display_name: 多舵轮轮速阶跃
        selected_task: velocity_step
        reference_path_id: control_line_low_speed
        control_axis: longitudinal_control
        controller_algorithm: pid
        control_role: wheel_speed_inner_loop
@@ -341,6 +253,7 @@ chassis_profiles:
      - task_code: control.multi_steer.module_steering_response
        display_name: 多舵轮模块转角响应轨迹
        selected_task: trajectory_tracking
        reference_path_id: control_lateral_offset_low_speed
        control_axis: lateral_control
        controller_algorithm: pid
        control_role: module_steering_inner_loop
@@ -352,26 +265,10 @@ chassis_profiles:
          required_external_pose_source_id: workshop_external_localization
          max_external_pose_age_ms: 100.0
          min_external_pose_quality_score: 0.7
          path:
            - x_m: 0.0
              y_m: 0.0
              yaw_rad: 0.0
              target_speed_ms: 0.06
            - x_m: 0.3
              y_m: 0.18
              yaw_rad: 0.0
              target_speed_ms: 0.06
            - x_m: 0.6
              y_m: -0.18
              yaw_rad: 0.0
              target_speed_ms: 0.06
            - x_m: 0.9
              y_m: 0.0
              yaw_rad: 0.0
              target_speed_ms: 0.06
      - task_code: control.multi_steer.stop_accuracy
        display_name: 多舵轮停车精度
        selected_task: stop_accuracy
        reference_path_id: control_stop_accuracy_1_2m
        control_axis: longitudinal_control
        controller_algorithm: pid
        control_role: wheel_speed_inner_loop
@@ -16,6 +16,12 @@ except ImportError as exc:
 SCRIPT_DIR = Path(__file__).resolve().parent
 if str(SCRIPT_DIR) not in sys.path:
    sys.path.insert(0, str(SCRIPT_DIR))
 REFERENCE_PATH_TOOL_DIR = SCRIPT_DIR.parents[0] / "workshop_reference_paths"
 if str(REFERENCE_PATH_TOOL_DIR) not in sys.path:
    sys.path.insert(0, str(REFERENCE_PATH_TOOL_DIR))
 from calibration_reference_paths import path_points_as_trajectory  # noqa: E402
 from calibration_reference_paths import selected_path_metadata  # noqa: E402
 from stage_control_parameter_commit import (
    CHASSIS_TYPES,
@@ -204,9 +210,15 @@ def validate_trajectory_payload(
    payload: dict[str, Any],
    action_name: str,
    safety: dict[str, float],
    allow_reference_path: bool = False,
 ) -> None:
    validate_positive(payload.get("timeout_sec"), f"{action_name}.trajectory_tracking.timeout_sec")
-    path = require_list(payload.get("path"), f"{action_name}.trajectory_tracking.path")
+    raw_path = payload.get("path")
    if raw_path in (None, ""):
        if not allow_reference_path:
            raise ValueError(f"{action_name}.trajectory_tracking.path 不能为空，或在动作上配置 reference_path_id。")
    else:
        path = require_list(raw_path, f"{action_name}.trajectory_tracking.path")
        if len(path) < 2:
            raise ValueError(f"{action_name}.trajectory_tracking.path 至少需要 2 个轨迹点。")
@@ -289,8 +301,15 @@ def validate_action(
    payload_key = TASK_PAYLOAD_KEYS[selected_task]
    payload = require_map(action.get(payload_key), f"{action_name}.{payload_key}")
    if action.get("reference_path_id") not in (None, ""):
        require_string(action.get("reference_path_id"), f"{action_name}.reference_path_id")
    if selected_task == "trajectory_tracking":
-        validate_trajectory_payload(payload, action_name, safety)
+        validate_trajectory_payload(
            payload,
            action_name,
            safety,
            allow_reference_path=action.get("reference_path_id") not in (None, ""),
        )
    elif selected_task == "velocity_step":
        validate_velocity_step_payload(payload, action_name, safety)
    elif selected_task in {"acceleration_deceleration", "accel_decel"}:
@@ -349,7 +368,11 @@ def add_if_present(metadata: dict[str, Any], key: str, payload: dict[str, Any],
        metadata[key] = payload[source_key]
-def flatten_trajectory_payload(metadata: dict[str, Any], payload: dict[str, Any]) -> None:
+def flatten_trajectory_payload(
    metadata: dict[str, Any],
    payload: dict[str, Any],
    selected_path_points: list[dict[str, float]] | None = None,
 ) -> None:
    metadata["control.stop_at_end"] = payload.get("stop_at_end", True)
    metadata["control.timeout_sec"] = payload["timeout_sec"]
    add_if_present(metadata, "trajectory_tracking.required_external_pose_source_id", payload, "required_external_pose_source_id")
@@ -360,17 +383,30 @@ def flatten_trajectory_payload(metadata: dict[str, Any], payload: dict[str, Any]
    add_if_present(metadata, "trajectory_tracking.total_segments", payload, "total_segments")
    add_if_present(metadata, "trajectory_tracking.is_final_segment", payload, "is_final_segment")
-    for index, point in enumerate(payload["path"]):
+    path = selected_path_points if selected_path_points is not None else payload["path"]
    for index, point in enumerate(path):
        metadata[f"traj_pt_{index}_x_m"] = point["x_m"]
        metadata[f"traj_pt_{index}_y_m"] = point["y_m"]
        metadata[f"traj_pt_{index}_yaw_rad"] = point["yaw_rad"]
        metadata[f"traj_pt_{index}_speed_ms"] = point["target_speed_ms"]
-def flatten_action_metadata(action: dict[str, Any], chassis_type: str) -> dict[str, Any]:
+def flatten_action_metadata(
    action: dict[str, Any],
    chassis_type: str,
    profile: dict[str, Any],
    profile_path: Path | None = None,
 ) -> dict[str, Any]:
    selected_task = action["selected_task"]
    payload_key = TASK_PAYLOAD_KEYS[selected_task]
    payload = action[payload_key]
    reference_metadata, selected_path = selected_path_metadata(
        profile.get("reference_path_dir", "reference_paths"),
        profile_path,
        str(action.get("reference_path_id", "")),
        chassis_type,
        TASK_TYPE_TO_METADATA[selected_task],
    )
    metadata: dict[str, Any] = {
        "control.task_type": TASK_TYPE_TO_METADATA[selected_task],
        "control.chassis_type": chassis_type,
@@ -379,9 +415,13 @@ def flatten_action_metadata(action: dict[str, Any], chassis_type: str) -> dict[s
        "control.role": action["control_role"],
    }
    add_if_present(metadata, "control.loop_name", action, "loop_name")
    metadata.update(reference_metadata)
    if selected_task == "trajectory_tracking":
-        flatten_trajectory_payload(metadata, payload)
+        selected_points = path_points_as_trajectory(selected_path)
        if len(selected_points) < 2:
            raise ValueError(f"{action['task_code']} 选择的 reference_path 至少需要 2 个点。")
        flatten_trajectory_payload(metadata, payload, selected_points)
    elif selected_task == "velocity_step":
        metadata["velocity_step.target_velocity_ms"] = payload["target_velocity_ms"]
        metadata["velocity_step.hold_time_sec"] = payload["hold_time_sec"]
@@ -399,11 +439,15 @@ def flatten_action_metadata(action: dict[str, Any], chassis_type: str) -> dict[s
    return metadata
-def export_requested_tasks(profile: dict[str, Any], chassis_type: str) -> dict[str, Any]:
+def export_requested_tasks(
    profile: dict[str, Any],
    chassis_type: str,
    profile_path: Path | None = None,
 ) -> dict[str, Any]:
    section = profile["chassis_profiles"][chassis_type]
    tasks: list[dict[str, Any]] = []
    for action in section["actions"]:
-        metadata = flatten_action_metadata(action, chassis_type)
+        metadata = flatten_action_metadata(action, chassis_type, profile, profile_path)
        task_params = [make_task_param(key, metadata[key]) for key in sorted(metadata)]
        tasks.append({
            "stage_type": "CONTROL_CALIBRATION_STAGE",
@@ -458,7 +502,7 @@ def main() -> int:
    if args.format == "requested_tasks":
        if not args.chassis_type:
            raise ValueError("--format requested_tasks 必须指定 --chassis-type。")
-        output = export_requested_tasks(profile, args.chassis_type)
+        output = export_requested_tasks(profile, args.chassis_type, profile_path)
    else:
        output = build_summary(profile)
@@ -0,0 +1,21 @@
 schema_version: 1
 path_id: control_arc_low_speed
 display_name: 运控低速圆弧跟踪
 module_type: control
 frame_id: workshop
 path_type: arc
 recommended_task_types:
  - trajectory_tracking
 points:
  - x_m: 0.0
    y_m: 0.0
    yaw_rad: 0.0
    target_speed_ms: 0.08
  - x_m: 0.5
    y_m: 0.08
    yaw_rad: 0.15
    target_speed_ms: 0.08
  - x_m: 1.0
    y_m: 0.28
    yaw_rad: 0.30
    target_speed_ms: 0.08
@@ -0,0 +1,25 @@
 schema_version: 1
 path_id: control_lateral_offset_low_speed
 display_name: 运控横向偏移低速跟踪
 module_type: control
 frame_id: workshop
 path_type: lateral_offset
 recommended_task_types:
  - trajectory_tracking
 points:
  - x_m: 0.0
    y_m: 0.0
    yaw_rad: 0.0
    target_speed_ms: 0.08
  - x_m: 0.4
    y_m: 0.2
    yaw_rad: 0.0
    target_speed_ms: 0.08
  - x_m: 0.8
    y_m: 0.2
    yaw_rad: 0.0
    target_speed_ms: 0.08
  - x_m: 1.2
    y_m: 0.0
    yaw_rad: 0.0
    target_speed_ms: 0.08
@@ -0,0 +1,23 @@
 schema_version: 1
 path_id: control_line_low_speed
 display_name: 运控低速直线跟踪
 module_type: control
 frame_id: workshop
 path_type: straight_line
 recommended_task_types:
  - trajectory_tracking
  - velocity_step
  - accel_decel
 points:
  - x_m: 0.0
    y_m: 0.0
    yaw_rad: 0.0
    target_speed_ms: 0.10
  - x_m: 0.8
    y_m: 0.0
    yaw_rad: 0.0
    target_speed_ms: 0.12
  - x_m: 1.6
    y_m: 0.0
    yaw_rad: 0.0
    target_speed_ms: 0.10
@@ -0,0 +1,25 @@
 schema_version: 1
 path_id: control_s_curve_low_speed
 display_name: 运控低速 S 形跟踪
 module_type: control
 frame_id: workshop
 path_type: s_curve
 recommended_task_types:
  - trajectory_tracking
 points:
  - x_m: 0.0
    y_m: 0.0
    yaw_rad: 0.0
    target_speed_ms: 0.10
  - x_m: 0.6
    y_m: 0.10
    yaw_rad: 0.12
    target_speed_ms: 0.12
  - x_m: 1.2
    y_m: -0.10
    yaw_rad: -0.12
    target_speed_ms: 0.12
  - x_m: 1.8
    y_m: 0.0
    yaw_rad: 0.0
    target_speed_ms: 0.10
@@ -0,0 +1,17 @@
 schema_version: 1
 path_id: control_stop_accuracy_1_2m
 display_name: 运控停车精度 1.2m
 module_type: control
 frame_id: workshop
 path_type: stop_accuracy
 recommended_task_types:
  - stop_accuracy
 points:
  - x_m: 0.0
    y_m: 0.0
    yaw_rad: 0.0
    target_speed_ms: 0.10
  - x_m: 1.2
    y_m: 0.0
    yaw_rad: 0.0
    target_speed_ms: 0.0
@@ -0,0 +1,15 @@
 profile_name: control_calibration_reference_paths
 frame_id: workshop
 default_path_id_by_chassis:
  ackermann: control_s_curve_low_speed
  differential: control_line_low_speed
  single_steer_wheel: control_arc_low_speed
  multi_steer_wheel: control_lateral_offset_low_speed
 default_path_id_by_task_type:
  trajectory_tracking: control_s_curve_low_speed
  velocity_step: control_line_low_speed
  accel_decel: control_line_low_speed
  acceleration_deceleration: control_line_low_speed
  stop_accuracy: control_stop_accuracy_1_2m
@@ -0,0 +1,51 @@
 # 标定参考路径工具
 三类标定任务都使用同一套路径 YAML 协议，但路径文件分别放在各自模块目录：
 - `src/site_deployment/workshop_chassis_calibration_real/reference_paths/`
 - `src/site_deployment/workshop_control_calibration_real/reference_paths/`
 - `src/site_deployment/workshop_sensor_calibration_real/reference_paths/`
 每条路径一个 YAML 文件，必须包含 `path_id` 和中文 `display_name`。`index.yaml` 只负责给 `chassis_type` 或任务类型配置默认路径，不存放路径点。
 查看某个目录中的可选路径：
 ```bash
 python3 src/site_deployment/workshop_reference_paths/calibration_reference_paths.py \
  --path-dir src/site_deployment/workshop_control_calibration_real/reference_paths
 ```
 导出一条路径的总控 metadata：
 ```bash
 python3 src/site_deployment/workshop_reference_paths/calibration_reference_paths.py \
  --path-dir src/site_deployment/workshop_control_calibration_real/reference_paths \
  --path-id control_s_curve_low_speed \
  --format metadata
 ```
 从外部真值话题录制新路径：
 ```bash
 python3 src/site_deployment/workshop_reference_paths/record_calibration_reference_path.py \
  --output-dir src/site_deployment/workshop_control_calibration_real/reference_paths \
  --path-id site_a_control_s_curve \
  --display-name 现场A运控S形路径 \
  --module-type control \
  --source external_pose \
  --duration-sec 20.0 \
  --target-speed-ms 0.10 \
  --recommended-task-type trajectory_tracking
 ```
 也可以从已有 CSV 生成路径，CSV 支持 `x_m/y_m/yaw_rad` 或 `odom_x_m/odom_y_m/odom_yaw_rad`：
 ```bash
 python3 src/site_deployment/workshop_reference_paths/record_calibration_reference_path.py \
  --output-dir src/site_deployment/workshop_chassis_calibration_real/reference_paths \
  --path-id site_a_chassis_line \
  --display-name 现场A底盘直线路径 \
  --module-type chassis \
  --input-csv /data/agv_calib/site_a/session_001/external/truth_trajectory.csv \
  --recommended-task-type straight_line
 ```
@@ -0,0 +1,377 @@
 #!/usr/bin/env python3
 """校验、选择和导出标定参考路径目录。"""
 from __future__ import annotations
 import argparse
 import sys
 from pathlib import Path
 from typing import Any
 try:
    import yaml
 except ImportError as exc:
    raise SystemExit("缺少 PyYAML，请先在当前 Python 环境安装 yaml 模块。") from exc
 SCRIPT_DIR = Path(__file__).resolve().parent
 SUPPORTED_CHASSIS_TYPES = {
    "ackermann",
    "differential",
    "single_steer_wheel",
    "multi_steer_wheel",
 }
 def parse_args() -> argparse.Namespace:
    parser = argparse.ArgumentParser(description="校验并导出标定参考路径目录。")
    parser.add_argument(
        "--path-dir",
        required=True,
        help="参考路径 YAML 文件目录。目录中每条路径一个 YAML 文件。",
    )
    parser.add_argument("--path-id", default="", help="要导出的路径 ID。")
    parser.add_argument(
        "--chassis-type",
        choices=sorted(SUPPORTED_CHASSIS_TYPES),
        default="",
        help="未指定 --path-id 时，从目录 index.yaml 的 default_path_id_by_chassis 中选择。",
    )
    parser.add_argument(
        "--task-type",
        default="",
        help="未指定 --path-id/--chassis-type 时，从目录 index.yaml 的 default_path_id_by_task_type 中选择。",
    )
    parser.add_argument(
        "--format",
        choices=["summary", "metadata", "path"],
        default="summary",
        help="summary 输出摘要；metadata 输出总控 task_params；path 输出单条路径。",
    )
    parser.add_argument("-o", "--output", default="", help="输出文件路径；不填写时输出到标准输出。")
    return parser.parse_args()
 def load_yaml(path: Path) -> dict[str, Any]:
    with path.open("r", encoding="utf-8") as stream:
        data = yaml.safe_load(stream)
    if data is None:
        return {}
    if not isinstance(data, dict):
        raise ValueError(f"{path} 的顶层结构必须是 YAML map。")
    return data
 def require_map(value: Any, field_name: str) -> dict[str, Any]:
    if not isinstance(value, dict):
        raise ValueError(f"{field_name} 必须是 YAML map。")
    return value
 def require_list(value: Any, field_name: str) -> list[Any]:
    if not isinstance(value, list):
        raise ValueError(f"{field_name} 必须是 YAML list。")
    return value
 def require_string(value: Any, field_name: str) -> str:
    if not isinstance(value, str) or not value.strip():
        raise ValueError(f"{field_name} 必须是非空字符串。")
    return value.strip()
 def as_float(value: Any, field_name: str) -> float:
    try:
        return float(value)
    except (TypeError, ValueError) as exc:
        raise ValueError(f"{field_name} 必须是数字，当前值为 {value!r}。") from exc
 def stringify_value(value: Any) -> str:
    if isinstance(value, bool):
        return "true" if value else "false"
    if isinstance(value, float):
        return f"{value:.9g}"
    return str(value)
 def resolve_path_dir(raw_path: str | Path, owner_profile_path: Path | None = None) -> Path:
    path = Path(str(raw_path)).expanduser()
    if path.is_absolute():
        return path.resolve(strict=False)
    if owner_profile_path is not None:
        owner_relative = (owner_profile_path.parent / path).resolve(strict=False)
        if owner_relative.exists():
            return owner_relative
    return path.resolve(strict=False)
 def path_yaml_files(path_dir: Path) -> list[Path]:
    if not path_dir.exists():
        raise FileNotFoundError(f"参考路径目录不存在: {path_dir}")
    if not path_dir.is_dir():
        raise ValueError(f"参考路径路径必须是目录: {path_dir}")
    return sorted(
        path
        for path in path_dir.glob("*.yaml")
        if path.name not in {"index.yaml", "_index.yaml"}
    )
 def load_path_index(path_dir: Path) -> dict[str, Any]:
    for filename in ("index.yaml", "_index.yaml"):
        index_path = path_dir / filename
        if index_path.exists():
            return load_yaml(index_path)
    return {}
 def validate_point(point_value: Any, path_name: str, point_index: int) -> None:
    point = require_map(point_value, f"{path_name}.points[{point_index}]")
    as_float(point.get("x_m"), f"{path_name}.points[{point_index}].x_m")
    as_float(point.get("y_m"), f"{path_name}.points[{point_index}].y_m")
    if "z_m" in point:
        as_float(point.get("z_m"), f"{path_name}.points[{point_index}].z_m")
    if "yaw_rad" in point:
        as_float(point.get("yaw_rad"), f"{path_name}.points[{point_index}].yaw_rad")
    if "target_speed_ms" in point:
        speed = as_float(point.get("target_speed_ms"), f"{path_name}.points[{point_index}].target_speed_ms")
        if speed < 0.0:
            raise ValueError(f"{path_name}.points[{point_index}].target_speed_ms 不能小于 0。")
 def validate_reference_path(path: dict[str, Any], source_path: Path | None = None) -> dict[str, Any]:
    schema_version = int(path.get("schema_version", 0))
    if schema_version != 1:
        raise ValueError(f"{source_path or path.get('path_id', '')} schema_version 必须为 1。")
    path_id = require_string(path.get("path_id"), "path_id")
    require_string(path.get("display_name"), f"{path_id}.display_name")
    if "frame_id" in path:
        require_string(path.get("frame_id"), f"{path_id}.frame_id")
    if "path_type" in path:
        require_string(path.get("path_type"), f"{path_id}.path_type")
    if "module_type" in path:
        require_string(path.get("module_type"), f"{path_id}.module_type")
    points = require_list(path.get("points"), f"{path_id}.points")
    if not points:
        raise ValueError(f"{path_id}.points 不能为空。")
    for index, point in enumerate(points):
        validate_point(point, path_id, index)
    for task_type in path.get("recommended_task_types", []) or []:
        require_string(task_type, f"{path_id}.recommended_task_types[]")
    if source_path is not None:
        path["_source_file"] = str(source_path.resolve(strict=False))
    return path
 def load_reference_path_dir(raw_path_dir: str | Path, owner_profile_path: Path | None = None) -> tuple[dict[str, Any], Path]:
    path_dir = resolve_path_dir(raw_path_dir, owner_profile_path)
    index = load_path_index(path_dir)
    paths: list[dict[str, Any]] = []
    seen: set[str] = set()
    for path_file in path_yaml_files(path_dir):
        path = validate_reference_path(load_yaml(path_file), path_file)
        path_id = str(path["path_id"])
        if path_id in seen:
            raise ValueError(f"重复的 path_id：{path_id}")
        seen.add(path_id)
        paths.append(path)
    if not paths:
        raise ValueError(f"参考路径目录没有路径 YAML: {path_dir}")
    profile = {
        "schema_version": 1,
        "profile_name": index.get("profile_name", path_dir.name),
        "frame_id": index.get("frame_id", "workshop"),
        "default_path_id_by_chassis": index.get("default_path_id_by_chassis", {}) or {},
        "default_path_id_by_task_type": index.get("default_path_id_by_task_type", {}) or {},
        "paths": paths,
    }
    validate_path_dir_profile(profile)
    return profile, path_dir
 def validate_path_dir_profile(profile: dict[str, Any]) -> dict[str, Any]:
    indexed = path_map(profile)
    defaults_by_chassis = require_map(
        profile.get("default_path_id_by_chassis", {}) or {},
        "default_path_id_by_chassis",
    )
    for chassis_type, path_id in defaults_by_chassis.items():
        if chassis_type not in SUPPORTED_CHASSIS_TYPES:
            raise ValueError(f"default_path_id_by_chassis 包含未知底盘类型：{chassis_type}")
        if str(path_id) not in indexed:
            raise ValueError(f"default_path_id_by_chassis.{chassis_type} 引用了不存在的路径：{path_id}")
    defaults_by_task_type = require_map(
        profile.get("default_path_id_by_task_type", {}) or {},
        "default_path_id_by_task_type",
    )
    for task_type, path_id in defaults_by_task_type.items():
        require_string(str(task_type), "default_path_id_by_task_type key")
        if str(path_id) not in indexed:
            raise ValueError(f"default_path_id_by_task_type.{task_type} 引用了不存在的路径：{path_id}")
    return profile
 def path_map(profile: dict[str, Any]) -> dict[str, dict[str, Any]]:
    indexed: dict[str, dict[str, Any]] = {}
    for path in require_list(profile.get("paths"), "paths"):
        path_id = require_string(path.get("path_id"), "path_id")
        indexed[path_id] = path
    return indexed
 def select_path(
    profile: dict[str, Any],
    path_id: str = "",
    chassis_type: str = "",
    task_type: str = "",
 ) -> dict[str, Any]:
    indexed = path_map(profile)
    selected_id = path_id.strip()
    if not selected_id and chassis_type:
        selected_id = str((profile.get("default_path_id_by_chassis", {}) or {}).get(chassis_type, ""))
    if not selected_id and task_type:
        selected_id = str((profile.get("default_path_id_by_task_type", {}) or {}).get(task_type, ""))
    if not selected_id:
        raise ValueError("未指定 path_id，也没有可用的默认参考路径。")
    if selected_id not in indexed:
        raise ValueError(f"参考路径不存在：{selected_id}")
    return indexed[selected_id]
 def flatten_path_metadata(path: dict[str, Any], profile: dict[str, Any], path_dir: Path | None = None) -> dict[str, str]:
    points = require_list(path.get("points"), f"{path.get('path_id', '')}.points")
    metadata: dict[str, str] = {
        "reference_path.id": require_string(path.get("path_id"), "path_id"),
        "reference_path.display_name": str(path.get("display_name", path.get("path_id", ""))),
        "reference_path.frame_id": str(path.get("frame_id", profile.get("frame_id", "workshop"))),
        "reference_path.path_type": str(path.get("path_type", "polyline")),
        "reference_path.point_count": str(len(points)),
    }
    if path_dir is not None:
        metadata["reference_path.path_dir"] = str(path_dir)
    if path.get("_source_file"):
        metadata["reference_path.file"] = str(path["_source_file"])
    if path.get("module_type"):
        metadata["reference_path.module_type"] = str(path["module_type"])
    if path.get("description"):
        metadata["reference_path.description"] = str(path["description"])
    for index, point_value in enumerate(points):
        point = require_map(point_value, f"{path['path_id']}.points[{index}]")
        prefix = f"reference_path.pt_{index}"
        metadata[f"{prefix}_x_m"] = stringify_value(as_float(point.get("x_m"), f"{prefix}_x_m"))
        metadata[f"{prefix}_y_m"] = stringify_value(as_float(point.get("y_m"), f"{prefix}_y_m"))
        metadata[f"{prefix}_z_m"] = stringify_value(as_float(point.get("z_m", 0.0), f"{prefix}_z_m"))
        metadata[f"{prefix}_yaw_rad"] = stringify_value(as_float(point.get("yaw_rad", 0.0), f"{prefix}_yaw_rad"))
        metadata[f"{prefix}_speed_ms"] = stringify_value(as_float(point.get("target_speed_ms", 0.0), f"{prefix}_speed_ms"))
    return metadata
 def selected_path_metadata(
    reference_path_dir: str | Path,
    owner_profile_path: Path | None = None,
    path_id: str = "",
    chassis_type: str = "",
    task_type: str = "",
 ) -> tuple[dict[str, str], dict[str, Any]]:
    profile, path_dir = load_reference_path_dir(reference_path_dir, owner_profile_path)
    selected = select_path(profile, path_id, chassis_type, task_type)
    return flatten_path_metadata(selected, profile, path_dir), selected
 def path_points_as_trajectory(path: dict[str, Any]) -> list[dict[str, float]]:
    points = require_list(path.get("points"), f"{path.get('path_id', '')}.points")
    trajectory: list[dict[str, float]] = []
    for index, point_value in enumerate(points):
        point = require_map(point_value, f"{path.get('path_id', '')}.points[{index}]")
        trajectory.append({
            "x_m": as_float(point.get("x_m"), f"points[{index}].x_m"),
            "y_m": as_float(point.get("y_m"), f"points[{index}].y_m"),
            "yaw_rad": as_float(point.get("yaw_rad", 0.0), f"points[{index}].yaw_rad"),
            "target_speed_ms": as_float(
                point.get("target_speed_ms", 0.0),
                f"points[{index}].target_speed_ms",
            ),
        })
    return trajectory
 def make_task_param(key: str, value: Any) -> dict[str, str]:
    return {"key": key, "value": stringify_value(value)}
 def build_summary(profile: dict[str, Any], path_dir: Path) -> dict[str, Any]:
    return {
        "schema_version": profile["schema_version"],
        "profile_name": profile.get("profile_name", ""),
        "path_dir": str(path_dir),
        "frame_id": profile.get("frame_id", "workshop"),
        "default_path_id_by_chassis": profile.get("default_path_id_by_chassis", {}) or {},
        "default_path_id_by_task_type": profile.get("default_path_id_by_task_type", {}) or {},
        "paths": [
            {
                "path_id": path["path_id"],
                "display_name": path.get("display_name", ""),
                "module_type": path.get("module_type", ""),
                "path_type": path.get("path_type", "polyline"),
                "point_count": len(path.get("points", []) or []),
                "recommended_task_types": path.get("recommended_task_types", []) or [],
                "file": path.get("_source_file", ""),
            }
            for path in profile["paths"]
        ],
    }
 def render_yaml(data: dict[str, Any]) -> str:
    return yaml.safe_dump(data, sort_keys=False, allow_unicode=True)
 def main() -> int:
    args = parse_args()
    profile, path_dir = load_reference_path_dir(args.path_dir)
    if args.format == "summary":
        output = build_summary(profile, path_dir)
    else:
        selected = select_path(profile, args.path_id, args.chassis_type, args.task_type)
        if args.format == "path":
            output = {
                "schema_version": 1,
                "path_dir": str(path_dir),
                "reference_path": {
                    key: value
                    for key, value in selected.items()
                    if not key.startswith("_")
                },
            }
        else:
            metadata = flatten_path_metadata(selected, profile, path_dir)
            output = {
                "schema_version": 1,
                "reference_path_id": metadata["reference_path.id"],
                "display_name": metadata["reference_path.display_name"],
                "task_params": [
                    make_task_param(key, metadata[key])
                    for key in sorted(metadata)
                ],
            }
    rendered = render_yaml(output)
    if args.output:
        output_path = Path(args.output).expanduser().resolve(strict=False)
        output_path.parent.mkdir(parents=True, exist_ok=True)
        output_path.write_text(rendered, encoding="utf-8")
        print(f"[OK] 已写入参考路径输出：{output_path}", file=sys.stderr)
    else:
        print(rendered, end="")
    return 0
 if __name__ == "__main__":
    try:
        raise SystemExit(main())
    except Exception as exc:
        print(f"[错误] {exc}", file=sys.stderr)
        raise SystemExit(1)
@@ -0,0 +1,249 @@
 #!/usr/bin/env python3
 """从 ROS 话题或 CSV 录制一条标定参考路径 YAML。"""
 from __future__ import annotations
 import argparse
 import csv
 import math
 import signal
 import sys
 import time
 from pathlib import Path
 from typing import Any
 try:
    import yaml
 except ImportError as exc:
    raise SystemExit("缺少 PyYAML，请先在当前 Python 环境安装 yaml 模块。") from exc
 SCRIPT_DIR = Path(__file__).resolve().parent
 if str(SCRIPT_DIR) not in sys.path:
    sys.path.insert(0, str(SCRIPT_DIR))
 from calibration_reference_paths import validate_reference_path  # noqa: E402
 def parse_args() -> argparse.Namespace:
    parser = argparse.ArgumentParser(description="录制一条标定参考路径，并写成单独 YAML 文件。")
    parser.add_argument("--output-dir", required=True, help="路径 YAML 输出目录，例如某模块的 reference_paths。")
    parser.add_argument("--path-id", required=True, help="路径 ID，同时作为默认文件名。")
    parser.add_argument("--display-name", required=True, help="中文路径名称，例如：底盘直线前进 1m。")
    parser.add_argument(
        "--module-type",
        choices=["chassis", "control", "sensor"],
        required=True,
        help="路径归属模块。",
    )
    parser.add_argument(
        "--allowed-chassis-type",
        choices=["ackermann", "differential", "single_steer_wheel", "multi_steer_wheel"],
        action="append",
        default=[],
        help="底盘路径允许出现在哪些底盘类型下，可重复传入。",
    )
    parser.add_argument("--path-type", default="recorded", help="路径类型，例如 straight_line、arc、recorded。")
    parser.add_argument("--frame-id", default="workshop", help="路径坐标系。")
    parser.add_argument("--description", default="", help="路径说明。")
    parser.add_argument(
        "--recommended-task-type",
        action="append",
        default=[],
        help="推荐使用的任务类型，可重复传入。",
    )
    parser.add_argument("--target-speed-ms", type=float, default=0.0, help="写入路径点的默认目标速度。")
    parser.add_argument("--duration-sec", type=float, default=0.0, help="ROS 录制时长；0 表示 Ctrl+C 结束。")
    parser.add_argument("--min-distance-step-m", type=float, default=0.03, help="相邻保留点的最小平移距离。")
    parser.add_argument("--min-yaw-step-rad", type=float, default=0.03, help="相邻保留点的最小 yaw 变化。")
    parser.add_argument("--overwrite", action="store_true", help="允许覆盖同名路径文件。")
    source = parser.add_mutually_exclusive_group(required=True)
    source.add_argument("--input-csv", default="", help="从 CSV 生成路径，不启动 ROS。")
    source.add_argument(
        "--source",
        choices=["external_pose", "chassis_telemetry"],
        help="从 ROS 话题录制路径。",
    )
    parser.add_argument("--topic", default="", help="ROS 话题；不填时按 source 使用默认话题。")
    return parser.parse_args()
 def yaw_delta(a: float, b: float) -> float:
    delta = (a - b + math.pi) % (2.0 * math.pi) - math.pi
    return abs(delta)
 def should_keep_point(
    points: list[dict[str, float]],
    point: dict[str, float],
    min_distance_step_m: float,
    min_yaw_step_rad: float,
 ) -> bool:
    if not points:
        return True
    previous = points[-1]
    distance = math.hypot(point["x_m"] - previous["x_m"], point["y_m"] - previous["y_m"])
    return distance >= min_distance_step_m or yaw_delta(point["yaw_rad"], previous["yaw_rad"]) >= min_yaw_step_rad
 def append_decimated(points: list[dict[str, float]], point: dict[str, float], args: argparse.Namespace) -> None:
    if should_keep_point(points, point, args.min_distance_step_m, args.min_yaw_step_rad):
        points.append(point)
 def csv_float(row: dict[str, str], keys: tuple[str, ...], default: float = 0.0) -> float:
    for key in keys:
        value = row.get(key)
        if value not in (None, ""):
            return float(value)
    return default
 def read_points_from_csv(path: Path, args: argparse.Namespace) -> list[dict[str, float]]:
    points: list[dict[str, float]] = []
    with path.open("r", newline="", encoding="utf-8") as stream:
        reader = csv.DictReader(stream)
        for row in reader:
            if row.get("pose_valid") not in (None, "", "1", "true", "True", "TRUE"):
                continue
            point = {
                "x_m": csv_float(row, ("x_m", "odom_x_m")),
                "y_m": csv_float(row, ("y_m", "odom_y_m")),
                "z_m": csv_float(row, ("z_m",), 0.0),
                "yaw_rad": csv_float(row, ("yaw_rad", "odom_yaw_rad"), 0.0),
                "target_speed_ms": args.target_speed_ms,
            }
            append_decimated(points, point, args)
    return points
 def default_topic(source: str) -> str:
    if source == "external_pose":
        return "/workshop/external_localization/vehicle/pose"
    if source == "chassis_telemetry":
        return "/chassis/telemetry"
    raise ValueError(f"不支持的 source: {source}")
 def read_points_from_ros(args: argparse.Namespace) -> list[dict[str, float]]:
    try:
        import rclpy
        from calibration_chassis_interfaces.msg import ChassisTelemetry
        from calibration_external_localization_interfaces.msg import ExternalLocalizationTelemetry
    except ImportError as exc:
        raise RuntimeError(f"缺少 ROS 2 Python 环境或消息包: {exc}") from exc
    points: list[dict[str, float]] = []
    stop_requested = False
    topic = args.topic or default_topic(args.source)
    def on_signal(signum: int, frame: Any) -> None:
        nonlocal stop_requested
        (void_signum, void_frame) = (signum, frame)
        _ = (void_signum, void_frame)
        stop_requested = True
    previous_sigint_handler = signal.signal(signal.SIGINT, on_signal)
    previous_sigterm_handler = signal.signal(signal.SIGTERM, on_signal)
    rclpy.init(args=None)
    node = rclpy.create_node("calibration_reference_path_recorder")
    def on_external_pose(msg: Any) -> None:
        if hasattr(msg, "pose_valid") and not bool(msg.pose_valid):
            return
        pose = msg.workshop_pose
        point = {
            "x_m": float(pose.x_m),
            "y_m": float(pose.y_m),
            "z_m": float(getattr(pose, "z_m", 0.0)),
            "yaw_rad": float(pose.yaw_rad),
            "target_speed_ms": args.target_speed_ms,
        }
        append_decimated(points, point, args)
    def on_chassis_telemetry(msg: Any) -> None:
        point = {
            "x_m": float(msg.odom_x_m),
            "y_m": float(msg.odom_y_m),
            "z_m": 0.0,
            "yaw_rad": float(msg.odom_yaw_rad),
            "target_speed_ms": args.target_speed_ms,
        }
        append_decimated(points, point, args)
    try:
        if args.source == "external_pose":
            node.create_subscription(ExternalLocalizationTelemetry, topic, on_external_pose, 50)
        elif args.source == "chassis_telemetry":
            node.create_subscription(ChassisTelemetry, topic, on_chassis_telemetry, 50)
        else:
            raise ValueError(f"不支持的 source: {args.source}")
        print(f"[*] 正在录制参考路径: topic={topic}, display_name={args.display_name}", file=sys.stderr)
        start = time.monotonic()
        while rclpy.ok() and not stop_requested:
            rclpy.spin_once(node, timeout_sec=0.1)
            if args.duration_sec > 0.0 and time.monotonic() - start >= args.duration_sec:
                break
    finally:
        node.destroy_node()
        rclpy.shutdown()
        signal.signal(signal.SIGINT, previous_sigint_handler)
        signal.signal(signal.SIGTERM, previous_sigterm_handler)
    return points
 def build_path_document(points: list[dict[str, float]], args: argparse.Namespace) -> dict[str, Any]:
    if not points:
        raise ValueError("没有录到任何路径点。")
    document: dict[str, Any] = {
        "schema_version": 1,
        "path_id": args.path_id,
        "display_name": args.display_name,
        "module_type": args.module_type,
        "frame_id": args.frame_id,
        "path_type": args.path_type,
        "points": points,
    }
    if args.description:
        document["description"] = args.description
    if args.recommended_task_type:
        document["recommended_task_types"] = args.recommended_task_type
    if args.module_type == "chassis" and args.allowed_chassis_type:
        document["allowed_chassis_types"] = sorted(set(args.allowed_chassis_type))
    validate_reference_path(dict(document))
    return document
 def write_path_document(document: dict[str, Any], args: argparse.Namespace) -> Path:
    output_dir = Path(args.output_dir).expanduser().resolve(strict=False)
    output_dir.mkdir(parents=True, exist_ok=True)
    output_path = output_dir / f"{args.path_id}.yaml"
    if output_path.exists() and not args.overwrite:
        raise FileExistsError(f"路径文件已存在，若要覆盖请加 --overwrite: {output_path}")
    output_path.write_text(
        yaml.safe_dump(document, sort_keys=False, allow_unicode=True),
        encoding="utf-8",
    )
    return output_path
 def main() -> int:
    args = parse_args()
    if args.input_csv:
        points = read_points_from_csv(Path(args.input_csv).expanduser().resolve(strict=False), args)
    else:
        points = read_points_from_ros(args)
    document = build_path_document(points, args)
    output_path = write_path_document(document, args)
    print(f"[OK] 已写入参考路径: {output_path}", file=sys.stderr)
    print(yaml.safe_dump({"path_file": str(output_path), "point_count": len(points)}, sort_keys=False, allow_unicode=True), end="")
    return 0
 if __name__ == "__main__":
    try:
        raise SystemExit(main())
    except Exception as exc:
        print(f"[错误] {exc}", file=sys.stderr)
        raise SystemExit(1)
@@ -23,6 +23,14 @@
 src/site_deployment/workshop_sensor_calibration_real/config/sensor_calibration_profile.yaml
 ```
 传感器参考路径单独放在：
 ```bash
 src/site_deployment/workshop_sensor_calibration_real/reference_paths/
 ```
 每条路径一个 YAML 文件，必须配置 `path_id` 和中文 `display_name`。`sensor_calibration_profile.yaml` 中每个任务通过 `reference_path_id` 选择路径，导出任务时会自动带上 `reference_path.*` metadata，便于 UI 或 Isaac 显示当前标定采集路径。
 校验并查看摘要：
 ```bash
@@ -4,6 +4,7 @@
 schema_version: 1
 profile_name: workshop_sensor_calibration_profile
 reference_path_dir: ../reference_paths
 data_capture:
  required_data_inputs:
@@ -25,6 +26,7 @@ tasks:
    display_name: 前视相机内参
    enabled: true
    selected_task: camera_intrinsic
    reference_path_id: sensor_static_front_station
    sensor_id: demo_front_camera
    task_subtype: front_camera_intrinsic
    camera_intrinsic:
@@ -36,6 +38,7 @@ tasks:
    display_name: 下视相机内参
    enabled: true
    selected_task: camera_intrinsic
    reference_path_id: sensor_static_front_station
    sensor_id: demo_down_camera
    task_subtype: downward_camera_intrinsic
    camera_intrinsic:
@@ -47,6 +50,7 @@ tasks:
    display_name: IMU 内参
    enabled: true
    selected_task: imu_intrinsic
    reference_path_id: sensor_imu_motion_line
    sensor_id: demo_imu
    task_subtype: imu_intrinsic
    imu_intrinsic:
@@ -58,6 +62,7 @@ tasks:
    display_name: 前视相机到 base_link 外参
    enabled: true
    selected_task: sensor_extrinsic
    reference_path_id: sensor_extrinsic_slow_line
    sensor_id: demo_front_camera
    task_subtype: front_camera_extrinsic
    sensor_extrinsic:
@@ -69,6 +74,7 @@ tasks:
    display_name: 下视相机到 base_link 外参
    enabled: true
    selected_task: sensor_extrinsic
    reference_path_id: sensor_extrinsic_slow_line
    sensor_id: demo_down_camera
    task_subtype: downward_camera_extrinsic
    sensor_extrinsic:
@@ -80,6 +86,7 @@ tasks:
    display_name: 2D LiDAR 到 base_link 外参
    enabled: true
    selected_task: sensor_extrinsic
    reference_path_id: sensor_extrinsic_slow_line
    sensor_id: demo_lidar_2d
    task_subtype: lidar_2d_extrinsic
    sensor_extrinsic:
@@ -91,6 +98,7 @@ tasks:
    display_name: 3D LiDAR 到 base_link 外参
    enabled: true
    selected_task: sensor_extrinsic
    reference_path_id: sensor_extrinsic_slow_line
    sensor_id: demo_lidar_3d
    task_subtype: lidar_3d_extrinsic
    sensor_extrinsic:
@@ -102,6 +110,7 @@ tasks:
    display_name: IMU 到 base_link 外参
    enabled: true
    selected_task: sensor_extrinsic
    reference_path_id: sensor_extrinsic_slow_line
    sensor_id: demo_imu
    task_subtype: imu_extrinsic
    sensor_extrinsic:
@@ -113,6 +122,7 @@ tasks:
    display_name: 手眼相机眼在手上
    enabled: true
    selected_task: hand_eye
    reference_path_id: hand_eye_pose_sweep
    sensor_id: demo_arm_camera
    task_subtype: eye_in_hand
    hand_eye:
@@ -0,0 +1,21 @@
 schema_version: 1
 path_id: hand_eye_pose_sweep
 display_name: 手眼标定位姿序列
 module_type: sensor
 frame_id: workshop
 path_type: pose_sweep
 recommended_task_types:
  - hand_eye
 points:
  - x_m: 0.0
    y_m: 0.0
    yaw_rad: 0.0
    target_speed_ms: 0.0
  - x_m: 0.15
    y_m: 0.0
    yaw_rad: 0.15
    target_speed_ms: 0.0
  - x_m: -0.15
    y_m: 0.0
    yaw_rad: -0.15
    target_speed_ms: 0.0
@@ -0,0 +1,8 @@
 profile_name: sensor_calibration_reference_paths
 frame_id: workshop
 default_path_id_by_task_type:
  camera_intrinsic: sensor_static_front_station
  imu_intrinsic: sensor_imu_motion_line
  sensor_extrinsic: sensor_extrinsic_slow_line
  hand_eye: hand_eye_pose_sweep
@@ -0,0 +1,21 @@
 schema_version: 1
 path_id: sensor_extrinsic_slow_line
 display_name: 传感器外参低速直线采样
 module_type: sensor
 frame_id: workshop
 path_type: sampling_line
 recommended_task_types:
  - sensor_extrinsic
 points:
  - x_m: 0.0
    y_m: 0.0
    yaw_rad: 0.0
    target_speed_ms: 0.05
  - x_m: 0.4
    y_m: 0.0
    yaw_rad: 0.0
    target_speed_ms: 0.05
  - x_m: 0.8
    y_m: 0.0
    yaw_rad: 0.0
    target_speed_ms: 0.05
@@ -0,0 +1,17 @@
 schema_version: 1
 path_id: sensor_imu_motion_line
 display_name: IMU 内参低速运动段
 module_type: sensor
 frame_id: workshop
 path_type: imu_motion
 recommended_task_types:
  - imu_intrinsic
 points:
  - x_m: 0.0
    y_m: 0.0
    yaw_rad: 0.0
    target_speed_ms: 0.05
  - x_m: 0.5
    y_m: 0.0
    yaw_rad: 0.0
    target_speed_ms: 0.05
@@ -0,0 +1,13 @@
 schema_version: 1
 path_id: sensor_static_front_station
 display_name: 传感器静态采集点
 module_type: sensor
 frame_id: workshop
 path_type: static_station
 recommended_task_types:
  - camera_intrinsic
 points:
  - x_m: 0.0
    y_m: 0.0
    yaw_rad: 0.0
    target_speed_ms: 0.0
@@ -13,6 +13,13 @@ try:
 except ImportError as exc:
    raise SystemExit("缺少 PyYAML，请先在当前 Python 环境安装 yaml 模块。") from exc
 SCRIPT_DIR = Path(__file__).resolve().parent
 REFERENCE_PATH_TOOL_DIR = SCRIPT_DIR.parents[0] / "workshop_reference_paths"
 if str(REFERENCE_PATH_TOOL_DIR) not in sys.path:
    sys.path.insert(0, str(REFERENCE_PATH_TOOL_DIR))
 from calibration_reference_paths import selected_path_metadata  # noqa: E402
 TASK_GROUPS = {
    "camera_intrinsic",
@@ -224,6 +231,8 @@ def validate_task(task: dict[str, Any], index: int) -> None:
        raise ValueError(
            f"{task_name}.task_subtype={task_subtype} 不适用于 selected_task={selected_task}。"
        )
    if task.get("reference_path_id") not in (None, ""):
        require_string(task.get("reference_path_id"), f"{task_name}.reference_path_id")
    if selected_task == "camera_intrinsic":
        validate_camera_intrinsic(task, task_name)
@@ -276,12 +285,24 @@ def task_matches_requested(task: dict[str, Any], requested_tasks: set[str]) -> b
    return TASK_GROUP_TO_REQUESTED_TASK[selected_task] in requested_tasks
-def flatten_task_metadata(task: dict[str, Any]) -> list[dict[str, str]]:
+def flatten_task_metadata(
    task: dict[str, Any],
    profile: dict[str, Any],
    profile_path: Path | None = None,
 ) -> list[dict[str, str]]:
    selected_task = str(task["selected_task"])
    reference_metadata, _ = selected_path_metadata(
        profile.get("reference_path_dir", "reference_paths"),
        profile_path,
        str(task.get("reference_path_id", "")),
        "",
        selected_task,
    )
    params = [
        make_task_param("sensor.sensor_id", task["sensor_id"]),
        make_task_param("sensor.task_subtype", task["task_subtype"]),
    ]
    params.extend(make_task_param(key, reference_metadata[key]) for key in sorted(reference_metadata))
    if selected_task == "camera_intrinsic":
        payload = task["camera_intrinsic"]
@@ -324,6 +345,7 @@ def flatten_task_metadata(task: dict[str, Any]) -> list[dict[str, str]]:
 def export_requested_tasks(
    profile: dict[str, Any],
    requested_tasks: set[str] | list[str] | str | None = None,
    profile_path: Path | None = None,
 ) -> dict[str, Any]:
    selected_requested_tasks = (
        requested_tasks
@@ -345,7 +367,7 @@ def export_requested_tasks(
            "reason": str(task.get("reason", "现场传感器标定 profile")),
            "task_code": str(task["task_code"]),
            "target_id": str(task["sensor_id"]),
-            "task_params": flatten_task_metadata(task),
+            "task_params": flatten_task_metadata(task, profile, profile_path),
        })
    return {
        "schema_version": 1,
@@ -382,7 +404,7 @@ def main() -> int:
    profile = validate_profile(load_yaml(profile_path))
    if args.format == "requested_tasks":
-        output = export_requested_tasks(profile, parse_requested_tasks(args.tasks))
+        output = export_requested_tasks(profile, parse_requested_tasks(args.tasks), profile_path)
    else:
        output = build_summary(profile)