rospy subscriber delay, not giving the latest msg
I've been trying to use rospy.subscribe to pointcloud2 msg, but I found that the subscriber are not giving the latest data as the output:
The second recieved data's time stamp is before the previous published data's time stamp, which is not expected. It seems that even I set the queue_size to 1 and buffer size to a much bigger one than the data size, there is still lots of data in the buffer which prevents reading the latest data.
My code is attached here, I did a point cloud clustering which is around 4hz while the pointcloud2 recieving rate is around 5hz. The clustering process took place in the callback function but it keeps processing the old data.
class point_cloud_subsriber():
def point_cloud_cb(self, msg):
self.width = msg.width
self.height = msg.height
self.source_frame= msg.header.frame_id
self.recieve_time = msg.header.stamp
rospy.loginfo("time stamp of recieved pointcloud2 is : %s and %s", str(msg.header.stamp.secs), str(msg.header.stamp.nsecs))
rospy.loginfo ("time now is: %s and %s ", str(rospy.Time.now().secs), str(rospy.Time.now().nsecs))
points = []
for data in pc2.read_points(msg,
field_names=('x', 'y', 'z', 'rgb'),
skip_nans=True):
s=struct.pack('>f', data[3])
i = struct.unpack('>l',s)[0]
pack = ctypes.c_uint32(i).value
if data[2] > -0.2 and data[0] < 10:
points.append([data[0], data[1], data[2]])
X = np.array(points)
kmeans = KMeans(n_clusters=1, random_state=0).fit(X)
labels = kmeans.labels_
centroids = kmeans.cluster_centers_
ds = X[np.where(labels==0)]
print("cluster centroid: ", centroids)
yellow = struct.unpack('I', struct.pack('BBBB', 0, 255, 255, 255))[0]
red = struct.unpack('I', struct.pack('BBBB', 0, 0, 180, 255))[0]
new_ds = np.insert(ds, 3, red, axis =1 )
out_cloud = new_ds
self.point_cloud_pub.publish(self.convert_to_pointcloud2(out_cloud.tolist(), self.source_frame))
def convert_to_pointcloud2(self, data,target_frame):
self.lock = FALSE
for per_list in data:
per_list[-1] = int(per_list[-1])
fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('rgba', 12, PointField.UINT32, 1),
]
header = Header()
header.frame_id = target_frame
pc_out = pc2.create_cloud(header,fields,data)
pc_out.header.stamp = rospy.Time.now()
self.publish_time = pc_out.header.stamp
rospy.loginfo("time stamp of published pointcloud2 is : %s and %s", str(pc_out.header.stamp.secs),str(pc_out.header.stamp.nsecs))
rospy.loginfo("timestamp diff = %s", str((pc_out.header.stamp - self.recieve_time).to_sec()))
return pc_out
def __init__(self):
self.width = 0
self.height= 0
self.source_frame = "default"
rospy.init_node('pointcloudsub', anonymous=True)
self.point_cloud_sub = rospy.Subscriber("/points2", PointCloud2, self.point_cloud_cb,queue_size=1,buff_size =2**24)#,buff_size = 9830000)#
self.point_cloud_pub = rospy.Publisher("point_cloud_transformed", PointCloud2,queue_size=1)
rospy.spin()
Any idea on how to subsribe to the latest msg? The msg that arrives when the callback is still processing the cluster should be discarded. Any help would be greatly appreciated!
What is the cpu utilization of this node? If it's 100%, rospy is not going to behave in a good way.