Replacing camera_info messages

asked 2012-01-19 00:19:29 -0600

Hordur
544 ●14 ●17 ●26

Is there any easy way to replace the camera_info messages stored in a bag file. In particular when using either the image_proc or stereo_image_proc nodes.

edit retag flag offensive close merge delete

add a comment

5

answered 2012-01-20 09:14:12 -0600

Patrick Mihelich
4336 ●32 ●37 ●44

Some simple operations can be done from the command line. For example, if you just want to remove the camera info messages, you can do:

rosbag filter orig.bag filtered.bag "m._type != 'sensor_msgs/CameraInfo'"

If you need to mutate messages in a bag file or some other more complicated operation, the easiest way is to write a Python script using the rosbag code API. Below is a script I used at some point to replace the value of header.frame_id for all CameraInfo and Image messages in one or more bags:

#!/usr/bin/env python

PKG = 'sensor_msgs'
import roslib; roslib.load_manifest(PKG)
import rospy
import rosrecord
import fileinput
import os

def fix(inbags):
  for b in inbags:
    print "Trying to migrating file: %s"%b
    outbag = b+'.tmp'
    rebag = rosrecord.Rebagger(outbag)
    try:
      for (topic, msg, t) in rosrecord.logplayer(b, raw=False):
        if msg._type == 'sensor_msgs/CameraInfo' or msg._type == 'sensor_msgs/Image':
          msg.header.frame_id = 'l_forearm_cam_optical_frame'
          rebag.add(topic, msg, t, raw=False)
        else:
          rebag.add(topic, msg, t, raw=False)
      rebag.close()
    except rosrecord.ROSRecordException, e:
      print " Migration failed: %s"%(e.message)
      os.remove(outbag)
      continue

    oldnamebase = b+'.old'
    oldname = oldnamebase
    i = 1
    while os.path.isfile(oldname):
      i=i+1
      oldname = oldnamebase + str(i)
    os.rename(b, oldname)
    os.rename(outbag, b)
    print " Migration successful.  Original stored as: %s"%oldname

if __name__ == '__main__':
  import sys
  if len(sys.argv) >= 2:
    fix(sys.argv[1:])
  else:
    print "usage: %s bag1 [bag2 bag3 ...]" % sys.argv[0]

edit flag offensive delete link

add a comment

1

answered 2013-02-16 11:55:17 -0600

sergey_alexandrov
260 ●4 ●12 ●11

You can use the following function to replace all camera info messages in a certain topic with another given camera info message:

def replace_camera_info_messages(bag, topic, camera_info):                  
    import os.path                                                          
    import rosbag                                                           
    orig = os.path.splitext(bag)[0] + '.orig.bag'                           
    os.rename(bag, orig)                                                    
    inbag = rosbag.Bag(orig, 'r')                                           
    outbag = rosbag.Bag(bag, 'w')                                           
    for t, msg, ts in inbag.read_messages():                                
        if t == topic:                                                      
            camera_info.header.seq = msg.header.seq                         
            camera_info.header.stamp = msg.header.stamp                     
            outbag.write(t, camera_info, ts)                                
        else:                                                               
            outbag.write(t, msg, ts)                                        
    inbag.close()                                                           
    outbag.close()