ROS Approximate time synchronizer not entering callback

asked 2020-06-11 10:52:05 -0500

ogbf3 gravatar image

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
edit retag flag offensive close merge delete