00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 import roslib.message; roslib.load_manifest('tug_ist_diagnosis_observers')
00026 import rospy
00027 import sys
00028 import xmlrpclib
00029 import os
00030 from tug_ist_diagnosis_msgs.msg import Observations
00031 import time
00032 import numpy as np
00033 import math
00034 import time
00035 import thread
00036
00037 class Regression(object):
00038 def __init__(self,ws,pb,nb):
00039 self.s = [[],[],[],[]]
00040 self.t = []
00041 self.prev_value = 0
00042 self.ws = ws
00043 self.n = None
00044 self.pb = pb;
00045 self.nb = nb;
00046 self.choice = 'n'
00047 self.value = 0
00048
00049 def show(self):
00050 print self.s
00051
00052 def find(self, value,time,ch):
00053 self.choice = ch
00054 self.value = value
00055 self.s[0].append(value)
00056 self.s[1].append(value)
00057 self.s[2].append(value)
00058 self.s[3].append(value)
00059 self.t.append(time)
00060 trend = self.find_slope(self.pb,self.nb)
00061 return trend
00062
00063 def find_slope(self,pb,nb):
00064 if self.choice == 'y':
00065 r1 = self.linear_regression(self.ws,self.s[0],self.t)
00066 else:
00067 r1 = self.value
00068 self.s[1].pop()
00069 self.s[1].append(r1)
00070 r2 = self.linear_regression(self.ws,self.s[1],self.t)
00071 self.s[2].pop()
00072 self.s[2].append(r2)
00073 r3 = self.linear_regression(self.ws,self.s[2],self.t)
00074 self.s[3].pop()
00075 self.s[3].append(r3)
00076 self.remove_tails()
00077 if r1<nb:
00078 self.prev_value = -1
00079 return -1
00080 elif r1>pb:
00081 self.prev_value = +1
00082 return +1
00083 elif ~(( (r2>nb)&(r2<pb) ) & ( (r3>nb)&(r3<pb) )):
00084 self.prev_value = 0
00085 return 0
00086 else:
00087 return self.prev_value
00088
00089
00090 def remove_tails(self):
00091 i = 0
00092 while( i< (len(self.s[0])-self.n) ):
00093 self.s[0].pop(0)
00094 self.s[1].pop(0)
00095 self.s[2].pop(0)
00096 self.s[3].pop(0)
00097 self.t.pop(0)
00098
00099 def linear_regression(self, ws,s,t):
00100 Sum_xy = 0.0
00101 Sum_x = 0.0
00102 Sum_y = 0.0
00103 Sum_xx = 0.0
00104 last_indx = len(t) - 1
00105 i = last_indx
00106 n = 0
00107 while (i >-1) & ( (t[last_indx]-t[i]) < ws ):
00108 Sum_xy = Sum_xy + t[i] * s[i]
00109 Sum_x = Sum_x + t[i]
00110 Sum_y = Sum_y + s[i]
00111 Sum_xx = Sum_xx + t[i] * t[i]
00112 i = i - 1
00113 n = n + 1
00114
00115
00116 self.n = n
00117 if (n * Sum_xx - (Sum_x * Sum_x)) <> 0 :
00118 slope = (n*Sum_xy - Sum_x * Sum_y)/(n * Sum_xx - (Sum_x * Sum_x))
00119 return slope
00120 else:
00121 return 0
00122
00123
00124 class Qualitative_Observer(object):
00125
00126 def __init__(self):
00127 rospy.init_node('BinaryQObs', anonymous=True)
00128 self.caller_id = '/script'
00129 self.m = xmlrpclib.ServerProxy(os.environ['ROS_MASTER_URI'])
00130 self.param_field1 = rospy.get_param('~field1', 'pose.pose.position.x')
00131 self.param_topic1 = rospy.get_param('~topic1', '/pose')
00132 self.param_ws1 = rospy.get_param('~ws1', 1000)
00133 self.param_pb1 = rospy.get_param('~pb1', 0.0000005)
00134 self.param_nb1 = rospy.get_param('~nb1', -0.0000005)
00135 self.param_field2 = rospy.get_param('~field2', 'pose.pose.position.y')
00136 self.param_topic2 = rospy.get_param('~topic2', '/pose')
00137 self.param_ws2 = rospy.get_param('~ws2', 1000)
00138 self.param_pb2 = rospy.get_param('~pb2', 0.0000005)
00139 self.param_nb2 = rospy.get_param('~nb2', -0.0000005)
00140 self.mismatch_thr = rospy.get_param('~thr', 30)
00141 self.mode = rospy.get_param('~mode', 'LIN')
00142 reg_param = rospy.get_param('~reg','yy')
00143 if len(reg_param) != 2:
00144 print 'r1 parameter should be nn/yy/ny/yn'
00145 sys.exit(os.EX_USAGE)
00146 else:
00147 self.r11 = reg_param[0]
00148 self.r12 = reg_param[1]
00149 self.ws1 = float(self.param_ws1)/1000.0
00150 self.ws2 = float(self.param_ws2)/1000.0
00151 self.topic1 = ""
00152 self.topic1_type = ""
00153 self.topic2 = ""
00154 self.topic2_type = ""
00155 self.Trend1 = None
00156 self.Trend2 = None
00157 self.data = None
00158 self.num1 = 0
00159 self.num2 = 0
00160 self.sum1 = 0
00161 self.sum2 = 0
00162 self.queu = [[0.0 for i in xrange(100)],[0.0 for i in xrange(100)]]
00163 self.pub = rospy.Publisher('/observations', Observations)
00164 self.curr_t1 = None
00165 self.init_t = time.time()
00166 self.prev_t = time.time()
00167 self.prev_data = 0
00168 self.curr_t2 = None
00169 self.mismatch = 0
00170 thread.start_new_thread(self.check_thread,(self.param_topic1,self.param_topic2,0.2))
00171
00172
00173 def start(self):
00174 self.params1 = []
00175 field=self.param_field1
00176 i=field.count('.')
00177 for p in xrange(i):
00178 i=field.find('.')
00179 self.params1.append(field[0:i])
00180 field = field[i+1:len(field)]
00181 self.params1.append(field)
00182
00183 self.params2 = []
00184 field=self.param_field2
00185 i=field.count('.')
00186 for p in xrange(i):
00187 i=field.find('.')
00188 self.params2.append(field[0:i])
00189 field = field[i+1:len(field)]
00190 self.params2.append(field)
00191
00192 topic1_found = False
00193 topic2_found = False
00194 firstcheck = True
00195 while firstcheck:
00196 pubcode, statusMessage, topicList = self.m.getPublishedTopics(self.caller_id, "")
00197 for item in topicList:
00198 if item[0] == self.param_topic1:
00199 self.topic1 = item[0]
00200 self.topic1_type = item[1]
00201 topic1_found = True
00202 if item[0] == self.param_topic2:
00203 self.topic2 = item[0]
00204 self.topic2_type = item[1]
00205 topic2_found = True
00206 if topic1_found == True & topic2_found == True:
00207 firstcheck = False
00208 msg_class1 = roslib.message.get_message_class(self.topic1_type)
00209 msg_class2 = roslib.message.get_message_class(self.topic2_type)
00210 if self.mode == 'DRV1':
00211 rospy.Subscriber(self.topic1, msg_class1, self.drv_call_back)
00212 rospy.Subscriber(self.topic2, msg_class2, self.call_back2)
00213 self.regression1 = Regression(self.ws1,self.param_pb1,self.param_nb1)
00214 self.regression2 = Regression(self.ws2,self.param_pb2,self.param_nb2)
00215 else:
00216 rospy.Subscriber(self.topic1, msg_class1, self.call_back1)
00217 rospy.Subscriber(self.topic2, msg_class2, self.call_back2)
00218 self.regression1 = Regression(self.ws1,self.param_pb1,self.param_nb1)
00219 self.regression2 = Regression(self.ws2,self.param_pb2,self.param_nb2)
00220 thread.start_new_thread(self.BQObs_thread,(self.param_topic1,self.param_topic2,0.2))
00221 rospy.spin()
00222 time.sleep(1)
00223
00224
00225
00226 def call_back1(self,data):
00227 self.curr_t1 = time.time() - self.init_t
00228 curr_data = self.extract_data(data,self.params1)
00229 self.sum1 = self.sum1 + curr_data
00230 self.num1 = self.num1 + 1
00231
00232 def drv_call_back(self,data):
00233 self.curr_t1 = time.time() - self.init_t
00234 curr_t = time.time()
00235 delta_t = curr_t - self.prev_t
00236 curr_data = self.extract_data(data,self.params1)
00237 delta_data = curr_data - self.prev_data
00238 self.prev_data = curr_data
00239 self.prev_t = curr_t
00240 drv = delta_data/delta_t
00241 self.sum1 = self.sum1 + drv
00242 self.num1 = self.num1 + 1
00243
00244 def call_back2(self,data):
00245 self.curr_t2 = time.time() - self.init_t
00246 curr_data = self.extract_data(data,self.params2)
00247 self.sum2 = self.sum2 + curr_data
00248 self.num2 = self.num2 + 1
00249
00250 def extract_data(self,data,params):
00251 c = 0
00252 while (c < len(params)):
00253 for f in data.__slots__:
00254 if f == params[c]:
00255 data = getattr(data, f)
00256 break
00257 c = c + 1
00258 return data
00259
00260
00261 def publish_output(self):
00262 if self.topic1[0] == '/':
00263 self.topic1 = self.topic1[1:len(self.topic1)]
00264 if self.topic2[0] == '/':
00265 self.topic2 = self.topic2[1:len(self.topic2)]
00266 obs_msg = []
00267 if self.Trend1 == self.Trend2:
00268 if self.mismatch > 0:
00269 self.mismatch = self.mismatch - 1
00270 else:
00271 self.mismatch = self.mismatch + 1
00272
00273
00274 field1 = self.param_field1.replace('.', '_')
00275 field2 = self.param_field2.replace('.', '_')
00276 if self.mismatch <= self.mismatch_thr:
00277 rospy.loginfo('matched('+self.topic1+','+field1+','+self.topic2+','+field2+')')
00278 obs_msg.append('matched('+self.topic1+','+field1+','+self.topic2+','+field2+')')
00279 self.pub.publish(Observations(time.time(),obs_msg))
00280 else:
00281 rospy.loginfo('~matched('+self.topic1+','+field1+','+self.topic2+','+field2+')')
00282 obs_msg.append('~matched('+self.topic1+','+field1+','+self.topic2+','+field2+')')
00283 self.pub.publish(Observations(time.time(),obs_msg))
00284
00285 def BQObs_thread(self,topic1,topic2,sleeptime,*args):
00286 while True:
00287 t1 = 0
00288 t2 = 0
00289 pubcode, statusMessage, topicList = self.m.getPublishedTopics(self.caller_id, "")
00290 for item in topicList:
00291 if item[0] == topic1:
00292 t1 = 1
00293 if item[0] == topic2:
00294 t2 = 1
00295
00296 if t1 == 0:
00297 t1 = 1
00298 self.pub.publish(Observations(time.time(),['~matched('+topic1[1:len(topic1)]+','+topic2[1:len(topic2)]+')']))
00299 if t2 == 0:
00300 t2 = 1
00301 self.pub.publish(Observations(time.time(),['~matched('+topic1[1:len(topic1)]+','+topic2[1:len(topic2)]+')']))
00302 print('Num1='+str(self.num1)+',Num2'+str(self.num2))
00303 if (self.num1 != 0) & (self.num2 != 0):
00304 data1 = self.sum1/self.num1
00305 data2 = self.sum2/self.num2
00306 self.Trend1 = self.regression1.find(data1,self.curr_t1,self.r11)
00307 self.Trend2 = self.regression2.find(data2,self.curr_t2,self.r12)
00308 rospy.loginfo(self.topic1+'_avg_data='+ str(data1) +','+self.topic1+'Trend='+ str(self.Trend1) +','+self.topic2+'Trend='+str(self.Trend2)+','+self.topic2+'_avg_data='+str(data2))
00309 self.num1 = 0
00310 self.num2 = 0
00311 self.sum1 = 0.0
00312 self.sum2 = 0.0
00313 self.publish_output()
00314 time.sleep(sleeptime)
00315
00316 def check_thread(self,topic1,topic2,sleeptime,*args):
00317 try:
00318 while not rospy.is_shutdown():
00319 t1 = 0
00320 t2 = 0
00321 m = xmlrpclib.ServerProxy(os.environ['ROS_MASTER_URI'])
00322 pubcode, statusMessage, topicList = m.getPublishedTopics(self.caller_id, "")
00323 for item in topicList:
00324 if item[0] == topic1:
00325 t1 = 1
00326 if item[0] == topic2:
00327 t2 = 1
00328 if t1 == 0 | t2 == 0:
00329 t1 = 1
00330 self.pub.publish(Observations(time.time(),['~matched('+topic1[1:len(topic1)]+','+topic2[1:len(topic2)]+')']))
00331 time.sleep(sleeptime)
00332 except:
00333 print "An unhandled exception occured, here's the traceback!"
00334 traceback.print_exc()
00335
00336
00337 def report_error():
00338 print 'rosrun tug_ist_diagnosis_observers BiQObs.py <Topic> <Field_variable> <WindowSize>'
00339 print 'e.g rosrun tug_ist_diagnosis_observers QObs.py _topic:=/odom _field:=pose.pose.position.x _ws:=1000'
00340 sys.exit(os.EX_USAGE)
00341
00342 if __name__=="__main__":
00343 if len(sys.argv) < 3:
00344 report_error()
00345 QObs = Qualitative_Observer()
00346 QObs.start()
00347
00348