#!/usr/bin/env python3

import rospy

import numpy as np

from sensor_msgs.msg import PointCloud2, PointField

from sensor_msgs import point_cloud2

from geometry_msgs.msg import PoseStamped

import tf.transformations as tf_trans

# 全局变量：缓存最新的真值位姿（位置+姿态）

latest_pose_gt = None

latest_pose_time = None

def pose_gt_callback(pose_msg):

“””订阅真值位姿的回调函数，缓存最新位姿”””

global latest_pose_gt, latest_pose_time

latest_pose_gt = pose_msg

latest_pose_time = pose_msg.header.stamp

def lidar_to_world(cloud_msg):

“””将lidar坐标系点云转换到世界坐标系（手动计算，无TF）”””

global latest_pose_gt, latest_pose_time

# 检查是否有可用的真值位姿

if latest_pose_gt is None:

rospy.logwarn(“No ground truth pose available!”)

return None

# 匹配点云和位姿的时间（允许100ms内的时间差）

time_diff = abs((cloud_msg.header.stamp – latest_pose_time).to_sec())

if time_diff > 0.1:

rospy.logwarn(f”Pose and cloud time mismatch: {time_diff}s”)

# 仍然使用最新位姿（比赛场景下可接受）

# 1. 提取真值位姿的位置和姿态

# 位置（x,y,z）

px = latest_pose_gt.pose.position.x

py = latest_pose_gt.pose.position.y

pz = latest_pose_gt.pose.position.z

# 四元数（x,y,z,w）

qx = latest_pose_gt.pose.orientation.x

qy = latest_pose_gt.pose.orientation.y

qz = latest_pose_gt.pose.orientation.z

qw = latest_pose_gt.pose.orientation.w

# 2. 构建旋转矩阵（四元数→旋转矩阵）

rot_mat = tf_trans.quaternion_matrix([qx, qy, qz, qw])[:3, :3]

# 3. 解析原始点云数据

# 提取点云的每个点（x,y,z）

gen = point_cloud2.read_points(cloud_msg, field_names=(“x”, “y”, “z”), skip_nans=True)

points_lidar = np.array(list(gen)).T # 形状：(3, N)

if points_lidar.size == 0:

rospy.logwarn(“Empty point cloud!”)

return None

# 4. 坐标变换：世界坐标 = 旋转矩阵×雷达坐标 + 平移向量

# 旋转：雷达坐标系→无人机本体坐标系

points_body = rot_mat @ points_lidar

# 平移：无人机本体坐标系→世界坐标系

points_world = points_body + np.array([[px], [py], [pz]])

# 5. 构建世界坐标系下的新点云

# 复制原始点云的header和结构，只替换点数据

cloud_world = PointCloud2()

cloud_world.header = cloud_msg.header

cloud_world.header.frame_id = “world” # 标记为世界坐标系

cloud_world.height = cloud_msg.height

cloud_world.width = cloud_msg.width

cloud_world.fields = cloud_msg.fields

cloud_world.is_bigendian = cloud_msg.is_bigendian

cloud_world.point_step = cloud_msg.point_step

cloud_world.row_step = cloud_msg.row_step

cloud_world.is_dense = cloud_msg.is_dense

# 将变换后的点重新打包成PointCloud2格式

points_list = []

for i in range(points_world.shape[1]):

x = points_world[0, i]

y = points_world[1, i]

z = points_world[2, i]

# 保留原始点云的其他字段（如intensity），这里只处理x/y/z

# 如果你的点云有其他字段，需要补充

points_list.append((x, y, z))

# 生成最终的点云数据

cloud_world.data = point_cloud2.create_cloud(

cloud_world.header,

cloud_msg.fields,

points_list

).data

return cloud_world

def lidar_callback(cloud_msg, pub):

“””订阅原始点云的回调函数，转换后发布”””

cloud_world = lidar_to_world(cloud_msg)

if cloud_world is not None:

pub.publish(cloud_world)

if __name__ == ‘__main__’:

# 初始化ROS节点

rospy.init_node(‘lidar_to_world_with_pose_gt’, anonymous=True)

# 发布转换后的世界坐标系点云

pub_world_lidar = rospy.Publisher(

‘/drone_1/lidar_world’, # 新的点云话题名

PointCloud2,

queue_size=10

)

# 订阅真值位姿

rospy.Subscriber(

‘/airsim_node/drone_1/debug/pose_gt’,

PoseStamped,

pose_gt_callback,

queue_size=10

)

# 订阅原始lidar点云（frame_id=lidar），并传入发布器

rospy.Subscriber(

‘/airsim_node/drone_1/lidar’, # 替换为你的原始lidar话题

PointCloud2,

lambda msg: lidar_callback(msg, pub_world_lidar),

queue_size=10

)

rospy.loginfo(“Lidar to world converter (with pose GT) started!”)

rospy.spin()