play.py
Go to the documentation of this file.
1 #!/usr/bin/python
2 
3 import rospy
4 import rosbag
5 import sys
6 
7 # F#@K IT WE'LL DO IT LIVE
8 
9 if __name__=='__main__':
10  rospy.init_node('rosbaglive')
11  bagfn = None
12  should_loop = False
13  loop_sleep = 0.1
14 
15  for arg in sys.argv[1:]:
16  if ".bag" in arg:
17  bagfn = arg
18  elif arg=='-l':
19  should_loop = True
20  elif arg[0:2]=='-d':
21  loop_sleep = float(arg[2:])
22 
23  if bagfn is None:
24  rospy.logerr("No Bag specified!")
25  exit(1)
26 
27  bag = rosbag.Bag(bagfn)
28  pubs = {}
29  rospy.loginfo('Start read')
30  last = None
31  data = []
32 
33  for topic, msg, t in bag.read_messages():
34  if topic not in pubs:
35  pub = rospy.Publisher(topic, type(msg), latch=('map' in topic))
36  pubs[topic] = pub
37 
38  if t!=last:
39  data.append( (t, []) )
40  last = t
41  data[-1][1].append( (topic, msg) )
42  rospy.loginfo('Done read')
43  start = rospy.Time.now()
44  sim_start = None
45  while not rospy.is_shutdown():
46  for t, msgs in data:
47  now = rospy.Time.now()
48  if sim_start is None:
49  sim_start = t
50  else:
51  real_time = now - start
52  sim_time = t - sim_start
53  if sim_time > real_time:
54  rospy.sleep( sim_time - real_time)
55 
56  for (topic, msg) in msgs:
57  if 'header' in dir(msg):
58  msg.header.stamp = now
59  elif 'transforms' in dir(msg):
60  for tf in msg.transforms:
61  tf.header.stamp = now
62  pub = pubs[topic]
63  pub.publish(msg)
64  if rospy.is_shutdown():
65  break
66  if not should_loop:
67  break
68 
69  rospy.sleep(loop_sleep)
70  bag.close()
71 


rosbaglive
Author(s): David V. Lu!!
autogenerated on Fri Jun 7 2019 22:01:13