msg_saver.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 import roslib;
00003 roslib.load_manifest('camera_pose_calibration')
00004 
00005 import rospy
00006 import camera_pose_calibration.msg
00007 import std_msgs.msg
00008 import rosbag
00009 import sys
00010 import os
00011 import rostopic
00012 
00013 def callback(msg):
00014     global topic_name, bag_filename
00015     if (bag_filename):
00016         bag = rosbag.Bag(bag_filename, 'w')
00017         bag.write(topic_name, msg)
00018         print "Saving msg on [%s]" % topic_name
00019         bag.close()
00020 
00021 if len(rospy.myargv()) >= 3:
00022     msg_type = std_msgs.msg.Empty
00023     topic_name = rospy.myargv()[1]
00024     topic_type = rospy.myargv()[2]
00025     bag_filename = None
00026     if len(rospy.myargv()) >= 4:
00027         bag_filename = rospy.myargv()[3]
00028 else:
00029     #print ""
00030     print "   Message Saver:"
00031     print "      Saves the latest message on a topic into a bagfile, and republishes saved message on bootup."
00032     print "      This is a really convenient way to store state in a system."
00033     print "   Usage: "
00034     print "      msg_saver.py [topic] [type] [filename]"
00035     sys.exit(-1)
00036 
00037 
00038 print "Topic Name: %s" % topic_name
00039 #pub = rospy.Publisher(topic_name, msg_type, latch=True)
00040 
00041 # Figure out the message class, and then create the publisher
00042 try:
00043     msg_class = roslib.message.get_message_class(topic_type)
00044 except:
00045     raise ROSTopicException("invalid topic type: %s"%topic_type)
00046 if msg_class is None:
00047     pkg = roslib.names.resource_name_package(topic_type)
00048     raise ROSTopicException("invalid message type: %s.\nIf this is a valid message type, perhaps you need to type 'rosmake %s'"%(topic_type, pkg))
00049 # disable /rosout and /rostime as this causes blips in the pubsub network due to rostopic pub often exiting quickly
00050 rospy.init_node("msg_saver", disable_rosout=True, disable_rostime=True)
00051 
00052 pub = rospy.Publisher(topic_name, msg_class, latch=True)
00053 
00054 # Get the last message on the topic of interest
00055 
00056 if bag_filename:
00057     if (os.path.isfile(bag_filename)):
00058         print "Found file [%s]" % bag_filename
00059         bag = rosbag.Bag(bag_filename)
00060         last_msg = None
00061         for topic, msg, t in bag.read_messages(topics=[topic_name]):
00062             last_msg = msg
00063         if last_msg is not None:
00064             pub.publish(last_msg)
00065         else:
00066             print "Couldn't find any messages on topic [%s]" % topic_name
00067         bag.close()
00068     else:
00069         print "Couldn't find file [%s]. Skipping publishing" % bag_filename
00070 
00071 # Listen on the same topic, so that we can update the bag
00072 sub = rospy.Subscriber(topic_name, msg_class, callback)
00073 
00074 rospy.spin()
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


camera_pose_calibration
Author(s): Vijay Pradeep, Wim Meeussen
autogenerated on Thu Aug 15 2013 10:18:08