ROS Approximate time synchronizer not entering callback
I have created an approximate time synchronizer to sync stereo camera images are GPS. The time synchronizer does not enter the callback. I have already tried playing around with slop and queue_size parameters.
#!/usr/bin/env python2
import roslib
import sys
import rospy
import cv2
import numpy
import message_filters
from std_msgs.msg import String
from sensor_msgs.msg import Image, CompressedImage
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import NavSatFix
from std_msgs.msg import Float64
class sorghum_gps:
def __init__(self,arg1):
self.bridge = CvBridge()
self.image_pub0 = rospy.Publisher("image_topic_0",Image)
self.image_pub1 = rospy.Publisher("image_topic_1",Image)
self.gps_latitude_pub = rospy.Publisher("gps/lat",Float64)
self.gps_longitude_pub=rospy.Publisher("gps/long",Float64)
self.lat=0.0
self.long=0.0
self.image_sub0 = message_filters.Subscriber("/cam0/image_raw",Image)
self.image_sub1 = message_filters.Subscriber("/cam1/image_raw",Image)
self.gps_sub = message_filters.Subscriber("/fix",NavSatFix)
self.ts = message_filters.ApproximateTimeSynchronizer([self.image_sub0,self.image_sub1, self.gps_sub],5,1)
self.ts.registerCallback(self.callback0)
self.im_count=0
self.directory = arg1[1]
print(self.directory)
def callback0(self,data1, data2, data3):
try:
cv_image0 = self.bridge.imgmsg_to_cv2(data1, "bgr8")
cv_image1 = self.bridge.imgmsg_to_cv2(data2, "bgr8")
self.image_multiplier = 4
#self.image_pub1.publish(self.bridge.cv2_to_imgmsg(cv_image1, "bgr8"))
self.image_pub0.publish(self.bridge.cv2_to_imgmsg(cv_image0, "bgr8"))
self.gps_latitude_pub.publish(data3.latitude)
self.gps_longitude_pub.publish(data3.longitude)
cv2.imwrite(self.directory+'/0/'+str(self.im_count)+'.jpg',cv_image0)
cv2.imwrite(self.directory+'/1/'+str(self.im_count)+'.jpg',cv_image1)
#cv2.imwrite(self.directory+'/1/'+str(self.im_count)+'.jpg',cv_image1)
print('got_data')
#print(self.directory+'/0/'+str(self.im_count)+'.jpg')
self.im_count=1+self.im_count
except CvBridgeError as e:
print(e)
def call1(self, data):
print(data)
def main(args):
print(args[1], 'RUNNING')
rospy.init_node('node_gps',anonymous=True)
gp = sorghum_gps(args)
try:
rospy.spin()
except KeyboardInterrupt:
print("Shutting down")
cv2.destroyAllWindows()
if __name__ == '__main__':
main(sys.argv)
Here are the details of the bag file using rosbag info
types: sensor_msgs/Image [060021388200f6f0f447d0fcd9c64743]
sensor_msgs/NavSatFix [2d3a8cd499b9b4a0249fb98fd05cfa48] topics: /fix 7748 msgs : sensor_msgs/NavSatFix
/cam0/image_raw 1942 msgs : sensor_msgs/Image
/cam1/image_raw 1942 msgs : sensor_msgs/Image