diff --git a/agv_calib_brain/.isaac_cache/workshop_scene_manifest.json b/agv_calib_brain/.isaac_cache/workshop_scene_manifest.json
index 36dd910..dc532a5 100644
--- a/agv_calib_brain/.isaac_cache/workshop_scene_manifest.json
+++ b/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,
- "roll_rad": 0.0,
- "pitch_rad": -1.5707963267948966,
- "yaw_rad": 0.0
+ "z_m": 1.18,
+ "roll_rad": 1.5707963267948966,
+ "pitch_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
diff --git a/agv_calib_brain/src/simulation/isaac_workshop_sim/assets/urdf/ackermann_front_steer_rear_drive.urdf b/agv_calib_brain/src/simulation/isaac_workshop_sim/assets/urdf/ackermann_front_steer_rear_drive.urdf
index dfa90f8..9658f22 100644
--- a/agv_calib_brain/src/simulation/isaac_workshop_sim/assets/urdf/ackermann_front_steer_rear_drive.urdf
+++ b/agv_calib_brain/src/simulation/isaac_workshop_sim/assets/urdf/ackermann_front_steer_rear_drive.urdf
@@ -277,7 +277,7 @@
-
+
@@ -293,14 +293,14 @@
-
+
-
+
@@ -309,14 +309,14 @@
-
+
-
+
@@ -325,7 +325,7 @@
-
+
diff --git a/agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/build_calibration_room.py b/agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/build_calibration_room.py
index 65d5e29..23deb62 100644
--- a/agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/build_calibration_room.py
+++ b/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,
- 'pitch': -math.pi / 2, # 相机朝下 -90度
- 'yaw': 0.0,
+ # 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': {
@@ -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,
- "pitch_rad": -math.pi / 2.0,
- "yaw_rad": 0.0,
+ "roll_rad": math.pi / 2.0,
+ "pitch_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
diff --git a/agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/calibration_targets.py b/agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/calibration_targets.py
new file mode 100644
index 0000000..7dddf4e
--- /dev/null
+++ b/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],
+ )
diff --git a/agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/image_utils.py b/agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/image_utils.py
new file mode 100644
index 0000000..3387991
--- /dev/null
+++ b/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
diff --git a/agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/urdf_utils.py b/agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/urdf_utils.py
new file mode 100644
index 0000000..b05d654
--- /dev/null
+++ b/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
diff --git a/agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/usd_utils.py b/agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/usd_utils.py
new file mode 100644
index 0000000..637f451
--- /dev/null
+++ b/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
diff --git a/agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/utils.py b/agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/utils.py
new file mode 100644
index 0000000..b8b1eb4
--- /dev/null
+++ b/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())