用单线雷达模拟多线雷达，复制单层点云改变z轴信息为多层点云

单线雷达数据
模拟效果

#!/usr/bin/env python
# coding:utf-8

import rospy
from sensor_msgs.msg import PointCloud2
from sensor_msgs import point_cloud2 
from sensor_msgs.msg import PointField

import sensor_msgs.point_cloud2 as pc2


def callback_pointcloud(data):
    assert isinstance(data, PointCloud2)
    gen = point_cloud2.read_points(data)
    #print type(gen)
    #for p in gen:
    #    print p
    #    print (p[0],p[1],p[2])
    #    print "---------s"
    layer,length,cp_arr=copy_pointcloud(gen,16,0.1)
    publish_msg(layer,length,cp_arr)

def copy_pointcloud(data,layer,zdis):
        ss=[]
        for p in data:
            ss.append((p[0],p[1],p[2]))

        ps=[]
        for i in range (layer):
            h=zdis*i
            #pc=[]
            #print (i, h)
            for p in ss:
                ps.append((p[0],p[1],p[2]+h))                
                pass
            #ps.append(pc)
        return layer,len(ss),ps

def publish_msg(layer,length,points):
        msg = PointCloud2()
        
        msg.header.stamp = rospy.Time().now()
        msg.header.frame_id = "map"
        

        '''
        msg.fields = [
            PointField('x', 0, PointField.FLOAT32, 1),
            PointField('y', 4, PointField.FLOAT32, 1),
            PointField('z', 8, PointField.FLOAT32, 1)]
        msg.is_bigendian = False
        msg.point_step = 12
        msg.row_step = msg.point_step * length
        # msg.is_dense = int(np.isfinite(points).all())
        msg.is_dense = False
        msg.data =points #np.asarray(points, np.float32).tostring()
        '''
        msg = pc2.create_cloud_xyz32(msg.header, points)
        msg.point_step = 12
        msg.row_step = msg.point_step * length
        msg.height = layer
        msg.width = len(points)/layer
        print msg.fields
        print (msg.width,msg.height)
        #pub_cloud.publish(pcloud)


        rospy.Publisher('points_raw_fake', PointCloud2, queue_size=1).publish(msg)
        #pub.publish(msg)

def listener():
    rospy.init_node('fake_lidar', anonymous=True)
    rospy.Subscriber('/scan_matched_points2', PointCloud2, callback_pointcloud)
    rospy.spin()

if __name__ == '__main__':
    listener()