rospy.init_node("lidarSender",anonymous=True)
pointcloud_publish = rospy.Publisher('/velodyne_points', PointCloud2, queue_size=10)
flag = -1
ref_lat = 0
ref_lon = 0
ref_alt = 0
C_EARTH = 6378137
def publish_pointcloud(file_name,t):
print(file_name)
Fieldx = PointField('x', 0, 7, 1) # field name, offset, datatype(float32), count
Fieldy = PointField('y', 4, 7, 1) # field name, offset, datatype(float32), count
Fieldz = PointField('z', 8, 7, 1) # field name, offset, datatype(float32), count
Fieldi = PointField('intensity', 16, 2, 1) # field name, offset, datatype(uint8), count
Fieldt = PointField('timestamp', 24, 8, 1) # field name, offset, datatype(float64), count
#Fieldr = PointField('ring', 28, 2, 1)
#Fieldl = PointField('label', 32, 2, 1)
pointField = [Fieldx, Fieldy, Fieldz, Fieldi, Fieldt]
data = np.loadtxt(file_name)
header = Header(stamp = t)
header.frame_id = 'map'
sensor_msg_pointcloud = point_cloud2.create_cloud(header=header, fields=pointField, points=data)
pointcloud_publish.publish(sensor_msg_pointcloud).
While it responsed with this.
jinxiao@jinxiao:~/YU/catkin_20190118$ rosrun points_preprocessor_usi groundplanfit
[ INFO] [1547882021.742146913]: Inititalizing Ground Plane Fitter...
[ INFO] [1547882021.748947743]: Input Point Cloud: /velodyne_points
[ INFO] [1547882021.755725621]: Sensor Model: 32
[ INFO] [1547882021.762239499]: Sensor Height: 2.500000
[ INFO] [1547882021.769587839]: Num of Segments: 1
[ INFO] [1547882021.776253835]: Num of Iteration: 3
[ INFO] [1547882021.782429419]: Num of LPR: 20
[ INFO] [1547882021.788627200]: Seeds Threshold: 1.200000
[ INFO] [1547882021.794751334]: Distance Threshold: 0.300000
[ INFO] [1547882021.883559641]: No Ground Output Point Cloud: /points_no_ground
[ INFO] [1547882021.889763761]: Only Ground Output Point Cloud: /points_ground
Failed to find match for field 'intensity'.
Failed to find match for field 'ring'.
Failed to find match for field 'intensity'.
Failed to find match for field 'ring'.
Failed to find match for field 'intensity'.
Failed to find match for field 'ring'.
Failed to find match for field 'intensity'.
Failed to find match for field 'ring'.
Failed to find match for field 'intensity'.
Failed to find match for field 'ring'.