feat:修复了车载Camera和Lidar问题....
This commit is contained in:
li-shihao-code
@@ -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
|
||||
|
||||
+6
-6
@@ -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>
|
||||
|
||||
+104
-728
@@ -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
|
||||
|
||||
@@ -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],
|
||||
)
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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())
|
||||
Reference in New Issue
Block a user