From 6593ab0e67a7a34166b7125ff0bac52e710777d0 Mon Sep 17 00:00:00 2001 From: li-shihao-code <2469171725@qq.com> Date: Mon, 4 May 2026 11:13:59 +0800 Subject: [PATCH] =?UTF-8?q?feat:=E5=AE=8C=E5=96=84=E4=BB=BF=E7=9C=9F?= =?UTF-8?q?=E7=8E=AF=E5=A2=83?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../scripts/build_calibration_room.py | 189 ++++++++++++++++++ 1 file changed, 189 insertions(+) diff --git a/agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/build_calibration_room.py b/agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/build_calibration_room.py index cbc5aaa..65d5e29 100644 --- a/agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/build_calibration_room.py +++ b/agv_calib_brain/src/simulation/isaac_workshop_sim/scripts/build_calibration_room.py @@ -6,6 +6,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()