#!/usr/bin/env python3
"""车间总控端到端验收脚本。

使用前先启动仿真/本地 demo 栈。本脚本负责：
1. 注册一份最小车辆画像；
2. 创建 workshop session；
3. 调用 /workshop_v2/execute_session action；
4. 检查最终报告和阶段结果。

默认验收到传感器内参链路；传感器外参需要有效观测帧，等观测数据链路就绪后可通过
--tasks external,chassis,control,sensor_intrinsic,sensor_extrinsic 显式加入。
"""

from __future__ import annotations

import argparse
import os
import sys
import time
from pathlib import Path
from typing import Iterable

try:
    import rclpy
    from rclpy.action import ActionClient
except ImportError as exc:  # pragma: no cover - 只在未 source ROS 环境时触发
    print(f"[FAIL] 无法导入 ROS2 Python 模块: {exc}", file=sys.stderr)
    print("请先执行: source install/setup.bash", file=sys.stderr)
    raise

from calibration_common_interfaces.msg import KeyValuePair, RequestHeader
from calibration_external_localization_interfaces.msg import ExternalLocalizationTelemetry
from rcl_interfaces.msg import Parameter, ParameterType, ParameterValue
from rcl_interfaces.srv import SetParameters
from calibration_vehicle_profile_interfaces.msg import (
    CalibrationAbilityType,
    CalibrationCapability,
    CameraMountType,
    ChassisType,
    ControlAxisType,
    ControllerAlgorithmType,
    ControllerProfile,
    ControllerSelection,
    SensorProfile,
    SensorType,
    VehicleProfile,
    WorkflowStageType,
)
from calibration_workshop_orchestration_interfaces.action import ExecuteWorkshopSession
from calibration_workshop_orchestration_interfaces.msg import (
    RequestedCalibrationTask,
    StageExecutionPolicy,
    WorkshopOperatorInfo,
    WorkshopSessionConfig,
)
from calibration_workshop_orchestration_interfaces.srv import (
    CreateWorkshopSession,
    GetWorkshopReport,
    GetWorkshopSession,
)
from calibration_vehicle_profile_interfaces.srv import RegisterOrUpdateVehicleProfile


TASK_STAGE_TYPES = {
    "external": WorkflowStageType.EXTERNAL_REFERENCE_READY_CHECK_STAGE,
    "chassis": WorkflowStageType.CHASSIS_CALIBRATION_STAGE,
    "control": WorkflowStageType.CONTROL_CALIBRATION_STAGE,
    "sensor_intrinsic": WorkflowStageType.SENSOR_INTRINSIC_CALIBRATION_STAGE,
    "sensor_extrinsic": WorkflowStageType.SENSOR_EXTRINSIC_CALIBRATION_STAGE,
    "hand_eye": WorkflowStageType.HAND_EYE_CALIBRATION_STAGE,
}

DEFAULT_TASKS = "external,chassis,control,sensor_intrinsic"


def now_us() -> int:
    return int(time.time() * 1_000_000)


def make_header(vehicle_id: str, task_id: str, operator_id: str) -> RequestHeader:
    header = RequestHeader()
    header.session_id = f"orchestrator_smoke_{now_us()}"
    header.task_id = task_id
    header.vehicle_id = vehicle_id
    header.request_id = f"{task_id}_{now_us()}"
    header.client_send_timestamp_us = now_us()
    header.operator_id = operator_id
    header.workshop_host = "smoke_test_workshop_orchestrator"
    return header


def kv(key: str, value: str) -> KeyValuePair:
    item = KeyValuePair()
    item.key = key
    item.value = value
    return item


def make_capability(ability: int, message: str) -> CalibrationCapability:
    cap = CalibrationCapability()
    cap.ability_type.value = ability
    cap.supported = True
    cap.message = message
    return cap


def make_controller_profile(axis: int, default_algorithm: int) -> ControllerProfile:
    profile = ControllerProfile()
    profile.control_axis.value = axis
    for algorithm in (
        ControllerAlgorithmType.PID,
        ControllerAlgorithmType.MPC,
        ControllerAlgorithmType.LQR,
        ControllerAlgorithmType.PURE_PURSUIT,
    ):
        item = ControllerAlgorithmType()
        item.value = algorithm
        profile.supported_algorithms.append(item)
    profile.default_algorithm.value = default_algorithm
    return profile


def make_controller_selection(axis: int, algorithm: int, is_default: bool) -> ControllerSelection:
    selection = ControllerSelection()
    selection.control_axis.value = axis
    selection.algorithm_type.value = algorithm
    selection.enabled = True
    selection.need_tuning = True
    selection.is_default = is_default
    return selection


def make_sensor(sensor_id: str, sensor_type: int, mount_type: int, name: str) -> SensorProfile:
    sensor = SensorProfile()
    sensor.sensor_id = sensor_id
    sensor.sensor_type.value = sensor_type
    sensor.sensor_name = name
    sensor.frame_id = f"{sensor_id}_frame"
    sensor.camera_mount_type.value = mount_type
    sensor.enabled = True
    sensor.needs_intrinsic_calibration = sensor_type in (
        SensorType.FRONT_CAMERA,
        SensorType.DOWNWARD_CAMERA,
        SensorType.IMU,
    )
    sensor.needs_extrinsic_calibration = True
    sensor.device_hint = f"/workshop/vehicle_sensor/{sensor_id}"
    sensor.selected_for_this_session = sensor_id == "demo_front_camera"
    sensor.supports_intrinsic_calibration = sensor.needs_intrinsic_calibration
    sensor.supports_extrinsic_calibration = True
    return sensor


def make_vehicle_profile(vehicle_id: str) -> VehicleProfile:
    profile = VehicleProfile()
    profile.base_info.vehicle_id = vehicle_id
    profile.base_info.vehicle_name = "仿真阿克曼小车"
    profile.base_info.model_name = "ackermann_sim"
    profile.base_info.serial_number = f"{vehicle_id}_serial"
    profile.base_info.manufacturer = "AutoCalib Workshop"
    profile.base_info.description = "orchestrator e2e smoke profile"
    profile.chassis_type.value = ChassisType.ACKERMANN
    profile.base_link_frame = "base_link"
    profile.profile_version = "orchestrator_smoke_v1"

    profile.capabilities.extend([
        make_capability(CalibrationAbilityType.EXTERNAL_LOCALIZATION_CALIBRATION, "支持外部真值接入"),
        make_capability(CalibrationAbilityType.CHASSIS_CALIBRATION, "支持底盘标定"),
        make_capability(CalibrationAbilityType.CONTROL_CALIBRATION, "支持运控参数标定"),
        make_capability(CalibrationAbilityType.SENSOR_CALIBRATION, "支持传感器标定"),
    ])

    profile.sensors.extend([
        make_sensor("demo_front_camera", SensorType.FRONT_CAMERA, CameraMountType.FRONT_MOUNTED, "前视相机"),
        make_sensor("demo_down_camera", SensorType.DOWNWARD_CAMERA, CameraMountType.DOWNWARD_MOUNTED, "下视相机"),
        make_sensor("demo_lidar_3d", SensorType.LIDAR_3D, CameraMountType.CAMERA_MOUNT_TYPE_UNSPECIFIED, "3D 激光雷达"),
        make_sensor("demo_lidar_2d", SensorType.LIDAR_2D, CameraMountType.CAMERA_MOUNT_TYPE_UNSPECIFIED, "2D 激光雷达"),
        make_sensor("demo_imu", SensorType.IMU, CameraMountType.CAMERA_MOUNT_TYPE_UNSPECIFIED, "IMU"),
    ])

    profile.controllers.extend([
        make_controller_profile(ControlAxisType.LATERAL_CONTROL, ControllerAlgorithmType.MPC),
        make_controller_profile(ControlAxisType.LONGITUDINAL_CONTROL, ControllerAlgorithmType.PID),
    ])
    profile.controller_selections.extend([
        make_controller_selection(ControlAxisType.LATERAL_CONTROL, ControllerAlgorithmType.MPC, True),
        make_controller_selection(ControlAxisType.LONGITUDINAL_CONTROL, ControllerAlgorithmType.PID, True),
    ])

    for stage_value in (
        WorkflowStageType.EXTERNAL_REFERENCE_READY_CHECK_STAGE,
        WorkflowStageType.CHASSIS_CALIBRATION_STAGE,
        WorkflowStageType.CONTROL_CALIBRATION_STAGE,
        WorkflowStageType.SENSOR_INTRINSIC_CALIBRATION_STAGE,
        WorkflowStageType.SENSOR_EXTRINSIC_CALIBRATION_STAGE,
    ):
        stage = WorkflowStageType()
        stage.value = stage_value
        profile.enabled_workflow_stages.append(stage)

    return profile


def parse_tasks(raw: str) -> list[str]:
    if raw.strip().lower() == "all":
        raw = DEFAULT_TASKS
    tasks = []
    for item in raw.split(","):
        name = item.strip()
        if not name:
            continue
        if name not in TASK_STAGE_TYPES:
            raise ValueError(f"不支持的任务名: {name}，可选: {', '.join(TASK_STAGE_TYPES)}")
        tasks.append(name)
    if not tasks:
        raise ValueError("至少需要指定一个任务")
    return tasks


def task_params(task_name: str, args: argparse.Namespace) -> list[KeyValuePair]:
    if task_name == "external":
        return [
            kv("external.static_sample_count", "1"),
            kv("external.dynamic_sample_count", "1"),
            kv("external.require_short_motion_segment", "false"),
            kv("external.max_position_stddev_m", "0.05"),
            kv("external.max_yaw_stddev_rad", "0.05"),
            kv("external.max_tracking_loss_ratio", "0.05"),
            kv("external.max_time_sync_offset_ms", "50.0"),
            kv("external.timeout_sec", "5.0"),
        ]
    if task_name == "chassis":
        return [
            kv("primitive_type", "straight_line"),
            kv("straight_line.target_distance_m", str(args.chassis_distance_m)),
            kv("straight_line.target_speed_ms", str(args.chassis_speed_mps)),
            kv("straight_line.reverse", "false"),
            kv("brake_when_finished", "true"),
            kv("timeout_sec", "10.0"),
        ]
    if task_name == "control":
        return [
            kv("control.task_type", "velocity_step"),
            kv("velocity_step.target_velocity_ms", str(args.control_velocity_mps)),
            kv("velocity_step.hold_time_sec", "0.5"),
            kv("velocity_step.settle_before_step_sec", "0.1"),
            kv("control.timeout_sec", "10.0"),
        ]
    if task_name == "sensor_intrinsic":
        return [
            kv("sensor.sensor_id", "demo_front_camera"),
            kv("sensor.task_subtype", "front_camera_intrinsic"),
            kv("camera_intrinsic.required_image_count", "1"),
            kv("camera_intrinsic.target_board_id", args.reference_target_id),
            kv("camera_intrinsic.timeout_sec", "5.0"),
        ]
    if task_name == "sensor_extrinsic":
        return [
            kv("sensor.sensor_id", "demo_front_camera"),
            kv("sensor.task_subtype", "front_camera_extrinsic"),
            kv("sensor_extrinsic.base_frame_id", "base_link"),
            kv("sensor_extrinsic.required_sample_count", "1"),
            kv("sensor_extrinsic.timeout_sec", "5.0"),
        ]
    if task_name == "hand_eye":
        return [
            kv("sensor.sensor_id", "demo_front_camera"),
            kv("sensor.task_subtype", "eye_in_hand"),
            kv("hand_eye.arm_id", "demo_arm"),
            kv("hand_eye.required_pose_count", "1"),
            kv("hand_eye.timeout_sec", "5.0"),
        ]
    return []


def make_requested_task(task_name: str, args: argparse.Namespace) -> RequestedCalibrationTask:
    task = RequestedCalibrationTask()
    task.stage_type.value = TASK_STAGE_TYPES[task_name]
    task.enabled = True
    task.require_manual_approval = False
    task.execution_policy.value = StageExecutionPolicy.REQUIRED
    task.reason = "orchestrator e2e smoke"
    task.task_code = task_name
    task.target_id = args.reference_target_id if task_name == "external" else ""
    task.task_params.extend(task_params(task_name, args))
    return task


def make_session_config(tasks: Iterable[str], args: argparse.Namespace) -> WorkshopSessionConfig:
    config = WorkshopSessionConfig()
    config.auto_commit_parameters = False
    config.require_manual_approval_before_commit = False
    config.run_validation_after_each_stage = False
    config.stop_on_first_failure = True
    config.allow_optional_stage_skip = False
    config.enable_auto_rollback_on_validation_failure = False
    config.allow_rebuild_execution_plan = False
    config.localization_source_id = args.reference_source_name
    config.workcell_zone_id = args.workcell_zone_id
    config.reference_target_id = args.reference_target_id
    for task_name in tasks:
        config.requested_tasks.append(make_requested_task(task_name, args))
    return config


class OrchestratorSmoke:
    def __init__(self, args: argparse.Namespace) -> None:
        self.args = args
        self.node = rclpy.create_node("smoke_test_workshop_orchestrator")
        self.feedback_messages: list[str] = []
        self.external_pub = self.node.create_publisher(
            ExternalLocalizationTelemetry,
            args.external_telemetry_topic,
            10,
        )
        self.external_timer = None
        if args.publish_fake_external_telemetry:
            period = 1.0 / max(args.fake_external_hz, 1.0)
            self.external_timer = self.node.create_timer(period, self.publish_external_telemetry)

    def destroy(self) -> None:
        self.node.destroy_node()

    def publish_external_telemetry(self) -> None:
        msg = ExternalLocalizationTelemetry()
        msg.hardware_timestamp_us = now_us()
        msg.pose_valid = True
        msg.workshop_pose.x_m = 0.0
        msg.workshop_pose.y_m = 0.0
        msg.workshop_pose.z_m = 0.0
        msg.workshop_pose.roll_rad = 0.0
        msg.workshop_pose.pitch_rad = 0.0
        msg.workshop_pose.yaw_rad = 0.0
        msg.position_stddev_m = 0.001
        msg.yaw_stddev_rad = 0.001
        msg.tracking_loss_ratio = 0.0
        msg.time_sync_offset_ms = 1.0
        msg.quality_score = 1.0
        msg.observed_target_count = 4
        msg.reference_source_name = self.args.reference_source_name
        msg.active_job_id = "orchestrator_e2e_smoke"
        self.external_pub.publish(msg)

    def spin_for(self, seconds: float) -> None:
        deadline = time.monotonic() + seconds
        while time.monotonic() < deadline:
            rclpy.spin_once(self.node, timeout_sec=0.05)

    def wait_for_service(self, client, name: str) -> None:
        if not client.wait_for_service(timeout_sec=self.args.service_timeout_sec):
            raise RuntimeError(f"服务不可用: {name}")

    def call_service(self, client, request, name: str):
        self.wait_for_service(client, name)
        future = client.call_async(request)
        deadline = time.monotonic() + self.args.service_timeout_sec
        while rclpy.ok() and not future.done() and time.monotonic() < deadline:
            rclpy.spin_once(self.node, timeout_sec=0.05)
        if not future.done():
            raise TimeoutError(f"等待服务响应超时: {name}")
        return future.result()

    def maybe_disable_wifi6_precheck(self) -> None:
        if not self.args.disable_wifi6_precheck:
            return
        node_name = self.args.orchestrator_node.strip("/")
        client = self.node.create_client(SetParameters, f"/{node_name}/set_parameters")
        request = SetParameters.Request()
        item = Parameter()
        item.name = "wifi6_precheck_enabled"
        item.value = ParameterValue()
        item.value.type = ParameterType.PARAMETER_BOOL
        item.value.bool_value = False
        request.parameters.append(item)
        response = self.call_service(client, request, f"/{node_name}/set_parameters")
        results = response.results
        if not results or not results[0].successful:
            reason = "" if not results else results[0].reason
            raise RuntimeError(f"设置 wifi6_precheck_enabled=false 失败: {reason}")
        print("[INFO] 已临时关闭 orchestrator WiFi6 预检。")

    def register_vehicle_profile(self, profile: VehicleProfile) -> None:
        client = self.node.create_client(
            RegisterOrUpdateVehicleProfile,
            "/vehicle_profile_manager/register_or_update_vehicle_profile",
        )
        request = RegisterOrUpdateVehicleProfile.Request()
        request.request.header = make_header(self.args.vehicle_id, "register_profile", self.args.operator_id)
        request.request.profile = profile
        response = self.call_service(
            client,
            request,
            "/vehicle_profile_manager/register_or_update_vehicle_profile",
        )
        if not response.response.success:
            raise RuntimeError(f"注册车辆画像失败: {response.response.message}")
        print(f"[PASS] 已注册车辆画像: {self.args.vehicle_id}")

    def create_session(self, profile: VehicleProfile, tasks: list[str]) -> str:
        client = self.node.create_client(CreateWorkshopSession, "/workshop_v2/create_session")
        request = CreateWorkshopSession.Request()
        request.request.header = make_header(self.args.vehicle_id, "create_session", self.args.operator_id)
        request.request.vehicle_profile_snapshot = profile
        request.request.operator_info = WorkshopOperatorInfo()
        request.request.operator_info.operator_id = self.args.operator_id
        request.request.operator_info.operator_name = "orchestrator smoke"
        request.request.operator_info.workstation_id = self.args.workstation_id
        request.request.operator_info.shift_id = "smoke"
        request.request.config = make_session_config(tasks, self.args)
        request.request.workshop_line_id = self.args.workshop_line_id
        request.request.auto_build_execution_plan = True
        response = self.call_service(client, request, "/workshop_v2/create_session")
        if not response.response.success:
            raise RuntimeError(f"创建 session 失败: {response.response.message}")
        response_session_id = response.response.session_id
        nested_session_id = response.response.session.session_id
        session_id = response_session_id or nested_session_id
        if not session_id:
            raise RuntimeError(
                "创建 session 成功但返回了空 session_id: "
                f"message={response.response.message!r}, "
                f"response_session_id={response_session_id!r}, "
                f"nested_session_id={nested_session_id!r}, "
                f"stage_count={len(response.response.session.stage_plan)}"
            )
        stage_ids = [stage.stage_id for stage in response.response.session.stage_plan]
        print(f"[PASS] 已创建 session: {session_id}")
        if response_session_id != nested_session_id:
            print(
                "[WARN] create_session 返回的顶层 session_id 与 session 快照不一致: "
                f"response={response_session_id!r}, nested={nested_session_id!r}"
            )
        print(f"[INFO] 计划阶段: {', '.join(stage_ids)}")
        return session_id

    def verify_session_visible(self, session_id: str) -> None:
        client = self.node.create_client(GetWorkshopSession, "/workshop_v2/get_session")
        request = GetWorkshopSession.Request()
        request.request.header = make_header(self.args.vehicle_id, "get_session", self.args.operator_id)
        request.request.session_id = session_id
        response = self.call_service(client, request, "/workshop_v2/get_session")
        if not response.response.success:
            raise RuntimeError(
                "创建后立即查询 session 失败，可能存在旧 orchestrator 节点或 service/action 连到不同实例: "
                f"session_id={session_id!r}, message={response.response.message!r}"
            )
        visible_id = response.response.session.session_id
        if visible_id != session_id:
            raise RuntimeError(
                "get_session 返回的 session_id 与请求不一致: "
                f"requested={session_id!r}, visible={visible_id!r}"
            )
        print(f"[PASS] session 可查询: {session_id}")

    def feedback_cb(self, feedback_msg) -> None:
        event = feedback_msg.feedback.feedback
        text = event.message or f"event_type={event.event_type.value}"
        if event.stage_id:
            text = f"{event.stage_id}: {text}"
        self.feedback_messages.append(text)
        if self.args.verbose_feedback:
            print(f"[FEEDBACK] {text}")

    def execute_session(self, session_id: str):
        client = ActionClient(self.node, ExecuteWorkshopSession, "/workshop_v2/execute_session")
        if not client.wait_for_server(timeout_sec=self.args.service_timeout_sec):
            raise RuntimeError("action server 不可用: /workshop_v2/execute_session")

        goal = ExecuteWorkshopSession.Goal()
        goal.goal.header = make_header(self.args.vehicle_id, "execute_session", self.args.operator_id)
        goal.goal.header.session_id = session_id
        goal.goal.session_id = session_id
        send_future = client.send_goal_async(goal, feedback_callback=self.feedback_cb)
        deadline = time.monotonic() + self.args.action_timeout_sec
        while rclpy.ok() and not send_future.done() and time.monotonic() < deadline:
            rclpy.spin_once(self.node, timeout_sec=0.05)
        if not send_future.done():
            raise TimeoutError("发送 execute_session goal 超时")

        goal_handle = send_future.result()
        if not goal_handle.accepted:
            raise RuntimeError(f"orchestrator 拒绝 execute_session goal: session_id={session_id!r}")
        print("[PASS] execute_session goal 已接受")

        result_future = goal_handle.get_result_async()
        while rclpy.ok() and not result_future.done() and time.monotonic() < deadline:
            rclpy.spin_once(self.node, timeout_sec=0.05)
        if not result_future.done():
            raise TimeoutError("等待 execute_session 结果超时")
        return result_future.result()

    def fetch_report(self, session_id: str):
        client = self.node.create_client(GetWorkshopReport, "/workshop_v2/get_report")
        request = GetWorkshopReport.Request()
        request.request.header = make_header(self.args.vehicle_id, "get_report", self.args.operator_id)
        request.request.session_id = session_id
        return self.call_service(client, request, "/workshop_v2/get_report")

    def run(self) -> int:
        tasks = parse_tasks(self.args.tasks)
        Path(self.args.sensor_storage_root).mkdir(parents=True, exist_ok=True)
        self.maybe_disable_wifi6_precheck()
        if self.args.publish_fake_external_telemetry:
            self.spin_for(self.args.external_warmup_sec)

        profile = make_vehicle_profile(self.args.vehicle_id)
        self.register_vehicle_profile(profile)
        session_id = self.create_session(profile, tasks)
        self.verify_session_visible(session_id)
        action_result = self.execute_session(session_id)
        report_response = action_result.result.result
        report = report_response.report

        if not report_response.success:
            action_status = getattr(action_result, "status", None)
            message = report_response.message or "(orchestrator 未返回失败原因)"
            print(
                f"[FAIL] execute_session 失败: {message}; "
                f"session_id={session_id}; action_status={action_status}",
                file=sys.stderr,
            )
            self.print_report(report)
            return 2

        if not report.overall_success:
            print("[FAIL] report.overall_success=false", file=sys.stderr)
            self.print_report(report)
            return 3

        expected_count = len(tasks)
        actual_count = len(report.stage_results)
        if self.args.strict_stage_count and actual_count != expected_count:
            print(
                f"[FAIL] 阶段结果数量不符合预期: expected={expected_count}, actual={actual_count}",
                file=sys.stderr,
            )
            self.print_report(report)
            return 4

        failed = [result for result in report.stage_results if not result.success or not result.auto_acceptance_passed]
        if failed:
            print("[FAIL] 存在未通过阶段", file=sys.stderr)
            self.print_report(report)
            return 5

        service_report = self.fetch_report(session_id)
        if not service_report.response.success:
            print(f"[FAIL] get_report 失败: {service_report.response.message}", file=sys.stderr)
            return 6

        print("[PASS] workshop orchestrator e2e smoke completed.")
        self.print_report(report)
        return 0

    @staticmethod
    def print_report(report) -> None:
        print(f"[REPORT] session_id={report.session_id} overall_success={report.overall_success}")
        print(f"[REPORT] summary={report.summary}")
        for file_ref in report.report_files:
            print(
                "[REPORT_FILE] "
                f"name={file_ref.file_name} "
                f"uri={file_ref.file_uri} "
                f"size={file_ref.size_bytes} "
                f"description={file_ref.description}"
            )
        for result in report.stage_results:
            print(
                "[STAGE] "
                f"id={result.stage_id} "
                f"success={result.success} "
                f"auto_acceptance={result.auto_acceptance_passed} "
                f"state={result.final_job_state.state} "
                f"summary={result.summary}"
            )


def parse_args() -> argparse.Namespace:
    parser = argparse.ArgumentParser(description="workshop_orchestrator_v2 端到端验收脚本")
    parser.add_argument("--vehicle-id", default="demo_agv_001")
    parser.add_argument("--tasks", default=DEFAULT_TASKS, help=f"逗号分隔任务列表，默认: {DEFAULT_TASKS}")
    parser.add_argument("--operator-id", default="smoke_operator")
    parser.add_argument("--workstation-id", default="sim_workstation")
    parser.add_argument("--workshop-line-id", default="sim_line_001")
    parser.add_argument("--workcell-zone-id", default="sim_workcell_zone")
    parser.add_argument("--reference-source-name", default="isaac_sim_truth_source")
    parser.add_argument("--reference-target-id", default="sim_reference_target")
    parser.add_argument("--external-telemetry-topic", default="/isaac/external_localization/telemetry")
    parser.add_argument("--publish-fake-external-telemetry", action=argparse.BooleanOptionalAction, default=True)
    parser.add_argument("--fake-external-hz", type=float, default=20.0)
    parser.add_argument("--external-warmup-sec", type=float, default=0.8)
    parser.add_argument("--sensor-storage-root", default="/tmp/agv_sensor_calibration")
    parser.add_argument("--chassis-distance-m", type=float, default=0.2)
    parser.add_argument("--chassis-speed-mps", type=float, default=0.1)
    parser.add_argument("--control-velocity-mps", type=float, default=0.1)
    parser.add_argument("--service-timeout-sec", type=float, default=10.0)
    parser.add_argument("--action-timeout-sec", type=float, default=240.0)
    parser.add_argument("--strict-stage-count", action=argparse.BooleanOptionalAction, default=True)
    parser.add_argument("--verbose-feedback", action="store_true")
    parser.add_argument(
        "--disable-wifi6-precheck",
        action="store_true",
        help="仅用于本地无 9000 WiFi6 gateway 的 demo；会临时关闭 orchestrator 的 WiFi6 预检参数",
    )
    parser.add_argument("--orchestrator-node", default="/workshop_orchestrator_v2")
    return parser.parse_args()


def main() -> int:
    args = parse_args()
    rclpy.init(args=None)
    runner = OrchestratorSmoke(args)
    try:
        return runner.run()
    except Exception as exc:
        print(f"[FAIL] {exc}", file=sys.stderr)
        return 1
    finally:
        runner.destroy()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == "__main__":
    raise SystemExit(main())
