play.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 import rospy
00004 import rosbag
00005 import sys
00006 
00007 # F#@K IT WE'LL DO IT LIVE
00008 
00009 if __name__=='__main__':
00010     rospy.init_node('rosbaglive')
00011     bagfn = None
00012     should_loop = False
00013     loop_sleep = 0.1
00014 
00015     for arg in sys.argv[1:]:
00016         if ".bag" in arg:
00017             bagfn = arg
00018         elif arg=='-l':
00019             should_loop = True
00020         elif arg[0:2]=='-d':
00021             loop_sleep = float(arg[2:])
00022             
00023     if bagfn is None:
00024         rospy.logerr("No Bag specified!")
00025         exit(1)
00026 
00027     bag = rosbag.Bag(bagfn)
00028     pubs = {}
00029     rospy.loginfo('Start read')
00030     last = None
00031     data = []
00032 
00033     for topic, msg, t in bag.read_messages():
00034         if topic not in pubs:
00035             pub = rospy.Publisher(topic, type(msg), latch=('map' in topic))
00036             pubs[topic] = pub
00037 
00038         if t!=last:
00039             data.append( (t, []) )
00040             last = t
00041         data[-1][1].append( (topic, msg) )
00042     rospy.loginfo('Done read')
00043     start = rospy.Time.now()
00044     sim_start = None
00045     while not rospy.is_shutdown():
00046         for t, msgs in data:
00047             now = rospy.Time.now()      
00048             if sim_start is None:
00049                 sim_start = t
00050             else:
00051                 real_time = now - start
00052                 sim_time = t - sim_start
00053                 if sim_time > real_time:
00054                     rospy.sleep( sim_time - real_time)
00055 
00056             for (topic, msg) in msgs:
00057                 if 'header' in dir(msg):
00058                     msg.header.stamp = now
00059                 elif 'transforms' in dir(msg):
00060                     for tf in msg.transforms:
00061                         tf.header.stamp = now
00062                 pub = pubs[topic]
00063                 pub.publish(msg)
00064             if rospy.is_shutdown():
00065                 break
00066         if not should_loop:
00067             break
00068 
00069         rospy.sleep(loop_sleep)
00070     bag.close()
00071 


rosbaglive
Author(s): David V. Lu!!
autogenerated on Thu Jun 6 2019 22:02:27