Skip to content

Instantly share code, notes, and snippets.

@LimHyungTae
Created July 15, 2020 08:53
Show Gist options
  • Save LimHyungTae/e8317ff341e31f308d16a310e837ac06 to your computer and use it in GitHub Desktop.
Save LimHyungTae/e8317ff341e31f308d16a310e837ac06 to your computer and use it in GitHub Desktop.
Multithread to preserve the data on ROS
#!/usr/bin/env python
import rospy
from std_msgs.msg import String
from sensor_msgs.msg import CompressedImage, Image
from cv_bridge import CvBridge
import cv2
import threading
import time
images=[]
lock = threading.Lock()
COUNT = None
def ImgCallBack(data):
global COUNT
print(str(data.header.seq) + " is coming!")
# data = bridge.compressed_imgmsg_to_cv2(data)
# cv2.imshow("test2", data)
# cv2.waitKey(10)
lock.acquire()
images.append(data)
print(len(images), " IS IN,CALLBACK")
lock.release()
if COUNT is None:
COUNT = data.header.seq
else:
if (data.header.seq - COUNT) != 1:
print("= = = = = = = = = = = = = = = = = = = = ")
print("Wrong! %d %d" % (COUNT, data.header.seq))
print("= = = = = = = = = = = = = = = = = = = = ")
else:
pass
COUNT = data.header.seq
def test():
while(True):
image = None
lock.acquire()
if(len(images)>0):
print(len(images), "IS IN, TEST")
image = images.pop(0)
lock.release()
if(image is not None):
print(image.header.seq)
time.sleep(0.1)
class Thread(threading.Thread):
def __init__(self,t,*args):
threading.Thread.__init__(self,target=t,args=args)
self.start()
def listener():
# In ROS, nodes are uniquely named. If two nodes with the same
# name are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('listener', anonymous=True)
rospy.Subscriber('/cam0/image_raw', Image, ImgCallBack)
threadtest = threading.Thread(target=test)
threadtest.start()
threadtest.join()
rospy.spin()
if __name__ == '__main__':
listener()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment