import argparse
import json
import math
import os
import signal
import time
import xml.etree.ElementTree as ET
from pathlib import Path

# 导入本地工具模块
from utils import (
    chassis_type_value,
    clamp,
    controller_algorithm_value,
    make_mesh_path_absolute,
    normalize_angle,
    quat_wxyz_to_yaw,
    resolve_topic,
    vehicle_state_from_isaac,
)
from urdf_utils import prepare_isaac_urdf
from image_utils import create_charuco_image, create_checkerboard_image

# TF 发布相关导入（用于发布 base_link 和传感器静态 TF）
try:
    from geometry_msgs.msg import TransformStamped
    from tf2_ros import TransformBroadcaster, StaticTransformBroadcaster
    TF_AVAILABLE = True
except ImportError:
    TF_AVAILABLE = False


def parse_args():
    parser = argparse.ArgumentParser(description="Isaac 标定车间构建脚本")
    parser.add_argument("--headless", action="store_true", help="以无界面模式启动 Isaac Sim")
    parser.add_argument("--skip-final-close", action="store_true", help="退出时不调用 SimulationApp.close()，用于定位 Isaac 关闭阶段崩溃")
    parser.add_argument("--room-length", type=float, default=10.0, help="车间长度（米）")
    parser.add_argument("--room-width", type=float, default=6.0, help="车间宽度（米）")
    parser.add_argument("--room-height", type=float, default=3.5, help="车间高度（米）")
    parser.add_argument("--wall-thickness", type=float, default=0.2, help="墙体厚度（米）")
    parser.add_argument("--light-intensity", type=float, default=30000.0, help="顶灯强度")
    parser.add_argument("--checkerboard-rows", type=int, default=6, help="棋盘格行数")
    parser.add_argument("--checkerboard-cols", type=int, default=9, help="棋盘格列数")
    parser.add_argument("--checkerboard-square-size", type=float, default=0.12, help="棋盘格单格边长（米）")
    parser.add_argument("--camera-resolution-width", type=int, default=1280, help="相机宽度")
    parser.add_argument("--camera-resolution-height", type=int, default=720, help="相机高度")
    parser.add_argument("--camera-frequency", type=float, default=20.0, help="相机频率（Hz）")
    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("--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="不创建顶置相机")
    parser.add_argument("--disable-lidars", action="store_true", help="不创建四角激光雷达")
    parser.add_argument("--disable-vehicle-camera", action="store_true", help="不创建车载前视相机")
    parser.add_argument("--disable-vehicle-lidar", action="store_true", help="不创建车载 3D 激光雷达")
    parser.add_argument("--disable-down-camera", action="store_true", help="不创建车载下视相机")
    parser.add_argument("--disable-vehicle-2d-lidar", action="store_true", help="不创建车载 2D 激光雷达")
    parser.add_argument("--disable-vehicle-imu", action="store_true", help="不创建车载 IMU")
    parser.add_argument("--vehicle-camera-topic", type=str, default="/sensor/front_camera/image_raw", help="车载前视相机图像话题")
    parser.add_argument("--down-camera-topic", type=str, default="/sensor/down_camera/image_raw", help="车载下视相机图像话题")
    parser.add_argument("--vehicle-lidar-topic", type=str, default="/sensor/lidar_3d/pointcloud", help="车载 3D 激光雷达点云话题")
    parser.add_argument("--vehicle-2d-lidar-topic", type=str, default="/sensor/lidar_2d/scan", help="车载 2D 激光雷达 LaserScan 话题")
    parser.add_argument("--vehicle-imu-topic", type=str, default="/sensor/imu/data", help="车载 IMU 话题")
    parser.add_argument("--vehicle-camera-x", type=float, default=1.12, help="车载前视相机相对 base_link X 坐标")
    parser.add_argument("--vehicle-camera-y", type=float, default=0.0, help="车载前视相机相对 base_link Y 坐标")
    parser.add_argument("--vehicle-camera-z", type=float, default=1.18, help="车载前视相机相对 base_link Z 坐标")
    parser.add_argument("--down-camera-x", type=float, default=0.40, help="车载下视相机相对 base_link X 坐标")
    parser.add_argument("--down-camera-y", type=float, default=0.0, help="车载下视相机相对 base_link Y 坐标")
    parser.add_argument("--down-camera-z", type=float, default=0.20, help="车载下视相机相对 base_link Z 坐标")
    parser.add_argument("--disable-down-camera-intrinsic-target", action="store_true", help="不创建下视相机 3D ChArUco 内参标定台")
    parser.add_argument("--down-camera-target-x", type=float, default=0.0, help="下视相机内参标定台中心 X 坐标")
    parser.add_argument("--down-camera-target-y", type=float, default=-2.15, help="下视相机内参标定台中心 Y 坐标")
    parser.add_argument("--down-camera-target-length", type=float, default=2.70, help="下视相机内参标定台长度（米）")
    parser.add_argument("--down-camera-target-width", type=float, default=0.90, help="下视相机内参标定台宽度（米）")
    parser.add_argument("--down-camera-target-max-height", type=float, default=0.045, help="下视相机内参标定台最高点高度（米）")
    parser.add_argument("--down-camera-target-squares-x", type=int, default=30, help="下视相机 ChArUco 纹理 X 方向格数")
    parser.add_argument("--down-camera-target-squares-y", type=int, default=10, help="下视相机 ChArUco 纹理 Y 方向格数")
    parser.add_argument("--vehicle-lidar-x", type=float, default=0.55, help="车载 3D LiDAR 相对 base_link X 坐标")
    parser.add_argument("--vehicle-lidar-y", type=float, default=0.0, help="车载 LiDAR 相对 base_link Y 坐标")
    parser.add_argument("--vehicle-lidar-z", type=float, default=1.20, help="车载 3D LiDAR 相对 base_link Z 坐标")
    parser.add_argument("--vehicle-2d-lidar-x", type=float, default=0.70, help="车载 2D LiDAR 相对 base_link X 坐标")
    parser.add_argument("--vehicle-2d-lidar-y", type=float, default=0.0, help="车载 2D LiDAR 相对 base_link Y 坐标")
    parser.add_argument("--vehicle-2d-lidar-z", type=float, default=1.00, help="车载 2D LiDAR 相对 base_link Z 坐标")
    parser.add_argument("--vehicle-imu-x", type=float, default=0.40, help="车载 IMU 相对 base_link X 坐标")
    parser.add_argument("--vehicle-imu-y", type=float, default=0.0, help="车载 IMU 相对 base_link Y 坐标")
    parser.add_argument("--vehicle-imu-z", type=float, default=0.26, help="车载 IMU 相对 base_link Z 坐标")
    parser.add_argument("--disable-2d-lidar-targets", action="store_true", help="不创建 2D 激光雷达角落外参标定靶标")
    parser.add_argument("--lidar-2d-target-width", type=float, default=0.82, help="2D LiDAR 靶标水平宽度（米）")
    parser.add_argument("--lidar-2d-target-height", type=float, default=1.60, help="2D LiDAR 靶标有效高度（米）")
    parser.add_argument("--lidar-2d-target-thickness", type=float, default=0.04, help="2D LiDAR 靶标板厚（米）")
    parser.add_argument("--lidar-2d-target-bottom-z", type=float, default=0.08, help="2D LiDAR 靶标底部高度（米）")
    parser.add_argument("--lidar-2d-target-slope-angle-deg", type=float, default=45.0, help="2D LiDAR 斜板相对垂直基准面的夹角（度）")
    parser.add_argument("--disable-external-telemetry", action="store_true", help="不发布 external 仿真遥测")
    parser.add_argument("--external-telemetry-topic", type=str, default="/isaac/external_localization/telemetry", help="external 仿真遥测话题")
    parser.add_argument("--external-reference-source-name", type=str, default="isaac_sim_truth_source", help="external 真值源名称")
    parser.add_argument("--external-workcell-zone-id", type=str, default="isaac_workcell_zone_a", help="external 所属工位 ID")
    parser.add_argument("--external-publish-hz", type=float, default=20.0, help="external 遥测发布频率（Hz）")
    parser.add_argument("--external-position-noise-stddev-m", type=float, default=0.005, help="位置噪声标准差（米）")
    parser.add_argument("--external-yaw-noise-stddev-rad", type=float, default=0.003, help="航向噪声标准差（弧度）")
    parser.add_argument("--external-position-stddev-m", type=float, default=0.01, help="上报的位置标准差（米）")
    parser.add_argument("--external-yaw-stddev-rad", type=float, default=0.01, help="上报的航向标准差（弧度）")
    parser.add_argument("--external-tracking-loss-ratio", type=float, default=0.0, help="上报的跟踪丢失比例")
    parser.add_argument("--external-time-sync-offset-ms", type=float, default=2.0, help="上报的时间同步偏差（毫秒）")
    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("--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）")
    parser.add_argument(
        "--chassis-type",
        choices=["differential", "ackermann", "single_steer", "multi_steer"],
        default="ackermann",
        help="仿真底盘类型，用于遥测消息",
    )
    parser.add_argument("--wheel-radius", type=float, default=0.10, help="遥测换算用轮半径（米）")
    parser.add_argument("--wheel-track", type=float, default=0.52, help="差速/多轮底盘轮距（米）")
    parser.add_argument("--wheel-base", type=float, default=0.80, help="阿克曼底盘轴距（米）")
    parser.add_argument("--odom-noise-stddev-m", type=float, default=0.002, help="底盘里程计位置噪声标准差（米）")
    parser.add_argument("--yaw-noise-stddev-rad", type=float, default=0.001, help="底盘里程计 yaw 噪声标准差（弧度）")
    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-y-m", type=float, default=0.0, help="运控直线参考轨迹的 Y 坐标（米）")
    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）")
    parser.add_argument("--disable-sensor-telemetry", action="store_true", help="不发布传感器标定遥测")
    parser.add_argument("--sensor-telemetry-topic", type=str, default="/sensor_calibration/telemetry", help="传感器标定遥测话题")
    parser.add_argument("--sensor-telemetry-hz", type=float, default=10.0, help="传感器标定遥测发布频率（Hz）")
    parser.add_argument("--front-camera-sensor-id", type=str, default="demo_front_camera", help="前视相机传感器 ID")
    parser.add_argument("--sensor-target-sample-count", type=int, default=20, help="传感器标定目标样本数")
    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("--straight-track-length", type=float, default=5.0, help="底盘直线标定路线长度（米）")
    parser.add_argument("--straight-track-width", type=float, default=0.7, help="底盘直线标定路线宽度（米）")
    parser.add_argument("--arc-track-radius", type=float, default=1.6, help="圆弧/转向标定路线半径（米）")
    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 坐标")
    parser.add_argument("--vehicle-initial-y", type=float, default=0.0, help="车辆初始 Y 坐标")
    parser.add_argument("--vehicle-initial-z", type=float, default=0.0, help="车辆初始 Z 坐标")
    parser.add_argument(
        "--vehicle-source",
        choices=["urdf"],
        default="urdf",
        help="车辆来源；当前固定为 URDF 路径直接导入",
    )
    parser.add_argument("--max-steer-angle-deg", type=float, default=32.0, help="阿克曼前轮最大转角（度）")
    parser.add_argument("--max-speed-ms", type=float, default=1.2, help="阿克曼小车最大车速（m/s）")
    parser.add_argument("--urdf-path", type=str, default="", help="车辆 URDF 路径；默认使用仓库内置阿克曼小车 URDF")
    parser.add_argument("--usd-path", type=str, default="", help=argparse.SUPPRESS)
    parser.add_argument("--disable-urdf-visual-origin-fix", action="store_true", help=argparse.SUPPRESS)
    parser.add_argument("--force-rebuild-usd", action="store_true", help=argparse.SUPPRESS)
    parser.add_argument("--scene-manifest-path", type=str, default="", help="输出车间场景 manifest JSON 的路径")
    args, _ = parser.parse_known_args()
    return args


ARGS = parse_args()
SCRIPT_PATH = Path(__file__).resolve()
SHUTDOWN_SIGNAL = None
SHUTDOWN_REQUESTED = False


def _record_shutdown_signal(signum, _frame):
    global SHUTDOWN_SIGNAL, SHUTDOWN_REQUESTED
    SHUTDOWN_SIGNAL = signum
    SHUTDOWN_REQUESTED = True
    print(f"\n[INFO] 收到退出信号: {signal.Signals(signum).name}({signum})", flush=True)


def install_shutdown_signal_handlers():
    signal.signal(signal.SIGINT, _record_shutdown_signal)
    signal.signal(signal.SIGTERM, _record_shutdown_signal)


def ros_context_is_shutdown():
    return rclpy is not None and not rclpy.ok()


def is_ros_context_invalid_error(exc):
    text = str(exc)
    return "publisher's context is invalid" in text or "context is invalid" in text


install_shutdown_signal_handlers()


def patch_pillow_compat_for_isaac_extensions():
    try:
        import PIL._util as pil_util
    except Exception as exc:
        print(f"[WARN] Pillow 兼容补丁跳过: {exc}")
        return

    if hasattr(pil_util, "is_directory"):
        return

    def is_directory(path):
        return isinstance(path, (bytes, str, os.PathLike)) and os.path.isdir(path)

    pil_util.is_directory = is_directory
    print("[*] 已补齐 PIL._util.is_directory 兼容接口，用于 Isaac UI 测试扩展加载。")


patch_pillow_compat_for_isaac_extensions()


def find_project_root(script_path):
    for parent in [script_path.parent, *script_path.parents]:
        if (parent / "src" / "simulation" / "isaac_workshop_sim").is_dir():
            return parent
    raise RuntimeError(f"无法从脚本路径定位项目根目录: {script_path}")


PROJECT_ROOT = find_project_root(SCRIPT_PATH)
ISAAC_SIM_ROOT = SCRIPT_PATH.parents[1]
WORKSPACE_ROOT = PROJECT_ROOT.parent
MODELS_ROOT = WORKSPACE_ROOT / "models"
ASSETS_ROOT = ISAAC_SIM_ROOT / "assets"
DEFAULT_ACKERMANN_URDF_PATH = ASSETS_ROOT / "urdf" / "ackermann_front_steer_rear_drive.urdf"
GENERATED_ROOT = PROJECT_ROOT / ".isaac_cache"
GENERATED_ROOT.mkdir(parents=True, exist_ok=True)


def validate_args(args):
    numeric_positive_fields = {
        "room_length": args.room_length,
        "room_width": args.room_width,
        "room_height": args.room_height,
        "wall_thickness": args.wall_thickness,
        "checkerboard_square_size": args.checkerboard_square_size,
        "camera_focal_length": args.camera_focal_length,
    }
    for name, value in numeric_positive_fields.items():
        if value <= 0:
            raise ValueError(f"{name} 必须大于 0，当前值={value}")

    if args.checkerboard_rows <= 0 or args.checkerboard_cols <= 0:
        raise ValueError("checkerboard_rows 和 checkerboard_cols 必须大于 0。")
    if args.camera_resolution_width <= 0 or args.camera_resolution_height <= 0:
        raise ValueError("camera resolution 必须大于 0。")
    if args.camera_frequency <= 0:
        raise ValueError("camera_frequency 必须大于 0。")
    if args.external_publish_hz < 0 or args.chassis_telemetry_hz < 0 or args.control_telemetry_hz < 0 or args.sensor_telemetry_hz < 0:
        raise ValueError("发布频率不能为负数。")
    if not 0.0 <= args.external_pose_drop_rate <= 1.0:
        raise ValueError("external_pose_drop_rate 必须在 [0,1] 范围内。")
    if not 0.0 <= args.external_quality_score <= 1.0:
        raise ValueError("external_quality_score 必须在 [0,1] 范围内。")
    if not 0.0 <= args.sensor_quality_score <= 1.0:
        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.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:
        raise ValueError("2D LiDAR 靶标底部高度不能为负数。")
    if not 5.0 <= args.lidar_2d_target_slope_angle_deg <= 85.0:
        raise ValueError("2D LiDAR 斜板角度应在 [5,85] 度范围内。")
    if args.down_camera_target_length <= 0 or args.down_camera_target_width <= 0:
        raise ValueError("下视相机内参标定台长度和宽度必须大于 0。")
    if args.down_camera_target_max_height <= 0:
        raise ValueError("下视相机内参标定台最高点必须大于 0。")
    if args.down_camera_target_max_height <= 0.012:
        raise ValueError("下视相机内参标定台最高点必须高于低平面 0.012m。")
    if args.down_camera_target_max_height >= 0.08:
        raise ValueError("下视相机内参标定台最高点应小于默认底盘最小离地间隙 0.08m。")
    if args.down_camera_target_squares_x < 4 or args.down_camera_target_squares_y < 4:
        raise ValueError("下视相机 ChArUco 纹理格数至少为 4x4。")
    down_target_square_x = args.down_camera_target_length / args.down_camera_target_squares_x
    down_target_square_y = args.down_camera_target_width / args.down_camera_target_squares_y
    square_aspect_error = abs(down_target_square_x - down_target_square_y) / max(down_target_square_x, down_target_square_y)
    if square_aspect_error > 0.05:
        raise ValueError("下视相机 ChArUco 物理单格应接近正方形，请同步调整 target 尺寸和 squares 数。")
    target_half_x = args.down_camera_target_length / 2.0
    target_half_y = args.down_camera_target_width / 2.0
    if abs(args.down_camera_target_x) + target_half_x > args.room_length / 2.0:
        raise ValueError("下视相机内参标定台 X 方向超出车间范围。")
    if abs(args.down_camera_target_y) + target_half_y > args.room_width / 2.0:
        raise ValueError("下视相机内参标定台 Y 方向超出车间范围。")

    if args.max_steer_angle_deg <= 0 or args.max_speed_ms <= 0:
        raise ValueError("max_steer_angle_deg 和 max_speed_ms 必须大于 0。")

    urdf_path = Path(args.urdf_path).expanduser().resolve() if args.urdf_path else DEFAULT_ACKERMANN_URDF_PATH
    if not urdf_path.exists():
        raise FileNotFoundError(f"未找到车辆 URDF: {urdf_path}")


validate_args(ARGS)
os.environ["OMNI_KIT_ACCEPT_EULA"] = "YES"

from isaacsim import SimulationApp

simulation_app = SimulationApp({"headless": ARGS.headless})

from omni.isaac.core.utils.extensions import enable_extension

enable_extension("omni.isaac.ros2_bridge")
enable_extension("omni.isaac.sensor")
simulation_app.update()

import numpy as np
from PIL import Image
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 omni.isaac.core import World
from omni.isaac.core.objects import FixedCuboid
from omni.isaac.core.robots import Robot
from omni.isaac.core.utils.prims import create_prim
from omni.isaac.core.utils.rotations import euler_angles_to_quat
from omni.isaac.core.utils.viewports import set_camera_view

# 导入本地模块（需要在 Isaac Sim 初始化后）
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,
)

try:
    import rclpy
except ImportError:
    rclpy = None

try:
    from calibration_external_localization_interfaces.msg import ExternalLocalizationTelemetry
except ImportError:
    ExternalLocalizationTelemetry = None

try:
    from calibration_sensor_interfaces.msg import CaptureIntentType, SensorCalibrationTelemetry
except ImportError:
    CaptureIntentType = None
    SensorCalibrationTelemetry = None

try:
    from calibration_vehicle_profile_interfaces.msg import (
        ChassisType,
        ControllerAlgorithmType,
        SensorType,
    )
except ImportError:
    ChassisType = None
    ControllerAlgorithmType = None
    SensorType = None

try:
    from calibration_chassis_interfaces.msg import ChassisTelemetry, WheelModuleState
except ImportError:
    ChassisTelemetry = None
    WheelModuleState = None

try:
    from calibration_control_interfaces.msg import ControlTelemetry
except ImportError:
    ControlTelemetry = None

try:
    from sensor_msgs.msg import Imu, LaserScan
except ImportError:
    Imu = None
    LaserScan = None


def add_corner_rotary_lidars(room_length, room_width, height, lidar_config, topic_prefix):
    """添加四角旋转 LiDAR"""
    from omni.isaac.core.utils.rotations import euler_angles_to_quat
    import omni.graph.core as og
    import omni.kit.commands
    import omni.replicator.core as rep
    from pxr import Gf

    offset = 0.3
    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"
    nodes = [
        ("OnTick", "omni.graph.action.OnTick"),
        ("ReadSimTime", "omni.isaac.core_nodes.IsaacReadSimulationTime"),
        ("PublishTF", "omni.isaac.ros2_bridge.ROS2PublishTransformTree"),
    ]
    connections = [
        ("OnTick.outputs:tick", "PublishTF.inputs:execIn"),
        ("ReadSimTime.outputs:simulationTime", "PublishTF.inputs:timeStamp"),
    ]
    set_values = []
    lidar_paths = []

    for cfg in lidar_configs:
        lidar_path = f"/World/Sensors/Lidar_{cfg['name']}"
        lidar_paths.append(lidar_path)
        quat = euler_angles_to_quat(np.array([0, 15.0, cfg["yaw"]]), degrees=True)
        orientation = Gf.Quatd(quat[0], quat[1], quat[2], quat[3])
        omni.kit.commands.execute(
            "IsaacSensorCreateRtxLidar",
            path=lidar_path,
            parent=None,
            config=lidar_config,
            translation=Gf.Vec3d(*cfg["pos"]),
            orientation=orientation,
        )
        render_product = rep.create.render_product(lidar_path, [1, 1])
        helper_name = f"ROS2LidarHelper_{cfg['name']}"
        nodes.append((helper_name, "omni.isaac.ros2_bridge.ROS2RtxLidarHelper"))
        connections.append(("OnTick.outputs:tick", f"{helper_name}.inputs:execIn"))
        set_values.extend([
            (f"{helper_name}.inputs:renderProductPath", str(render_product.path)),
            (f"{helper_name}.inputs:topicName", f"{topic_prefix}/{cfg['name'].lower()}/pointcloud"),
            (f"{helper_name}.inputs:frameId", f"Lidar_{cfg['name']}"),
            (f"{helper_name}.inputs:type", "point_cloud"),
            (f"{helper_name}.inputs:fullScan", True),
        ])

    set_values.append(("PublishTF.inputs:targetPrims", lidar_paths))
    og.Controller.edit(
        {"graph_path": graph_path, "evaluator_name": "execution"},
        {keys.CREATE_NODES: nodes, keys.CONNECT: connections, keys.SET_VALUES: set_values},
    )



class ExternalTruthTelemetryPublisher:
    def __init__(self, args):
        self.enabled = not args.disable_external_telemetry
        self.topic = args.external_telemetry_topic
        self.publish_period_sec = 0.0 if args.external_publish_hz <= 0.0 else 1.0 / args.external_publish_hz
        self.reference_source_name = args.external_reference_source_name
        self.workcell_zone_id = args.external_workcell_zone_id
        self.position_noise_stddev_m = max(0.0, args.external_position_noise_stddev_m)
        self.yaw_noise_stddev_rad = max(0.0, args.external_yaw_noise_stddev_rad)
        self.position_stddev_m = max(0.0, args.external_position_stddev_m)
        self.yaw_stddev_rad = max(0.0, args.external_yaw_stddev_rad)
        self.tracking_loss_ratio = clamp(args.external_tracking_loss_ratio, 0.0, 1.0)
        self.time_sync_offset_ms = abs(args.external_time_sync_offset_ms)
        self.quality_score = clamp(args.external_quality_score, 0.0, 1.0)
        self.observed_target_count = max(0, args.external_observed_target_count)
        self.pose_drop_rate = clamp(args.external_pose_drop_rate, 0.0, 1.0)
        self.last_publish_wall_time = 0.0
        self.rng = np.random.default_rng(args.random_seed)
        self.node = None
        self.publisher = None

        if not self.enabled:
            print("[*] 已禁用 external 仿真遥测发布。")
            return

        if rclpy is None or ExternalLocalizationTelemetry is None:
            print("[WARN] 未找到 rclpy 或 calibration_external_localization_interfaces，external 遥测发布已禁用。")
            self.enabled = False
            return

        if not rclpy.ok():
            rclpy.init(args=None)

        self.node = rclpy.create_node("isaac_external_truth_publisher")
        self.publisher = self.node.create_publisher(ExternalLocalizationTelemetry, self.topic, 10)
        print(f"[*] external 仿真遥测发布已启用: topic={self.topic}, source={self.reference_source_name}, zone={self.workcell_zone_id}")

    def publish(self, agv):
        if not self.enabled or self.publisher is None:
            return

        now = time.time()
        if self.publish_period_sec > 0.0 and (now - self.last_publish_wall_time) < self.publish_period_sec:
            return

        position, quat = agv.get_world_pose()
        yaw_rad = quat_wxyz_to_yaw(quat)
        pose_valid = self.rng.random() >= self.pose_drop_rate

        msg = ExternalLocalizationTelemetry()
        msg.hardware_timestamp_us = time.time_ns() // 1000
        msg.pose_valid = pose_valid
        msg.workshop_pose.x_m = float(position[0] + self.rng.normal(0.0, self.position_noise_stddev_m))
        msg.workshop_pose.y_m = float(position[1] + self.rng.normal(0.0, self.position_noise_stddev_m))
        msg.workshop_pose.z_m = float(position[2])
        msg.workshop_pose.roll_rad = 0.0
        msg.workshop_pose.pitch_rad = 0.0
        msg.workshop_pose.yaw_rad = float(yaw_rad + self.rng.normal(0.0, self.yaw_noise_stddev_rad))
        msg.position_stddev_m = self.position_stddev_m
        msg.yaw_stddev_rad = self.yaw_stddev_rad
        msg.tracking_loss_ratio = self.tracking_loss_ratio if pose_valid else max(self.tracking_loss_ratio, 1.0)
        msg.time_sync_offset_ms = self.time_sync_offset_ms
        msg.quality_score = self.quality_score if pose_valid else 0.0
        msg.observed_target_count = self.observed_target_count if pose_valid else 0
        msg.reference_source_name = self.reference_source_name
        msg.active_job_id = f"{self.workcell_zone_id}:isaac_external_truth"

        self.publisher.publish(msg)
        self.last_publish_wall_time = now

    def shutdown(self):
        if self.node is not None:
            self.node.destroy_node()
            self.node = None




class VehicleTfPublisher:
    """发布车辆 base_link 动态 TF 和传感器静态 TF"""

    def __init__(self, args):
        self.enabled = TF_AVAILABLE and not getattr(args, 'disable_vehicle_tf', False)
        self.tf_publish_hz = getattr(args, 'vehicle_tf_publish_hz', 50.0)
        self.publish_period_sec = 0.0 if self.tf_publish_hz <= 0.0 else 1.0 / self.tf_publish_hz
        self.last_publish_time = 0.0
        self.node = None
        self.tf_broadcaster = None
        self.static_tf_broadcaster = None
        self.static_tfs_published = False

        # 传感器相对于 base_link 的安装位置（从 args 读取）
        self.sensor_configs = {
            'front_camera_link': {
                'x': args.vehicle_camera_x,
                'y': args.vehicle_camera_y,
                'z': args.vehicle_camera_z,
                # USD 相机默认看向局部 -Z；这里让光轴朝 base_link +X，画面上方朝 base_link +Z，避免图像顺时针旋转 90°。
                'roll': math.pi / 2,
                'pitch': 0.0,
                'yaw': -math.pi / 2,
                'enabled': not args.disable_vehicle_camera,
            },
            'down_camera_link': {
                'x': args.down_camera_x,
                'y': args.down_camera_y,
                'z': args.down_camera_z,
                'roll': 0.0,
                'pitch': 0.0,  # 朝下相机，垂直向下
                'yaw': 0.0,
                'enabled': not args.disable_down_camera,
            },
            'lidar_3d_link': {
                'x': args.vehicle_lidar_x,
                'y': args.vehicle_lidar_y,
                'z': args.vehicle_lidar_z,
                'roll': 0.0,
                'pitch': 0.0,
                'yaw': 0.0,
                'enabled': not args.disable_vehicle_lidar,
            },
            'lidar_2d_link': {
                'x': args.vehicle_2d_lidar_x,
                'y': args.vehicle_2d_lidar_y,
                'z': args.vehicle_2d_lidar_z,
                'roll': 0.0,
                'pitch': 0.0,
                'yaw': 0.0,
                'enabled': not args.disable_vehicle_2d_lidar,
            },
            'imu_link': {
                'x': args.vehicle_imu_x,
                'y': args.vehicle_imu_y,
                'z': args.vehicle_imu_z,
                'roll': 0.0,
                'pitch': 0.0,
                'yaw': 0.0,
                'enabled': not args.disable_vehicle_imu,
            },
        }

        if not self.enabled:
            print("[*] 车辆 TF 发布已禁用（TF_AVAILABLE={}）或参数禁用。".format(TF_AVAILABLE))
            return

        if rclpy is None:
            print("[WARN] rclpy 不可用，车辆 TF 发布已禁用。")
            self.enabled = False
            return

        if not rclpy.ok():
            rclpy.init(args=None)

        self.node = rclpy.create_node("isaac_vehicle_tf_publisher")
        self.tf_broadcaster = TransformBroadcaster(self.node)
        self.static_tf_broadcaster = StaticTransformBroadcaster(self.node)
        print(f"[*] 车辆 TF 发布已启用: hz={self.tf_publish_hz}")

    def _create_transform(self, frame_id, child_frame_id, x, y, z, roll, pitch, yaw, timestamp=None):
        """创建 TransformStamped 消息"""
        t = TransformStamped()
        if timestamp is None:
            timestamp = self.node.get_clock().now().to_msg()
        t.header.stamp = timestamp
        t.header.frame_id = frame_id
        t.child_frame_id = child_frame_id
        x = float(x)
        y = float(y)
        z = float(z)
        roll = float(roll)
        pitch = float(pitch)
        yaw = float(yaw)
        t.transform.translation.x = x
        t.transform.translation.y = y
        t.transform.translation.z = z

        # 欧拉角转四元数
        cy = math.cos(yaw * 0.5)
        sy = math.sin(yaw * 0.5)
        cp = math.cos(pitch * 0.5)
        sp = math.sin(pitch * 0.5)
        cr = math.cos(roll * 0.5)
        sr = math.sin(roll * 0.5)

        t.transform.rotation.w = float(cr * cp * cy + sr * sp * sy)
        t.transform.rotation.x = float(sr * cp * cy - cr * sp * sy)
        t.transform.rotation.y = float(cr * sp * cy + sr * cp * sy)
        t.transform.rotation.z = float(cr * cp * sy - sr * sp * cy)

        return t

    def publish_static_tfs(self):
        """发布传感器相对于 base_link 的静态 TF（只发布一次）"""
        if not self.enabled or self.static_tfs_published:
            return

        transforms = []
        timestamp = self.node.get_clock().now().to_msg()

        for child_frame, config in self.sensor_configs.items():
            if not config['enabled']:
                continue
            t = self._create_transform(
                'base_link', child_frame,
                config['x'], config['y'], config['z'],
                config['roll'], config['pitch'], config['yaw'],
                timestamp
            )
            transforms.append(t)
            print(f"[*] 静态 TF: base_link -> {child_frame} ({config['x']:.3f}, {config['y']:.3f}, {config['z']:.3f})")

        if transforms:
            self.static_tf_broadcaster.sendTransform(transforms)
            self.static_tfs_published = True
            print(f"[*] 已发布 {len(transforms)} 个传感器静态 TF")

    def publish(self, agv):
        """发布 base_link 相对于 world 的动态 TF"""
        if not self.enabled:
            return

        now = time.time()
        if self.publish_period_sec > 0.0 and (now - self.last_publish_time) < self.publish_period_sec:
            return

        # 首先发布静态 TF（只发布一次）
        if not self.static_tfs_published:
            self.publish_static_tfs()

        # 获取车辆世界位姿
        position, quat = agv.get_world_pose()

        # 四元数 (w, x, y, z) 转欧拉角（用于调试）
        qw, qx, qy, qz = (float(quat[0]), float(quat[1]), float(quat[2]), float(quat[3]))
        yaw = math.atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz))

        # 发布 world -> base_link 动态 TF
        timestamp = self.node.get_clock().now().to_msg()
        t = self._create_transform(
            'world', 'base_link',
            position[0], position[1], position[2],
            0.0, 0.0, yaw,  # roll/pitch 设为 0，只保留 yaw
            timestamp
        )
        # 使用原始四元数覆盖（更精确）
        t.transform.rotation.w = float(qw)
        t.transform.rotation.x = float(qx)
        t.transform.rotation.y = float(qy)
        t.transform.rotation.z = float(qz)

        self.tf_broadcaster.sendTransform(t)
        self.last_publish_time = now

    def shutdown(self):
        if self.node is not None:
            self.node.destroy_node()
            self.node = None

class VehicleImuTopicPublisher:
    def __init__(self, args):
        self.enabled = not args.disable_vehicle_imu
        self.topic = args.vehicle_imu_topic
        self.publish_period_sec = 0.0 if args.chassis_telemetry_hz <= 0.0 else 1.0 / args.chassis_telemetry_hz
        self.last_publish_wall_time = 0.0
        self.last_sample_wall_time = None
        self.last_linear_velocity = None
        self.node = None
        self.publisher = None

        if not self.enabled:
            print("[*] 已禁用车载 IMU 话题发布。")
            return

        if rclpy is None or Imu is None:
            print("[WARN] 未找到 rclpy 或 sensor_msgs，车载 IMU 话题发布已禁用。")
            self.enabled = False
            return

        if not rclpy.ok():
            rclpy.init(args=None)

        self.node = rclpy.create_node("isaac_vehicle_imu_topic_publisher")
        self.publisher = self.node.create_publisher(Imu, self.topic, 10)
        print(f"[*] 车载 IMU 话题发布已启用: topic={self.topic}")

    @staticmethod
    def _stamp_now(msg):
        now_ns = time.time_ns()
        msg.header.stamp.sec = int(now_ns // 1_000_000_000)
        msg.header.stamp.nanosec = int(now_ns % 1_000_000_000)

    def publish(self, agv):
        if not self.enabled or self.publisher is None or agv is None:
            return

        now = time.time()
        if self.publish_period_sec > 0.0 and (now - self.last_publish_wall_time) < self.publish_period_sec:
            return

        _position, quat = agv.get_world_pose()
        yaw_rad = quat_wxyz_to_yaw(quat)
        linear_velocity = np.array(agv.get_linear_velocity(), dtype=float)
        try:
            angular_velocity = np.array(agv.get_angular_velocity(), dtype=float)
        except Exception:
            angular_velocity = np.array([0.0, 0.0, 0.0], dtype=float)

        if self.last_linear_velocity is None or self.last_sample_wall_time is None:
            world_accel = np.array([0.0, 0.0, 0.0], dtype=float)
        else:
            dt = max(now - self.last_sample_wall_time, 1e-3)
            world_accel = (linear_velocity - self.last_linear_velocity) / dt

        cos_yaw = math.cos(yaw_rad)
        sin_yaw = math.sin(yaw_rad)
        body_accel_x = cos_yaw * world_accel[0] + sin_yaw * world_accel[1]
        body_accel_y = -sin_yaw * world_accel[0] + cos_yaw * world_accel[1]
        body_ang_x = cos_yaw * angular_velocity[0] + sin_yaw * angular_velocity[1]
        body_ang_y = -sin_yaw * angular_velocity[0] + cos_yaw * angular_velocity[1]

        msg = Imu()
        self._stamp_now(msg)
        msg.header.frame_id = "imu_link"
        msg.orientation.w = float(quat[0])
        msg.orientation.x = float(quat[1])
        msg.orientation.y = float(quat[2])
        msg.orientation.z = float(quat[3])
        msg.angular_velocity.x = float(body_ang_x)
        msg.angular_velocity.y = float(body_ang_y)
        msg.angular_velocity.z = float(angular_velocity[2])
        msg.linear_acceleration.x = float(body_accel_x)
        msg.linear_acceleration.y = float(body_accel_y)
        msg.linear_acceleration.z = float(world_accel[2] + 9.80665)
        msg.orientation_covariance = [0.001, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.001]
        msg.angular_velocity_covariance = [0.0025, 0.0, 0.0, 0.0, 0.0025, 0.0, 0.0, 0.0, 0.0025]
        msg.linear_acceleration_covariance = [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]

        self.publisher.publish(msg)
        self.last_publish_wall_time = now
        self.last_sample_wall_time = now
        self.last_linear_velocity = linear_velocity

    def shutdown(self):
        if self.node is not None:
            self.node.destroy_node()
            self.node = None


class VehicleLaserScanTopicPublisher:
    def __init__(self, args, target_specs):
        self.enabled = not args.disable_vehicle_2d_lidar
        self.args = args
        self.target_specs = list(target_specs or [])
        self.topic = args.vehicle_2d_lidar_topic
        self.publish_period_sec = 0.0 if args.chassis_telemetry_hz <= 0.0 else 1.0 / args.chassis_telemetry_hz
        self.angle_min = -math.pi
        self.angle_max = math.pi
        self.angle_increment = math.radians(1.0)
        self.range_min = 0.05
        self.range_max = max(12.0, math.hypot(args.room_length, args.room_width))
        self.last_publish_wall_time = 0.0
        self.node = None
        self.publisher = None

        if not self.enabled:
            print("[*] 已禁用车载 2D LiDAR LaserScan 发布。")
            return

        if rclpy is None or LaserScan is None:
            print("[WARN] 未找到 rclpy 或 sensor_msgs，车载 2D LiDAR LaserScan 发布已禁用。")
            self.enabled = False
            return

        if not rclpy.ok():
            rclpy.init(args=None)

        self.node = rclpy.create_node("isaac_vehicle_lidar_2d_scan_publisher")
        self.publisher = self.node.create_publisher(LaserScan, self.topic, 10)
        print(f"[*] 车载 2D LiDAR LaserScan 发布已启用: topic={self.topic}")

    @staticmethod
    def _stamp_now(msg):
        now_ns = time.time_ns()
        msg.header.stamp.sec = int(now_ns // 1_000_000_000)
        msg.header.stamp.nanosec = int(now_ns % 1_000_000_000)

    @staticmethod
    def _axis_hit(origin, direction, fixed_axis, fixed_value, range_axis, range_min, range_max):
        denom = direction[fixed_axis]
        if abs(denom) < 1e-9:
            return None
        t = (fixed_value - origin[fixed_axis]) / denom
        if t <= 0.0:
            return None
        crossed = origin[range_axis] + t * direction[range_axis]
        if range_min <= crossed <= range_max:
            return t
        return None

    def _target_segments_at_scan_height(self, scan_z):
        segments = []
        for spec in self.target_specs:
            position = spec.get("position", [0.0, 0.0, 0.0])
            scale = spec.get("scale", [0.0, 0.0, 0.0])
            if spec.get("wall") == "front":
                x = float(position[0])
                if spec.get("type") == "height_encoding_slope":
                    angle_rad = math.radians(float(spec.get("slope_angle_deg", self.args.lidar_2d_target_slope_angle_deg)))
                    x -= (float(scan_z) - float(position[2])) * math.tan(angle_rad)
                y_min = float(position[1]) - float(scale[1]) / 2.0
                y_max = float(position[1]) + float(scale[1]) / 2.0
                segments.append(("x", x, y_min, y_max))
            elif spec.get("wall") == "right":
                y = float(position[1])
                x_min = float(position[0]) - float(scale[0]) / 2.0
                x_max = float(position[0]) + float(scale[0]) / 2.0
                segments.append(("y", y, x_min, x_max))
        return segments

    def _raycast(self, origin_xy, beam_yaw, scan_z):
        direction = np.array([math.cos(beam_yaw), math.sin(beam_yaw)], dtype=float)
        half_length = self.args.room_length / 2.0
        half_width = self.args.room_width / 2.0
        best_range = self.range_max
        best_intensity = 5.0

        wall_hits = [
            self._axis_hit(origin_xy, direction, 0, half_length, 1, -half_width, half_width),
            self._axis_hit(origin_xy, direction, 0, -half_length, 1, -half_width, half_width),
            self._axis_hit(origin_xy, direction, 1, half_width, 0, -half_length, half_length),
            self._axis_hit(origin_xy, direction, 1, -half_width, 0, -half_length, half_length),
        ]
        for hit_range in wall_hits:
            if hit_range is not None and self.range_min <= hit_range < best_range:
                best_range = hit_range
                best_intensity = 10.0

        for axis, value, seg_min, seg_max in self._target_segments_at_scan_height(scan_z):
            if axis == "x":
                hit_range = self._axis_hit(origin_xy, direction, 0, value, 1, seg_min, seg_max)
            else:
                hit_range = self._axis_hit(origin_xy, direction, 1, value, 0, seg_min, seg_max)
            if hit_range is not None and self.range_min <= hit_range < best_range:
                best_range = hit_range
                best_intensity = 80.0

        return float(best_range), float(best_intensity)

    def publish(self, agv):
        if not self.enabled or self.publisher is None or agv is None:
            return

        now = time.time()
        if self.publish_period_sec > 0.0 and (now - self.last_publish_wall_time) < self.publish_period_sec:
            return

        position, quat = agv.get_world_pose()
        yaw_rad = quat_wxyz_to_yaw(quat)
        cos_yaw = math.cos(yaw_rad)
        sin_yaw = math.sin(yaw_rad)
        lidar_local_x = self.args.vehicle_2d_lidar_x
        lidar_local_y = self.args.vehicle_2d_lidar_y
        origin_xy = np.array([
            float(position[0]) + cos_yaw * lidar_local_x - sin_yaw * lidar_local_y,
            float(position[1]) + sin_yaw * lidar_local_x + cos_yaw * lidar_local_y,
        ])
        scan_z = float(position[2]) + self.args.vehicle_2d_lidar_z

        beam_count = int(round((self.angle_max - self.angle_min) / self.angle_increment)) + 1
        ranges = []
        intensities = []
        for index in range(beam_count):
            angle = self.angle_min + index * self.angle_increment
            hit_range, intensity = self._raycast(origin_xy, yaw_rad + angle, scan_z)
            ranges.append(hit_range)
            intensities.append(intensity)

        msg = LaserScan()
        self._stamp_now(msg)
        msg.header.frame_id = "lidar_2d_link"
        msg.angle_min = float(self.angle_min)
        msg.angle_max = float(self.angle_max)
        msg.angle_increment = float(self.angle_increment)
        msg.time_increment = 0.0
        msg.scan_time = float(self.publish_period_sec if self.publish_period_sec > 0.0 else 0.05)
        msg.range_min = float(self.range_min)
        msg.range_max = float(self.range_max)
        msg.ranges = ranges
        msg.intensities = intensities

        self.publisher.publish(msg)
        self.last_publish_wall_time = now

    def shutdown(self):
        if self.node is not None:
            self.node.destroy_node()
            self.node = None


class ChassisCalibrationTelemetryPublisher:
    def __init__(self, args):
        self.enabled = not args.disable_chassis_telemetry
        self.topic = args.chassis_telemetry_topic
        self.publish_period_sec = 0.0 if args.chassis_telemetry_hz <= 0.0 else 1.0 / args.chassis_telemetry_hz
        self.chassis_type = args.chassis_type
        self.wheel_radius = args.wheel_radius
        self.wheel_track = args.wheel_track
        self.wheel_base = args.wheel_base
        self.odom_noise_stddev_m = args.odom_noise_stddev_m
        self.yaw_noise_stddev_rad = args.yaw_noise_stddev_rad
        self.rng = np.random.default_rng(args.random_seed + 17)
        self.last_publish_wall_time = 0.0
        self.encoder_ticks = {}
        self.node = None
        self.publisher = None

        if not self.enabled:
            print("[*] 已禁用底盘标定遥测发布。")
            return

        if rclpy is None or ChassisTelemetry is None or WheelModuleState is None or ChassisType is None:
            print("[WARN] 未找到底盘标定 ROS 消息，底盘遥测发布已禁用。")
            self.enabled = False
            return

        if not rclpy.ok():
            rclpy.init(args=None)

        self.node = rclpy.create_node("isaac_chassis_calibration_telemetry_publisher")
        self.publisher = self.node.create_publisher(ChassisTelemetry, self.topic, 10)
        print(f"[*] 底盘标定遥测发布已启用: topic={self.topic}, chassis_type={self.chassis_type}")

    def make_module_state(self, module_id, wheel_speed_rpm, steer_angle_deg, dt):
        ticks_per_rev = 2048.0
        previous_ticks = self.encoder_ticks.get(module_id, 0.0)
        next_ticks = previous_ticks + wheel_speed_rpm / 60.0 * ticks_per_rev * max(dt, 0.0)
        self.encoder_ticks[module_id] = next_ticks

        module = WheelModuleState()
        module.module_id = module_id
        module.encoder_ticks = int(next_ticks)
        module.wheel_speed_rpm = float(wheel_speed_rpm)
        module.steer_angle_deg = float(steer_angle_deg)
        module.motor_current_amp = float(0.8 + 0.15 * abs(wheel_speed_rpm) / 100.0)
        return module

    def build_modules(self, forward_velocity, angular_velocity, dt):
        wheel_circumference = 2.0 * math.pi * self.wheel_radius
        if self.chassis_type == "differential":
            left_speed = forward_velocity - angular_velocity * self.wheel_track / 2.0
            right_speed = forward_velocity + angular_velocity * self.wheel_track / 2.0
            return [
                self.make_module_state("left_drive", left_speed / wheel_circumference * 60.0, 0.0, dt),
                self.make_module_state("right_drive", right_speed / wheel_circumference * 60.0, 0.0, dt),
            ]

        if self.chassis_type == "ackermann":
            steer_rad = 0.0
            if abs(forward_velocity) > 0.02:
                steer_rad = math.atan(self.wheel_base * angular_velocity / forward_velocity)
            wheel_speed_rpm = forward_velocity / wheel_circumference * 60.0
            steer_deg = math.degrees(steer_rad)
            return [
                self.make_module_state("front_left", wheel_speed_rpm, steer_deg, dt),
                self.make_module_state("front_right", wheel_speed_rpm, steer_deg, dt),
                self.make_module_state("rear_left", wheel_speed_rpm, 0.0, dt),
                self.make_module_state("rear_right", wheel_speed_rpm, 0.0, dt),
            ]

        wheel_speed_rpm = forward_velocity / wheel_circumference * 60.0
        steer_angle = math.degrees(math.atan2(angular_velocity * self.wheel_track, max(abs(forward_velocity), 0.05)))
        module_ids = ["front_left", "front_right", "rear_left", "rear_right"]
        return [self.make_module_state(module_id, wheel_speed_rpm, steer_angle, dt) for module_id in module_ids]

    def publish(self, agv):
        if not self.enabled or self.publisher is None:
            return

        now = time.time()
        if self.publish_period_sec > 0.0 and (now - self.last_publish_wall_time) < self.publish_period_sec:
            return

        dt = self.publish_period_sec if self.last_publish_wall_time == 0.0 else now - self.last_publish_wall_time
        position, yaw_rad, linear_velocity, angular_velocity = vehicle_state_from_isaac(agv)
        forward_axis = np.array([math.cos(yaw_rad), math.sin(yaw_rad)])
        lateral_axis = np.array([-math.sin(yaw_rad), math.cos(yaw_rad)])
        planar_velocity = np.array([linear_velocity[0], linear_velocity[1]])
        forward_velocity = float(np.dot(planar_velocity, forward_axis))
        lateral_velocity = float(np.dot(planar_velocity, lateral_axis))
        yaw_rate = float(angular_velocity[2])

        msg = ChassisTelemetry()
        msg.hardware_timestamp_us = time.time_ns() // 1000
        msg.chassis_type.value = chassis_type_value(self.chassis_type)
        msg.odom_x_m = float(position[0] + self.rng.normal(0.0, self.odom_noise_stddev_m))
        msg.odom_y_m = float(position[1] + self.rng.normal(0.0, self.odom_noise_stddev_m))
        msg.odom_yaw_rad = float(normalize_angle(yaw_rad + self.rng.normal(0.0, self.yaw_noise_stddev_rad)))
        msg.linear_velocity_ms = float(forward_velocity)
        msg.angular_velocity_rads = float(yaw_rate)
        msg.modules = self.build_modules(forward_velocity, yaw_rate, dt)
        msg.estop_engaged = False
        msg.driver_error_code = 0
        msg.active_job_id = "isaac_chassis_calibration"
        msg.lateral_slip_estimate = float(lateral_velocity)
        msg.curvature_estimate = float(yaw_rate / forward_velocity if abs(forward_velocity) > 0.02 else 0.0)

        self.publisher.publish(msg)
        self.last_publish_wall_time = now

    def shutdown(self):
        if self.node is not None:
            self.node.destroy_node()
            self.node = None


class ControlCalibrationTelemetryPublisher:
    def __init__(self, args):
        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
        self.publisher = None

        if not self.enabled:
            print("[*] 已禁用运控评估遥测发布。")
            return

        if rclpy is None or ControlTelemetry is None or ControllerAlgorithmType is None:
            print("[WARN] 未找到运控标定 ROS 消息，运控遥测发布已禁用。")
            self.enabled = False
            return

        if not rclpy.ok():
            rclpy.init(args=None)

        self.node = rclpy.create_node("isaac_control_calibration_telemetry_publisher")
        self.publisher = self.node.create_publisher(ControlTelemetry, self.topic, 10)
        print(f"[*] 运控评估遥测发布已启用: topic={self.topic}, parameter_version={self.parameter_version}")

    def publish(self, agv):
        if not self.enabled or self.publisher is None:
            return

        now = time.time()
        if self.publish_period_sec > 0.0 and (now - self.last_publish_wall_time) < self.publish_period_sec:
            return

        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)
        heading_error = float(normalize_angle(yaw_rad))
        speed_error = float(forward_velocity - self.reference_speed_ms)
        steering_output = float(clamp(-0.8 * lateral_error - 1.2 * heading_error, -1.0, 1.0))
        throttle_output = float(clamp(-2.0 * speed_error, 0.0, 1.0))
        brake_output = float(clamp(2.0 * speed_error, 0.0, 1.0))

        msg = ControlTelemetry()
        msg.hardware_timestamp_us = time.time_ns() // 1000
        msg.odom_x_m = float(position[0])
        msg.odom_y_m = float(position[1])
        msg.odom_yaw_rad = float(yaw_rad)
        msg.linear_velocity_ms = float(forward_velocity)
        msg.angular_velocity_rads = float(angular_velocity[2])
        msg.lateral_error_m = float(lateral_error)
        msg.heading_error_rad = float(heading_error)
        msg.speed_error_ms = float(speed_error)
        msg.steering_output = float(steering_output)
        msg.throttle_output = float(throttle_output)
        msg.brake_output = float(brake_output)
        msg.saturation_flag = abs(steering_output) > 0.98 or throttle_output > 0.98 or brake_output > 0.98
        msg.active_job_id = "isaac_control_calibration"
        msg.parameter_version = self.parameter_version
        msg.active_lateral_algorithm.value = controller_algorithm_value("pure_pursuit")
        msg.active_longitudinal_algorithm.value = controller_algorithm_value("pid")

        self.publisher.publish(msg)
        self.last_publish_wall_time = now

    def shutdown(self):
        if self.node is not None:
            self.node.destroy_node()
            self.node = None


class SensorCalibrationTelemetryPublisher:
    def __init__(self, args):
        self.enabled = not args.disable_sensor_telemetry
        self.topic = args.sensor_telemetry_topic
        self.publish_period_sec = 0.0 if args.sensor_telemetry_hz <= 0.0 else 1.0 / args.sensor_telemetry_hz
        self.sensor_id = args.front_camera_sensor_id
        self.target_sample_count = max(0, args.sensor_target_sample_count)
        self.quality_score = clamp(args.sensor_quality_score, 0.0, 1.0)
        self.target_detected = not args.sensor_target_missing
        self.last_publish_wall_time = 0.0
        self.collected_sample_count = 0
        self.node = None
        self.publisher = None

        if not self.enabled:
            print("[*] 已禁用传感器标定遥测发布。")
            return

        if rclpy is None or SensorCalibrationTelemetry is None or CaptureIntentType is None or SensorType is None:
            print("[WARN] 未找到传感器标定 ROS 消息，传感器遥测发布已禁用。")
            self.enabled = False
            return

        if not rclpy.ok():
            rclpy.init(args=None)

        self.node = rclpy.create_node("isaac_sensor_calibration_telemetry_publisher")
        self.publisher = self.node.create_publisher(SensorCalibrationTelemetry, self.topic, 10)
        print(f"[*] 传感器标定遥测发布已启用: topic={self.topic}, sensor_id={self.sensor_id}")

    def publish(self):
        if not self.enabled or self.publisher is None:
            return

        now = time.time()
        if self.publish_period_sec > 0.0 and (now - self.last_publish_wall_time) < self.publish_period_sec:
            return

        if self.target_detected:
            self.collected_sample_count += 1

        msg = SensorCalibrationTelemetry()
        msg.hardware_timestamp_us = time.time_ns() // 1000
        msg.sensor_id = self.sensor_id
        msg.sensor_type.value = SensorType.FRONT_CAMERA
        msg.collected_sample_count = (
            min(self.collected_sample_count, self.target_sample_count)
            if self.target_sample_count > 0
            else self.collected_sample_count
        )
        msg.target_sample_count = self.target_sample_count
        msg.target_detected = self.target_detected
        msg.quality_score = self.quality_score if self.target_detected else 0.0
        msg.active_job_id = f"{self.sensor_id}:isaac_sensor_telemetry"
        msg.capture_intent.value = CaptureIntentType.CAMERA_EXTRINSIC_CAPTURE

        self.publisher.publish(msg)
        self.last_publish_wall_time = now

    def shutdown(self):
        if self.node is not None:
            self.node.destroy_node()
            self.node = None


class AckermannUrdfJointDriver:
    def __init__(self, args):
        self.wheel_base = args.wheel_base
        self.wheel_radius = args.wheel_radius
        self.max_steer_angle_rad = math.radians(args.max_steer_angle_deg)
        self.max_speed_ms = args.max_speed_ms
        self.joint_indices = {}
        self.warned = False

    def configure(self, robot):
        joint_names = [
            "front_left_steer_joint",
            "front_right_steer_joint",
            "front_left_wheel_joint",
            "front_right_wheel_joint",
            "rear_left_wheel_joint",
            "rear_right_wheel_joint",
        ]
        for name in joint_names:
            try:
                self.joint_indices[name] = int(robot.get_dof_index(name))
            except Exception:
                continue

        if len(self.joint_indices) < len(joint_names):
            missing = sorted(set(joint_names) - set(self.joint_indices))
            print(f"[WARN] 部分阿克曼关节未找到，车轮动画可能不完整: {missing}")
        else:
            print("[*] 阿克曼 URDF 关节动画已启用：前轮转向，后轮驱动。")

    def steering_from_cmd(self, linear_x, angular_z):
        linear_x = clamp(float(linear_x), -self.max_speed_ms, self.max_speed_ms)
        angular_z = float(angular_z)
        if abs(linear_x) < 0.02:
            return clamp(math.copysign(self.max_steer_angle_rad, angular_z), -self.max_steer_angle_rad, self.max_steer_angle_rad) if abs(angular_z) > 0.02 else 0.0
        steer = math.atan(self.wheel_base * angular_z / linear_x)
        return clamp(steer, -self.max_steer_angle_rad, self.max_steer_angle_rad)

    def apply(self, robot, linear_x, angular_z):
        if not self.joint_indices:
            return

        steer = self.steering_from_cmd(linear_x, angular_z)
        wheel_velocity = clamp(float(linear_x), -self.max_speed_ms, self.max_speed_ms) / self.wheel_radius

        try:
            steer_indices = [
                self.joint_indices[name]
                for name in ("front_left_steer_joint", "front_right_steer_joint")
                if name in self.joint_indices
            ]
            if steer_indices:
                robot.set_joint_positions(np.array([steer] * len(steer_indices)), joint_indices=np.array(steer_indices))

            wheel_indices = [
                self.joint_indices[name]
                for name in (
                    "front_left_wheel_joint",
                    "front_right_wheel_joint",
                    "rear_left_wheel_joint",
                    "rear_right_wheel_joint",
                )
                if name in self.joint_indices
            ]
            if wheel_indices:
                robot.set_joint_velocities(np.array([wheel_velocity] * len(wheel_indices)), joint_indices=np.array(wheel_indices))
        except Exception as exc:
            if not self.warned:
                print(f"[WARN] 阿克曼关节动画写入失败，车辆根位姿控制仍继续: {exc}")
                self.warned = True


class IsaacWorkshopRuntime:
    def __init__(self, args):
        self.args = args
        self.camera_topic = resolve_topic(args.ros_topic_prefix, "/camera/image_raw")
        self.lidar_topic_prefix = resolve_topic(args.ros_topic_prefix, "/lidar")
        self.cmd_vel_topic = args.cmd_vel_topic
        self.vehicle_camera_topic = args.vehicle_camera_topic
        self.down_camera_topic = args.down_camera_topic
        self.vehicle_lidar_topic = args.vehicle_lidar_topic
        self.vehicle_2d_lidar_topic = args.vehicle_2d_lidar_topic
        self.vehicle_imu_topic = args.vehicle_imu_topic
        self.checkerboard_texture_path = GENERATED_ROOT / "checkerboard.png"
        self.down_camera_charuco_texture_path = GENERATED_ROOT / "down_camera_charuco.png"
        self.source_urdf_path = Path(args.urdf_path).expanduser().resolve() if args.urdf_path else DEFAULT_ACKERMANN_URDF_PATH
        should_fix_legacy_ack_m = (
            not args.disable_urdf_visual_origin_fix
            and self.source_urdf_path.name == "ack_m.urdf"
        )
        self.urdf_path = (
            prepare_isaac_urdf(self.source_urdf_path, GENERATED_ROOT / "ack_m_isaac_fixed.urdf")
            if should_fix_legacy_ack_m
            else self.source_urdf_path
        )
        self.usd_path = None
        self.vehicle_prim_path = ""
        self.vehicle = None
        self.sensor_render_products = []
        self.calibration_board_specs = []
        self.lidar_2d_target_specs = []
        self.down_camera_target_specs = []
        self.scene_manifest_path = (
            Path(args.scene_manifest_path).expanduser().resolve()
            if args.scene_manifest_path
            else GENERATED_ROOT / "workshop_scene_manifest.json"
        )

        if not self.source_urdf_path.exists():
            raise FileNotFoundError(f"未找到车辆 URDF: {self.source_urdf_path}")

    def create_ros_camera_render_product(self, prim_path, position, orientation, focal_length):
        create_prim(
            prim_path=prim_path,
            prim_type="Camera",
            position=np.array(position),
            orientation=np.array(orientation),
            attributes={
                "focalLength": float(focal_length),
                # 下视/近距离相机需要较小近裁剪面，否则离地几厘米时画面会被 near plane 裁成黑屏。
                "clippingRange": (0.005, 1000.0),
            },
        )
        render_product = rep.create.render_product(
            prim_path,
            (self.args.camera_resolution_width, self.args.camera_resolution_height),
        )
        self.sensor_render_products.append(render_product)
        return str(render_product.path)

    def write_scene_manifest(self):
        manifest = {
            "vehicle": {
                "vehicle_id": self.args.vehicle_id,
                "vehicle_source": "urdf_direct",
                "source_urdf_path": str(self.source_urdf_path),
                "urdf_path": str(self.urdf_path),
                "usd_path": None,
                "stage_prim_path": self.vehicle_prim_path,
                "drive_wheels": ["rear_left_wheel_link", "rear_right_wheel_link"],
                "steer_wheels": ["front_left_wheel_link", "front_right_wheel_link"],
                "initial_pose": {
                    "x_m": self.args.vehicle_initial_x,
                    "y_m": self.args.vehicle_initial_y,
                    "z_m": self.args.vehicle_initial_z,
                    "yaw_rad": 0.0,
                },
            },
            "workshop": {
                "workcell_zone_id": self.args.external_workcell_zone_id,
                "room_length_m": self.args.room_length,
                "room_width_m": self.args.room_width,
                "room_height_m": self.args.room_height,
                "checkerboard": {
                    "rows": self.args.checkerboard_rows,
                    "cols": self.args.checkerboard_cols,
                    "square_size_m": self.args.checkerboard_square_size,
                    "texture_path": str(self.checkerboard_texture_path),
                    "reserved_wall_zones": lidar_2d_checkerboard_reserved_zones(self.args),
                    "layout": self.calibration_board_specs,
                },
                "down_camera_intrinsic_target": {
                    "enabled": not self.args.disable_down_camera_intrinsic_target,
                    "texture_path": str(self.down_camera_charuco_texture_path),
                    "pattern": "charuco",
                    "squares_x": self.args.down_camera_target_squares_x,
                    "squares_y": self.args.down_camera_target_squares_y,
                    "square_size_x_m": self.args.down_camera_target_length / self.args.down_camera_target_squares_x,
                    "square_size_y_m": self.args.down_camera_target_width / self.args.down_camera_target_squares_y,
                    "layout": self.down_camera_target_specs,
                },
            },
            "ros_topics": {
                "cmd_vel": self.cmd_vel_topic,
                "camera_image": self.camera_topic,
                "lidar_prefix": self.lidar_topic_prefix,
                "front_camera_image": self.vehicle_camera_topic,
                "down_camera_image": self.down_camera_topic,
                "lidar_3d_pointcloud": self.vehicle_lidar_topic,
                "lidar_2d_scan": self.vehicle_2d_lidar_topic,
                "imu": self.vehicle_imu_topic,
                "external_telemetry": self.args.external_telemetry_topic,
                "chassis_telemetry": self.args.chassis_telemetry_topic,
                "control_telemetry": self.args.control_telemetry_topic,
                "sensor_telemetry": self.args.sensor_telemetry_topic,
            },
            "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": {
                "localization_source_id": self.args.external_reference_source_name,
                "reference_source_name": self.args.external_reference_source_name,
                "workcell_zone_id": self.args.external_workcell_zone_id,
                "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,
            },
            "sensors": [
                {
                    "sensor_id": self.args.front_camera_sensor_id,
                    "sensor_type": "front_camera",
                    "frame_id": "front_camera_link",
                    "image_topic": self.vehicle_camera_topic,
                    "telemetry_topic": self.args.sensor_telemetry_topic,
                    "mount_pose": {
                        "x_m": self.args.vehicle_camera_x,
                        "y_m": self.args.vehicle_camera_y,
                        "z_m": self.args.vehicle_camera_z,
                        "roll_rad": math.pi / 2.0,
                        "pitch_rad": 0.0,
                        "yaw_rad": -math.pi / 2.0,
                    },
                },
                {
                    "sensor_id": "demo_down_camera",
                    "sensor_type": "down_camera",
                    "frame_id": "down_camera_link",
                    "image_topic": self.down_camera_topic,
                    "mount_pose": {
                        "x_m": self.args.down_camera_x,
                        "y_m": self.args.down_camera_y,
                        "z_m": self.args.down_camera_z,
                        "roll_rad": 0.0,
                        "pitch_rad": 0.0,
                        "yaw_rad": 0.0,
                    },
                },
                {
                    "sensor_id": "demo_lidar_3d",
                    "sensor_type": "lidar_3d",
                    "frame_id": "lidar_3d_link",
                    "pointcloud_topic": self.vehicle_lidar_topic,
                    "mount_pose": {
                        "x_m": self.args.vehicle_lidar_x,
                        "y_m": self.args.vehicle_lidar_y,
                        "z_m": self.args.vehicle_lidar_z,
                        "roll_rad": 0.0,
                        "pitch_rad": 0.0,
                        "yaw_rad": 0.0,
                    },
                },
                {
                    "sensor_id": "demo_lidar_2d",
                    "sensor_type": "lidar_2d",
                    "frame_id": "lidar_2d_link",
                    "scan_topic": self.vehicle_2d_lidar_topic,
                    "mount_pose": {
                        "x_m": self.args.vehicle_2d_lidar_x,
                        "y_m": self.args.vehicle_2d_lidar_y,
                        "z_m": self.args.vehicle_2d_lidar_z,
                        "roll_rad": 0.0,
                        "pitch_rad": 0.0,
                        "yaw_rad": 0.0,
                    },
                },
                {
                    "sensor_id": "demo_imu",
                    "sensor_type": "imu",
                    "frame_id": "imu_link",
                    "imu_topic": self.vehicle_imu_topic,
                    "mount_pose": {
                        "x_m": self.args.vehicle_imu_x,
                        "y_m": self.args.vehicle_imu_y,
                        "z_m": self.args.vehicle_imu_z,
                        "roll_rad": 0.0,
                        "pitch_rad": 0.0,
                        "yaw_rad": 0.0,
                    },
                },
            ],
            "lidar_2d_calibration_targets": {
                "enabled": not self.args.disable_2d_lidar_targets,
                "target_width_m": self.args.lidar_2d_target_width,
                "target_height_m": self.args.lidar_2d_target_height,
                "target_thickness_m": self.args.lidar_2d_target_thickness,
                "target_bottom_z_m": self.args.lidar_2d_target_bottom_z,
                "slope_angle_deg": self.args.lidar_2d_target_slope_angle_deg,
                "targets": self.lidar_2d_target_specs,
            },
            "orchestrator_session_config_hint": {
                "vehicle_id": self.args.vehicle_id,
                "localization_source_id": self.args.external_reference_source_name,
                "workcell_zone_id": self.args.external_workcell_zone_id,
                "reference_target_id": "isaac_external_truth",
            },
        }

        self.scene_manifest_path.parent.mkdir(parents=True, exist_ok=True)
        with self.scene_manifest_path.open("w", encoding="utf-8") as fp:
            json.dump(manifest, fp, ensure_ascii=False, indent=2)
            fp.write("\n")
        print(f"[*] 场景 manifest 已写入: {self.scene_manifest_path}")

    def import_vehicle_to_stage(self, world):
        status, import_config = omni.kit.commands.execute("URDFCreateImportConfig")
        if not status:
            raise RuntimeError("创建 URDF import_config 失败。")

        import_config.merge_fixed_joints = False
        import_config.convex_decomp = False
        import_config.fix_base = False
        import_config.make_default_prim = True
        if hasattr(import_config, "import_inertia_tensor"):
            import_config.import_inertia_tensor = True

        print(f"[*] 直接从 URDF 路径导入车辆: {self.urdf_path}")
        status, imported_prim_path = omni.kit.commands.execute(
            "URDFParseAndImportFile",
            urdf_path=str(self.urdf_path),
            import_config=import_config,
        )
        if not status or not imported_prim_path:
            raise RuntimeError(f"URDF 导入失败: {self.urdf_path}")

        self.vehicle_prim_path = str(imported_prim_path)
        self.vehicle = world.scene.add(
            Robot(
                prim_path=self.vehicle_prim_path,
                name="agv_vehicle",
                position=np.array([
                    self.args.vehicle_initial_x,
                    self.args.vehicle_initial_y,
                    self.args.vehicle_initial_z,
                ]),
            )
        )
        print(f"[*] 车辆已导入 stage: {self.vehicle_prim_path}")

        stage = omni.usd.get_context().get_stage()
        physx_rb = PhysxSchema.PhysxRigidBodyAPI.Get(stage, self.vehicle_prim_path)
        if physx_rb:
            physx_rb.GetSleepThresholdAttr().Set(0.0)

    def find_vehicle_prim_path_by_name(self, preferred_names):
        stage = omni.usd.get_context().get_stage()
        vehicle_root = self.vehicle_prim_path.rstrip("/")
        preferred = set(preferred_names)

        for prim in stage.Traverse():
            prim_path = str(prim.GetPath())
            if prim_path != vehicle_root and not prim_path.startswith(f"{vehicle_root}/"):
                continue
            if prim.GetName() in preferred:
                return prim_path

        return ""

    def find_vehicle_rigid_body_prim_path(self, preferred_names):
        stage = omni.usd.get_context().get_stage()
        vehicle_root = self.vehicle_prim_path.rstrip("/")
        preferred = set(preferred_names)
        fallback_path = ""

        for prim in stage.Traverse():
            prim_path = str(prim.GetPath())
            if prim_path != vehicle_root and not prim_path.startswith(f"{vehicle_root}/"):
                continue

            rigid_body = PhysxSchema.PhysxRigidBodyAPI.Get(stage, prim_path)
            if not rigid_body:
                continue

            prim_name = prim.GetName()
            if prim_name in preferred:
                return prim_path
            if not fallback_path:
                fallback_path = prim_path

        return fallback_path

    def get_vehicle_sensor_mount_parent_path(self):
        """返回车载传感器的 USD 父 prim，优先挂到车辆 base_link 下。"""
        base_link_path = self.find_vehicle_prim_path_by_name(["base_link"])
        if base_link_path:
            return base_link_path

        rigid_body_path = self.find_vehicle_rigid_body_prim_path(["base_link"])
        if rigid_body_path:
            print(f"[WARN] 未按名称找到 base_link，车载传感器改挂到车辆刚体 prim: {rigid_body_path}")
            return rigid_body_path

        print(f"[WARN] 未找到车辆 base_link/刚体 prim，车载传感器改挂到车辆根 prim: {self.vehicle_prim_path}")
        return self.vehicle_prim_path

    def hide_sensor_placeholder_visuals(self):
        """隐藏 URDF 里的传感器占位外壳，避免相机/LiDAR 打到自己的可视化模型。"""
        stage = omni.usd.get_context().get_stage()
        hidden_paths = []
        for link_name in ("front_camera_link", "down_camera_link", "lidar_3d_link", "lidar_2d_link"):
            link_path = self.find_vehicle_prim_path_by_name([link_name])
            if not link_path:
                continue
            prim = stage.GetPrimAtPath(link_path)
            if not prim or not prim.IsValid():
                continue
            UsdGeom.Imageable(prim).MakeInvisible()
            hidden_paths.append(link_path)

        if hidden_paths:
            print(f"[*] 已隐藏传感器 URDF 占位外壳，避免相机/LiDAR 自遮挡: {hidden_paths}")

    def add_vehicle_sensor_publishers(self):
        if (
            self.args.disable_vehicle_camera
            and self.args.disable_down_camera
            and self.args.disable_vehicle_lidar
            and self.args.disable_vehicle_2d_lidar
            and self.args.disable_vehicle_imu
        ):
            return

        keys = og.Controller.Keys
        nodes = [("OnTick", "omni.graph.action.OnTick")]
        connections = []
        set_values = []
        vehicle_root = self.vehicle_prim_path
        vehicle_sensor_parent_path = self.get_vehicle_sensor_mount_parent_path()
        print(f"[*] 车载传感器 USD 父 prim: {vehicle_sensor_parent_path}（安装参数按 base_link 局部坐标解释）")

        if not self.args.disable_vehicle_camera:
            # 光轴朝 base_link +X，画面上方朝 +Z，右方朝 -Y；修正原先顺时针旋转 90° 的图像。
            camera_quat = np.array([0.5, 0.5, -0.5, -0.5])
            vehicle_camera_render_product = self.create_ros_camera_render_product(
                prim_path=f"{vehicle_sensor_parent_path}/SimFrontCamera",
                position=[
                    self.args.vehicle_camera_x,
                    self.args.vehicle_camera_y,
                    self.args.vehicle_camera_z,
                ],
                orientation=camera_quat,
                focal_length=self.args.camera_focal_length,
            )
            nodes.append(("VehicleFrontCamera", "omni.isaac.ros2_bridge.ROS2CameraHelper"))
            connections.append(("OnTick.outputs:tick", "VehicleFrontCamera.inputs:execIn"))
            set_values.extend([
                ("VehicleFrontCamera.inputs:renderProductPath", vehicle_camera_render_product),
                ("VehicleFrontCamera.inputs:topicName", self.vehicle_camera_topic),
                ("VehicleFrontCamera.inputs:frameId", "front_camera_link"),
                ("VehicleFrontCamera.inputs:type", "rgb"),
            ])

        if not self.args.disable_down_camera:
            down_camera_render_product = self.create_ros_camera_render_product(
                prim_path=f"{vehicle_sensor_parent_path}/SimDownCamera",
                position=[
                    self.args.down_camera_x,
                    self.args.down_camera_y,
                    self.args.down_camera_z,
                ],
                orientation=[1.0, 0.0, 0.0, 0.0],
                focal_length=self.args.camera_focal_length,
            )
            nodes.append(("VehicleDownCamera", "omni.isaac.ros2_bridge.ROS2CameraHelper"))
            connections.append(("OnTick.outputs:tick", "VehicleDownCamera.inputs:execIn"))
            set_values.extend([
                ("VehicleDownCamera.inputs:renderProductPath", down_camera_render_product),
                ("VehicleDownCamera.inputs:topicName", self.down_camera_topic),
                ("VehicleDownCamera.inputs:frameId", "down_camera_link"),
                ("VehicleDownCamera.inputs:type", "rgb"),
            ])

        if not self.args.disable_vehicle_lidar:
            lidar_parent_path = vehicle_sensor_parent_path
            lidar_path = f"{lidar_parent_path}/SimLidar3D"
            lidar_quat = euler_angles_to_quat(np.array([0.0, 0.0, 0.0]), degrees=True)
            omni.kit.commands.execute(
                "IsaacSensorCreateRtxLidar",
                path=lidar_path,
                parent=None,
                config=self.args.lidar_3d_config,
                translation=Gf.Vec3d(
                    self.args.vehicle_lidar_x,
                    self.args.vehicle_lidar_y,
                    self.args.vehicle_lidar_z,
                ),
                orientation=Gf.Quatd(lidar_quat[0], lidar_quat[1], lidar_quat[2], lidar_quat[3]),
            )
            render_product = rep.create.render_product(lidar_path, [360, 32])
            self.sensor_render_products.append(render_product)
            nodes.append(("VehicleLidar3D", "omni.isaac.ros2_bridge.ROS2RtxLidarHelper"))
            connections.append(("OnTick.outputs:tick", "VehicleLidar3D.inputs:execIn"))
            set_values.extend([
                ("VehicleLidar3D.inputs:renderProductPath", str(render_product.path)),
                ("VehicleLidar3D.inputs:topicName", self.vehicle_lidar_topic),
                ("VehicleLidar3D.inputs:frameId", "lidar_3d_link"),
                ("VehicleLidar3D.inputs:type", "point_cloud"),
                ("VehicleLidar3D.inputs:fullScan", True),
            ])

        if not self.args.disable_vehicle_2d_lidar:
            lidar_2d_parent_path = vehicle_sensor_parent_path
            lidar_2d_path = f"{lidar_2d_parent_path}/SimLidar2D"
            lidar_2d_quat = euler_angles_to_quat(np.array([0.0, 0.0, 0.0]), degrees=True)
            omni.kit.commands.execute(
                "IsaacSensorCreateRtxLidar",
                path=lidar_2d_path,
                parent=None,
                config=self.args.lidar_config,
                translation=Gf.Vec3d(
                    self.args.vehicle_2d_lidar_x,
                    self.args.vehicle_2d_lidar_y,
                    self.args.vehicle_2d_lidar_z,
                ),
                orientation=Gf.Quatd(lidar_2d_quat[0], lidar_2d_quat[1], lidar_2d_quat[2], lidar_2d_quat[3]),
            )
            render_product_2d = rep.create.render_product(lidar_2d_path, [1, 1])
            self.sensor_render_products.append(render_product_2d)
            print(f"[*] 2D LiDAR RTX sensor prim 已创建: {lidar_2d_path}")
            print(f"[*] 2D LiDAR ROS topic 由 Isaac 几何扫描发布器输出: {self.vehicle_2d_lidar_topic}")

        if not self.args.disable_vehicle_imu:
            imu_parent_path = self.find_vehicle_rigid_body_prim_path(["base_link", "imu_link"])
            if not imu_parent_path:
                print("[INFO] 未找到可挂载 IMU 的车辆刚体 link，跳过 Isaac IMU sensor prim；IMU ROS topic 继续由车体运动学发布器输出。")
                imu_parent_path = ""
            imu_path = f"{imu_parent_path}/SimImu" if imu_parent_path else ""
            if imu_parent_path:
                imu_quat = Gf.Quatd(1.0, 0.0, 0.0, 0.0)
                status, _sensor = omni.kit.commands.execute(
                    "IsaacSensorCreateImuSensor",
                    path="/SimImu",
                    parent=imu_parent_path,
                    sensor_period=-1.0,
                    translation=Gf.Vec3d(
                        self.args.vehicle_imu_x,
                        self.args.vehicle_imu_y,
                        self.args.vehicle_imu_z,
                    ),
                    orientation=imu_quat,
                )
                if status:
                    print(f"[*] 车载 IMU sensor prim 已挂载到刚体 link: {imu_parent_path}, sensor={imu_path}")
                    print(f"[*] IMU ROS topic 由 Isaac 车辆状态发布器输出: {self.vehicle_imu_topic}")
                else:
                    print(f"[WARN] IsaacSensorCreateImuSensor 创建失败，parent={imu_parent_path}，IMU ROS topic 将继续由 Isaac 车辆状态发布器输出。")

        og.Controller.edit(
            {"graph_path": "/World/ROS2_Vehicle_Sensors_Graph", "evaluator_name": "execution"},
            {keys.CREATE_NODES: nodes, keys.CONNECT: connections, keys.SET_VALUES: set_values},
        )

    def build_workshop(self):
        world = World(stage_units_in_meters=1.0)
        room_length = self.args.room_length
        room_width = self.args.room_width
        room_height = self.args.room_height
        wall_thickness = self.args.wall_thickness
        floor_color = np.array([0.2, 0.2, 0.2])
        wall_color = np.array([0.8, 0.8, 0.8])

        world.scene.add(FixedCuboid(
            prim_path="/World/Workshop/Floor",
            name="floor",
            position=np.array([0, 0, -wall_thickness / 2]),
            scale=np.array([room_length + 2 * wall_thickness, room_width + 2 * wall_thickness, wall_thickness]),
            color=floor_color,
        ))
        world.scene.add(FixedCuboid(
            prim_path="/World/Workshop/Ceiling",
            name="ceiling",
            position=np.array([0, 0, room_height + wall_thickness / 2]),
            scale=np.array([room_length + 2 * wall_thickness, room_width + 2 * wall_thickness, wall_thickness]),
            color=wall_color,
        ))
        world.scene.add(FixedCuboid(
            prim_path="/World/Workshop/Wall_Front",
            name="wall_front",
            position=np.array([room_length / 2 + wall_thickness / 2, 0, room_height / 2]),
            scale=np.array([wall_thickness, room_width, room_height]),
            color=wall_color,
        ))
        world.scene.add(FixedCuboid(
            prim_path="/World/Workshop/Wall_Back",
            name="wall_back",
            position=np.array([-room_length / 2 - wall_thickness / 2, 0, room_height / 2]),
            scale=np.array([wall_thickness, room_width, room_height]),
            color=wall_color,
        ))
        world.scene.add(FixedCuboid(
            prim_path="/World/Workshop/Wall_Left",
            name="wall_left",
            position=np.array([0, room_width / 2 + wall_thickness / 2, room_height / 2]),
            scale=np.array([room_length + 2 * wall_thickness, wall_thickness, room_height]),
            color=wall_color,
        ))
        world.scene.add(FixedCuboid(
            prim_path="/World/Workshop/Wall_Right",
            name="wall_right",
            position=np.array([0, -room_width / 2 - wall_thickness / 2, room_height / 2]),
            scale=np.array([room_length + 2 * wall_thickness, wall_thickness, room_height]),
            color=wall_color,
        ))

        light_positions = [
            (room_length / 4, room_width / 4, room_height - 0.5),
            (room_length / 4, -room_width / 4, room_height - 0.5),
            (-room_length / 4, room_width / 4, room_height - 0.5),
            (-room_length / 4, -room_width / 4, room_height - 0.5),
        ]
        for index, position in enumerate(light_positions):
            create_prim(
                prim_path=f"/World/Workshop/Lights/Light_{index}",
                prim_type="SphereLight",
                position=np.array(position),
                attributes={
                    "inputs:radius": 0.3,
                    "inputs:intensity": self.args.light_intensity,
                    "inputs:color": (1.0, 1.0, 0.95),
                },
            )

        checkerboard_texture = create_checkerboard_image(
            self.checkerboard_texture_path,
            rows=self.args.checkerboard_rows,
            cols=self.args.checkerboard_cols,
            square_size_px=500,
        )
        stage = omni.usd.get_context().get_stage()
        material = create_raw_usd_material(stage, "/World/Workshop/Materials/CheckerboardMat", checkerboard_texture)
        down_camera_target_material = None
        if not self.args.disable_down_camera_intrinsic_target:
            down_camera_charuco_texture = create_charuco_image(
                self.down_camera_charuco_texture_path,
                squares_x=self.args.down_camera_target_squares_x,
                squares_y=self.args.down_camera_target_squares_y,
                square_size_px=90,
            )
            down_camera_target_material = create_raw_usd_material(
                stage,
                "/World/Workshop/Materials/DownCameraCharucoMat",
                down_camera_charuco_texture,
            )

        board_width = (self.args.checkerboard_cols + 2) * self.args.checkerboard_square_size
        board_height = (self.args.checkerboard_rows + 2) * self.args.checkerboard_square_size
        if self.args.disable_calibration_boards:
            print("[*] 已禁用多姿态棋盘格标定目标阵列。")
        else:
            self.calibration_board_specs = add_calibration_boards(
                stage,
                self.args,
                board_width,
                board_height,
                material,
            )
            print(f"[*] 已创建 {len(self.calibration_board_specs)} 个多姿态棋盘格标定目标。")

        if self.args.disable_2d_lidar_targets:
            print("[*] 已禁用 2D LiDAR 角落外参标定靶标。")
        else:
            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:
            self.down_camera_target_specs = add_down_camera_intrinsic_target(
                world,
                stage,
                self.args,
                down_camera_target_material,
            )
            print(f"[*] 已创建 {len(self.down_camera_target_specs)} 个下视相机 3D ChArUco 内参标定台。")

        if not self.args.disable_camera:
            camera_height = room_height - 0.1
            camera_render_product = self.create_ros_camera_render_product(
                prim_path="/World/Workshop/CalibrationCamera",
                position=[0.0, 0.0, camera_height],
                orientation=[1.0, 0.0, 0.0, 0.0],
                focal_length=self.args.camera_focal_length,
            )

            keys = og.Controller.Keys
            og.Controller.edit(
                {"graph_path": "/World/ROS2_Camera_Graph", "evaluator_name": "execution"},
                {
                    keys.CREATE_NODES: [
                        ("OnTick", "omni.graph.action.OnTick"),
                        ("ROS2Camera", "omni.isaac.ros2_bridge.ROS2CameraHelper"),
                    ],
                    keys.CONNECT: [("OnTick.outputs:tick", "ROS2Camera.inputs:execIn")],
                    keys.SET_VALUES: [
                        ("ROS2Camera.inputs:renderProductPath", camera_render_product),
                        ("ROS2Camera.inputs:topicName", self.camera_topic),
                        ("ROS2Camera.inputs:type", "rgb"),
                    ],
                },
            )

        if not self.args.disable_lidars:
            add_corner_rotary_lidars(
                room_length=room_length,
                room_width=room_width,
                height=room_height - 0.2,
                lidar_config=self.args.lidar_config,
                topic_prefix=self.lidar_topic_prefix,
            )

        self.import_vehicle_to_stage(world)
        self.hide_sensor_placeholder_visuals()
        self.add_vehicle_sensor_publishers()

        keys = og.Controller.Keys
        og.Controller.edit(
            {"graph_path": "/World/ROS2_Twist_Graph", "evaluator_name": "execution"},
            {
                keys.CREATE_NODES: [
                    ("OnTick", "omni.graph.action.OnTick"),
                    ("TwistSub", "omni.isaac.ros2_bridge.ROS2SubscribeTwist"),
                ],
                keys.CONNECT: [("OnTick.outputs:tick", "TwistSub.inputs:execIn")],
                keys.SET_VALUES: [("TwistSub.inputs:topicName", self.cmd_vel_topic)],
            },
        )

        return world


def main():
    runtime = IsaacWorkshopRuntime(ARGS)
    telemetry_publisher = ExternalTruthTelemetryPublisher(ARGS)
    vehicle_tf_publisher = VehicleTfPublisher(ARGS)
    vehicle_imu_publisher = VehicleImuTopicPublisher(ARGS)
    chassis_telemetry_publisher = ChassisCalibrationTelemetryPublisher(ARGS)
    control_telemetry_publisher = ControlCalibrationTelemetryPublisher(ARGS)
    sensor_telemetry_publisher = SensorCalibrationTelemetryPublisher(ARGS)
    world = runtime.build_workshop()
    vehicle_laser_scan_publisher = VehicleLaserScanTopicPublisher(ARGS, runtime.lidar_2d_target_specs)
    # rclpy.init() may install its own signal handlers; restore ours before entering the long-running loop.
    install_shutdown_signal_handlers()
    runtime.write_scene_manifest()
    world.reset()

    if not ARGS.headless:
        set_camera_view(eye=np.array([0.001, 0.0, 3.4]), target=np.array([0.0, 0.0, 0.0]))

    print("======================================================")
    print(" 🎯 Isaac 标定车间已启动")
    print(f"  - headless: {ARGS.headless}")
    print(f"  - 车辆 URDF: {runtime.urdf_path}")
    print(f"  - 车辆 prim: {runtime.vehicle_prim_path}")
    print(f"  - 相机 topic: {runtime.camera_topic}")
    print(f"  - 激光雷达前缀: {runtime.lidar_topic_prefix}")
    print(f"  - 前视相机 topic: {runtime.vehicle_camera_topic}")
    print(f"  - 下视相机 topic: {runtime.down_camera_topic}")
    print(f"  - 3D LiDAR topic: {runtime.vehicle_lidar_topic}")
    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"  - external telemetry topic: {ARGS.external_telemetry_topic}")
    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}")
    print(f"  - sensor telemetry topic: {ARGS.sensor_telemetry_topic}")
    print(f"  - scene manifest: {runtime.scene_manifest_path}")
    print("======================================================")

    agv = runtime.vehicle or world.scene.get_object("agv_vehicle")
    ackermann_joint_driver = AckermannUrdfJointDriver(ARGS)
    ackermann_joint_driver.configure(agv)
    render_sensor_outputs = (
        not ARGS.disable_camera
        or not ARGS.disable_lidars
        or not ARGS.disable_vehicle_camera
        or not ARGS.disable_down_camera
        or not ARGS.disable_vehicle_lidar
    )
    render_frame = (not ARGS.headless) or render_sensor_outputs
    if ARGS.headless and render_sensor_outputs:
        print("[*] headless 模式下启用离屏渲染，用于相机和 RTX LiDAR render product 发布。")

    frame_count = 0
    loop_started_wall_time = time.time()
    exit_reason = "unknown"
    ros_context_shutdown_seen = False
    ros_publishers = (
        (telemetry_publisher, (agv,)),
        (vehicle_tf_publisher, (agv,)),
        (vehicle_imu_publisher, (agv,)),
        (vehicle_laser_scan_publisher, (agv,)),
        (chassis_telemetry_publisher, (agv,)),
        (control_telemetry_publisher, (agv,)),
        (sensor_telemetry_publisher, ()),
    )
    ros_context_required = any(getattr(publisher, "node", None) is not None for publisher, _ in ros_publishers)
    try:
        while True:
            if SHUTDOWN_REQUESTED:
                exit_reason = f"收到退出信号 {signal.Signals(SHUTDOWN_SIGNAL).name if SHUTDOWN_SIGNAL else 'UNKNOWN'}"
                break
            if ros_context_required and ros_context_is_shutdown():
                ros_context_shutdown_seen = True
                exit_reason = "ROS context 已关闭"
                break
            if not simulation_app.is_running():
                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"))
                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()
                    current_linear_velocity = agv.get_linear_velocity()

                    q = Gf.Quatd(float(quat[0]), float(quat[1]), float(quat[2]), float(quat[3]))
                    rot_mat = Gf.Matrix3d(Gf.Rotation(q))
                    world_linear_velocity = Gf.Vec3d(*lin_vel) * rot_mat
                    world_angular_velocity = Gf.Vec3d(*ang_vel) * rot_mat

                    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}")

            world.step(render=render_frame)
            try:
                for publisher, publish_args in ros_publishers:
                    if SHUTDOWN_REQUESTED:
                        exit_reason = f"收到退出信号 {signal.Signals(SHUTDOWN_SIGNAL).name if SHUTDOWN_SIGNAL else 'UNKNOWN'}"
                        break
                    if ros_context_required and ros_context_is_shutdown():
                        ros_context_shutdown_seen = True
                        exit_reason = "ROS context 已关闭"
                        break
                    publisher.publish(*publish_args)
                else:
                    frame_count += 1
                    continue
                break
            except Exception as exc:
                if is_ros_context_invalid_error(exc):
                    ros_context_shutdown_seen = True
                    exit_reason = "ROS context 已关闭，停止发布"
                    break
                raise
    except Exception as exc:
        exit_reason = f"未捕获异常: {type(exc).__name__}: {exc}"
        print(f"\n[ERROR] Isaac 主循环异常退出: {exit_reason}", flush=True)
        raise
    finally:
        elapsed_sec = time.time() - loop_started_wall_time
        print(
            f"\n[*] Isaac 主循环退出: reason={exit_reason}, "
            f"frames={frame_count}, elapsed_sec={elapsed_sec:.3f}",
            flush=True,
        )
        telemetry_publisher.shutdown()
        vehicle_tf_publisher.shutdown()
        vehicle_imu_publisher.shutdown()
        vehicle_laser_scan_publisher.shutdown()
        chassis_telemetry_publisher.shutdown()
        control_telemetry_publisher.shutdown()
        sensor_telemetry_publisher.shutdown()
        if rclpy is not None and rclpy.ok():
            rclpy.shutdown()
        if SHUTDOWN_REQUESTED or ros_context_shutdown_seen:
            print("[*] 停止信号/ROS context 关闭期间跳过 SimulationApp.close()，避免 Isaac 退出阶段崩溃。", flush=True)
            os._exit(0)
        elif ARGS.skip_final_close:
            print("[*] 已按 --skip-final-close 跳过 SimulationApp.close()。", flush=True)
        else:
            print("[*] 正在调用 SimulationApp.close()。如这里发生 segfault，请加 --skip-final-close 复测。", flush=True)
            simulation_app.close()


if __name__ == "__main__":
    main()
