step_bag.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import sys
00003 import roslib; roslib.load_manifest('modular_cloud_matcher')
00004 import rospy
00005 import rosbag
00006 import tf
00007 from sensor_msgs.msg import *
00008 
00009 
00010 if len(sys.argv) < 3:
00011         sys.exit('Usage: %s bagName topicName <number of scan to jump> <list of scan to jump>' % sys.argv[0])
00012 
00013 if len(sys.argv) == 3:
00014         jumpScan = 0
00015 else:
00016         jumpScan = int(sys.argv[3])
00017 
00018 blackList = []
00019 if len(sys.argv) >= 5:
00020         blackList = sys.argv[4:]
00021 
00022 
00023 bagName = sys.argv[1]
00024 topicName = sys.argv[2]
00025 
00026 rospy.init_node('republishBag')
00027 pub = rospy.Publisher(topicName, PointCloud2)
00028 
00029 bag = rosbag.Bag(bagName)
00030 
00031 i = 0
00032 for topic, msg, t in bag.read_messages(topics=[topicName, "/tf"]):
00033         if(topic == "/tf"):
00034                 if(msg.transforms[0].header.frame_id == "/map"):
00035                         if(msg.transforms[0].child_frame_id == "/odom"):
00036                                 br = tf.TransformBroadcaster()
00037                                 br.sendTransform((msg.transforms[0].transform.translation.x, msg.transforms[0].transform.translation.y, msg.transforms[0].transform.translation.z), 
00038                                         (msg.transforms[0].transform.rotation.x, msg.transforms[0].transform.rotation.y, msg.transforms[0].transform.rotation.z, msg.transforms[0].transform.rotation.w),
00039                                         msg.transforms[0].header.stamp,
00040                                         "/odom",
00041                                         "/map")
00042         else:
00043 
00044                 if str(i) in blackList:
00045                         print "Scan " + str(i) + " is black-listed. Skipping it\n"
00046 
00047                 elif (i >= jumpScan):
00048                         print "----------------\nPoint cloud " + str(i)
00049                         pointCloud = msg
00050                         pub.publish(pointCloud)
00051                         raw_input('Press Enter...\n')
00052 
00053                 else:
00054                         print "Skipping scan " + str(i)
00055                 
00056                 i = i + 1
00057 
00058 
00059 bag.close()
00060 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends Defines


modular_cloud_matcher_experiments
Author(s): François Pomerleau and Stéphane Magnenat
autogenerated on Tue Dec 18 2012 01:32:59