Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 import rospy
00011 import rocon_gateway
00012 import rocon_console.console as console
00013 from gateway_msgs.msg import ConnectionType
00014 import sys
00015 import itertools
00016 import rosgraph
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 if __name__ == '__main__':
00038 rospy.init_node('bench_master_api')
00039 master = rosgraph.Master(rospy.get_name())
00040 connection_cache = rocon_gateway.ConnectionCache(master.getSystemState)
00041 times = {}
00042 names = ['start_time', 'get_system_state', 'extract_action_servers', 'extract_action_clients', 'get_pubs', 'get_subs', 'get_services', 'get_action_servers', 'get_action_clients']
00043 for name in names:
00044 times[name] = 0.0
00045 while not rospy.is_shutdown():
00046 try:
00047 connections = {}
00048 times['start_time'] = rospy.get_time()
00049 publishers, subscribers, services = connection_cache._get_system_state()
00050 times['get_system_state'] = rospy.get_time()
00051 action_servers, publishers, subscribers = connection_cache._get_action_servers(publishers, subscribers)
00052 times['extract_action_servers'] = rospy.get_time()
00053 action_clients, publishers, subscribers = connection_cache._get_action_clients(publishers, subscribers)
00054 times['extract_action_clients'] = rospy.get_time()
00055 connections[ConnectionType.PUBLISHER] = connection_cache._get_connections_from_pub_sub_list(publishers, ConnectionType.PUBLISHER)
00056 times['get_pubs'] = rospy.get_time()
00057 connections[ConnectionType.SUBSCRIBER] = connection_cache._get_connections_from_pub_sub_list(subscribers, ConnectionType.SUBSCRIBER)
00058 times['get_subs'] = rospy.get_time()
00059 connections[ConnectionType.SERVICE] = connection_cache._get_connections_from_service_list(services, ConnectionType.SERVICE)
00060 times['get_services'] = rospy.get_time()
00061 connections[ConnectionType.ACTION_SERVER] = connection_cache._get_connections_from_action_list(action_servers, ConnectionType.ACTION_SERVER)
00062 times['get_action_servers'] = rospy.get_time()
00063 connections[ConnectionType.ACTION_CLIENT] = connection_cache._get_connections_from_action_list(action_clients, ConnectionType.ACTION_CLIENT)
00064 times['get_action_clients'] = rospy.get_time()
00065 print(console.bold + "Benchmarks" + console.reset)
00066 for i in range(1, len(names)):
00067 print(console.cyan + " %s: " % names[i] + console.yellow + "%s" % (times[names[i]] - times[names[i-1]]) + console.reset)
00068 print(console.green + " total_time: " + console.magenta + "%s" % (times[names[-1]] - times[names[0]]) + console.reset)
00069 except Exception as e:
00070 print(console.red + "Lost contact with the master [%s][%s]" % (str(e), type(e)) + console.reset)
00071 break