feat:修复了车载Camera和Lidar问题....

agv_calib_brain/.isaac_cache/workshop_scene_manifest.json

@@ -637,7 +637,7 @@
     }
   },
   "ros_topics": {
-    "cmd_vel": "/vehicle/demo_agv_001/actuator/cmd_vel",
+    "cmd_vel": "/cmd_vel",
     "camera_image": "/AutoCalib_Workshop/camera/image_raw",
     "lidar_prefix": "/AutoCalib_Workshop/lidar",
     "front_camera_image": "/sensor/front_camera/image_raw",
@@ -692,12 +692,12 @@
       "image_topic": "/sensor/front_camera/image_raw",
       "telemetry_topic": "/sensor_calibration/telemetry",
       "mount_pose": {
-        "x_m": 0.82,
+        "x_m": 1.12,
         "y_m": 0.0,
-        "z_m": 0.32,
+        "z_m": 1.18,
-        "roll_rad": 0.0,
+        "roll_rad": 1.5707963267948966,
-        "pitch_rad": -1.5707963267948966,
+        "pitch_rad": 0.0,
-        "yaw_rad": 0.0
+        "yaw_rad": -1.5707963267948966
       }
     },
     {
@@ -708,7 +708,7 @@
       "mount_pose": {
         "x_m": 0.4,
         "y_m": 0.0,
-        "z_m": 0.3,
+        "z_m": 0.2,
         "roll_rad": 0.0,
         "pitch_rad": 0.0,
         "yaw_rad": 0.0
@@ -722,7 +722,7 @@
       "mount_pose": {
         "x_m": 0.55,
         "y_m": 0.0,
-        "z_m": 0.46,
+        "z_m": 1.2,
         "roll_rad": 0.0,
         "pitch_rad": 0.0,
         "yaw_rad": 0.0
@@ -736,7 +736,7 @@
       "mount_pose": {
         "x_m": 0.7,
         "y_m": 0.0,
-        "z_m": 0.24,
+        "z_m": 1.0,
         "roll_rad": 0.0,
         "pitch_rad": 0.0,
         "yaw_rad": 0.0

agv_calib_brain/src/simulation/isaac_workshop_sim/assets/urdf/ackermann_front_steer_rear_drive.urdf

@@ -277,7 +277,7 @@
   </link>
 
   <joint name="front_camera_joint" type="fixed">
-    <origin xyz="0.82 0 0.32" rpy="0 0 0"/>
+    <origin xyz="1.12 0 1.18" rpy="0 0 0"/>
     <parent link="base_link"/>
     <child link="front_camera_link"/>
   </joint>
@@ -293,14 +293,14 @@
   </link>
 
   <joint name="down_camera_joint" type="fixed">
-    <origin xyz="0.40 0 0.30" rpy="0 0 0"/>
+    <origin xyz="0.40 0 0.20" rpy="0 0 0"/>
     <parent link="base_link"/>
     <child link="down_camera_link"/>
   </joint>
 
   <link name="lidar_3d_link">
     <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <origin xyz="0 0 -0.06" rpy="0 0 0"/>
       <geometry>
         <cylinder radius="0.055" length="0.055"/>
       </geometry>
@@ -309,14 +309,14 @@
   </link>
 
   <joint name="lidar_3d_joint" type="fixed">
-    <origin xyz="0.55 0 0.46" rpy="0 0 0"/>
+    <origin xyz="0.55 0 1.20" rpy="0 0 0"/>
     <parent link="base_link"/>
     <child link="lidar_3d_link"/>
   </joint>
 
   <link name="lidar_2d_link">
     <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <origin xyz="0 0 -0.05" rpy="0 0 0"/>
       <geometry>
         <cylinder radius="0.040" length="0.040"/>
       </geometry>
@@ -325,7 +325,7 @@
   </link>
 
   <joint name="lidar_2d_joint" type="fixed">
-    <origin xyz="0.70 0 0.24" rpy="0 0 0"/>
+    <origin xyz="0.70 0 1.00" rpy="0 0 0"/>
     <parent link="base_link"/>
     <child link="lidar_2d_link"/>
   </joint>

agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/build_calibration_room.py

@@ -6,6 +6,20 @@ 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
@@ -33,6 +47,7 @@ def parse_args():
     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="不创建车载前视相机")
@@ -45,12 +60,12 @@ def parse_args():
     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=0.82, help="车载前视相机相对 base_link X 坐标")
+    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=0.32, help="车载前视相机相对 base_link Z 坐标")
+    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.30, help="车载下视相机相对 base_link Z 坐标")
+    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 坐标")
@@ -61,10 +76,10 @@ def parse_args():
     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=0.46, help="车载 3D LiDAR 相对 base_link Z 坐标")
+    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=0.24, help="车载 2D LiDAR 相对 base_link Z 坐标")
+    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 坐标")
@@ -264,6 +279,16 @@ 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:
@@ -309,202 +334,14 @@ except ImportError:
     LaserScan = None
 
 
-def clamp(value, low, high):
-    return max(low, min(high, value))
-
-
-def resolve_topic(prefix, suffix):
-    normalized_prefix = prefix.rstrip("/")
-    normalized_suffix = suffix if suffix.startswith("/") else f"/{suffix}"
-    return f"{normalized_prefix}{normalized_suffix}" if normalized_prefix else normalized_suffix
-
-
-def quat_wxyz_to_yaw(quat_wxyz):
-    w, x, y, z = quat_wxyz
-    siny_cosp = 2.0 * (w * z + x * y)
-    cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
-    return math.atan2(siny_cosp, cosy_cosp)
-
-
-def normalize_angle(angle):
-    return math.atan2(math.sin(angle), math.cos(angle))
-
-
-def chassis_type_value(name):
-    if ChassisType is None:
-        return 0
-    mapping = {
-        "ackermann": ChassisType.ACKERMANN,
-        "differential": ChassisType.DIFFERENTIAL,
-        "single_steer": ChassisType.SINGLE_STEER_WHEEL,
-        "multi_steer": ChassisType.MULTI_STEER_WHEEL,
-    }
-    return mapping.get(name, ChassisType.CHASSIS_TYPE_UNSPECIFIED)
-
-
-def controller_algorithm_value(name):
-    if ControllerAlgorithmType is None:
-        return 0
-    mapping = {
-        "pid": ControllerAlgorithmType.PID,
-        "mpc": ControllerAlgorithmType.MPC,
-        "lqr": ControllerAlgorithmType.LQR,
-        "pure_pursuit": ControllerAlgorithmType.PURE_PURSUIT,
-    }
-    return mapping.get(name, ControllerAlgorithmType.CONTROLLER_ALGORITHM_UNSPECIFIED)
-
-
-def vehicle_state_from_isaac(agv):
-    position, quat = agv.get_world_pose()
-    yaw_rad = quat_wxyz_to_yaw(quat)
-    linear_velocity = agv.get_linear_velocity()
-    try:
-        angular_velocity = agv.get_angular_velocity()
-    except Exception:
-        angular_velocity = np.array([0.0, 0.0, 0.0])
-    return position, yaw_rad, linear_velocity, angular_velocity
-
-
-def make_mesh_path_absolute(mesh_filename, source_dir):
-    mesh_path = Path(mesh_filename)
-    if mesh_path.is_absolute():
-        return str(mesh_path)
-    return str((source_dir / mesh_path).resolve())
-
-
-def prepare_isaac_urdf(source_urdf_path, output_urdf_path):
-    tree = ET.parse(source_urdf_path)
-    root = tree.getroot()
-    source_dir = source_urdf_path.parent
-
-    local_mesh_links = {
-        "left_steering_hinge",
-        "right_steering_hinge",
-        "left_wheel",
-        "right_wheel",
-        "left_rear_wheel",
-        "right_rear_wheel",
-        "camera",
-        "laser",
-    }
-
-    for link in root.findall("link"):
-        link_name = link.get("name", "")
-        for section_name in ("visual", "collision"):
-            section = link.find(section_name)
-            if section is None:
-                continue
-            mesh = section.find("geometry/mesh")
-            if mesh is not None and mesh.get("filename"):
-                mesh.set("filename", make_mesh_path_absolute(mesh.get("filename"), source_dir))
-            if link_name in local_mesh_links:
-                origin = section.find("origin")
-                if origin is None:
-                    origin = ET.SubElement(section, "origin")
-                origin.set("xyz", "0 0 0")
-                origin.set("rpy", "0 0 0")
-
-    output_urdf_path.parent.mkdir(parents=True, exist_ok=True)
-    tree.write(output_urdf_path, encoding="utf-8", xml_declaration=True)
-    return output_urdf_path
-
-
-def create_checkerboard_image(filepath, rows=6, cols=9, square_size_px=500):
-    width = cols * square_size_px
-    height = rows * square_size_px
-    img = np.ones((height, width, 3), dtype=np.uint8) * 255
-
-    for r in range(rows):
-        for c in range(cols):
-            if (r + c) % 2 == 1:
-                img[r * square_size_px:(r + 1) * square_size_px, c * square_size_px:(c + 1) * square_size_px] = 0
-
-    border = square_size_px
-    img_with_border = np.pad(
-        img,
-        pad_width=((border, border), (border, border), (0, 0)),
-        mode="constant",
-        constant_values=255,
-    )
-
-    abs_filepath = Path(filepath).resolve()
-    abs_filepath.parent.mkdir(parents=True, exist_ok=True)
-    Image.fromarray(img_with_border).save(abs_filepath)
-    usd_filepath = str(abs_filepath).replace("\\", "/")
-    print(f"[*] 棋盘格纹理已生成: {usd_filepath}")
-    return usd_filepath
-
-
-def create_charuco_image(filepath, squares_x=30, squares_y=10, square_size_px=90):
-    width = squares_x * square_size_px
-    height = squares_y * square_size_px
-    abs_filepath = Path(filepath).resolve()
-    abs_filepath.parent.mkdir(parents=True, exist_ok=True)
-
-    try:
-        import cv2
-        dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_250)
-        try:
-            board = cv2.aruco.CharucoBoard((squares_x, squares_y), 1.0, 0.70, dictionary)
-        except TypeError:
-            board = cv2.aruco.CharucoBoard_create(squares_x, squares_y, 1.0, 0.70, dictionary)
-        if hasattr(board, "generateImage"):
-            img = board.generateImage((width, height), marginSize=0)
-        else:
-            img = board.draw((width, height), marginSize=0)
-        if len(img.shape) == 2:
-            img = np.repeat(img[:, :, None], 3, axis=2)
-        Image.fromarray(img).save(abs_filepath)
-        usd_filepath = str(abs_filepath).replace("\\", "/")
-        print(f"[*] ChArUco 纹理已生成: {usd_filepath}")
-        return usd_filepath
-    except Exception as exc:
-        print(f"[WARN] OpenCV ChArUco 生成失败，使用内置 ChArUco 风格纹理: {exc}")
-
-    img = np.ones((height, width, 3), dtype=np.uint8) * 255
-    marker_cells = 6
-    marker_margin = max(3, square_size_px // 7)
-    marker_size = square_size_px - 2 * marker_margin
-    marker_cell_px = max(1, marker_size // marker_cells)
-    marker_px = marker_cell_px * marker_cells
-
-    def marker_bits(marker_id):
-        marker = np.ones((marker_cells, marker_cells), dtype=np.uint8) * 255
-        marker[0, :] = 0
-        marker[-1, :] = 0
-        marker[:, 0] = 0
-        marker[:, -1] = 0
-        state = (marker_id + 1) * 1103515245 + 12345
-        for r in range(1, marker_cells - 1):
-            for c in range(1, marker_cells - 1):
-                state = (state * 1664525 + 1013904223 + r * 97 + c * 193) & 0xFFFFFFFF
-                marker[r, c] = 0 if (state & 1) else 255
-        return marker
-
-    marker_id = 0
-    for r in range(squares_y):
-        for c in range(squares_x):
-            y0 = r * square_size_px
-            x0 = c * square_size_px
-            if (r + c) % 2 == 1:
-                img[y0:y0 + square_size_px, x0:x0 + square_size_px] = 0
-                continue
-            marker = marker_bits(marker_id)
-            marker_img = np.kron(marker, np.ones((marker_cell_px, marker_cell_px), dtype=np.uint8))
-            marker_img = marker_img[:marker_px, :marker_px]
-            marker_rgb = np.repeat(marker_img[:, :, None], 3, axis=2)
-            marker_y = y0 + (square_size_px - marker_px) // 2
-            marker_x = x0 + (square_size_px - marker_px) // 2
-            img[marker_y:marker_y + marker_px, marker_x:marker_x + marker_px] = marker_rgb
-            marker_id += 1
-
-    Image.fromarray(img).save(abs_filepath)
-    usd_filepath = str(abs_filepath).replace("\\", "/")
-    print(f"[*] ChArUco 风格纹理已生成: {usd_filepath}")
-    return usd_filepath
-
-
 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
@@ -562,521 +399,6 @@ def add_corner_rotary_lidars(room_length, room_width, height, lidar_config, topi
     )
 
 
-def create_raw_usd_material(stage, mat_path, tex_path):
-    material = UsdShade.Material.Define(stage, mat_path)
-    pbr_shader = UsdShade.Shader.Define(stage, f"{mat_path}/PBRShader")
-    pbr_shader.CreateIdAttr("UsdPreviewSurface")
-    pbr_shader.CreateInput("roughness", Sdf.ValueTypeNames.Float).Set(1.0)
-    pbr_shader.CreateInput("metallic", Sdf.ValueTypeNames.Float).Set(0.0)
-
-    tex_sampler = UsdShade.Shader.Define(stage, f"{mat_path}/diffuseTexture")
-    tex_sampler.CreateIdAttr("UsdUVTexture")
-    tex_sampler.CreateInput("file", Sdf.ValueTypeNames.Asset).Set(Sdf.AssetPath(tex_path))
-    tex_sampler.CreateInput("magFilter", Sdf.ValueTypeNames.Token).Set("nearest")
-    tex_sampler.CreateInput("minFilter", Sdf.ValueTypeNames.Token).Set("nearest")
-
-    st_reader = UsdShade.Shader.Define(stage, f"{mat_path}/stReader")
-    st_reader.CreateIdAttr("UsdPrimvarReader_float2")
-    st_reader.CreateInput("varname", Sdf.ValueTypeNames.Token).Set("st")
-
-    tex_sampler.CreateInput("st", Sdf.ValueTypeNames.Float2).ConnectToSource(st_reader.ConnectableAPI(), "result")
-    pbr_shader.CreateInput("diffuseColor", Sdf.ValueTypeNames.Color3f).ConnectToSource(tex_sampler.ConnectableAPI(), "rgb")
-    material.CreateSurfaceOutput().ConnectToSource(pbr_shader.ConnectableAPI(), "surface")
-    return material
-
-
-def create_textured_board(stage, prim_path, width, height, center, euler_rot_deg, usd_material):
-    mesh = UsdGeom.Mesh.Define(stage, prim_path)
-    half_width, half_height = width / 2.0, height / 2.0
-    mesh.GetPointsAttr().Set(Vt.Vec3fArray([
-        Gf.Vec3f(-half_width, -half_height, 0),
-        Gf.Vec3f(half_width, -half_height, 0),
-        Gf.Vec3f(half_width, half_height, 0),
-        Gf.Vec3f(-half_width, half_height, 0),
-    ]))
-    mesh.GetFaceVertexCountsAttr().Set([4])
-    mesh.GetFaceVertexIndicesAttr().Set([0, 1, 2, 3])
-    mesh.GetNormalsAttr().Set([Gf.Vec3f(0, 0, 1)] * 4)
-    mesh.SetNormalsInterpolation(UsdGeom.Tokens.vertex)
-
-    primvars_api = UsdGeom.PrimvarsAPI(mesh)
-    st_primvar = primvars_api.CreatePrimvar("st", Sdf.ValueTypeNames.TexCoord2fArray, UsdGeom.Tokens.vertex)
-    st_primvar.Set([Gf.Vec2f(0, 0), Gf.Vec2f(1, 0), Gf.Vec2f(1, 1), Gf.Vec2f(0, 1)])
-    mesh.GetExtentAttr().Set([Gf.Vec3f(-half_width, -half_height, -0.01), Gf.Vec3f(half_width, half_height, 0.01)])
-
-    xform = UsdGeom.Xformable(mesh)
-    xform.AddTranslateOp().Set(Gf.Vec3d(*center))
-    xform.AddRotateXYZOp().Set(Gf.Vec3f(*euler_rot_deg))
-    UsdShade.MaterialBindingAPI.Apply(mesh.GetPrim()).Bind(usd_material)
-    return mesh
-
-
-def create_textured_top_strip(stage, prim_path, x_edges, y_min, y_max, z_values, usd_material):
-    mesh = UsdGeom.Mesh.Define(stage, prim_path)
-    x0 = x_edges[0]
-    x1 = x_edges[-1]
-    total_length = max(x1 - x0, 1e-6)
-
-    points = []
-    st_values = []
-    for x, z in zip(x_edges, z_values):
-        u = (x - x0) / total_length
-        points.append(Gf.Vec3f(x, y_min, z))
-        points.append(Gf.Vec3f(x, y_max, z))
-        st_values.append(Gf.Vec2f(u, 0.0))
-        st_values.append(Gf.Vec2f(u, 1.0))
-
-    face_vertex_counts = []
-    face_vertex_indices = []
-    for index in range(len(x_edges) - 1):
-        face_vertex_counts.append(4)
-        face_vertex_indices.extend([2 * index, 2 * (index + 1), 2 * (index + 1) + 1, 2 * index + 1])
-
-    mesh.GetPointsAttr().Set(Vt.Vec3fArray(points))
-    mesh.GetFaceVertexCountsAttr().Set(face_vertex_counts)
-    mesh.GetFaceVertexIndicesAttr().Set(face_vertex_indices)
-    mesh.GetExtentAttr().Set([
-        Gf.Vec3f(min(x_edges), y_min, min(z_values) - 0.005),
-        Gf.Vec3f(max(x_edges), y_max, max(z_values) + 0.005),
-    ])
-
-    primvars_api = UsdGeom.PrimvarsAPI(mesh)
-    st_primvar = primvars_api.CreatePrimvar("st", Sdf.ValueTypeNames.TexCoord2fArray, UsdGeom.Tokens.vertex)
-    st_primvar.Set(st_values)
-    UsdShade.MaterialBindingAPI.Apply(mesh.GetPrim()).Bind(usd_material)
-    return mesh
-
-
-def add_down_camera_intrinsic_target(world, stage, args, material):
-    length = args.down_camera_target_length
-    width = args.down_camera_target_width
-    center_x = args.down_camera_target_x
-    center_y = args.down_camera_target_y
-    z_low = 0.012
-    z_high = args.down_camera_target_max_height
-    x_start = center_x - length / 2.0
-    x_low_end = x_start + length * 0.22
-    x_ramp_end = x_start + length * 0.74
-    x_end = center_x + length / 2.0
-    y_min = center_y - width / 2.0
-    y_max = center_y + width / 2.0
-    base_thickness = 0.006
-
-    world.scene.add(FixedCuboid(
-        prim_path="/World/Workshop/DownCameraIntrinsicTarget/Base",
-        name="down_camera_intrinsic_target_base",
-        position=np.array([center_x, center_y, base_thickness / 2.0]),
-        scale=np.array([length + 0.08, width + 0.08, base_thickness]),
-        color=np.array([0.045, 0.050, 0.052]),
-    ))
-
-    create_textured_top_strip(
-        stage,
-        "/World/Workshop/DownCameraIntrinsicTarget/CharucoRampSurface",
-        [x_start, x_low_end, x_ramp_end, x_end],
-        y_min,
-        y_max,
-        [z_low, z_low, z_high, z_high],
-        material,
-    )
-
-    specs = [{
-        "id": "down_camera_3d_charuco_ramp",
-        "target_type": "down_camera_intrinsic",
-        "pattern": "charuco",
-        "purpose": "intrinsic_calibration_with_depth_and_pose_gradient",
-        "center": [center_x, center_y, (z_low + z_high) / 2.0],
-        "length_m": length,
-        "width_m": width,
-        "squares_x": args.down_camera_target_squares_x,
-        "squares_y": args.down_camera_target_squares_y,
-        "square_size_m": width / args.down_camera_target_squares_y,
-        "min_height_m": z_low,
-        "max_height_m": z_high,
-        "recommended_drive_axis": "+x",
-        "recommended_speed_mps": 0.10,
-        "surfaces": [
-            {
-                "id": "low_flat_charuco",
-                "type": "flat",
-                "x_range_m": [x_start, x_low_end],
-                "z_range_m": [z_low, z_low],
-            },
-            {
-                "id": "continuous_slope_charuco",
-                "type": "continuous_slope",
-                "x_range_m": [x_low_end, x_ramp_end],
-                "z_range_m": [z_low, z_high],
-            },
-            {
-                "id": "high_flat_charuco",
-                "type": "flat",
-                "x_range_m": [x_ramp_end, x_end],
-                "z_range_m": [z_high, z_high],
-            },
-        ],
-    }]
-    return specs
-
-
-def lidar_2d_target_wall_slots(args):
-    if args.disable_2d_lidar_targets:
-        return {}
-
-    width = args.lidar_2d_target_width
-    height = args.lidar_2d_target_height
-    gap = 0.12
-    edge_margin = 0.18
-    bay_margin = 0.07
-    bay_width = max(1.90, 2.0 * width + gap + 2.0 * bay_margin)
-    bay_z_min = 0.0
-    bay_z_max = args.room_height
-    center_spacing = width + gap
-
-    def group_offsets(span, side):
-        usable_min = -span / 2.0 + edge_margin
-        usable_max = span / 2.0 - edge_margin
-        usable_width = max(0.0, usable_max - usable_min)
-        effective_bay_width = min(bay_width, usable_width) if usable_width > 0.0 else bay_width
-        if usable_width <= effective_bay_width:
-            axis_min = usable_min
-            axis_max = usable_max
-        elif side == "max":
-            axis_max = usable_max
-            axis_min = axis_max - effective_bay_width
-        else:
-            axis_min = usable_min
-            axis_max = axis_min + effective_bay_width
-        group_center = (axis_min + axis_max) / 2.0
-        vertical_offset = group_center - center_spacing / 2.0
-        slope_offset = group_center + center_spacing / 2.0
-        return vertical_offset, slope_offset, axis_min, axis_max
-
-    front_vertical, front_slope, front_min, front_max = group_offsets(args.room_width, "min")
-    _, _, right_min, right_max = group_offsets(args.room_length, "max")
-    right_vertical = (right_min + right_max) / 2.0
-
-    slots = {
-        "front": {
-            "vertical_offset": front_vertical,
-            "slope_offset": front_slope,
-            "reserved_axis_min": front_min,
-            "reserved_axis_max": front_max,
-            "reserved_z_min": bay_z_min,
-            "reserved_z_max": bay_z_max,
-        },
-        "right": {
-            "vertical_offset": right_vertical,
-            "reserved_axis_min": right_min,
-            "reserved_axis_max": right_max,
-            "reserved_z_min": bay_z_min,
-            "reserved_z_max": bay_z_max,
-        },
-    }
-    return slots
-
-
-def lidar_2d_checkerboard_reserved_zones(args):
-    slots = lidar_2d_target_wall_slots(args)
-    reserved_zones = {}
-    for wall, slot in slots.items():
-        reserved_zones[wall] = [{
-            "axis_min": slot["reserved_axis_min"],
-            "axis_max": slot["reserved_axis_max"],
-            "z_min": slot["reserved_z_min"],
-            "z_max": slot["reserved_z_max"],
-            "reason": "2d_lidar_extrinsic_corner_bay",
-        }]
-    return reserved_zones
-
-
-def calibration_board_layout(args, board_width, board_height):
-    wall_standoff = 0.015
-    front_x = args.room_length / 2.0 - wall_standoff
-    back_x = -args.room_length / 2.0 + wall_standoff
-    left_y = args.room_width / 2.0 - wall_standoff
-    right_y = -args.room_width / 2.0 + wall_standoff
-    horizontal_gap = 0.18
-    vertical_gap = 0.16
-    side_margin = 0.35
-    bottom_margin = 0.32
-    top_margin = 0.28
-
-    def axis_positions(span, item_size, margin, gap):
-        available = span - 2.0 * margin
-        count = max(1, int((available + gap) // (item_size + gap)))
-        if count == 1:
-            return [0.0]
-        used = count * item_size + (count - 1) * gap
-        start = -used / 2.0 + item_size / 2.0
-        return [start + index * (item_size + gap) for index in range(count)]
-
-    def z_positions():
-        available = args.room_height - bottom_margin - top_margin
-        count = max(1, int((available + vertical_gap) // (board_height + vertical_gap)))
-        if count == 1:
-            return [bottom_margin + board_height / 2.0]
-        used = count * board_height + (count - 1) * vertical_gap
-        start = bottom_margin + board_height / 2.0 + max(0.0, available - used) / 2.0
-        return [start + index * (board_height + vertical_gap) for index in range(count)]
-
-    zs = z_positions()
-    front_back_offsets = axis_positions(args.room_width, board_width, side_margin, horizontal_gap)
-    side_offsets = axis_positions(args.room_length, board_width, side_margin, horizontal_gap)
-    reserved_zones = lidar_2d_checkerboard_reserved_zones(args)
-
-    board_specs = []
-
-    def overlaps_reserved_zone(wall, offset, z):
-        board_axis_min = offset - board_width / 2.0
-        board_axis_max = offset + board_width / 2.0
-        board_z_min = z - board_height / 2.0
-        board_z_max = z + board_height / 2.0
-        for zone in reserved_zones.get(wall, []):
-            axis_overlaps = board_axis_min < zone["axis_max"] and board_axis_max > zone["axis_min"]
-            z_overlaps = board_z_min < zone["z_max"] and board_z_max > zone["z_min"]
-            if axis_overlaps and z_overlaps:
-                return True
-        return False
-
-    def add_wall_grid(wall, fixed_value, offsets, rotation_deg):
-        for row, z in enumerate(zs):
-            for col, offset in enumerate(offsets):
-                if overlaps_reserved_zone(wall, offset, z):
-                    continue
-                if wall == "front":
-                    position = [fixed_value, offset, z]
-                elif wall == "back":
-                    position = [fixed_value, offset, z]
-                elif wall == "left":
-                    position = [offset, fixed_value, z]
-                else:
-                    position = [offset, fixed_value, z]
-
-                board_specs.append({
-                    "id": f"{wall}_wall_r{row:02d}_c{col:02d}",
-                    "wall": wall,
-                    "mount": "flush",
-                    "purpose": "wall_checkerboard_array",
-                    "position": position,
-                    "rotation_deg": rotation_deg,
-                })
-
-    add_wall_grid("front", front_x, front_back_offsets, [90, 0, 90])
-    add_wall_grid("back", back_x, front_back_offsets, [90, 0, -90])
-    add_wall_grid("left", left_y, side_offsets, [90, 0, 0])
-    add_wall_grid("right", right_y, side_offsets, [90, 0, 180])
-
-    return board_specs
-
-
-def add_calibration_boards(stage, args, board_width, board_height, material):
-    board_specs = calibration_board_layout(args, board_width, board_height)
-    for spec in board_specs:
-        create_textured_board(
-            stage,
-            f"/World/Workshop/CalibrationBoards/{spec['id']}",
-            board_width,
-            board_height,
-            spec["position"],
-            spec["rotation_deg"],
-            material,
-        )
-    return board_specs
-
-
-def add_lidar_2d_panel(world, prim_path, name, position, scale, color, rotation_deg=None):
-    orientation = None
-    if rotation_deg is not None:
-        orientation = np.array(euler_angles_to_quat(np.array(rotation_deg), degrees=True))
-    world.scene.add(FixedCuboid(
-        prim_path=prim_path,
-        name=name,
-        position=np.array(position),
-        orientation=orientation,
-        scale=np.array(scale),
-        color=np.array(color),
-    ))
-
-
-def add_2d_lidar_calibration_targets(world, args):
-    thickness = args.lidar_2d_target_thickness
-    width = args.lidar_2d_target_width
-    height = args.lidar_2d_target_height
-    bottom_z = args.lidar_2d_target_bottom_z
-    center_z = bottom_z + height / 2.0
-    angle_deg = args.lidar_2d_target_slope_angle_deg
-    angle_rad = math.radians(angle_deg)
-    sloped_length = height / max(math.cos(angle_rad), 1e-3)
-    standoff = 0.06
-    slots = lidar_2d_target_wall_slots(args)
-
-    front_wall_x = args.room_length / 2.0
-    right_wall_y = -args.room_width / 2.0
-
-    front_base_x = front_wall_x - standoff - thickness / 2.0
-    side_right_base_y = right_wall_y + standoff + thickness / 2.0
-
-    front_slope_center_x = front_wall_x - standoff - height / 2.0 - thickness
-
-    front_vertical_y = slots["front"]["vertical_offset"]
-    front_slope_y = slots["front"]["slope_offset"]
-    right_vertical_x = slots["right"]["vertical_offset"]
-
-    vertical_color = [0.92, 0.82, 0.18]
-    slope_color = [0.12, 0.65, 0.95]
-    specs = []
-
-    def add_spec(spec):
-        specs.append(spec)
-        return spec
-
-    front_vertical = add_spec({
-        "id": "front_vertical_reference_panel",
-        "wall": "front",
-        "type": "vertical_reference",
-        "position": [front_base_x, front_vertical_y, center_z],
-        "scale": [thickness, width, height],
-        "rotation_deg": [0.0, 0.0, 0.0],
-        "nominal_plane": "x = room_length/2 - standoff - thickness",
-    })
-    add_lidar_2d_panel(
-        world,
-        "/World/Workshop/Lidar2DCalibrationTargets/FrontVerticalReference",
-        "front_vertical_reference_panel",
-        front_vertical["position"],
-        front_vertical["scale"],
-        vertical_color,
-    )
-
-    front_slope = add_spec({
-        "id": "front_45deg_height_encoding_panel",
-        "wall": "front",
-        "type": "height_encoding_slope",
-        "position": [front_slope_center_x, front_slope_y, center_z],
-        "scale": [thickness, width, sloped_length],
-        "rotation_deg": [0.0, -angle_deg, 0.0],
-        "slope_angle_deg": angle_deg,
-        "height_to_range_sign": "higher_scan_plane_farther_from_front_wall",
-    })
-    add_lidar_2d_panel(
-        world,
-        "/World/Workshop/Lidar2DCalibrationTargets/FrontSlope45",
-        "front_45deg_height_encoding_panel",
-        front_slope["position"],
-        front_slope["scale"],
-        slope_color,
-        front_slope["rotation_deg"],
-    )
-
-    right_vertical = add_spec({
-        "id": "right_vertical_reference_panel",
-        "wall": "right",
-        "type": "vertical_reference",
-        "position": [right_vertical_x, side_right_base_y, center_z],
-        "scale": [width, thickness, height],
-        "rotation_deg": [0.0, 0.0, 0.0],
-        "nominal_plane": "y = -room_width/2 + standoff + thickness",
-    })
-    add_lidar_2d_panel(
-        world,
-        "/World/Workshop/Lidar2DCalibrationTargets/RightVerticalReference",
-        "right_vertical_reference_panel",
-        right_vertical["position"],
-        right_vertical["scale"],
-        vertical_color,
-    )
-
-    return specs
-
-
-def add_floor_marker(world, prim_path, name, position, scale, color):
-    world.scene.add(FixedCuboid(
-        prim_path=prim_path,
-        name=name,
-        position=np.array(position),
-        scale=np.array(scale),
-        color=np.array(color),
-    ))
-
-
-def add_calibration_floor_fixtures(world, args):
-    z = 0.012
-    thickness = 0.012
-    length = args.straight_track_length
-    width = args.straight_track_width
-
-    add_floor_marker(
-        world,
-        "/World/Workshop/CalibrationFixtures/StraightTrack",
-        "straight_track",
-        [0.0, 0.0, z],
-        [length, width, thickness],
-        [0.08, 0.12, 0.10],
-    )
-    add_floor_marker(
-        world,
-        "/World/Workshop/CalibrationFixtures/StraightCenterLine",
-        "straight_center_line",
-        [0.0, 0.0, z + thickness],
-        [length, 0.035, thickness],
-        [0.95, 0.95, 0.15],
-    )
-    for side, y in (("Left", width / 2.0), ("Right", -width / 2.0)):
-        add_floor_marker(
-            world,
-            f"/World/Workshop/CalibrationFixtures/StraightBoundary{side}",
-            f"straight_boundary_{side.lower()}",
-            [0.0, y, z + thickness],
-            [length, 0.025, thickness],
-            [0.95, 0.78, 0.08],
-        )
-
-    stop_x = min(length / 2.0 - 0.35, args.room_length / 2.0 - 1.0)
-    add_floor_marker(
-        world,
-        "/World/Workshop/CalibrationFixtures/StopAccuracyTarget",
-        "stop_accuracy_target",
-        [stop_x, 0.0, z + 2.0 * thickness],
-        [0.6, 0.9, thickness],
-        [0.88, 0.10, 0.10],
-    )
-
-    lateral_x = -min(length / 2.0 - 0.8, args.room_length / 2.0 - 1.2)
-    add_floor_marker(
-        world,
-        "/World/Workshop/CalibrationFixtures/LateralMotionPad",
-        "lateral_motion_pad",
-        [lateral_x, 0.0, z + thickness],
-        [0.75, 2.0, thickness],
-        [0.10, 0.34, 0.88],
-    )
-
-    arc_center = np.array([0.0, -args.room_width * 0.20])
-    arc_radius = min(args.arc_track_radius, args.room_width * 0.32)
-    marker_count = 25
-    for index in range(marker_count):
-        theta = math.radians(20.0 + 140.0 * index / (marker_count - 1))
-        x = arc_center[0] + arc_radius * math.cos(theta)
-        y = arc_center[1] + arc_radius * math.sin(theta)
-        add_floor_marker(
-            world,
-            f"/World/Workshop/CalibrationFixtures/ArcMarker_{index:02d}",
-            f"arc_marker_{index:02d}",
-            [x, y, z + 3.0 * thickness],
-            [0.10, 0.10, thickness],
-            [0.12, 0.75, 0.35],
-        )
-
-    add_floor_marker(
-        world,
-        "/World/Workshop/CalibrationFixtures/SensorCapturePad",
-        "sensor_capture_pad",
-        [0.0, args.room_width * 0.28, z + thickness],
-        [1.2, 0.9, thickness],
-        [0.42, 0.18, 0.78],
-    )
-
 
 class ExternalTruthTelemetryPublisher:
     def __init__(self, args):
@@ -1175,9 +497,10 @@ class VehicleTfPublisher:
                 'x': args.vehicle_camera_x,
                 'y': args.vehicle_camera_y,
                 'z': args.vehicle_camera_z,
-                'roll': 0.0,
+                # USD 相机默认看向局部 -Z；这里让光轴朝 base_link +X，画面上方朝 base_link +Z，避免图像顺时针旋转 90°。
-                'pitch': -math.pi / 2,  # 相机朝下 -90度
+                'roll': math.pi / 2,
-                'yaw': 0.0,
+                'pitch': 0.0,
+                'yaw': -math.pi / 2,
                 'enabled': not args.disable_vehicle_camera,
             },
             'down_camera_link': {
@@ -1944,6 +1267,8 @@ class IsaacWorkshopRuntime:
             orientation=np.array(orientation),
             attributes={
                 "focalLength": float(focal_length),
+                # 下视/近距离相机需要较小近裁剪面，否则离地几厘米时画面会被 near plane 裁成黑屏。
+                "clippingRange": (0.005, 1000.0),
             },
         )
         render_product = rep.create.render_product(
@@ -2044,9 +1369,9 @@ class IsaacWorkshopRuntime:
                         "x_m": self.args.vehicle_camera_x,
                         "y_m": self.args.vehicle_camera_y,
                         "z_m": self.args.vehicle_camera_z,
-                        "roll_rad": 0.0,
+                        "roll_rad": math.pi / 2.0,
-                        "pitch_rad": -math.pi / 2.0,
+                        "pitch_rad": 0.0,
-                        "yaw_rad": 0.0,
+                        "yaw_rad": -math.pi / 2.0,
                     },
                 },
                 {
@@ -2169,6 +1494,20 @@ class IsaacWorkshopRuntime:
         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("/")
@@ -2192,6 +1531,37 @@ class IsaacWorkshopRuntime:
 
         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
@@ -2207,11 +1577,14 @@ class IsaacWorkshopRuntime:
         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:
-            camera_quat = euler_angles_to_quat(np.array([0.0, -90.0, 0.0]), degrees=True)
+            # 光轴朝 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_root}/SimFrontCamera",
+                prim_path=f"{vehicle_sensor_parent_path}/SimFrontCamera",
                 position=[
                     self.args.vehicle_camera_x,
                     self.args.vehicle_camera_y,
@@ -2231,7 +1604,7 @@ class IsaacWorkshopRuntime:
 
         if not self.args.disable_down_camera:
             down_camera_render_product = self.create_ros_camera_render_product(
-                prim_path=f"{vehicle_root}/SimDownCamera",
+                prim_path=f"{vehicle_sensor_parent_path}/SimDownCamera",
                 position=[
                     self.args.down_camera_x,
                     self.args.down_camera_y,
@@ -2250,13 +1623,14 @@ class IsaacWorkshopRuntime:
             ])
 
         if not self.args.disable_vehicle_lidar:
-            lidar_path = f"{vehicle_root}/SimLidar3D"
+            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_config,
+                config=self.args.lidar_3d_config,
                 translation=Gf.Vec3d(
                     self.args.vehicle_lidar_x,
                     self.args.vehicle_lidar_y,
@@ -2264,7 +1638,7 @@ class IsaacWorkshopRuntime:
                 ),
                 orientation=Gf.Quatd(lidar_quat[0], lidar_quat[1], lidar_quat[2], lidar_quat[3]),
             )
-            render_product = rep.create.render_product(lidar_path, [1, 1])
+            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"))
@@ -2277,7 +1651,8 @@ class IsaacWorkshopRuntime:
             ])
 
         if not self.args.disable_vehicle_2d_lidar:
-            lidar_2d_path = f"{vehicle_root}/SimLidar2D"
+            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",
@@ -2489,6 +1864,7 @@ class IsaacWorkshopRuntime:
             )
 
         self.import_vehicle_to_stage(world)
+        self.hide_sensor_placeholder_visuals()
         self.add_vehicle_sensor_publishers()
 
         keys = og.Controller.Keys

agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/calibration_targets.py

@@ -0,0 +1,452 @@
+"""标定板和标靶创建工具
+
+需要在 Isaac Sim 初始化后导入。
+"""
+
+import math
+
+import numpy as np
+from omni.isaac.core.objects import FixedCuboid
+from omni.isaac.core.utils.rotations import euler_angles_to_quat
+
+from usd_utils import create_textured_board, create_textured_top_strip
+
+
+def add_down_camera_intrinsic_target(world, stage, args, material):
+    """添加下视相机内参标定台"""
+    length = args.down_camera_target_length
+    width = args.down_camera_target_width
+    center_x = args.down_camera_target_x
+    center_y = args.down_camera_target_y
+    z_low = 0.012
+    z_high = args.down_camera_target_max_height
+    x_start = center_x - length / 2.0
+    x_low_end = x_start + length * 0.22
+    x_ramp_end = x_start + length * 0.74
+    x_end = center_x + length / 2.0
+    y_min = center_y - width / 2.0
+    y_max = center_y + width / 2.0
+    base_thickness = 0.006
+
+    world.scene.add(FixedCuboid(
+        prim_path="/World/Workshop/DownCameraIntrinsicTarget/Base",
+        name="down_camera_intrinsic_target_base",
+        position=np.array([center_x, center_y, base_thickness / 2.0]),
+        scale=np.array([length + 0.08, width + 0.08, base_thickness]),
+        color=np.array([0.045, 0.050, 0.052]),
+    ))
+
+    create_textured_top_strip(
+        stage,
+        "/World/Workshop/DownCameraIntrinsicTarget/CharucoRampSurface",
+        [x_start, x_low_end, x_ramp_end, x_end],
+        y_min,
+        y_max,
+        [z_low, z_low, z_high, z_high],
+        material,
+    )
+
+    specs = [{
+        "id": "down_camera_3d_charuco_ramp",
+        "target_type": "down_camera_intrinsic",
+        "pattern": "charuco",
+        "purpose": "intrinsic_calibration_with_depth_and_pose_gradient",
+        "center": [center_x, center_y, (z_low + z_high) / 2.0],
+        "length_m": length,
+        "width_m": width,
+        "squares_x": args.down_camera_target_squares_x,
+        "squares_y": args.down_camera_target_squares_y,
+        "square_size_m": width / args.down_camera_target_squares_y,
+        "min_height_m": z_low,
+        "max_height_m": z_high,
+        "recommended_drive_axis": "+x",
+        "recommended_speed_mps": 0.10,
+        "surfaces": [
+            {
+                "id": "low_flat_charuco",
+                "type": "flat",
+                "x_range_m": [x_start, x_low_end],
+                "z_range_m": [z_low, z_low],
+            },
+            {
+                "id": "continuous_slope_charuco",
+                "type": "continuous_slope",
+                "x_range_m": [x_low_end, x_ramp_end],
+                "z_range_m": [z_low, z_high],
+            },
+            {
+                "id": "high_flat_charuco",
+                "type": "flat",
+                "x_range_m": [x_ramp_end, x_end],
+                "z_range_m": [z_high, z_high],
+            },
+        ],
+    }]
+    return specs
+
+
+def lidar_2d_target_wall_slots(args):
+    """计算 2D LiDAR 靶标墙面插槽位置"""
+    if args.disable_2d_lidar_targets:
+        return {}
+
+    width = args.lidar_2d_target_width
+    height = args.lidar_2d_target_height
+    gap = 0.12
+    edge_margin = 0.18
+    bay_margin = 0.07
+    bay_width = max(1.90, 2.0 * width + gap + 2.0 * bay_margin)
+    bay_z_min = 0.0
+    bay_z_max = args.room_height
+    center_spacing = width + gap
+
+    def group_offsets(span, side):
+        usable_min = -span / 2.0 + edge_margin
+        usable_max = span / 2.0 - edge_margin
+        usable_width = max(0.0, usable_max - usable_min)
+        effective_bay_width = min(bay_width, usable_width) if usable_width > 0.0 else bay_width
+        if usable_width <= effective_bay_width:
+            axis_min = usable_min
+            axis_max = usable_max
+        elif side == "max":
+            axis_max = usable_max
+            axis_min = axis_max - effective_bay_width
+        else:
+            axis_min = usable_min
+            axis_max = axis_min + effective_bay_width
+        group_center = (axis_min + axis_max) / 2.0
+        vertical_offset = group_center - center_spacing / 2.0
+        slope_offset = group_center + center_spacing / 2.0
+        return vertical_offset, slope_offset, axis_min, axis_max
+
+    front_vertical, front_slope, front_min, front_max = group_offsets(args.room_width, "min")
+    _, _, right_min, right_max = group_offsets(args.room_length, "max")
+    right_vertical = (right_min + right_max) / 2.0
+
+    slots = {
+        "front": {
+            "vertical_offset": front_vertical,
+            "slope_offset": front_slope,
+            "reserved_axis_min": front_min,
+            "reserved_axis_max": front_max,
+            "reserved_z_min": bay_z_min,
+            "reserved_z_max": bay_z_max,
+        },
+        "right": {
+            "vertical_offset": right_vertical,
+            "reserved_axis_min": right_min,
+            "reserved_axis_max": right_max,
+            "reserved_z_min": bay_z_min,
+            "reserved_z_max": bay_z_max,
+        },
+    }
+    return slots
+
+
+def lidar_2d_checkerboard_reserved_zones(args):
+    """获取 2D LiDAR 标靶预留区域"""
+    slots = lidar_2d_target_wall_slots(args)
+    reserved_zones = {}
+    for wall, slot in slots.items():
+        reserved_zones[wall] = [{
+            "axis_min": slot["reserved_axis_min"],
+            "axis_max": slot["reserved_axis_max"],
+            "z_min": slot["reserved_z_min"],
+            "z_max": slot["reserved_z_max"],
+            "reason": "2d_lidar_extrinsic_corner_bay",
+        }]
+    return reserved_zones
+
+
+def calibration_board_layout(args, board_width, board_height):
+    """计算标定板布局"""
+    wall_standoff = 0.015
+    front_x = args.room_length / 2.0 - wall_standoff
+    back_x = -args.room_length / 2.0 + wall_standoff
+    left_y = args.room_width / 2.0 - wall_standoff
+    right_y = -args.room_width / 2.0 + wall_standoff
+    horizontal_gap = 0.18
+    vertical_gap = 0.16
+    side_margin = 0.35
+    bottom_margin = 0.32
+    top_margin = 0.28
+
+    def axis_positions(span, item_size, margin, gap):
+        available = span - 2.0 * margin
+        count = max(1, int((available + gap) // (item_size + gap)))
+        if count == 1:
+            return [0.0]
+        used = count * item_size + (count - 1) * gap
+        start = -used / 2.0 + item_size / 2.0
+        return [start + index * (item_size + gap) for index in range(count)]
+
+    def z_positions():
+        available = args.room_height - bottom_margin - top_margin
+        count = max(1, int((available + vertical_gap) // (board_height + vertical_gap)))
+        if count == 1:
+            return [bottom_margin + board_height / 2.0]
+        used = count * board_height + (count - 1) * vertical_gap
+        start = bottom_margin + board_height / 2.0 + max(0.0, available - used) / 2.0
+        return [start + index * (board_height + vertical_gap) for index in range(count)]
+
+    zs = z_positions()
+    front_back_offsets = axis_positions(args.room_width, board_width, side_margin, horizontal_gap)
+    side_offsets = axis_positions(args.room_length, board_width, side_margin, horizontal_gap)
+    reserved_zones = lidar_2d_checkerboard_reserved_zones(args)
+
+    board_specs = []
+
+    def overlaps_reserved_zone(wall, offset, z):
+        board_axis_min = offset - board_width / 2.0
+        board_axis_max = offset + board_width / 2.0
+        board_z_min = z - board_height / 2.0
+        board_z_max = z + board_height / 2.0
+        for zone in reserved_zones.get(wall, []):
+            axis_overlaps = board_axis_min < zone["axis_max"] and board_axis_max > zone["axis_min"]
+            z_overlaps = board_z_min < zone["z_max"] and board_z_max > zone["z_min"]
+            if axis_overlaps and z_overlaps:
+                return True
+        return False
+
+    def add_wall_grid(wall, fixed_value, offsets, rotation_deg):
+        for row, z in enumerate(zs):
+            for col, offset in enumerate(offsets):
+                if overlaps_reserved_zone(wall, offset, z):
+                    continue
+                if wall == "front":
+                    position = [fixed_value, offset, z]
+                elif wall == "back":
+                    position = [fixed_value, offset, z]
+                elif wall == "left":
+                    position = [offset, fixed_value, z]
+                else:
+                    position = [offset, fixed_value, z]
+
+                board_specs.append({
+                    "id": f"{wall}_wall_r{row:02d}_c{col:02d}",
+                    "wall": wall,
+                    "mount": "flush",
+                    "purpose": "wall_checkerboard_array",
+                    "position": position,
+                    "rotation_deg": rotation_deg,
+                })
+
+    add_wall_grid("front", front_x, front_back_offsets, [90, 0, 90])
+    add_wall_grid("back", back_x, front_back_offsets, [90, 0, -90])
+    add_wall_grid("left", left_y, side_offsets, [90, 0, 0])
+    add_wall_grid("right", right_y, side_offsets, [90, 0, 180])
+
+    return board_specs
+
+
+def add_calibration_boards(stage, args, board_width, board_height, material):
+    """添加墙面标定板"""
+    board_specs = calibration_board_layout(args, board_width, board_height)
+    for spec in board_specs:
+        create_textured_board(
+            stage,
+            f"/World/Workshop/CalibrationBoards/{spec['id']}",
+            board_width,
+            board_height,
+            spec["position"],
+            spec["rotation_deg"],
+            material,
+        )
+    return board_specs
+
+
+def add_lidar_2d_panel(world, prim_path, name, position, scale, color, rotation_deg=None):
+    """添加 2D LiDAR 标定面板"""
+    orientation = None
+    if rotation_deg is not None:
+        orientation = np.array(euler_angles_to_quat(np.array(rotation_deg), degrees=True))
+    world.scene.add(FixedCuboid(
+        prim_path=prim_path,
+        name=name,
+        position=np.array(position),
+        orientation=orientation,
+        scale=np.array(scale),
+        color=np.array(color),
+    ))
+
+
+def add_2d_lidar_calibration_targets(world, args):
+    """添加 2D LiDAR 标定靶标"""
+    thickness = args.lidar_2d_target_thickness
+    width = args.lidar_2d_target_width
+    height = args.lidar_2d_target_height
+    bottom_z = args.lidar_2d_target_bottom_z
+    center_z = bottom_z + height / 2.0
+    angle_deg = args.lidar_2d_target_slope_angle_deg
+    angle_rad = math.radians(angle_deg)
+    sloped_length = height / max(math.cos(angle_rad), 1e-3)
+    standoff = 0.06
+    slots = lidar_2d_target_wall_slots(args)
+
+    front_wall_x = args.room_length / 2.0
+    right_wall_y = -args.room_width / 2.0
+
+    front_base_x = front_wall_x - standoff - thickness / 2.0
+    side_right_base_y = right_wall_y + standoff + thickness / 2.0
+
+    front_slope_center_x = front_wall_x - standoff - height / 2.0 - thickness
+
+    front_vertical_y = slots["front"]["vertical_offset"]
+    front_slope_y = slots["front"]["slope_offset"]
+    right_vertical_x = slots["right"]["vertical_offset"]
+
+    vertical_color = [0.92, 0.82, 0.18]
+    slope_color = [0.12, 0.65, 0.95]
+    specs = []
+
+    def add_spec(spec):
+        specs.append(spec)
+        return spec
+
+    front_vertical = add_spec({
+        "id": "front_vertical_reference_panel",
+        "wall": "front",
+        "type": "vertical_reference",
+        "position": [front_base_x, front_vertical_y, center_z],
+        "scale": [thickness, width, height],
+        "rotation_deg": [0.0, 0.0, 0.0],
+        "nominal_plane": "x = room_length/2 - standoff - thickness",
+    })
+    add_lidar_2d_panel(
+        world,
+        "/World/Workshop/Lidar2DCalibrationTargets/FrontVerticalReference",
+        "front_vertical_reference_panel",
+        front_vertical["position"],
+        front_vertical["scale"],
+        vertical_color,
+    )
+
+    front_slope = add_spec({
+        "id": "front_45deg_height_encoding_panel",
+        "wall": "front",
+        "type": "height_encoding_slope",
+        "position": [front_slope_center_x, front_slope_y, center_z],
+        "scale": [thickness, width, sloped_length],
+        "rotation_deg": [0.0, -angle_deg, 0.0],
+        "slope_angle_deg": angle_deg,
+        "height_to_range_sign": "higher_scan_plane_farther_from_front_wall",
+    })
+    add_lidar_2d_panel(
+        world,
+        "/World/Workshop/Lidar2DCalibrationTargets/FrontSlope45",
+        "front_45deg_height_encoding_panel",
+        front_slope["position"],
+        front_slope["scale"],
+        slope_color,
+        front_slope["rotation_deg"],
+    )
+
+    right_vertical = add_spec({
+        "id": "right_vertical_reference_panel",
+        "wall": "right",
+        "type": "vertical_reference",
+        "position": [right_vertical_x, side_right_base_y, center_z],
+        "scale": [width, thickness, height],
+        "rotation_deg": [0.0, 0.0, 0.0],
+        "nominal_plane": "y = -room_width/2 + standoff + thickness",
+    })
+    add_lidar_2d_panel(
+        world,
+        "/World/Workshop/Lidar2DCalibrationTargets/RightVerticalReference",
+        "right_vertical_reference_panel",
+        right_vertical["position"],
+        right_vertical["scale"],
+        vertical_color,
+    )
+
+    return specs
+
+
+def add_floor_marker(world, prim_path, name, position, scale, color):
+    """添加地面标记"""
+    world.scene.add(FixedCuboid(
+        prim_path=prim_path,
+        name=name,
+        position=np.array(position),
+        scale=np.array(scale),
+        color=np.array(color),
+    ))
+
+
+def add_calibration_floor_fixtures(world, args):
+    """添加地面标定设施"""
+    z = 0.012
+    thickness = 0.012
+    length = args.straight_track_length
+    width = args.straight_track_width
+
+    add_floor_marker(
+        world,
+        "/World/Workshop/CalibrationFixtures/StraightTrack",
+        "straight_track",
+        [0.0, 0.0, z],
+        [length, width, thickness],
+        [0.08, 0.12, 0.10],
+    )
+    add_floor_marker(
+        world,
+        "/World/Workshop/CalibrationFixtures/StraightCenterLine",
+        "straight_center_line",
+        [0.0, 0.0, z + thickness],
+        [length, 0.035, thickness],
+        [0.95, 0.95, 0.15],
+    )
+    for side, y in (("Left", width / 2.0), ("Right", -width / 2.0)):
+        add_floor_marker(
+            world,
+            f"/World/Workshop/CalibrationFixtures/StraightBoundary{side}",
+            f"straight_boundary_{side.lower()}",
+            [0.0, y, z + thickness],
+            [length, 0.025, thickness],
+            [0.95, 0.78, 0.08],
+        )
+
+    stop_x = min(length / 2.0 - 0.35, args.room_length / 2.0 - 1.0)
+    add_floor_marker(
+        world,
+        "/World/Workshop/CalibrationFixtures/StopAccuracyTarget",
+        "stop_accuracy_target",
+        [stop_x, 0.0, z + 2.0 * thickness],
+        [0.6, 0.9, thickness],
+        [0.88, 0.10, 0.10],
+    )
+
+    lateral_x = -min(length / 2.0 - 0.8, args.room_length / 2.0 - 1.2)
+    add_floor_marker(
+        world,
+        "/World/Workshop/CalibrationFixtures/LateralMotionPad",
+        "lateral_motion_pad",
+        [lateral_x, 0.0, z + thickness],
+        [0.75, 2.0, thickness],
+        [0.10, 0.34, 0.88],
+    )
+
+    arc_center = np.array([0.0, -args.room_width * 0.20])
+    arc_radius = min(args.arc_track_radius, args.room_width * 0.32)
+    marker_count = 25
+    for index in range(marker_count):
+        theta = math.radians(20.0 + 140.0 * index / (marker_count - 1))
+        x = arc_center[0] + arc_radius * math.cos(theta)
+        y = arc_center[1] + arc_radius * math.sin(theta)
+        add_floor_marker(
+            world,
+            f"/World/Workshop/CalibrationFixtures/ArcMarker_{index:02d}",
+            f"arc_marker_{index:02d}",
+            [x, y, z + 3.0 * thickness],
+            [0.10, 0.10, thickness],
+            [0.12, 0.75, 0.35],
+        )
+
+    add_floor_marker(
+        world,
+        "/World/Workshop/CalibrationFixtures/SensorCapturePad",
+        "sensor_capture_pad",
+        [0.0, args.room_width * 0.28, z + thickness],
+        [1.2, 0.9, thickness],
+        [0.42, 0.18, 0.78],
+    )

agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/image_utils.py

@@ -0,0 +1,108 @@
+"""图像生成工具模块
+
+提供棋盘格和 ChArUco 纹理生成功能。
+"""
+
+from pathlib import Path
+
+import numpy as np
+from PIL import Image
+
+
+def create_checkerboard_image(filepath, rows=6, cols=9, square_size_px=500):
+    """创建棋盘格纹理图像"""
+    width = cols * square_size_px
+    height = rows * square_size_px
+    img = np.ones((height, width, 3), dtype=np.uint8) * 255
+
+    for r in range(rows):
+        for c in range(cols):
+            if (r + c) % 2 == 1:
+                img[r * square_size_px:(r + 1) * square_size_px,
+                    c * square_size_px:(c + 1) * square_size_px] = 0
+
+    border = square_size_px
+    img_with_border = np.pad(
+        img,
+        pad_width=((border, border), (border, border), (0, 0)),
+        mode="constant",
+        constant_values=255,
+    )
+
+    abs_filepath = Path(filepath).resolve()
+    abs_filepath.parent.mkdir(parents=True, exist_ok=True)
+    Image.fromarray(img_with_border).save(abs_filepath)
+    usd_filepath = str(abs_filepath).replace("\\", "/")
+    print(f"[*] 棋盘格纹理已生成: {usd_filepath}")
+    return usd_filepath
+
+
+def create_charuco_image(filepath, squares_x=30, squares_y=10, square_size_px=90):
+    """创建 ChArUco 纹理图像"""
+    width = squares_x * square_size_px
+    height = squares_y * square_size_px
+    abs_filepath = Path(filepath).resolve()
+    abs_filepath.parent.mkdir(parents=True, exist_ok=True)
+
+    try:
+        import cv2
+        dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_250)
+        try:
+            board = cv2.aruco.CharucoBoard((squares_x, squares_y), 1.0, 0.70, dictionary)
+        except TypeError:
+            board = cv2.aruco.CharucoBoard_create(squares_x, squares_y, 1.0, 0.70, dictionary)
+        if hasattr(board, "generateImage"):
+            img = board.generateImage((width, height), marginSize=0)
+        else:
+            img = board.draw((width, height), marginSize=0)
+        if len(img.shape) == 2:
+            img = np.repeat(img[:, :, None], 3, axis=2)
+        Image.fromarray(img).save(abs_filepath)
+        usd_filepath = str(abs_filepath).replace("\\", "/")
+        print(f"[*] ChArUco 纹理已生成: {usd_filepath}")
+        return usd_filepath
+    except Exception as exc:
+        print(f"[WARN] OpenCV ChArUco 生成失败，使用内置 ChArUco 风格纹理: {exc}")
+
+    # 备用实现
+    img = np.ones((height, width, 3), dtype=np.uint8) * 255
+    marker_cells = 6
+    marker_margin = max(3, square_size_px // 7)
+    marker_size = square_size_px - 2 * marker_margin
+    marker_cell_px = max(1, marker_size // marker_cells)
+    marker_px = marker_cell_px * marker_cells
+
+    def marker_bits(marker_id):
+        marker = np.ones((marker_cells, marker_cells), dtype=np.uint8) * 255
+        marker[0, :] = 0
+        marker[-1, :] = 0
+        marker[:, 0] = 0
+        marker[:, -1] = 0
+        state = (marker_id + 1) * 1103515245 + 12345
+        for r in range(1, marker_cells - 1):
+            for c in range(1, marker_cells - 1):
+                state = (state * 1664525 + 1013904223 + r * 97 + c * 193) & 0xFFFFFFFF
+                marker[r, c] = 0 if (state & 1) else 255
+        return marker
+
+    marker_id = 0
+    for r in range(squares_y):
+        for c in range(squares_x):
+            y0 = r * square_size_px
+            x0 = c * square_size_px
+            if (r + c) % 2 == 1:
+                img[y0:y0 + square_size_px, x0:x0 + square_size_px] = 0
+                continue
+            marker = marker_bits(marker_id)
+            marker_img = np.kron(marker, np.ones((marker_cell_px, marker_cell_px), dtype=np.uint8))
+            marker_img = marker_img[:marker_px, :marker_px]
+            marker_rgb = np.repeat(marker_img[:, :, None], 3, axis=2)
+            marker_y = y0 + (square_size_px - marker_px) // 2
+            marker_x = x0 + (square_size_px - marker_px) // 2
+            img[marker_y:marker_y + marker_px, marker_x:marker_x + marker_px] = marker_rgb
+            marker_id += 1
+
+    Image.fromarray(img).save(abs_filepath)
+    usd_filepath = str(abs_filepath).replace("\\", "/")
+    print(f"[*] ChArUco 风格纹理已生成: {usd_filepath}")
+    return usd_filepath

agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/urdf_utils.py

@@ -0,0 +1,50 @@
+"""URDF 处理工具模块
+
+提供 URDF 文件处理功能。
+"""
+
+import xml.etree.ElementTree as ET
+from pathlib import Path
+
+from utils import make_mesh_path_absolute
+
+
+def prepare_isaac_urdf(source_urdf_path, output_urdf_path):
+    """准备 Isaac Sim 可用的 URDF 文件
+
+    处理网格路径和关节原点设置。
+    """
+    tree = ET.parse(source_urdf_path)
+    root = tree.getroot()
+    source_dir = source_urdf_path.parent
+
+    local_mesh_links = {
+        "left_steering_hinge",
+        "right_steering_hinge",
+        "left_wheel",
+        "right_wheel",
+        "left_rear_wheel",
+        "right_rear_wheel",
+        "camera",
+        "laser",
+    }
+
+    for link in root.findall("link"):
+        link_name = link.get("name", "")
+        for section_name in ("visual", "collision"):
+            section = link.find(section_name)
+            if section is None:
+                continue
+            mesh = section.find("geometry/mesh")
+            if mesh is not None and mesh.get("filename"):
+                mesh.set("filename", make_mesh_path_absolute(mesh.get("filename"), source_dir))
+            if link_name in local_mesh_links:
+                origin = section.find("origin")
+                if origin is None:
+                    origin = ET.SubElement(section, "origin")
+                origin.set("xyz", "0 0 0")
+                origin.set("rpy", "0 0 0")
+
+    output_urdf_path.parent.mkdir(parents=True, exist_ok=True)
+    tree.write(output_urdf_path, encoding="utf-8", xml_declaration=True)
+    return output_urdf_path

agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/usd_utils.py

@@ -0,0 +1,94 @@
+"""USD 材质和几何体创建工具
+
+需要在 Isaac Sim 初始化后导入。
+"""
+
+from pxr import Gf, Sdf, UsdGeom, UsdShade, Vt
+
+
+def create_raw_usd_material(stage, mat_path, tex_path):
+    """创建 USD PBR 材质"""
+    material = UsdShade.Material.Define(stage, mat_path)
+    pbr_shader = UsdShade.Shader.Define(stage, f"{mat_path}/PBRShader")
+    pbr_shader.CreateIdAttr("UsdPreviewSurface")
+    pbr_shader.CreateInput("roughness", Sdf.ValueTypeNames.Float).Set(1.0)
+    pbr_shader.CreateInput("metallic", Sdf.ValueTypeNames.Float).Set(0.0)
+
+    tex_sampler = UsdShade.Shader.Define(stage, f"{mat_path}/diffuseTexture")
+    tex_sampler.CreateIdAttr("UsdUVTexture")
+    tex_sampler.CreateInput("file", Sdf.ValueTypeNames.Asset).Set(Sdf.AssetPath(tex_path))
+    tex_sampler.CreateInput("magFilter", Sdf.ValueTypeNames.Token).Set("nearest")
+    tex_sampler.CreateInput("minFilter", Sdf.ValueTypeNames.Token).Set("nearest")
+
+    st_reader = UsdShade.Shader.Define(stage, f"{mat_path}/stReader")
+    st_reader.CreateIdAttr("UsdPrimvarReader_float2")
+    st_reader.CreateInput("varname", Sdf.ValueTypeNames.Token).Set("st")
+
+    tex_sampler.CreateInput("st", Sdf.ValueTypeNames.Float2).ConnectToSource(st_reader.ConnectableAPI(), "result")
+    pbr_shader.CreateInput("diffuseColor", Sdf.ValueTypeNames.Color3f).ConnectToSource(tex_sampler.ConnectableAPI(), "rgb")
+    material.CreateSurfaceOutput().ConnectToSource(pbr_shader.ConnectableAPI(), "surface")
+    return material
+
+
+def create_textured_board(stage, prim_path, width, height, center, euler_rot_deg, usd_material):
+    """创建带纹理的标定板"""
+    mesh = UsdGeom.Mesh.Define(stage, prim_path)
+    half_width, half_height = width / 2.0, height / 2.0
+    mesh.GetPointsAttr().Set(Vt.Vec3fArray([
+        Gf.Vec3f(-half_width, -half_height, 0),
+        Gf.Vec3f(half_width, -half_height, 0),
+        Gf.Vec3f(half_width, half_height, 0),
+        Gf.Vec3f(-half_width, half_height, 0),
+    ]))
+    mesh.GetFaceVertexCountsAttr().Set([4])
+    mesh.GetFaceVertexIndicesAttr().Set([0, 1, 2, 3])
+    mesh.GetNormalsAttr().Set([Gf.Vec3f(0, 0, 1)] * 4)
+    mesh.SetNormalsInterpolation(UsdGeom.Tokens.vertex)
+
+    primvars_api = UsdGeom.PrimvarsAPI(mesh)
+    st_primvar = primvars_api.CreatePrimvar("st", Sdf.ValueTypeNames.TexCoord2fArray, UsdGeom.Tokens.vertex)
+    st_primvar.Set([Gf.Vec2f(0, 0), Gf.Vec2f(1, 0), Gf.Vec2f(1, 1), Gf.Vec2f(0, 1)])
+    mesh.GetExtentAttr().Set([Gf.Vec3f(-half_width, -half_height, -0.01), Gf.Vec3f(half_width, half_height, 0.01)])
+
+    xform = UsdGeom.Xformable(mesh)
+    xform.AddTranslateOp().Set(Gf.Vec3d(*center))
+    xform.AddRotateXYZOp().Set(Gf.Vec3f(*euler_rot_deg))
+    UsdShade.MaterialBindingAPI.Apply(mesh.GetPrim()).Bind(usd_material)
+    return mesh
+
+
+def create_textured_top_strip(stage, prim_path, x_edges, y_min, y_max, z_values, usd_material):
+    """创建顶部纹理条带（用于下视相机标定台）"""
+    mesh = UsdGeom.Mesh.Define(stage, prim_path)
+    x0 = x_edges[0]
+    x1 = x_edges[-1]
+    total_length = max(x1 - x0, 1e-6)
+
+    points = []
+    st_values = []
+    for x, z in zip(x_edges, z_values):
+        u = (x - x0) / total_length
+        points.append(Gf.Vec3f(x, y_min, z))
+        points.append(Gf.Vec3f(x, y_max, z))
+        st_values.append(Gf.Vec2f(u, 0.0))
+        st_values.append(Gf.Vec2f(u, 1.0))
+
+    face_vertex_counts = []
+    face_vertex_indices = []
+    for index in range(len(x_edges) - 1):
+        face_vertex_counts.append(4)
+        face_vertex_indices.extend([2 * index, 2 * (index + 1), 2 * (index + 1) + 1, 2 * index + 1])
+
+    mesh.GetPointsAttr().Set(Vt.Vec3fArray(points))
+    mesh.GetFaceVertexCountsAttr().Set(face_vertex_counts)
+    mesh.GetFaceVertexIndicesAttr().Set(face_vertex_indices)
+    mesh.GetExtentAttr().Set([
+        Gf.Vec3f(min(x_edges), y_min, min(z_values) - 0.005),
+        Gf.Vec3f(max(x_edges), y_max, max(z_values) + 0.005),
+    ])
+
+    primvars_api = UsdGeom.PrimvarsAPI(mesh)
+    st_primvar = primvars_api.CreatePrimvar("st", Sdf.ValueTypeNames.TexCoord2fArray, UsdGeom.Tokens.vertex)
+    st_primvar.Set(st_values)
+    UsdShade.MaterialBindingAPI.Apply(mesh.GetPrim()).Bind(usd_material)
+    return mesh

agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/utils.py

@@ -0,0 +1,80 @@
+"""工具函数模块
+
+提供不依赖 omni.isaac 的纯工具函数。
+"""
+
+import math
+from pathlib import Path
+
+import numpy as np
+
+
+def clamp(value, low, high):
+    """将值限制在指定范围内"""
+    return max(low, min(high, value))
+
+
+def resolve_topic(prefix, suffix):
+    """解析 ROS 话题名"""
+    normalized_prefix = prefix.rstrip("/")
+    normalized_suffix = suffix if suffix.startswith("/") else f"/{suffix}"
+    return f"{normalized_prefix}{normalized_suffix}" if normalized_prefix else normalized_suffix
+
+
+def quat_wxyz_to_yaw(quat_wxyz):
+    """从四元数 (w, x, y, z) 提取偏航角"""
+    w, x, y, z = quat_wxyz
+    siny_cosp = 2.0 * (w * z + x * y)
+    cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
+    return math.atan2(siny_cosp, cosy_cosp)
+
+
+def normalize_angle(angle):
+    """将角度归一化到 [-pi, pi] 范围"""
+    return math.atan2(math.sin(angle), math.cos(angle))
+
+
+def chassis_type_value(name, chassis_type_cls=None):
+    """将底盘类型名称转换为枚举值"""
+    if chassis_type_cls is None:
+        return 0
+    mapping = {
+        "ackermann": chassis_type_cls.ACKERMANN,
+        "differential": chassis_type_cls.DIFFERENTIAL,
+        "single_steer": chassis_type_cls.SINGLE_STEER_WHEEL,
+        "multi_steer": chassis_type_cls.MULTI_STEER_WHEEL,
+    }
+    return mapping.get(name, chassis_type_cls.CHASSIS_TYPE_UNSPECIFIED)
+
+
+def controller_algorithm_value(name, controller_type_cls=None):
+    """将控制器算法名称转换为枚举值"""
+    if controller_type_cls is None:
+        return 0
+    mapping = {
+        "pid": controller_type_cls.PID,
+        "mpc": controller_type_cls.MPC,
+        "lqr": controller_type_cls.LQR,
+        "pure_pursuit": controller_type_cls.PURE_PURSUIT,
+    }
+    return mapping.get(name, controller_type_cls.CONTROLLER_ALGORITHM_UNSPECIFIED)
+
+
+def vehicle_state_from_isaac(agv):
+    """从 Isaac Sim 车辆对象获取状态"""
+    position, quat = agv.get_world_pose()
+    yaw_rad = quat_wxyz_to_yaw(quat)
+    linear_velocity = agv.get_linear_velocity()
+    try:
+        angular_velocity = agv.get_angular_velocity()
+    except Exception:
+        angular_velocity = np.array([0.0, 0.0, 0.0])
+    return position, yaw_rad, linear_velocity, angular_velocity
+
+
+def make_mesh_path_absolute(mesh_filename, source_dir):
+    """将相对网格路径转换为绝对路径"""
+    mesh_path = Path(mesh_filename)
+    if mesh_path.is_absolute():
+        return str(mesh_path)
+    return str((source_dir / mesh_path).resolve())