fix_timestamp_of_topic.py

Go to the documentation of this file.

#!/usr/bin/env python
import roslib
import os
import sys
import rospy
import rosbag

# Terminal colors for Ubuntu/Linux
SB_='\033[1m' #style bold
CY_='\033[33m' #color yellow
CR_='\033[1;31m' #color red
CLG_='\033[37m' #color light grey
_EC='\033[0m' #end color

# Get the bag
if len(sys.argv) < 4:
  print " "
  print CR_+"-------------------------------------------------------------------------------------------------"+_EC
  print CR_+"Error! "+_EC+SB_+" fix_timestamp_of_topic.py  BAG_FILE  TOPIC  TIME_DIFF(in s)"+_EC
  print CR_+"-------------------------------------------------------------------------------------------------"+_EC
  exit()
else:
  elbagin  = str(sys.argv[1])
  elbagout = elbagin[:-4]+"_ts_fixed.bag"  
  eltopic  = str(sys.argv[2])
  eldiff   = float(sys.argv[3])

print ""
print CY_+"-------------------------------------------------------------------------------------------------"+_EC
print SB_+"                         Fix topic timestamp from Bag"+_EC
print CY_+"-------------------------------------------------------------------------------------------------"+_EC

with rosbag.Bag(elbagout, 'w') as outbag:

  # calcular la diferencia entre t_ok - t_no_ok
  diff_stamp = rospy.Duration.from_sec(eldiff)

  # canviar el t_no_ok msg.header.stamp sumant la diferencia
  for topic, msg, t in rosbag.Bag(elbagin).read_messages():
    if topic == eltopic:
      print "  before",msg.header.stamp.to_sec()
      msg.header.stamp = msg.header.stamp + diff_stamp
      print "  fixed ",msg.header.stamp.to_sec()
      outbag.write(topic, msg,t)
      print CY_+"-------------------------"+_EC
    else:
      outbag.write(topic, msg,t)

print SB_+"                         End! the bag has been written"+_EC
print "  "+CY_+elbagout
print CY_+"-------------------------------------------------------------------------------------------------"+_EC