feat:完善了自动标定车间的环境
This commit is contained in:
li-shihao-code
@@ -0,0 +1,276 @@
|
||||
import os
|
||||
|
||||
os.environ["OMNI_KIT_ACCEPT_EULA"] = "YES"
|
||||
from isaacsim import SimulationApp
|
||||
|
||||
simulation_app = SimulationApp({"headless": False})
|
||||
|
||||
from omni.isaac.core.utils.extensions import enable_extension
|
||||
|
||||
enable_extension("omni.isaac.ros2_bridge")
|
||||
simulation_app.update()
|
||||
|
||||
import numpy as np
|
||||
from PIL import Image
|
||||
import omni.kit.commands
|
||||
from pathlib import Path
|
||||
|
||||
# 【🔥核心防坑：只依赖底层基石 USD API】
|
||||
import omni.usd
|
||||
from pxr import Gf, Sdf, UsdShade, UsdGeom, Vt
|
||||
|
||||
from omni.isaac.core import World
|
||||
from omni.isaac.core.objects import FixedCuboid
|
||||
from omni.isaac.core.utils.prims import create_prim
|
||||
from omni.isaac.core.utils.viewports import set_camera_view
|
||||
from omni.isaac.core.utils.rotations import euler_angles_to_quat
|
||||
|
||||
from omni.isaac.sensor import Camera
|
||||
import omni.replicator.core as rep
|
||||
import omni.graph.core as og
|
||||
|
||||
|
||||
def create_checkerboard_image(filepath="checkerboard.png", rows=6, cols=9, square_size_px=100):
|
||||
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)
|
||||
|
||||
pil_img = Image.fromarray(img_with_border)
|
||||
abs_filepath = Path(filepath).resolve()
|
||||
pil_img.save(abs_filepath)
|
||||
usd_filepath = str(abs_filepath).replace("\\", "/")
|
||||
print(f"[*] 棋盘格纹理已自动生成: {usd_filepath}")
|
||||
return usd_filepath
|
||||
|
||||
|
||||
def add_corner_rotary_lidars(room_length=10.0, room_width=6.0, height=3.5,
|
||||
lidar_config="Example_Rotary",
|
||||
topic_prefix="/workshop/lidar"):
|
||||
offset = 0.3
|
||||
x_pos = (room_length / 2.0) - offset
|
||||
y_pos = (room_width / 2.0) - offset
|
||||
|
||||
lidar_configs = [
|
||||
{"name": "FL", "pos": [x_pos, y_pos, height], "yaw": np.degrees(np.arctan2(-y_pos, -x_pos))},
|
||||
{"name": "FR", "pos": [x_pos, -y_pos, height], "yaw": np.degrees(np.arctan2(y_pos, -x_pos))},
|
||||
{"name": "BL", "pos": [-x_pos, y_pos, height], "yaw": np.degrees(np.arctan2(-y_pos, x_pos))},
|
||||
{"name": "BR", "pos": [-x_pos, -y_pos, height], "yaw": np.degrees(np.arctan2(y_pos, x_pos))}
|
||||
]
|
||||
|
||||
keys = og.Controller.Keys
|
||||
graph_path = "/World/ROS2_Lidar_Graph"
|
||||
|
||||
nodes = [
|
||||
("OnTick", "omni.graph.action.OnTick"),
|
||||
("ReadSimTime", "omni.isaac.core_nodes.IsaacReadSimulationTime"),
|
||||
("PublishTF", "omni.isaac.ros2_bridge.ROS2PublishTransformTree")
|
||||
]
|
||||
|
||||
connections = [
|
||||
("OnTick.outputs:tick", "PublishTF.inputs:execIn"),
|
||||
("ReadSimTime.outputs:simulationTime", "PublishTF.inputs:timeStamp")
|
||||
]
|
||||
|
||||
set_values = []
|
||||
lidar_paths = []
|
||||
|
||||
for cfg in lidar_configs:
|
||||
lidar_path = f"/World/Sensors/Lidar_{cfg['name']}"
|
||||
lidar_paths.append(lidar_path)
|
||||
|
||||
pitch_angle = 15.0
|
||||
quat = euler_angles_to_quat(np.array([0, pitch_angle, cfg['yaw']]), degrees=True)
|
||||
orientation = Gf.Quatd(quat[0], quat[1], quat[2], quat[3])
|
||||
|
||||
omni.kit.commands.execute(
|
||||
"IsaacSensorCreateRtxLidar", path=lidar_path, parent=None,
|
||||
config=lidar_config, translation=Gf.Vec3d(*cfg["pos"]), orientation=orientation
|
||||
)
|
||||
|
||||
render_product = rep.create.render_product(lidar_path, [1, 1])
|
||||
helper_name = f"ROS2LidarHelper_{cfg['name']}"
|
||||
nodes.append((helper_name, "omni.isaac.ros2_bridge.ROS2RtxLidarHelper"))
|
||||
connections.append(("OnTick.outputs:tick", f"{helper_name}.inputs:execIn"))
|
||||
|
||||
set_values.extend([
|
||||
(f"{helper_name}.inputs:renderProductPath", str(render_product.path)),
|
||||
(f"{helper_name}.inputs:topicName", f"{topic_prefix}/{cfg['name'].lower()}/pointcloud"),
|
||||
(f"{helper_name}.inputs:frameId", f"Lidar_{cfg['name']}"),
|
||||
(f"{helper_name}.inputs:type", "point_cloud"),
|
||||
(f"{helper_name}.inputs:fullScan", True)
|
||||
])
|
||||
|
||||
set_values.append(("PublishTF.inputs:targetPrims", lidar_paths))
|
||||
|
||||
og.Controller.edit({"graph_path": graph_path, "evaluator_name": "execution"},
|
||||
{keys.CREATE_NODES: nodes, keys.CONNECT: connections, keys.SET_VALUES: set_values})
|
||||
|
||||
|
||||
# ================= 【🔥纯血底层 API：手工构造材质与带 UV 的网格】 =================
|
||||
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))
|
||||
|
||||
# 🔥🔥🔥 核心修改 1：强制关闭 GPU 的双线性平滑插值，使用“最近邻(Nearest)”采样！🔥🔥🔥
|
||||
# 这一步能让黑白方块的交界处像刀切一样锐利，彻底消除模糊过渡带！
|
||||
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)
|
||||
w, h = width / 2.0, height / 2.0
|
||||
|
||||
points = Vt.Vec3fArray([Gf.Vec3f(-w, -h, 0), Gf.Vec3f(w, -h, 0), Gf.Vec3f(w, h, 0), Gf.Vec3f(-w, h, 0)])
|
||||
mesh.GetPointsAttr().Set(points)
|
||||
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(-w, -h, -0.01), Gf.Vec3f(w, h, 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 build_workshop():
|
||||
world = World(stage_units_in_meters=1.0)
|
||||
L, W, H, T = 10.0, 6.0, 3.5, 0.2
|
||||
floor_color = np.array([0.2, 0.2, 0.2])
|
||||
wall_color = np.array([0.8, 0.8, 0.8])
|
||||
|
||||
world.scene.add(FixedCuboid(prim_path="/World/Workshop/Floor", name="floor", position=np.array([0, 0, -T / 2]),
|
||||
scale=np.array([L + 2 * T, W + 2 * T, T]), color=floor_color))
|
||||
world.scene.add(
|
||||
FixedCuboid(prim_path="/World/Workshop/Ceiling", name="ceiling", position=np.array([0, 0, H + T / 2]),
|
||||
scale=np.array([L + 2 * T, W + 2 * T, T]), color=wall_color))
|
||||
world.scene.add(FixedCuboid(prim_path="/World/Workshop/Wall_Front", name="wall_front",
|
||||
position=np.array([L / 2 + T / 2, 0, H / 2]), scale=np.array([T, W, H]),
|
||||
color=wall_color))
|
||||
world.scene.add(FixedCuboid(prim_path="/World/Workshop/Wall_Back", name="wall_back",
|
||||
position=np.array([-L / 2 - T / 2, 0, H / 2]), scale=np.array([T, W, H]),
|
||||
color=wall_color))
|
||||
world.scene.add(FixedCuboid(prim_path="/World/Workshop/Wall_Left", name="wall_left",
|
||||
position=np.array([0, W / 2 + T / 2, H / 2]), scale=np.array([L + 2 * T, T, H]),
|
||||
color=wall_color))
|
||||
world.scene.add(FixedCuboid(prim_path="/World/Workshop/Wall_Right", name="wall_right",
|
||||
position=np.array([0, -W / 2 - T / 2, H / 2]), scale=np.array([L + 2 * T, T, H]),
|
||||
color=wall_color))
|
||||
|
||||
light_positions = [(L / 4, W / 4, H - 0.5), (L / 4, -W / 4, H - 0.5), (-L / 4, W / 4, H - 0.5),
|
||||
(-L / 4, -W / 4, H - 0.5)]
|
||||
for i, pos in enumerate(light_positions):
|
||||
create_prim(prim_path=f"/World/Workshop/Lights/Light_{i}", prim_type="SphereLight", position=np.array(pos),
|
||||
attributes={"inputs:radius": 0.3, "inputs:intensity": 30000.0, "inputs:color": (1.0, 1.0, 0.95)})
|
||||
|
||||
cb_rows, cb_cols = 6, 9
|
||||
cb_square_size = 0.20
|
||||
|
||||
# 🔥🔥🔥 核心修改 2：适当提升分辨率 🔥🔥🔥
|
||||
# 将 square_size_px 从 100 提高到 500！
|
||||
tex_path = create_checkerboard_image("checkerboard.png", rows=cb_rows, cols=cb_cols, square_size_px=500)
|
||||
|
||||
stage = omni.usd.get_context().get_stage()
|
||||
|
||||
mat_path = "/World/Workshop/Materials/CheckerboardMat"
|
||||
usd_material = create_raw_usd_material(stage, mat_path, tex_path)
|
||||
|
||||
board_w = (cb_cols + 2) * cb_square_size
|
||||
board_h = (cb_rows + 2) * cb_square_size
|
||||
z_height = H / 2.0
|
||||
offset = 0.05
|
||||
|
||||
board_configs = [
|
||||
("/World/Workshop/CalibrationBoards/Front", [L / 2 - offset, 0, z_height], [90, 0, 90]),
|
||||
("/World/Workshop/CalibrationBoards/Back", [-L / 2 + offset, 0, z_height], [90, 0, -90]),
|
||||
("/World/Workshop/CalibrationBoards/Left", [0, W / 2 - offset, z_height], [90, 0, 0]),
|
||||
("/World/Workshop/CalibrationBoards/Right", [0, -W / 2 + offset, z_height], [90, 0, 180])
|
||||
]
|
||||
|
||||
for path, pos, euler_rot in board_configs:
|
||||
create_textured_board(stage, path, board_w, board_h, pos, euler_rot, usd_material)
|
||||
|
||||
camera = Camera(prim_path="/World/Workshop/CalibrationCamera", position=np.array([-4.0, 0.0, 3.5]), frequency=20,
|
||||
resolution=(1280, 720))
|
||||
camera.set_world_pose(orientation=np.array([0.7071, 0.0, 0.7071, 0.0]))
|
||||
camera.initialize()
|
||||
|
||||
keys = og.Controller.Keys
|
||||
og.Controller.edit({"graph_path": "/World/ROS2_Camera_Graph", "evaluator_name": "execution"},
|
||||
{keys.CREATE_NODES: [("OnTick", "omni.graph.action.OnTick"),
|
||||
("ROS2Camera", "omni.isaac.ros2_bridge.ROS2CameraHelper")],
|
||||
keys.CONNECT: [("OnTick.outputs:tick", "ROS2Camera.inputs:execIn")],
|
||||
keys.SET_VALUES: [("ROS2Camera.inputs:renderProductPath", camera.get_render_product_path()),
|
||||
("ROS2Camera.inputs:topicName", "/AutoCalib_Workshop/camera/image_raw"),
|
||||
("ROS2Camera.inputs:type", "rgb")]})
|
||||
|
||||
add_corner_rotary_lidars(room_length=L, room_width=W, height=H - 0.2, lidar_config="Example_Rotary",
|
||||
topic_prefix="/AutoCalib_Workshop/lidar")
|
||||
|
||||
return world
|
||||
|
||||
|
||||
def main():
|
||||
world = build_workshop()
|
||||
|
||||
world.reset()
|
||||
|
||||
set_camera_view(eye=np.array([-4.0, 0.0, 2.0]), target=np.array([5.0, 0.0, 3.5]))
|
||||
|
||||
print("======================================================")
|
||||
print(" 🎯 标定车间完美运行！纯锐利边缘棋盘格已加载完毕！")
|
||||
print(" ---------------------------------------------------")
|
||||
print(" 💡 标定算法所需的关键真值参数 (Ground Truth)：")
|
||||
print(" - 内部角点维度 (Pattern Size) : 8 x 5")
|
||||
print(" - 绝对物理边长 (Square Size) : 0.20 米 (20cm)")
|
||||
print("======================================================")
|
||||
|
||||
while simulation_app.is_running():
|
||||
world.step(render=True)
|
||||
|
||||
simulation_app.close()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user