transform_message_saver.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 
00003 # yliu 
00004 # Aug 5, 2011 International Beer Day
00005 
00006 
00007 import roslib;roslib.load_manifest('camera_pose_toolkits')
00008 
00009 import rospy
00010 import camera_pose_calibration.msg
00011 import std_msgs.msg
00012 import rosbag
00013 import sys
00014 import os
00015 import rostopic
00016 import time
00017 from geometry_msgs.msg import TransformStamped
00018 
00019 def callback(msg):
00020     global topic_name, bag_filename, best_list
00021     if (bag_filename):
00022         if len(best_list) < 1:
00023             best_list.append(msg)
00024         else:
00025             for b in best_list:
00026                 # print msg.header.frame_id
00027                 # print msg.child_frame_id
00028                 # print b.header.frame_id
00029                 # print b.child_frame_id
00030                 # print (msg.header.frame_id == b.header.frame_id and msg.child_frame_id == b.child_frame_id)
00031                 if msg.header.frame_id == b.header.frame_id and msg.child_frame_id == b.child_frame_id:
00032                     print 'repeat' 
00033                     best_list[best_list.index(b)] = msg
00034                     break
00035             else:
00036                 print 'append'
00037                 best_list.append(msg)
00038         print best_list
00039 
00040         bag = rosbag.Bag(bag_filename, 'w')  #overwrite
00041         for b in best_list:
00042             bag.write(topic_name, b)
00043         
00044         print "Saving msg on [%s]" % topic_name
00045         print "number of messages in bag: %d" % len(best_list)
00046         bag.close()
00047 
00048                        
00049 
00050 
00051 
00052 if len(rospy.myargv()) >= 1:
00053     bag_filename = rospy.myargv()[1]
00054 else:
00055     print "   Usage: "
00056     print "      transform_msg_saver.py [bagfilename]"
00057     sys.exit(-1)
00058     
00059 topic_name = 'camera_pose_static_transform_update'
00060 
00061 
00062 rospy.init_node("transform_msg_saver")
00063 
00064 
00065 pub = rospy.Publisher(topic_name, TransformStamped, latch=True)
00066 
00067 # on boot up
00068 
00069 best_list = []
00070 
00071 if bag_filename:
00072     if (os.path.isfile(bag_filename)):
00073         print "Found file [%s]" % bag_filename
00074         bag = rosbag.Bag(bag_filename)
00075         for topic, msg, t in bag.read_messages(topics=[topic_name]):
00076             best_list.append (msg)
00077         else:
00078             print "Couldn't find any messages on topic [%s]" % topic_name
00079         bag.close()
00080 
00081         count = 0
00082         
00083         while pub.get_num_connections() == 0 and count < 700 :  # get number of connections to other ROS nodes for this topic
00084             time.sleep(0.01)
00085             count += 1
00086       
00087         if count == 700 :
00088             print 'trasform_message_saver :'
00089             print 'Waiting for subscriber(s) to topic [camera_pose_static_transform_update] has timed out.'
00090             print 'The on-start publishing starts without further waiting.'
00091             print 'If your bag file [%s] contains multiple previously saved messages, only the latest message is likely to be received.' % bag_filename
00092             print 'Just a heads-up.'
00093    
00094         for b in best_list:
00095             pub.publish(b)
00096             time.sleep(0.01)
00097     else:
00098         print "Couldn't find file [%s]. Skipping on-start publishing" % bag_filename
00099 
00100 # Listen on the same topic, so that we can update the bag
00101 sub = rospy.Subscriber(topic_name, TransformStamped, callback)
00102 
00103 rospy.spin()
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


camera_pose_toolkits
Author(s): Yiping Liu
autogenerated on Thu Aug 15 2013 12:02:52