feat:完善仿真环境

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

@@ -6,6 +6,14 @@ import time
 import xml.etree.ElementTree as ET
 from pathlib import Path
 
+# TF 发布相关导入（用于发布 base_link 和传感器静态 TF）
+try:
+    from geometry_msgs.msg import TransformStamped
+    from tf2_ros import TransformBroadcaster, StaticTransformBroadcaster
+    TF_AVAILABLE = True
+except ImportError:
+    TF_AVAILABLE = False
+
 
 def parse_args():
     parser = argparse.ArgumentParser(description="Isaac 标定车间构建脚本")
@@ -100,6 +108,8 @@ def parse_args():
     parser.add_argument("--control-reference-speed-ms", type=float, default=0.3, help="运控遥测参考速度（m/s）")
     parser.add_argument("--control-reference-y-m", type=float, default=0.0, help="运控直线参考轨迹的 Y 坐标（米）")
     parser.add_argument("--control-parameter-version", type=str, default="isaac_control_baseline_v1", help="当前控制参数版本")
+    parser.add_argument("--disable-vehicle-tf", action="store_true", help="不发布车辆 TF 变换")
+    parser.add_argument("--vehicle-tf-publish-hz", type=float, default=50.0, help="车辆 TF 发布频率（Hz）")
     parser.add_argument("--disable-sensor-telemetry", action="store_true", help="不发布传感器标定遥测")
     parser.add_argument("--sensor-telemetry-topic", type=str, default="/sensor_calibration/telemetry", help="传感器标定遥测话题")
     parser.add_argument("--sensor-telemetry-hz", type=float, default=10.0, help="传感器标定遥测发布频率（Hz）")
@@ -1144,6 +1154,181 @@ class ExternalTruthTelemetryPublisher:
             self.node = None
 
 
+
+
+class VehicleTfPublisher:
+    """发布车辆 base_link 动态 TF 和传感器静态 TF"""
+
+    def __init__(self, args):
+        self.enabled = TF_AVAILABLE and not getattr(args, 'disable_vehicle_tf', False)
+        self.tf_publish_hz = getattr(args, 'vehicle_tf_publish_hz', 50.0)
+        self.publish_period_sec = 0.0 if self.tf_publish_hz <= 0.0 else 1.0 / self.tf_publish_hz
+        self.last_publish_time = 0.0
+        self.node = None
+        self.tf_broadcaster = None
+        self.static_tf_broadcaster = None
+        self.static_tfs_published = False
+
+        # 传感器相对于 base_link 的安装位置（从 args 读取）
+        self.sensor_configs = {
+            'front_camera_link': {
+                '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,
+                'enabled': not args.disable_vehicle_camera,
+            },
+            'down_camera_link': {
+                'x': args.down_camera_x,
+                'y': args.down_camera_y,
+                'z': args.down_camera_z,
+                'roll': 0.0,
+                'pitch': 0.0,  # 朝下相机，垂直向下
+                'yaw': 0.0,
+                'enabled': not args.disable_down_camera,
+            },
+            'lidar_3d_link': {
+                'x': args.vehicle_lidar_x,
+                'y': args.vehicle_lidar_y,
+                'z': args.vehicle_lidar_z,
+                'roll': 0.0,
+                'pitch': 0.0,
+                'yaw': 0.0,
+                'enabled': not args.disable_vehicle_lidar,
+            },
+            'lidar_2d_link': {
+                'x': args.vehicle_2d_lidar_x,
+                'y': args.vehicle_2d_lidar_y,
+                'z': args.vehicle_2d_lidar_z,
+                'roll': 0.0,
+                'pitch': 0.0,
+                'yaw': 0.0,
+                'enabled': not args.disable_vehicle_2d_lidar,
+            },
+            'imu_link': {
+                'x': args.vehicle_imu_x,
+                'y': args.vehicle_imu_y,
+                'z': args.vehicle_imu_z,
+                'roll': 0.0,
+                'pitch': 0.0,
+                'yaw': 0.0,
+                'enabled': not args.disable_vehicle_imu,
+            },
+        }
+
+        if not self.enabled:
+            print("[*] 车辆 TF 发布已禁用（TF_AVAILABLE={}）或参数禁用。".format(TF_AVAILABLE))
+            return
+
+        if rclpy is None:
+            print("[WARN] rclpy 不可用，车辆 TF 发布已禁用。")
+            self.enabled = False
+            return
+
+        if not rclpy.ok():
+            rclpy.init(args=None)
+
+        self.node = rclpy.create_node("isaac_vehicle_tf_publisher")
+        self.tf_broadcaster = TransformBroadcaster(self.node)
+        self.static_tf_broadcaster = StaticTransformBroadcaster(self.node)
+        print(f"[*] 车辆 TF 发布已启用: hz={self.tf_publish_hz}")
+
+    def _create_transform(self, frame_id, child_frame_id, x, y, z, roll, pitch, yaw, timestamp=None):
+        """创建 TransformStamped 消息"""
+        t = TransformStamped()
+        if timestamp is None:
+            timestamp = self.node.get_clock().now().to_msg()
+        t.header.stamp = timestamp
+        t.header.frame_id = frame_id
+        t.child_frame_id = child_frame_id
+        t.transform.translation.x = x
+        t.transform.translation.y = y
+        t.transform.translation.z = z
+
+        # 欧拉角转四元数
+        cy = math.cos(yaw * 0.5)
+        sy = math.sin(yaw * 0.5)
+        cp = math.cos(pitch * 0.5)
+        sp = math.sin(pitch * 0.5)
+        cr = math.cos(roll * 0.5)
+        sr = math.sin(roll * 0.5)
+
+        t.transform.rotation.w = cr * cp * cy + sr * sp * sy
+        t.transform.rotation.x = sr * cp * cy - cr * sp * sy
+        t.transform.rotation.y = cr * sp * cy + sr * cp * sy
+        t.transform.rotation.z = cr * cp * sy - sr * sp * cy
+
+        return t
+
+    def publish_static_tfs(self):
+        """发布传感器相对于 base_link 的静态 TF（只发布一次）"""
+        if not self.enabled or self.static_tfs_published:
+            return
+
+        transforms = []
+        timestamp = self.node.get_clock().now().to_msg()
+
+        for child_frame, config in self.sensor_configs.items():
+            if not config['enabled']:
+                continue
+            t = self._create_transform(
+                'base_link', child_frame,
+                config['x'], config['y'], config['z'],
+                config['roll'], config['pitch'], config['yaw'],
+                timestamp
+            )
+            transforms.append(t)
+            print(f"[*] 静态 TF: base_link -> {child_frame} ({config['x']:.3f}, {config['y']:.3f}, {config['z']:.3f})")
+
+        if transforms:
+            self.static_tf_broadcaster.sendTransform(transforms)
+            self.static_tfs_published = True
+            print(f"[*] 已发布 {len(transforms)} 个传感器静态 TF")
+
+    def publish(self, agv):
+        """发布 base_link 相对于 world 的动态 TF"""
+        if not self.enabled:
+            return
+
+        now = time.time()
+        if self.publish_period_sec > 0.0 and (now - self.last_publish_time) < self.publish_period_sec:
+            return
+
+        # 首先发布静态 TF（只发布一次）
+        if not self.static_tfs_published:
+            self.publish_static_tfs()
+
+        # 获取车辆世界位姿
+        position, quat = agv.get_world_pose()
+
+        # 四元数 (w, x, y, z) 转欧拉角（用于调试）
+        qw, qx, qy, qz = quat
+        yaw = math.atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz))
+
+        # 发布 world -> base_link 动态 TF
+        timestamp = self.node.get_clock().now().to_msg()
+        t = self._create_transform(
+            'world', 'base_link',
+            position[0], position[1], position[2],
+            0.0, 0.0, yaw,  # roll/pitch 设为 0，只保留 yaw
+            timestamp
+        )
+        # 使用原始四元数覆盖（更精确）
+        t.transform.rotation.w = qw
+        t.transform.rotation.x = qx
+        t.transform.rotation.y = qy
+        t.transform.rotation.z = qz
+
+        self.tf_broadcaster.sendTransform(t)
+        self.last_publish_time = now
+
+    def shutdown(self):
+        if self.node is not None:
+            self.node.destroy_node()
+            self.node = None
+
 class VehicleImuTopicPublisher:
     def __init__(self, args):
         self.enabled = not args.disable_vehicle_imu
@@ -2325,6 +2510,7 @@ class IsaacWorkshopRuntime:
 def main():
     runtime = IsaacWorkshopRuntime(ARGS)
     telemetry_publisher = ExternalTruthTelemetryPublisher(ARGS)
+    vehicle_tf_publisher = VehicleTfPublisher(ARGS)
     vehicle_imu_publisher = VehicleImuTopicPublisher(ARGS)
     chassis_telemetry_publisher = ChassisCalibrationTelemetryPublisher(ARGS)
     control_telemetry_publisher = ControlCalibrationTelemetryPublisher(ARGS)
@@ -2352,6 +2538,7 @@ def main():
     print(f"  - /cmd_vel topic: {runtime.cmd_vel_topic}")
     print(f"  - external telemetry topic: {ARGS.external_telemetry_topic}")
     print(f"  - chassis telemetry topic: {ARGS.chassis_telemetry_topic}")
+    print(f"  - vehicle TF: enabled={not ARGS.disable_vehicle_tf}, hz={ARGS.vehicle_tf_publish_hz}")
     print(f"  - control telemetry topic: {ARGS.control_telemetry_topic}")
     print(f"  - sensor telemetry topic: {ARGS.sensor_telemetry_topic}")
     print(f"  - scene manifest: {runtime.scene_manifest_path}")
@@ -2399,6 +2586,7 @@ def main():
 
             world.step(render=render_frame)
             telemetry_publisher.publish(agv)
+            vehicle_tf_publisher.publish(agv)
             vehicle_imu_publisher.publish(agv)
             vehicle_laser_scan_publisher.publish(agv)
             chassis_telemetry_publisher.publish(agv)
@@ -2406,6 +2594,7 @@ def main():
             sensor_telemetry_publisher.publish()
     finally:
         telemetry_publisher.shutdown()
+        vehicle_tf_publisher.shutdown()
         vehicle_imu_publisher.shutdown()
         vehicle_laser_scan_publisher.shutdown()
         chassis_telemetry_publisher.shutdown()