feat: 完善车间环境中......
This commit is contained in:
li-shihao-code
+170
-111
@@ -1,12 +1,10 @@
|
||||
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()
|
||||
|
||||
@@ -15,12 +13,15 @@ from PIL import Image
|
||||
import omni.kit.commands
|
||||
from pathlib import Path
|
||||
|
||||
# 【🔥核心防坑：只依赖底层基石 USD API】
|
||||
# 【底层基石 USD API】
|
||||
import omni.usd
|
||||
from pxr import Gf, Sdf, UsdShade, UsdGeom, Vt
|
||||
from pxr import Gf, Sdf, UsdShade, UsdGeom, Vt, PhysxSchema
|
||||
|
||||
from omni.isaac.core import World
|
||||
from omni.isaac.core.objects import FixedCuboid
|
||||
# 【新增引入】DynamicCuboid 用于生成受物理世界重力影响的动态刚体车辆
|
||||
from omni.isaac.core.objects import DynamicCuboid
|
||||
|
||||
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
|
||||
@@ -28,9 +29,16 @@ 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
|
||||
from omni.isaac.core.objects import VisualCuboid
|
||||
|
||||
# 【引入 URDF 导入器和机器人核心类】
|
||||
import omni.kit.commands
|
||||
from omni.importer.urdf import _urdf
|
||||
from omni.isaac.core.robots import Robot
|
||||
from omni.isaac.core.utils.stage import add_reference_to_stage
|
||||
|
||||
|
||||
def create_checkerboard_image(filepath="checkerboard.png", rows=6, cols=9, square_size_px=100):
|
||||
def create_checkerboard_image(filepath="checkerboard.png", 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
|
||||
@@ -38,11 +46,10 @@ def create_checkerboard_image(filepath="checkerboard.png", rows=6, cols=9, squar
|
||||
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
|
||||
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)
|
||||
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()
|
||||
@@ -51,85 +58,54 @@ def create_checkerboard_image(filepath="checkerboard.png", rows=6, cols=9, squar
|
||||
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"):
|
||||
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": "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))}
|
||||
{"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")
|
||||
]
|
||||
|
||||
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)
|
||||
quat = euler_angles_to_quat(np.array([0, 15.0, cfg['yaw']]), degrees=True)
|
||||
orientation = Gf.Quatd(quat[0], quat[1], quat[2], quat[3])
|
||||
|
||||
omni.kit.commands.execute(
|
||||
"IsaacSensorCreateRtxLidar", path=lidar_path, parent=None,
|
||||
config=lidar_config, translation=Gf.Vec3d(*cfg["pos"]), orientation=orientation
|
||||
)
|
||||
|
||||
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)
|
||||
(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})
|
||||
|
||||
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) # 非金属
|
||||
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")
|
||||
|
||||
@@ -138,79 +114,51 @@ def create_raw_usd_material(stage, mat_path, tex_path):
|
||||
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")
|
||||
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.GetPointsAttr().Set(Vt.Vec3fArray([Gf.Vec3f(-w, -h, 0), Gf.Vec3f( w, -h, 0), Gf.Vec3f( w, h, 0), Gf.Vec3f(-w, h, 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(-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))
|
||||
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)]
|
||||
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)})
|
||||
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"
|
||||
@@ -222,31 +170,105 @@ def build_workshop():
|
||||
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])
|
||||
("/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_z = H - 0.1
|
||||
camera = Camera(prim_path="/World/Workshop/CalibrationCamera", position=np.array([0.0, 0.0, camera_z]), frequency=20, resolution=(1280, 720))
|
||||
camera.set_world_pose(orientation=np.array([1.0, 0.0, 0.0, 0.0]))
|
||||
camera.initialize()
|
||||
camera.set_focal_length(5.0)
|
||||
|
||||
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.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")]})
|
||||
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")
|
||||
add_corner_rotary_lidars(room_length=L, room_width=W, height=H - 0.2, lidar_config="Example_Rotary", topic_prefix="/AutoCalib_Workshop/lidar")
|
||||
|
||||
|
||||
# ================= 【🚗核心新增 1：构建物理层 AGV 底盘】 =================
|
||||
# 主车体：带质量的物理刚体
|
||||
# world.scene.add(
|
||||
# DynamicCuboid(
|
||||
# prim_path="/World/Workshop/Vehicle", name="agv_vehicle",
|
||||
# position=np.array([0.0, 0.0, 0.2]), # 中心高度 20cm，完美贴地防穿模
|
||||
# scale=np.array([0.8, 0.5, 0.3]), # 车辆尺寸：长0.8m x 宽0.5m x 高0.3m
|
||||
# color=np.array([0.2, 0.6, 1.0]), # 亮蓝色车身
|
||||
# mass=50.0 # 赋予 50kg 的真实物理质量
|
||||
# )
|
||||
# )
|
||||
|
||||
# 车头指示器：红色方块，挂载在车头正前方，明确指示 +X 前进方向
|
||||
# VisualCuboid(
|
||||
# prim_path="/World/Workshop/Vehicle/DirectionMarker",
|
||||
# name="direction_marker",
|
||||
# position=np.array([0.4, 0.0, 0.0]), # 相对车身局部前移
|
||||
# scale=np.array([0.1, 0.51, 0.31]),
|
||||
# color=np.array([1.0, 0.0, 0.0]) # 直接通过内置的 color 参数设置
|
||||
# )
|
||||
|
||||
# ================= 【🚗核心新增 1：导入真实 URDF 替换基础方块】 =================
|
||||
urdf_file_path = "/home/nvidia/study/AutoCalib-Workshop/models/ack_m.urdf"
|
||||
# 🚨 新增：指定转换后的 USD 文件保存在哪里（必须带 .usd 后缀）
|
||||
dest_usd_path = "/home/nvidia/study/AutoCalib-Workshop/models/ack_m.usd"
|
||||
|
||||
dest_prim_path = "/World/Workshop/Vehicle"
|
||||
|
||||
# 1. 配置 URDF 导入参数
|
||||
import_config = _urdf.ImportConfig()
|
||||
import_config.merge_fixed_joints = True # 🚨 关键修改：改为 True！将雷达、相机和空节点合并进主车身
|
||||
import_config.convex_decomp = False
|
||||
import_config.fix_base = False
|
||||
import_config.make_default_prim = True
|
||||
|
||||
# 2. 将 URDF 解析并保存为本地的 USD 文件
|
||||
omni.kit.commands.execute(
|
||||
"URDFParseAndImportFile",
|
||||
urdf_path=urdf_file_path,
|
||||
import_config=import_config,
|
||||
dest_path=dest_usd_path # 传入的是硬盘文件路径
|
||||
)
|
||||
|
||||
# 3. 🔥 将硬盘上的 USD 文件作为引用（Reference）挂载到场景树中
|
||||
add_reference_to_stage(usd_path=dest_usd_path, prim_path=dest_prim_path)
|
||||
|
||||
# 4. 包装为 Robot 对象
|
||||
world.scene.add(
|
||||
Robot(
|
||||
prim_path=dest_prim_path,
|
||||
name="agv_vehicle",
|
||||
position=np.array([0.0, 0.0, 0.05])
|
||||
)
|
||||
)
|
||||
# ========================================================================================
|
||||
|
||||
# 强制关闭物理引擎对车辆的“休眠优化(Sleep)”，确保它随时能被指令叫醒移动
|
||||
physx_rb = PhysxSchema.PhysxRigidBodyAPI.Get(stage, "/World/Workshop/Vehicle")
|
||||
if physx_rb:
|
||||
physx_rb.GetSleepThresholdAttr().Set(0.0)
|
||||
|
||||
# ================= 【🚗核心新增 2：无缝底层 Twist 订阅图】 =================
|
||||
og.Controller.edit({"graph_path": "/World/ROS2_Twist_Graph", "evaluator_name": "execution"},
|
||||
{
|
||||
keys.CREATE_NODES: [
|
||||
("OnTick", "omni.graph.action.OnTick"),
|
||||
("TwistSub", "omni.isaac.ros2_bridge.ROS2SubscribeTwist"), # <--- 已修正
|
||||
],
|
||||
keys.CONNECT: [
|
||||
("OnTick.outputs:tick", "TwistSub.inputs:execIn"),
|
||||
],
|
||||
keys.SET_VALUES: [
|
||||
("TwistSub.inputs:topicName", "/cmd_vel"),
|
||||
]
|
||||
})
|
||||
# ========================================================================================
|
||||
|
||||
return world
|
||||
|
||||
@@ -256,21 +278,58 @@ def main():
|
||||
|
||||
world.reset()
|
||||
|
||||
set_camera_view(eye=np.array([-4.0, 0.0, 2.0]), target=np.array([5.0, 0.0, 3.5]))
|
||||
# 上帝视角的俯视监控
|
||||
set_camera_view(eye=np.array([0.001, 0.0, 3.4]), target=np.array([0.0, 0.0, 0.0]))
|
||||
|
||||
print("======================================================")
|
||||
print(" 🎯 标定车间完美运行！纯锐利边缘棋盘格已加载完毕！")
|
||||
print(" 🎯 标定车间完美运行！全向 AGV (蓝身红头) 已就绪！")
|
||||
print(" ---------------------------------------------------")
|
||||
print(" 💡 标定算法所需的关键真值参数 (Ground Truth)：")
|
||||
print(" - 内部角点维度 (Pattern Size) : 8 x 5")
|
||||
print(" - 绝对物理边长 (Square Size) : 0.20 米 (20cm)")
|
||||
print(" 🎮 车辆控制指南：请打开您的**原生终端** (不激活 conda)，输入:")
|
||||
print(" ros2 topic pub /cmd_vel geometry_msgs/msg/Twist \"{linear: {x: 0.5}, angular: {z: 0.8}}\"")
|
||||
print("======================================================")
|
||||
|
||||
agv = world.scene.get_object("agv_vehicle")
|
||||
|
||||
while simulation_app.is_running():
|
||||
# ================= 【🚗核心闭环：实时提取 Twist，转换物理运动学】 =================
|
||||
try:
|
||||
# 1. 每一帧从底层 ActionGraph 中拉取解包出来的 ROS2 速度指令
|
||||
lin_vel = og.Controller.get(og.Controller.attribute("/World/ROS2_Twist_Graph/TwistSub.outputs:linearVelocity"))
|
||||
ang_vel = og.Controller.get(og.Controller.attribute("/World/ROS2_Twist_Graph/TwistSub.outputs:angularVelocity"))
|
||||
|
||||
if lin_vel is not None and ang_vel is not None and len(lin_vel) == 3:
|
||||
# 2. 获取车辆在物理世界中实时的绝对姿态四元数 [w, x, y, z] 和 自然下落速度
|
||||
pos, quat = agv.get_world_pose()
|
||||
curr_lin_vel = agv.get_linear_velocity()
|
||||
|
||||
# 3. 构造 3D 四元数和旋转矩阵
|
||||
q = Gf.Quatd(float(quat[0]), float(quat[1]), float(quat[2]), float(quat[3]))
|
||||
rot_mat = Gf.Matrix3d(Gf.Rotation(q))
|
||||
|
||||
# 🚨 核心修复：USD (pxr.Gf) 中，向量与矩阵相乘直接使用 * 运算符（行向量右乘矩阵）
|
||||
world_lin_vel = Gf.Vec3d(*lin_vel) * rot_mat
|
||||
world_ang_vel = Gf.Vec3d(*ang_vel) * rot_mat
|
||||
|
||||
# 4. 物理防翻车约束
|
||||
target_lin_vel = np.array([world_lin_vel[0], world_lin_vel[1], curr_lin_vel[2]])
|
||||
target_ang_vel = np.array([0.0, 0.0, world_ang_vel[2]])
|
||||
|
||||
# 5. 直接对车辆物理质心施加强制推演覆盖
|
||||
agv.set_linear_velocity(target_lin_vel)
|
||||
agv.set_angular_velocity(target_ang_vel)
|
||||
|
||||
# Debug 日志：确认 ROS2 话题是否真正连通
|
||||
if abs(lin_vel[0]) > 0.01 or abs(ang_vel[2]) > 0.01:
|
||||
print(f"\r[ROS2 Debug] 车辆移动中: 线速度 {lin_vel[0]:.2f}, 角速度 {ang_vel[2]:.2f}", end="")
|
||||
|
||||
except Exception as e:
|
||||
# 🚨 永远不要用 pass 吞掉这里的报错
|
||||
print(f"\n[ERROR] 运动学控制循环异常: {e}")
|
||||
# ============================================================================================
|
||||
|
||||
world.step(render=True)
|
||||
|
||||
simulation_app.close()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,301 @@
|
||||
<?xml version="1.0" ?>
|
||||
<robot name="qykj_ackm">
|
||||
<link name="base_link">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.0174221592416108 0 0.0010674027636553"/>
|
||||
<mass value="2.0946205"/>
|
||||
<inertia ixx="0.00725227532564324" ixy="1.826574180869E-05" ixz="1.74522061015835E-05" iyy="0.0112392911749803" iyz="-1.16120716894918E-06" izz="0.0572118967643173"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/base_link.stl"/>
|
||||
</geometry>
|
||||
<material name="base_material">
|
||||
<color rgba="1 0 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/base_link.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<link name="base_footprint">
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<mass value="0.01"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<sphere radius="0.001"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name="base_link2base_footprint" type="fixed">
|
||||
<parent link="base_footprint"/>
|
||||
<child link="base_link"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.055"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_steering_hinge">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="0.085506993927856"/>
|
||||
<inertia ixx="0.00342027975711424" ixy="-1.72810689862337E-03" ixz="2.68361824483564E-03" iyy="0.00342027975711424" iyz="1.65092622583439E-04" izz="0.00342027975711424"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.17831 -0.10435 -0.0070129"/>
|
||||
<geometry>
|
||||
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/left_front_steer_link.stl"/>
|
||||
</geometry>
|
||||
<material name="steering_hinge_material">
|
||||
<color rgba="0 0 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.17831 -0.10435 -0.0070129"/>
|
||||
<geometry>
|
||||
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/left_front_steer_link.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="left_steering_hinge_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.17831 0.10435 0.0070129"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="left_steering_hinge"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="8" lower="-0.5" upper="0.5" velocity="800"/>
|
||||
<dynamics damping="0.5"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_steering_hinge">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="0.085506993927856"/>
|
||||
<inertia ixx="0.00342027975711424" ixy="-1.72810689862337E-03" ixz="2.68361824483564E-03" iyy="0.00342027975711424" iyz="1.65092622583439E-04" izz="0.00342027975711424"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.17831 0.10435 -0.0070129"/>
|
||||
<geometry>
|
||||
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/right_front_steer_link.stl"/>
|
||||
</geometry>
|
||||
<material name="steering_hinge_material">
|
||||
<color rgba="0 0 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.17831 0.10435 -0.0070129"/>
|
||||
<geometry>
|
||||
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/right_front_steer_link.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="right_steering_hinge_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.17831 -0.10435 0.0070129"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="right_steering_hinge"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="8" lower="-0.5" upper="0.5" velocity="800"/>
|
||||
<dynamics damping="0.5"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_wheel">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 -0.01 0"/>
|
||||
<mass value="0.083615304385799"/>
|
||||
<inertia ixx="0.00010451913048224875" ixy="-1.02836030755788E-5" ixz="1.03858733509779E-10" iyy="0.000261297820954496875" iyz="-3.52038618447715E-09" izz="0.00010451913048224875"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.17831 -0.17835 -0.0070129"/>
|
||||
<geometry>
|
||||
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/left_front_wheel_link.stl"/>
|
||||
</geometry>
|
||||
<material name="wheel_material">
|
||||
<color rgba="0 0 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.17831 -0.17835 -0.0070129"/>
|
||||
<geometry>
|
||||
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/left_front_wheel_link.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="left_wheel_joint" type="continuous">
|
||||
<origin rpy="0 0 0" xyz="0 0.074 0"/>
|
||||
<parent link="left_steering_hinge"/>
|
||||
<child link="left_wheel"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="6" velocity="800"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_wheel">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0.01 0"/>
|
||||
<mass value="0.083615304385799"/>
|
||||
<inertia ixx="0.00010451913048224875" ixy="-1.02836030755788E-5" ixz="1.03858733509779E-10" iyy="0.000261297820954496875" iyz="-3.52038618447715E-09" izz="0.00010451913048224875"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.17831 0.17835 -0.0070129"/>
|
||||
<geometry>
|
||||
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/right_front_wheel_link.stl"/>
|
||||
</geometry>
|
||||
<material name="wheel_material">
|
||||
<color rgba="0 0 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.17831 0.17835 -0.0070129"/>
|
||||
<geometry>
|
||||
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/right_front_wheel_link.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="right_wheel_joint" type="continuous">
|
||||
<origin rpy="0 0 0" xyz="0 -0.074 0"/>
|
||||
<parent link="right_steering_hinge"/>
|
||||
<child link="right_wheel"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="6" velocity="800"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_rear_wheel">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 -0.01 0"/>
|
||||
<mass value="0.083615304385799"/>
|
||||
<inertia ixx="0.000645041181644452" ixy="-1.02836030755788E-5" ixz="-1.03858733509779E-10" iyy="0.00617238444547154" iyz="3.52038618447715E-09" izz="0.00064504895313491"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0.11072 -0.18046 -0.0099082"/>
|
||||
<geometry>
|
||||
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/left_back_wheel_link.stl"/>
|
||||
</geometry>
|
||||
<material name="wheel_material">
|
||||
<color rgba="0 0 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0.11072 -0.18046 -0.0099082"/>
|
||||
<geometry>
|
||||
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/left_back_wheel_link.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="left_rear_wheel_joint" type="continuous">
|
||||
<origin rpy="0 0 0" xyz="-0.11072 0.18046 0.0099082"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="left_rear_wheel"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="10" lower="-3.14159" upper="3.14159" velocity="800"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_rear_wheel">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0.01 0"/>
|
||||
<mass value="0.083615304385799"/>
|
||||
<inertia ixx="0.000645041181644452" ixy="-1.02836030755788E-5" ixz="-1.03858733509779E-10" iyy="0.00617238444547154" iyz="3.52038618447715E-09" izz="0.00064504895313491"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0.11072 0.18046 -0.0099082"/>
|
||||
<geometry>
|
||||
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/right_back_wheel_link.stl"/>
|
||||
</geometry>
|
||||
<material name="wheel_material">
|
||||
<color rgba="0 0 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0.11072 0.18046 -0.0099082"/>
|
||||
<geometry>
|
||||
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/right_back_wheel_link.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="right_rear_wheel_joint" type="continuous">
|
||||
<origin rpy="0 0 0" xyz="-0.11072 -0.18046 0.0099082"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="right_rear_wheel"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="10" lower="-3.14159" upper="3.14159" velocity="800"/>
|
||||
</joint>
|
||||
|
||||
<link name="camera">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="0.128875262431144"/>
|
||||
<inertia ixx="0.000226098241375409" ixy="-7.63151331329159E-09" ixz="-6.02732800767945E-07" iyy="1.50549325275966E-05" iyz="-7.62743002721805E-10" izz="0.000225285047549157"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.21669 0.00063349 -0.142"/>
|
||||
<geometry>
|
||||
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/camera.stl"/>
|
||||
</geometry>
|
||||
<material name="camera_material">
|
||||
<color rgba="0 0 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.21669 0.00063349 -0.142"/>
|
||||
<geometry>
|
||||
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/camera.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="camera_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0.21669 -0.00063349 0.142"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="camera"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="laser">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="0.136175408631878"/>
|
||||
<inertia ixx="5.05126026373552E-05" ixy="1.31150574294136E-08" ixz="1.65013289478E-06" iyy="7.08926003407945E-05" iyz="2.49898507713873E-09" izz="0.00010333009695655"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="-0.092303 -5.3876E-05 -0.2031"/>
|
||||
<geometry>
|
||||
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/laser_link.stl"/>
|
||||
</geometry>
|
||||
<material name="sensor_material">
|
||||
<color rgba="0 0 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.092303 -5.3876E-05 -0.2031"/>
|
||||
<geometry>
|
||||
<mesh filename="/home/nvidia/study/AutoCalib-Workshop/models/meshes/laser_link.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="laser_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0.092303 5.3876E-05 0.2031"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="laser"/>
|
||||
<axis xyz="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="imu">
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<mass value="0.005"/>
|
||||
<inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.005 0.005 0.007"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name="imu_joints" type="fixed">
|
||||
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0035"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="imu"/>
|
||||
</joint>
|
||||
</robot>
|
||||
Binary file not shown.
Executable
+277
@@ -0,0 +1,277 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="qykj_ackm">
|
||||
<!-- set param -->
|
||||
<xacro:property name="base_color" value="1 0 0 1" />
|
||||
<xacro:property name="wheel_color" value="0 0 0 1" />
|
||||
<xacro:property name="sensor_color" value="0 0 1 1" />
|
||||
<xacro:property name="camera_color" value="0 0 1 1" />
|
||||
<!-- Define meshes -->
|
||||
<xacro:property name="base_mesh" value="./meshes/base_link.STL" />
|
||||
<xacro:property name="left_wheel_mesh" value="./meshes/left_wheel.STL" />
|
||||
<xacro:property name="left_rear_wheel_mesh" value="./meshes/left_rear_wheel.STL" />
|
||||
<xacro:property name="right_wheel_mesh" value="./meshes/right_wheel.STL" />
|
||||
<xacro:property name="right_rear_wheel_mesh" value="./meshes/right_rear_wheel.STL" />
|
||||
<xacro:property name="laser_mesh" value="./meshes/laser.STL" />
|
||||
<xacro:property name="camera_mesh" value="./meshes/camera.STL" />
|
||||
|
||||
<!-- base_link and base_footprint -->
|
||||
<xacro:macro name="base_link" params="mesh color">
|
||||
<link name="base_link">
|
||||
<inertial>
|
||||
<origin xyz="-0.0174221592416108 0 0.0010674027636553" rpy="0 0 0" />
|
||||
<!-- <mass value="3.18924071684633" /> -->
|
||||
<mass value="2.0946205" />
|
||||
<inertia ixx="0.00725227532564324" ixy="1.826574180869E-05" ixz="1.74522061015835E-05" iyy="0.0112392911749803" iyz="-1.16120716894918E-06" izz="0.0572118967643173" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="${mesh}" />
|
||||
</geometry>
|
||||
<material name="base_material">
|
||||
<color rgba="${color}" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="${mesh}" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<!-- Base footprint -->
|
||||
<link name="base_footprint">
|
||||
<visual>
|
||||
<geometry>
|
||||
<sphere radius="0.001" />
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name="base_link2base_footprint" type="fixed">
|
||||
<parent link="base_footprint" />
|
||||
<child link="base_link" />
|
||||
<origin xyz="0 0 0.055" rpy="0 0 0" />
|
||||
</joint>
|
||||
</xacro:macro>
|
||||
|
||||
<!-- front wheel steering -->
|
||||
<xacro:macro name="steering_hinge" params="prefix color mass_center position rotation">
|
||||
<link name="${prefix}_steering_hinge">
|
||||
<inertial>
|
||||
<origin xyz="${mass_center}" rpy="0 0 0" />
|
||||
<mass value="0.085506993927856" />
|
||||
<inertia ixx="0.00342027975711424"
|
||||
ixy="-1.72810689862337E-03"
|
||||
ixz="2.68361824483564E-03"
|
||||
iyy="0.00342027975711424"
|
||||
iyz="1.65092622583439E-04"
|
||||
izz="0.00342027975711424" />
|
||||
<!-- <inertia ixx="0.00000000342027975711424" ixy="-1.72810689862337E-07" ixz="2.68361824483564E-07" iyy="0.0000000342027975711424" iyz="1.65092622583439E-08" izz="0.0000000342027975711424" /> -->
|
||||
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<sphere radius="0.005"/> <!-- 尝试增大半径 -->
|
||||
</geometry>
|
||||
<material name="steering_hinge_material">
|
||||
<color rgba="${color}" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<sphere radius="0.005"/> <!-- 尝试增大半径 -->
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="${prefix}_steering_hinge_joint" type="revolute">
|
||||
<origin xyz="${position}" rpy="${rotation}"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="${prefix}_steering_hinge"/>
|
||||
<axis xyz="0 0 1" />
|
||||
<limit lower="-0.5" upper="0.5" effort="8" velocity="800" />
|
||||
<damping>0.5</damping> <!-- 增加阻尼 -->
|
||||
</joint>
|
||||
</xacro:macro>
|
||||
|
||||
<!-- front wheel -->
|
||||
<xacro:macro name="wheel" params="prefix mesh mass_center color position">
|
||||
<link name="${prefix}_wheel">
|
||||
<inertial>
|
||||
<origin xyz="${mass_center}" rpy="0 0 0" />
|
||||
<mass value="0.083615304385799" />
|
||||
<!--qian <inertia ixx="0.00034504118176367" ixy="-1.05210123445392E-10" ixz="1.09117751056064E-10" iyy="0.000617238444498718" iyz="-3.52033611009814E-09" izz="0.00345048952965297" /> -->
|
||||
<!-- <inertia ixx="0.000645041181644452" ixy="-1.02836030755788E-5" ixz="1.03858733509779E-10" iyy="0.00617238444547154" iyz="-3.52038618447715E-09" izz="0.00064504895313491" /> -->
|
||||
<inertia ixx="0.00010451913048224875"
|
||||
ixy="-1.02836030755788E-5"
|
||||
ixz="1.03858733509779E-10"
|
||||
iyy="0.000261297820954496875"
|
||||
iyz="-3.52038618447715E-09"
|
||||
izz="0.00010451913048224875" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="${mesh}" />
|
||||
</geometry>
|
||||
<material name="wheel_material">
|
||||
<color rgba="${color}" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="${mesh}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="${prefix}_wheel_joint" type="continuous">
|
||||
<origin xyz="${position}" rpy="0 0 0" />
|
||||
<parent link="${prefix}_steering_hinge" />
|
||||
<child link="${prefix}_wheel" />
|
||||
<axis xyz="0 1 0" />
|
||||
<limit effort="6" velocity="800" />
|
||||
<!-- <limit lower="-3.14159" upper="3.14159" effort="8" velocity="800" /> -->
|
||||
</joint>
|
||||
</xacro:macro>
|
||||
|
||||
<!-- rear wheel -->
|
||||
<xacro:macro name="rear_wheel" params="prefix mesh mass_center color position">
|
||||
<link name="${prefix}_rear_wheel">
|
||||
<inertial>
|
||||
<origin xyz="${mass_center}" rpy="0 0 0" />
|
||||
<mass value="0.083615304385799" />
|
||||
<inertia ixx="0.000645041181644452" ixy="-1.02836030755788E-5" ixz="-1.03858733509779E-10" iyy="0.00617238444547154" iyz="3.52038618447715E-09" izz="0.00064504895313491" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="${mesh}" />
|
||||
</geometry>
|
||||
<material name="wheel_material">
|
||||
<color rgba="${color}" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="${mesh}" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="${prefix}_rear_wheel_joint" type="continuous">
|
||||
<origin xyz="${position}" rpy="0 0 0" />
|
||||
<parent link="base_link" />
|
||||
<child link="${prefix}_rear_wheel" />
|
||||
<axis xyz="0 1 0" />
|
||||
<limit lower="-3.14159" upper="3.14159" effort="10" velocity="800" />
|
||||
</joint>
|
||||
</xacro:macro>
|
||||
|
||||
<!-- laser -->
|
||||
<xacro:macro name="laser" params="name mesh color position">
|
||||
<link name="${name}">
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="0.136175408631878" />
|
||||
<inertia ixx="5.05126026373552E-05" ixy="1.31150574294136E-08" ixz="1.65013289478E-06" iyy="7.08926003407945E-05" iyz="2.49898507713873E-09" izz="0.00010333009695655" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="${mesh}" />
|
||||
</geometry>
|
||||
<material name="sensor_material">
|
||||
<color rgba="${color}" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="${mesh}" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="${name}_joint" type="fixed">
|
||||
<origin xyz="${position}" rpy="0 0 0" />
|
||||
<parent link="base_link" />
|
||||
<child link="${name}" />
|
||||
<axis xyz="0 0 0" />
|
||||
</joint>
|
||||
</xacro:macro>
|
||||
|
||||
<!-- Camera -->
|
||||
<xacro:macro name="camera" params="name mesh color position">
|
||||
<link name="${name}">
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="0.128875262431144" />
|
||||
<inertia ixx="0.000226098241375409" ixy="-7.63151331329159E-09" ixz="-6.02732800767945E-07" iyy="1.50549325275966E-05" iyz="-7.62743002721805E-10" izz="0.000225285047549157" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="${mesh}" />
|
||||
</geometry>
|
||||
<material name="camera_material">
|
||||
<color rgba="${color}" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="${mesh}" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="${name}_joint" type="fixed">
|
||||
<origin xyz="${position}" rpy="0 0 0" />
|
||||
<parent link="base_link" />
|
||||
<child link="${name}" />
|
||||
<axis xyz="0 0 0" />
|
||||
</joint>
|
||||
</xacro:macro>
|
||||
|
||||
<!-- add imu link -->
|
||||
<xacro:macro name="imu" params="name">
|
||||
<link name="${name}">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.005 0.005 0.007"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name ="imu_joints" type="fixed">
|
||||
<origin xyz="0.0 0.0 0.0035" rpy="0.0 0.0 0.0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="${name}"/>
|
||||
</joint>
|
||||
</xacro:macro>
|
||||
|
||||
<!-- create car body -->
|
||||
<xacro:base_link mesh="${base_mesh}" color="${base_color}" />
|
||||
|
||||
<!-- create front wheel steering -->
|
||||
<xacro:steering_hinge prefix="left" color="${wheel_color}" mass_center="0 0 0" position="0.17831 0.10435 0.0070129" rotation="0 0 0" />
|
||||
<xacro:steering_hinge prefix="right" color="${wheel_color}" mass_center="0 0 0" position="0.17831 -0.10435 0.0070129" rotation="0 0 0" />
|
||||
|
||||
<!-- create front wheel -->
|
||||
<xacro:wheel prefix="left" mesh="${left_wheel_mesh}" mass_center="0 -0.01 0" color="${wheel_color}" position="0 0.074 0" />
|
||||
<xacro:wheel prefix="right" mesh="${right_wheel_mesh}" mass_center="0 0.01 0" color="${wheel_color}" position="0 -0.074 0" />
|
||||
|
||||
<!-- create rear wheel -->
|
||||
<xacro:rear_wheel prefix="left" mesh="${left_rear_wheel_mesh}" mass_center="0 -0.01 0" color="${wheel_color}" position="-0.11072 0.18046 0.0099082" />
|
||||
<xacro:rear_wheel prefix="right" mesh="${right_rear_wheel_mesh}" mass_center="0 0.01 0" color="${wheel_color}" position="-0.11072 -0.18046 0.0099082" />
|
||||
|
||||
<!-- create camera -->
|
||||
<xacro:camera name="camera" mesh="${camera_mesh}" color="${camera_color}" position="0.21669 -0.00063349 0.142" />
|
||||
|
||||
<!-- create laser -->
|
||||
<xacro:laser name="laser" mesh="${laser_mesh}" color="${sensor_color}" position="0.092303 5.3876E-05 0.2031" />
|
||||
|
||||
<!-- imu -->
|
||||
<xacro:imu name="imu"/>
|
||||
|
||||
</robot>
|
||||
Executable
BIN
Binary file not shown.
Executable
BIN
Binary file not shown.
Executable
BIN
Binary file not shown.
Executable
BIN
Binary file not shown.
Executable
BIN
Binary file not shown.
Executable
BIN
Binary file not shown.
Executable
BIN
Binary file not shown.
Executable
BIN
Binary file not shown.
Executable
BIN
Binary file not shown.
Executable
BIN
Binary file not shown.
Reference in New Issue
Block a user