rospy subscriber delay, not giving the latest msg

asked 2021-06-23 23:43:53 -0600

sueee gravatar image

updated 2021-06-24 11:50:02 -0600

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: image description 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!

edit retag flag offensive close merge delete

Comments

What is the cpu utilization of this node? If it's 100%, rospy is not going to behave in a good way.

Mike Scheutzow gravatar image Mike Scheutzow  ( 2021-06-24 19:53:23 -0600 )edit