test_pointcloud.py

# 创建测试脚本

#!/usr/bin/env python
import rospy
import numpy as np
from sensor_msgs.msg import PointCloud2, PointField
import struct

def create_test_pointcloud():
    # 创建测试点云数据
    points = []
    for i in range(100):
        x = np.random.uniform(-10, 10)
        y = np.random.uniform(-10, 10) 
        z = np.random.uniform(0, 5)
        points.append([x, y, z])
    
    # 创建PointCloud2消息
    header = rospy.Header()
    header.stamp = rospy.Time.now()
    header.frame_id = "map"
    
    fields = [
        PointField('x', 0, PointField.FLOAT32, 1),
        PointField('y', 4, PointField.FLOAT32, 1),
        PointField('z', 8, PointField.FLOAT32, 1),
    ]
    
    cloud_data = []
    for p in points:
        cloud_data.append(struct.pack('fff', p[0], p[1], p[2]))
    
    cloud_msg = PointCloud2()
    cloud_msg.header = header
    cloud_msg.height = 1
    cloud_msg.width = len(points)
    cloud_msg.fields = fields
    cloud_msg.is_bigendian = False
    cloud_msg.point_step = 12
    cloud_msg.row_step = cloud_msg.point_step * cloud_msg.width
    cloud_msg.data = b''.join(cloud_data)
    cloud_msg.is_dense = True
    
    return cloud_msg

if __name__ == '__main__':
    rospy.init_node('test_pointcloud_publisher')
    
    pub1 = rospy.Publisher('/powerline_cloud_web', PointCloud2, queue_size=1)
    pub2 = rospy.Publisher('/environment_cloud_web', PointCloud2, queue_size=1)
    
    rate = rospy.Rate(1)  # 1Hz
    
    while not rospy.is_shutdown():
        msg = create_test_pointcloud()
        pub1.publish(msg)
        pub2.publish(msg)
        rospy.loginfo("发布测试点云数据")
        rate.sleep()