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                 self.max_cpu = 0
00039                 self.max_mem = 0
00040         def get_node_name(self):
00041                 return self.node_name
00042         def add_pub_topic(self,topic):
00043                 self.pub_topic_list.append(topic)
00044         def add_sub_topic(self,topic):
00045                 self.sub_topic_list.append(topic)
00046         def get_pub_topics(self):
00047                 return self.pub_topic_list
00048         def get_sub_topics(self):
00049                 return self.sub_topic_list
00050         def set_max_cpu(self,max_cpu):
00051                 self.max_cpu = max_cpu
00052         def get_max_cpu(self):
00053                 return self.max_cpu
00054         def set_max_mem(self,max_mem):
00055                 self.max_mem = max_mem
00056         def get_max_mem(self):
00057                 return self.max_mem
00058 
00059 
00060 class topic_data_structure(object):
00061         def __init__(self,topic_name,ws):
00062                 self.topic_name = topic_name
00063                 self.ws = ws
00064                 self.circular_queu = [0 for i in xrange(self.ws)]
00065                 self.frq_list = [0 for i in xrange(self.ws)]
00066                 self.calculated_frq = 0
00067                 self.calculated_frq_dev = 0
00068                 self.prev_t = time.time()
00069         def get_topic_name(self):
00070                 return self.topic_name
00071         def set_prev_t(self,prev_t):
00072                 self.prev_t = prev_t
00073         def get_prev_t(self):
00074                 return self.prev_t
00075         def calc_frq(self,delta_t):
00076                 self.circular_queu.pop(0)
00077                 self.circular_queu.append(delta_t)
00078                 sm = 0
00079                 for dt in self.circular_queu:
00080                         sm = sm + dt
00081                 self.calculated_frq = 1/(sm/self.ws)
00082                 self.frq_list.pop(0)
00083                 self.frq_list.append(self.calculated_frq)
00084         def calculate_frq_dev(self):
00085                 n = len(self.frq_list)
00086                 mean = sum(self.frq_list)/n
00087                 self.calculated_frq_dev = sqrt(sum((x-mean)**2 for x in self.frq_list)/n)       
00088         def get_calculated_frq(self):
00089                 return self.calculated_frq
00090         def get_calculated_frq_dev(self):
00091                 return self.calculated_frq_dev
00092         def print_frq_list(self):
00093                 for f in self.frq_list:
00094                         print f
00095                 
00096 
00097 class Generator(object):
00098         def __init__(self):
00099                 rospy.init_node('observers_generator', anonymous=False)
00100                 self.m = xmlrpclib.ServerProxy(os.environ['ROS_MASTER_URI'])
00101                 self.caller_id = '/script'
00102                 self.param_gen_ws = rospy.get_param('~gen_ws', 10)
00103                 self.param_g_ws = rospy.get_param('~gob_ws', 10)
00104                 self.param_p_ws = rospy.get_param('~pob_ws', 10)
00105                 self.p_mismatch_th = rospy.get_param('~pob_mismatch_th', 5)
00106                 self.brd_topic = rospy.get_param('~board_topic', '/board_measurments')
00107                 self.nodes_list = []
00108                 self.topics_list = []
00109                 self.topic_data_structure = []
00110                 self.node_data_structure = []
00111                 self.node_topic_list = []
00112                 self.topics_type_list = []
00113                 self.counter = 0
00114                 self.global_list = []
00115                 self.param_frq = 0
00116                 self.param_dev = 0
00117                 
00118 
00119         def start(self):
00120                 self.extract_topics()
00121                 self.extract_nodes()
00122                 self.make_obs_launch()
00123                 self.make_mdl_yaml()
00124                         
00125         def make_obs_launch(self):
00126                 temp_path = "/home/safdar/my_workspace/model_based_diagnosis/tug_ist_diagnosis_launch/launch/obs_auto.launch"
00127                 file = open(temp_path, 'wb')
00128                 file.write('<?xml version="1.0"?>\n<launch>\n')
00129                 if self.brd_topic in self.topics_list:
00130                         file.write('<node pkg="tug_ist_diagnosis_observers" type="HObs.py" name="BoardHObs" >\n')
00131                         file.write('</node>\n')
00132                 for nd_ds in self.node_data_structure:
00133                         node_name = nd_ds.get_node_name()
00134                         node_name = node_name[1:len(node_name)]
00135                         node_mxCpu = nd_ds.get_max_cpu()
00136                         node_mxMem = nd_ds.get_max_mem()
00137                         nobs_name = node_name+'NObs'
00138                         pubs = nd_ds.get_pub_topics()
00139                         if '/diagnostics' in pubs:
00140                                 dobs_name = node_name+'DObs'
00141                                 file.write('<node pkg="tug_ist_diagnosis_observers" type="DObs.py" name="'+dobs_name+'" >\n')
00142                                 file.write('\t<param name="dev_node" value="'+node_name+'" />\n')
00143                                 file.write('</node>\n')
00144                         file.write('<node pkg="tug_ist_diagnosis_observers" type="NObs.py" name="'+nobs_name+'" >\n')
00145                         file.write('\t<param name="node" value="'+node_name+'" />\n')
00146                         file.write('</node>\n')
00147                         pobs_name = node_name+'CpuPObs' 
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="cpu" />\n')
00151                         file.write('\t<param name="max_val" value="'+node_mxCpu+'" />\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                         pobs_name = node_name+'MemPObs' 
00156                         file.write('<node pkg="tug_ist_diagnosis_observers" type="PObs.py" name="'+pobs_name+'" >\n')
00157                         file.write('\t<param name="node" value="'+node_name+'" />\n')
00158                         file.write('\t<param name="property" value="mem" />\n')
00159                         file.write('\t<param name="max_val" value="'+node_mxMem+'" />\n')
00160                         file.write('\t<param name="mismatch_th" value="'+str(self.p_mismatch_th)+'" />\n')
00161                         file.write('\t<param name="ws" value="'+str(self.param_p_ws)+'" />\n')
00162                         file.write('</node>\n')
00163                 for t in self.topics_list:
00164                         for tpc_ds in self.topic_data_structure:
00165                                 if t == tpc_ds.get_topic_name():
00166                                         tpc_ds.calculate_frq_dev()
00167                                         self.param_frq = tpc_ds.get_calculated_frq()
00168                                         self.param_dev = tpc_ds.get_calculated_frq_dev()
00169                                         break
00170                         topic_name = t[1:len(t)]
00171                         gobs_name = topic_name+'GObs'
00172                         file.write('<node pkg="tug_ist_diagnosis_observers" type="GObs.py" name="'+gobs_name+'" >\n')
00173                         file.write('\t<param name="topic" value="'+topic_name+'" />\n')
00174                         file.write('\t<param name="frq" value="'+str(self.param_frq)+'" />\n')
00175                         file.write('\t<param name="dev" value="'+str(self.param_dev)+'" />\n')
00176                         file.write('\t<param name="ws" value="'+str(self.param_g_ws)+'" />\n')
00177                         file.write('</node>\n')
00178                 file.write('</launch>')
00179                 file.close()
00180 
00181         def make_mdl_yaml(self):
00182                 temp_path = "/home/safdar/my_workspace/model_based_diagnosis/tug_ist_diagnosis_model/diagnosis_model.yaml"
00183                 file = open(temp_path, 'wb')
00184                 file.write('ab: "AB"\nnab: "NAB"\nneg_prefix: "not_"\n\nprops:\n')
00185                 for n in self.nodes_list:
00186                         node_name = n[1:len(n)]
00187                         file.write('  - running('+node_name+')\n')
00188                 for t in self.topics_list:
00189                         topic_name = t[1:len(t)]
00190                         file.write('  - ok('+topic_name+')\n')
00191                 file.write('rules:\n')
00192                 for nd_ds in self.node_data_structure:
00193                         node_name = nd_ds.get_node_name()
00194                         node_name = node_name[1:len(node_name)]
00195                         file.write('  - NAB('+node_name+') -> running('+node_name+')\n')
00196                         pubs = nd_ds.get_pub_topics()
00197                         subs = nd_ds.get_sub_topics()
00198                         for p in pubs:
00199                                 str_subs = ''
00200                                 for s in subs:
00201                                         str_subs = str_subs + ', ok('+ s[1:len(s)] +')'
00202                                 file.write('  - NAB('+node_name+')'+str_subs+' -> ok('+p[1:len(p)]+')\n')
00203                 file.close()
00204         def extract_nodes(self):
00205                 code, statusMessage, sysState = self.m.getSystemState(self.caller_id)
00206                 for lst in sysState:
00207                         for row in lst:
00208                                 for node in row[1]:
00209                                         if (node == '/rosout') | (node == '/observers_generator') | (node == '/tf') | (node == '/board_controller') :
00210                                                 continue
00211                                         if node not in self.nodes_list:
00212                                                 self.nodes_list.append(node)
00213                                         
00214                 i = 0
00215                 for node in self.nodes_list:
00216                         self.node_data_structure.append(node_data_structure(node))
00217                         node_ds = self.node_data_structure[i]
00218                         self.extract_mem_cpu(node,node_ds)
00219                         for item in sysState[0]:
00220                                 if (node in item[1]) & (item[0] in self.topics_list):
00221                                         node_ds.add_pub_topic(item[0])
00222                         for item in sysState[1]:
00223                                 if (node in item[1]) & (item[0] in self.topics_list):
00224                                         node_ds.add_sub_topic(item[0])
00225                         i = i + 1
00226                 
00227         def extract_mem_cpu(self, node, node_ds):
00228                 a = subprocess.Popen("rosnode info " + node , shell=True,stdout=subprocess.PIPE)
00229                 parts = shlex.split(a.communicate()[0])
00230                 indx = parts.index("Pid:")
00231                 pid = parts[indx+1]
00232                 t = 0
00233                 maxMem = 0
00234                 maxCpu = 0
00235                 while t<=6:
00236                         p = subprocess.Popen("top -b -n 1 | grep -i %s" %pid, shell=True,stdout=subprocess.PIPE)
00237                         self.out = p.communicate()[0]
00238                         self.out1 = shlex.split(self.out)
00239                         cpu = self.out1[8]
00240                         mem = self.out1[9]
00241                         if cpu > maxCpu:
00242                                 maxCpu = cpu
00243                         if mem > maxMem:
00244                                 maxMem = mem
00245                         t = t + 1
00246                 node_ds.set_max_cpu(maxCpu)
00247                 node_ds.set_max_mem(maxMem)
00248                 
00249         def extract_topics(self):
00250                 pubcode, statusMessage, topicList = self.m.getPublishedTopics(self.caller_id, "")
00251                 i = 0
00252                 for topic in topicList:
00253                         if (topic[0] == '/rosout') | (topic[0] == '/rosout_agg') | (topic[0] == '/tf') | (topic[0] == '/map_metadata') | (topic[0] == '/map') | ('/board_controller' in topic[0]) :
00254                                 continue
00255                         if topic[0] not in self.topics_list:
00256                                 self.topics_list.append(topic[0])
00257                         print 'topic 0', topic[0],'  topic 1', topic[1]
00258                         msg_class = roslib.message.get_message_class(topic[1])
00259                         self.topic_data_structure.append(topic_data_structure(topic[0],self.param_gen_ws))
00260                         sb=rospy.Subscriber(topic[0], msg_class, self.callback, topic[0])
00261                         i = i + 1
00262                 thread.start_new_thread(self.spin_thread,('thread', 1))
00263                 time.sleep(3)
00264                 
00265         def callback(self,data,topic):
00266                 curr_t = time.time()
00267                 for tpc_ds in self.topic_data_structure:
00268                         if topic == tpc_ds.get_topic_name():
00269                                 delta_t = curr_t - tpc_ds.get_prev_t()
00270                                 tpc_ds.calc_frq(delta_t)
00271                                 tpc_ds.set_prev_t(curr_t)
00272                                 break
00273                 
00274         def average_delta_t(self):
00275                 s = 0
00276                 for val in self.circular_queu:
00277                         s = s + val
00278                 return s/self.param_ws
00279 
00280         def run_observers(self):
00281                 output = subprocess.Popen("roslaunch tug_ist_diagnosis_launch obs_auto.launch" , shell=True,stdout=subprocess.PIPE)
00282 
00283         def run_model_server(self):
00284                 command = "rosrun tug_ist_diagnosis_model diagnosis_model_server.py"
00285                 param = "_model:=/home/safdar/my_workspace/model_based_diagnosis/tug_ist_diagnosis_model/diagnosis_model.yaml"
00286                 output = subprocess.Popen(command+param , shell=True,stdout=subprocess.PIPE)
00287                 
00288         
00289         def report_error(self):
00290                 print '\n No Running System!'
00291                 self.calculate_frq_dev()
00292                 sys.exit(os.EX_USAGE)
00293         def spin_thread(self,string,st,*args):
00294                 rospy.spin()
00295                         
00296         
00297 if __name__ == '__main__':
00298       generator = Generator()
00299       generator.start()