random_play.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2008, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of Willow Garage, Inc. nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 #
00034 
00035 import unittest
00036 import rospy
00037 import rostest
00038 import sys
00039 from cStringIO import StringIO
00040 import time
00041 from random_messages import RandomMsgGen
00042 import subprocess
00043 import os
00044 
00045 import genpy
00046 
00047 DELAY = 0.5
00048 
00049 class RandomPlay(unittest.TestCase):
00050 
00051   def msg_cb_topic(self, topic):
00052     def msg_cb(msg):
00053       nowtime = rospy.Time.now()
00054 
00055       if self.start is None:
00056         self.start = nowtime
00057 
00058       nowtime -= self.start
00059 
00060       self.input.append((topic, msg, nowtime.to_sec()))
00061 
00062     return msg_cb
00063 
00064 
00065   def test_random_play(self):
00066     rospy.init_node('random_sub', anonymous=True)
00067 
00068     self.start = None
00069     self.input = []
00070 
00071     self.assertTrue(len(sys.argv[1]) > 3)
00072 
00073     seed    = int(sys.argv[1])
00074     topics  = int(sys.argv[2])
00075     length  = float(sys.argv[3])
00076     scale   = float(sys.argv[4])
00077     self.use_clock = bool(int(sys.argv[5]))
00078 
00079     rmg = RandomMsgGen(int(seed), topics, length)
00080 
00081     subscribers = {}
00082     for (topic, msg_class) in rmg.topics():
00083       subscribers[topic] = rospy.Subscriber(topic, msg_class, self.msg_cb_topic(topic))
00084 
00085     bagpath = os.path.join('/tmp', 'test_rosbag_random_record_%d.bag'%seed)
00086     cmd = ['rosbag', 'play', '-d', str(DELAY), '-r', str(scale)]
00087     
00088     rospy.loginfo(str(cmd))
00089 
00090     if (self.use_clock):
00091       cmd += ['--clock', '--hz', '100']
00092 
00093     cmd += [bagpath]
00094 
00095     f1 = subprocess.Popen(cmd)
00096 
00097     while (len(self.input) < rmg.message_count()):
00098 #      print "\n%d/%d\n"%(len(self.input), rmg.message_count())
00099       time.sleep(.1)
00100 
00101     self.assertEqual(len(self.input), rmg.message_count())
00102 
00103     max_late = 0
00104     max_early = 0
00105     avg_off = 0
00106     power = 0
00107 
00108     for (expect_topic, expect_msg, expect_time) in rmg.messages():
00109 
00110       if (not self.use_clock):
00111         expect_time /= scale
00112 
00113       buff = StringIO()
00114       expect_msg.serialize(buff)
00115       expect_msg.deserialize(buff.getvalue())
00116 
00117       msg_match = False
00118 
00119       for ind in xrange(0,100):
00120         (input_topic, input_msg, input_time) = self.input[ind]
00121 
00122         if (genpy.message.strify_message(expect_msg) == genpy.message.strify_message(input_msg)):
00123           msg_match = True
00124           del self.input[ind]
00125 
00126           # stats
00127           diff = input_time - expect_time
00128 
00129           if (diff < max_early):
00130             max_early = diff
00131 
00132           if (diff > max_late):
00133             max_late = diff
00134 
00135           avg_off += diff / rmg.message_count()
00136 
00137           power += (diff**2) / rmg.message_count()
00138 
00139           # Messages can arrive late, but never very early Both of
00140           # these bounds are much larger than they ought to be, but
00141           # you never know with a heavily loaded system.
00142           self.assertTrue(input_time - expect_time > -.5)
00143           self.assertTrue(abs(input_time - expect_time) < .5)
00144           break
00145 
00146       if not msg_match:
00147         print "No match at time: %f"%expect_time
00148 
00149       self.assertTrue(msg_match)
00150 
00151     print "%f %f %f %f"%(max_early, max_late, avg_off, power)
00152 
00153     (o1,e1) = f1.communicate()    
00154     self.assertEqual(f1.returncode, 0)
00155 
00156 if __name__ == '__main__':
00157   rostest.rosrun('test_rosbag', 'random_record_play', RandomPlay, sys.argv)


test_rosbag
Author(s): Jeremy Leibs
autogenerated on Sat Dec 28 2013 17:42:56