tug_ist_diagnosis_generator: observers_model

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 ##
00004 # observers_generator.py is a node that finds the nodes and topics and their related information in the running system.
00005 # After finding necessary information it creates launch files in tug_ist_diagnosis_launch package and executes them automatically. 
00006 # Copyright (c).2012. OWNER: Institute for Software Technology, TU Graz Austria.
00007 # Authors: Safdar Zaman, Gerald Steinbauer. (szaman@ist.tugraz.at, steinbauer@ist.tugraz.at)
00008 # All rights reserved.
00009 #    This program is free software: you can redistribute it and/or modify
00010 #    it under the terms of the GNU General Public License as published by
00011 #    the Free Software Foundation, either version 3 of the License, or
00012 #    (at your option) any later version.
00013 #
00014 #    This program is distributed in the hope that it will be useful,
00015 #    but WITHOUT ANY WARRANTY; without even the implied warranty of
00016 #    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00017 #    GNU General Public License for more details.
00018 #
00019 #    You should have received a copy of the GNU General Public License
00020 #    along with this program.  If not, see <http://www.gnu.org/licenses/>.
00021 ##
00022 
00023 import roslib.message; roslib.load_manifest('tug_ist_diagnosis_generator')
00024 import rospy
00025 import sys
00026 import xmlrpclib
00027 import os
00028 import subprocess
00029 import time
00030 import shlex
00031 import thread
00032 from math import sqrt
00033 class node_data_structure(object):
00034         def __init__(self,node_name):
00035                 self.node_name = node_name
00036                 self.pub_topic_list = []
00037                 self.sub_topic_list = []
00038         def get_node_name(self):
00039                 return self.node_name
00040         def add_pub_topic(self,topic):
00041                 self.pub_topic_list.append(topic)
00042         def add_sub_topic(self,topic):
00043                 self.sub_topic_list.append(topic)
00044         def get_pub_topics(self):
00045                 return self.pub_topic_list
00046         def get_sub_topics(self):
00047                 return self.sub_topic_list
00048         def print_pub(self):
00049                 for p in self.pub_topic_list:
00050                         print p
00051         def print_sub(self):
00052                 for s in self.sub_topic_list:
00053                         print s
00054 
00055 
00056 class topic_data_structure(object):
00057         def __init__(self,topic_name,ws):
00058                 self.topic_name = topic_name
00059                 self.ws = ws
00060                 self.circular_queu = [0 for i in xrange(self.ws)]
00061                 self.frq_list = [0 for i in xrange(self.ws)]
00062                 self.calculated_frq = 0
00063                 self.calculated_frq_dev = 0
00064                 self.prev_t = time.time()
00065         def get_topic_name(self):
00066                 return self.topic_name
00067         def set_prev_t(self,prev_t):
00068                 self.prev_t = prev_t
00069         def get_prev_t(self):
00070                 return self.prev_t
00071         def calc_frq(self,delta_t):
00072                 self.circular_queu.pop(0)
00073                 self.circular_queu.append(delta_t)
00074                 sm = 0
00075                 for dt in self.circular_queu:
00076                         sm = sm + dt
00077                 self.calculated_frq = 1/(sm/self.ws)
00078                 self.frq_list.pop(0)
00079                 self.frq_list.append(self.calculated_frq)
00080         def calculate_frq_dev(self):
00081                 n = len(self.frq_list)
00082                 mean = sum(self.frq_list)/n
00083                 self.calculated_frq_dev = sqrt(sum((x-mean)**2 for x in self.frq_list)/n)       
00084         def get_calculated_frq(self):
00085                 return self.calculated_frq
00086         def get_calculated_frq_dev(self):
00087                 return self.calculated_frq_dev
00088         def print_frq_list(self):
00089                 for f in self.frq_list:
00090                         print f
00091                 
00092 
00093 class Generator(object):
00094         def __init__(self):
00095                 rospy.init_node('observers_generator', anonymous=False)
00096                 self.m = xmlrpclib.ServerProxy(os.environ['ROS_MASTER_URI'])
00097                 self.caller_id = '/script'
00098                 self.param_f_ws = rospy.get_param('~frq_ws', 10)
00099                 self.param_g_ws = rospy.get_param('~gob_ws', 10)
00100                 self.param_p_ws = rospy.get_param('~pob_ws', 10)
00101                 self.p_mismatch_th = rospy.get_param('~p_mismatch_th', 5)
00102                 self.nodes_list = []
00103                 self.topics_list = []
00104                 self.topic_data_structure = []
00105                 self.node_data_structure = []
00106                 self.node_topic_list = []
00107                 self.topics_type_list = []
00108                 self.counter = 0
00109                 self.global_list = []
00110                 self.param_frq = 0
00111                 self.param_dev = 0
00112                 #t = Thread(target = self.spin_thread, args=(i)) 
00113                 
00114 
00115         def start(self):
00116                 self.extract_topics()
00117                 self.extract_nodes()
00118                 #self.calculate_frq_dev()
00119                 #self.make_obs_launch()
00120                 self.make_mdl_yaml()
00121                 #self.run_observers()
00122                 #self.run_model_server()
00123                         
00124         def make_obs_launch(self):
00125                 temp_path = "/home/safdar/my_workspace/model_based_diagnosis/tug_ist_diagnosis_launch/launch/obs_auto.launch"
00126                 file = open(temp_path, 'wb')
00127                 file.write('<?xml version="1.0"?>\n<launch>\n')
00128                 i = 0
00129                 ln = len(self.nodes_list)
00130                 while i<ln:
00131                         node_name = self.nodes_list[i]
00132                         node_name = node_name[1:len(node_name)]
00133                         node_mxCpu = self.nodes_list[i+1]
00134                         node_mxMem = self.nodes_list[i+2]
00135                         nobs_name = node_name+'NObs'    
00136                         file.write('<node pkg="tug_ist_diagnosis_observers" type="NObs.py" name="'+nobs_name+'" >\n')
00137                         file.write('\t<param name="node" value="'+node_name+'" />\n')
00138                         file.write('</node>\n')
00139                         pobs_name = node_name+'CpuPObs' 
00140                         file.write('<node pkg="tug_ist_diagnosis_observers" type="PObs.py" name="'+pobs_name+'" >\n')
00141                         file.write('\t<param name="node" value="'+node_name+'" />\n')
00142                         file.write('\t<param name="property" value="cpu" />\n')
00143                         file.write('\t<param name="max_val" value="'+node_mxCpu+'" />\n')
00144                         file.write('\t<param name="mismatch_th" value="'+str(self.p_mismatch_th)+'" />\n')
00145                         file.write('\t<param name="ws" value="'+str(self.param_p_ws)+'" />\n')
00146                         file.write('</node>\n')
00147                         pobs_name = node_name+'MemPObs' 
00148                         file.write('<node pkg="tug_ist_diagnosis_observers" type="PObs.py" name="'+pobs_name+'" >\n')
00149                         file.write('\t<param name="node" value="'+node_name+'" />\n')
00150                         file.write('\t<param name="property" value="mem" />\n')
00151                         file.write('\t<param name="max_val" value="'+node_mxMem+'" />\n')
00152                         file.write('\t<param name="mismatch_th" value="'+str(self.p_mismatch_th)+'" />\n')
00153                         file.write('\t<param name="ws" value="'+str(self.param_p_ws)+'" />\n')
00154                         file.write('</node>\n')
00155                         i = i + 3
00156                 for t in self.topics_list:
00157                         for tpc_ds in self.topic_data_structure:
00158                                 if t == tpc_ds.get_topic_name():
00159                                         tpc_ds.calculate_frq_dev()
00160                                         self.param_frq = tpc_ds.get_calculated_frq()
00161                                         self.param_dev = tpc_ds.get_calculated_frq_dev()
00162                                         break
00163                         topic_name = t[1:len(t)]
00164                         gobs_name = topic_name+'GObs'
00165                         file.write('<node pkg="tug_ist_diagnosis_observers" type="GObs.py" name="'+gobs_name+'" >\n')
00166                         file.write('\t<param name="topic" value="'+topic_name+'" />\n')
00167                         file.write('\t<param name="frq" value="'+str(self.param_frq)+'" />\n')
00168                         file.write('\t<param name="dev" value="'+str(self.param_dev)+'" />\n')
00169                         file.write('\t<param name="ws" value="'+str(self.param_g_ws)+'" />\n')
00170                         file.write('</node>\n')
00171                 file.write('</launch>')
00172                 file.close()
00173 
00174         def make_mdl_yaml(self):
00175                 temp_path = "/home/safdar/my_workspace/model_based_diagnosis/tug_ist_diagnosis_model/diagnosis_model.yaml"
00176                 file = open(temp_path, 'wb')
00177                 file.write('ab: "AB"\nnab: "NAB"\nneg_prefix: "not_"\n\nprops:\n')
00178                 i = 0
00179                 ln = len(self.nodes_list)
00180                 rules = ''
00181                 #while i<ln:
00182                 #       node_name = self.nodes_list[i]
00183                 #       node_name = node_name[1:len(node_name)]
00184                 #       file.write('  - running('+node_name+')\n')
00185                 #       i = i + 3
00186                 #       rules = rules + '  - NAB('+node_name+') -> running('+node_name+')\n'
00187                 for n in self.nodes_list:
00188                         node_name = n[1:len(n)]
00189                         file.write('  - running('+node_name+')\n')
00190                 for t in self.topics_list:
00191                         topic_name = t[1:len(t)]
00192                         file.write('  - ok('+topic_name+')\n')
00193                 file.write('rules:\n')
00194                 file.write(rules)
00195                 
00196                 #i = 0
00197                 #ln = len(self.node_topic_list)
00198                 #while i<ln:
00199                 #       node = self.node_topic_list[i]
00200                 #       topic = self.node_topic_list[i+1]
00201                 #       file.write('  - NAB('+node[1:len(node)]+') -> ok('+topic[1:len(topic)]+')\n')
00202                 #       i = i + 2
00203                 #file.close()
00204                 for nd_ds in self.node_data_structure:
00205                         node = nd_ds.get_node_name()
00206                         pubs = nd_ds.get_pub_topics()
00207                         subs = nd_ds.get_sub_topics()
00208                         for p in pubs:
00209                                 str_subs = ''
00210                                 for s in subs:
00211                                         str_subs = str_subs + ', ok('+ s[1:len(s)] +')'
00212                                 file.write('  - NAB('+node[1:len(node)]+')'+str_subs+' -> ok('+p[1:len(p)]+')\n')
00213                 file.close()
00214         def extract_nodes(self):
00215                 code, statusMessage, sysState = self.m.getSystemState(self.caller_id)
00216                 #print 'Publissh======>',sysState[0]
00217                 #for p in sysState[0]:
00218                 #       print p
00219                 #print 'Subscriber======>',sysState[1]
00220                 #for s in sysState[1]:
00221                 #       print s
00222                 for lst in sysState:
00223                         for row in lst:
00224                                 for node in row[1]:
00225                                         if (node == '/rosout') | (node == '/observers_generator') :
00226                                                 continue
00227                                         if node not in self.nodes_list:
00228                                                 self.nodes_list.append(node)
00229                                         #if (row[0] == '/rosout') | (row[0] == '/rosout_agg') :
00230                                         #       continue
00231                                         #self.node_topic_list.append(node)
00232                                         #self.extract_mem_cpu(node)
00233                                         #self.node_topic_list.append(row[0])
00234                 i = 0
00235                 for node in self.nodes_list:
00236                         #print 'node=',node
00237                         self.node_data_structure.append(node_data_structure(node))
00238                         #print 'class=',self.node_data_structure[i].get_node_name()
00239                         for item in sysState[0]:
00240                                 if (node in item[1]) & (item[0] in self.topics_list):
00241                                         self.node_data_structure[i].add_pub_topic(item[0])
00242                                         #print item[0],'is pub from', node
00243                         for item in sysState[1]:
00244                                 if (node in item[1]) & (item[0] in self.topics_list):
00245                                         self.node_data_structure[i].add_sub_topic(item[0])
00246                                         #print item[0],'is pub from', node
00247                         i = i + 1
00248                 #for nd_ds in self.node_data_structure: 
00249                 #       print 'Publisher for node', nd_ds.get_node_name()
00250                 #       nd_ds.print_pub()
00251                 #       print 'Subscriber for node', nd_ds.get_node_name()
00252                 #       nd_ds.print_sub()
00253                 
00254         def extract_mem_cpu(self, node):
00255                 a = subprocess.Popen("rosnode info " + node , shell=True,stdout=subprocess.PIPE)
00256                 parts = shlex.split(a.communicate()[0])
00257                 indx = parts.index("Pid:")
00258                 pid = parts[indx+1]
00259                 t = 0
00260                 maxMem = 0
00261                 maxCpu = 0
00262                 while t<=6:
00263                         p = subprocess.Popen("top -b -n 1 | grep -i %s" %pid, shell=True,stdout=subprocess.PIPE)
00264                         self.out = p.communicate()[0]
00265                         self.out1 = shlex.split(self.out)
00266                         cpu = self.out1[8]
00267                         mem = self.out1[9]
00268                         if cpu > maxCpu:
00269                                 maxCpu = cpu
00270                         if mem > maxMem:
00271                                 maxMem = mem
00272                         t = t + 1
00273                 self.nodes_list.append(maxCpu)
00274                 self.nodes_list.append(maxMem)
00275                 #print "Pid of node "+node+" is "+ pid + "with MaxCpu =" + maxCpu + "and MaxMem = " + maxMem
00276                 
00277         def extract_topics(self):
00278                 pubcode, statusMessage, topicList = self.m.getPublishedTopics(self.caller_id, "")
00279                 i = 0
00280                 for topic in topicList:
00281                         if (topic[0] == '/rosout') | (topic[0] == '/rosout_agg') :
00282                                 continue
00283                         if topic[0] not in self.topics_list:
00284                                 self.topics_list.append(topic[0])
00285                         msg_class = roslib.message.get_message_class(topic[1])
00286                         self.topic_data_structure.append(topic_data_structure(topic[0],self.param_f_ws))
00287                         sb=rospy.Subscriber(topic[0], msg_class, self.callback)
00288                         i = i + 1
00289                 #thread.start_new_thread(self.spin_thread,('v',0.5))
00290                 #time.sleep(5)
00291                 #for tpc_ds in self.topic_data_structure:
00292                         #print '===================='
00293                         #tpc_ds.print_frq_list()
00294                 #print tpc_ds.get_calculated_frq()
00295 
00296         def calc_frq_dev():
00297                 n = len(self.calc_frq)
00298                 mean = sum(self.calc_frq)/n
00299                 sd = sqrt(sum((x-mean)**2 for x in self.calc_frq) / n)  
00300                 print self.calc_frq
00301                 print 'sum=',sum(self.calc_frq),'Average=',mean,'sd=',sd
00302 
00303         def callback(self,data):
00304                 print data
00305                 topic = data._connection_header['topic']
00306                 curr_t = time.time()
00307                 for tpc_ds in self.topic_data_structure:
00308                         if topic == tpc_ds.get_topic_name():
00309                                 delta_t = curr_t - tpc_ds.get_prev_t()
00310                                 tpc_ds.calc_frq(delta_t)
00311                                 tpc_ds.set_prev_t(curr_t)
00312                                 break
00313                 
00314         def average_delta_t(self):
00315                 s = 0
00316                 for val in self.circular_queu:
00317                         s = s + val
00318                 return s/self.param_ws
00319 
00320         def run_observers(self):
00321                 output = subprocess.Popen("roslaunch tug_ist_diagnosis_launch obs_auto.launch" , shell=True,stdout=subprocess.PIPE)
00322 
00323         def run_model_server(self):
00324                 command = "rosrun tug_ist_diagnosis_model diagnosis_model_server.py"
00325                 param = " _model:=/home/safdar/my_workspace/model_based_diagnosis/tug_ist_diagnosis_model/diagnosis_model.yaml"
00326                 output = subprocess.Popen(command+param , shell=True,stdout=subprocess.PIPE)
00327                 
00328         
00329         def report_error(self):
00330                 print '\n No Running System!'
00331                 self.calculate_frq_dev()
00332                 sys.exit(os.EX_USAGE)
00333         def spin_thread(self,string,st,*args):
00334                 rospy.spin()
00335                         
00336         
00337 if __name__ == '__main__':
00338       generator = Generator()
00339       generator.start()