topic_compare.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 
00004 import imp
00005 import roslib
00006 import roslib.message
00007 import rospy
00008 import os, sys
00009 import time
00010 import traceback
00011 
00012 
00013 class ROSTopicCompare(object):
00014     subscriberArray = []
00015     topicSizesArray = []
00016     topicTimesArray = []
00017     scaleType = 0
00018     captureSize = 100
00019 
00020     def __init__(self, scale="KB", captureSize=100):
00021         if scale == "B":
00022             self.scaleType = 1
00023         elif scale == "KB":
00024             self.scaleType = 2
00025         elif scale == "MB":
00026             self.scaleType = 3
00027 
00028         self.captureSize = captureSize
00029 
00030     def _gen_callback(self):
00031         self.topicSizesArray.append([])
00032         self.topicTimesArray.append([])
00033         _topic_num = len(self.subscriberArray)
00034         def _callback(msg):
00035             topic_num = _topic_num
00036             try:
00037                 t = time.time()
00038                 self.topicTimesArray[topic_num].append(t)
00039                 self.topicSizesArray[topic_num].append(len(msg._buff))
00040                 assert(len(self.topicTimesArray[topic_num]) == len(self.topicSizesArray[topic_num]))
00041                 if len(self.topicTimesArray[topic_num]) > self.captureSize:
00042                     self.topicTimesArray[topic_num].pop(0)
00043                     self.topicSizesArray[topic_num].pop(0)
00044             except:
00045                 traceback.print_exc()
00046         return _callback
00047 
00048     def registerTopic(self, topic_name):
00049         sub = rospy.Subscriber(topic_name, rospy.AnyMsg, self._gen_callback())
00050         self.subscriberArray.append(sub)
00051         print "subscribed as %d: %s" % (len(self.subscriberArray)-1, topic_name)
00052 
00053     def getTotalBytes(self, i):
00054         return sum(self.topicSizesArray[i])
00055     def getMaxByte(self, i):
00056         return max(self.topicSizesArray[i])
00057     def getMinByte(self, i):
00058         return min(self.topicSizesArray[i])
00059     def getMessageNum(self, i):
00060         return len(self.topicTimesArray[i])
00061     def getStartTime(self, i):
00062         return self.topicTimesArray[i][0]
00063     def printBandWidth(self):
00064         current_time = time.time()
00065 
00066         # first lookup the longest topic name
00067         longest_topic_len = 0
00068         for i in range(len(self.subscriberArray)):
00069             if len(self.subscriberArray[i].name) > longest_topic_len:
00070                 longest_topic_len = len(self.subscriberArray[i].name)
00071         row_format = "{:>" + str(longest_topic_len + 1) + "}" + "{:>11}" * 5
00072         print ""
00073         print row_format.format("Name", "avg/sec", "avg/msg",
00074                                 "min/msg", "max/msg", "quant")
00075         for i in range(len(self.subscriberArray)):
00076             try:
00077                 n = self.getMessageNum(i)
00078                 start_time = self.getStartTime(i)
00079                 total_bytes = self.getTotalBytes(i)
00080                 bytes_per_sec = 1.0 * total_bytes / (current_time - start_time)
00081                 bytes_per_msg = total_bytes / n
00082                 max_size = self.getMaxByte(i)
00083                 min_size = self.getMinByte(i)
00084                 res = [bytes_per_sec, bytes_per_msg, min_size, max_size]
00085 
00086                 if self.scaleType == 1: # B
00087                     bps, bpm, ms, Ms = ["%.2fB" % v for v in res]
00088                 elif self.scaleType == 2: # KB
00089                     bps, bpm, ms, Ms = ["%.2fKB" % (v/1000) for v in res]
00090                 elif self.scaleType == 3: # MB
00091                     bps, bpm, ms, Ms = ["%.2fMB" % (v/1000000) for v in res]
00092                 else:
00093                     raise "error: invalid scale type"
00094                     quit()
00095                 print row_format.format(self.subscriberArray[i].name, bps, bpm, ms, Ms, n)
00096             except:
00097                 pass
00098 
00099 def onShutdown():
00100     i = 0
00101     print "subscribed topic:"
00102     for sub in tc.subscriberArray:
00103         print "%d: %s" % (i, sub.name)
00104         i += 1
00105 
00106 def fullUsage():
00107     print "[Usage] rosrun jsk_topic_tools topic_compare.py [Option] <topic_name_1>..<topic_name_n>"
00108     print "[Option]"
00109     print "-h: full usage"
00110     print "-b: show value as bytes"
00111     print "-m: show value as megabytes"
00112 
00113 if __name__ == '__main__':
00114     # check if valid argument
00115     if len(sys.argv) < 2:
00116         print "[Usage] rosrun jsk_topic_tools topic_compare.py <topic_name_1>..<topic_name_n>"
00117         quit()
00118 
00119     rospy.init_node("topic_compare")
00120 
00121     # check options
00122     if "-h" in sys.argv:
00123         fullUsage()
00124         quit()
00125     elif "-b" in sys.argv:
00126         tc = ROSTopicCompare("B")
00127     elif "-m" in sys.argv:
00128         tc = ROSTopicCompare("MB")
00129     else:
00130         tc = ROSTopicCompare()
00131 
00132     for name in sys.argv[1:]:
00133         if name[0] != "/":
00134             continue
00135         tc.registerTopic(name)
00136 
00137     rospy.on_shutdown(onShutdown)
00138 
00139     while not rospy.is_shutdown():
00140         rospy.rostime.wallsleep(1.0)
00141         tc.printBandWidth()


jsk_topic_tools
Author(s): Kei Okada , Yusuke Furuta
autogenerated on Sun Jan 25 2015 00:30:41