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
00026
00027 import roslib.message; roslib.load_manifest('tug_ist_diagnosis_observers')
00028 import rospy
00029 import sys
00030 import xmlrpclib
00031 import os
00032 from tug_ist_diagnosis_msgs.msg import Observations
00033 import time
00034 import numpy as np
00035 import math
00036 import time
00037 import thread
00038
00039 class Regression(object):
00040 def __init__(self,ws,b):
00041 self.s = [[],[],[],[]]
00042 self.t = []
00043 self.prev_value = 0
00044 self.ws = ws
00045 self.n = None
00046 self.b = b;
00047
00048 def show(self):
00049 print self.s
00050
00051 def find(self, value,time):
00052 self.s[0].append(value)
00053 self.s[1].append(value)
00054 self.s[2].append(value)
00055 self.s[3].append(value)
00056 self.t.append(time)
00057 trend = self.find_slope(self.b)
00058 return trend
00059
00060 def find_slope(self,b):
00061 r1 = self.linear_regression(self.ws,self.s[0],self.t)
00062 self.s[1].pop()
00063 self.s[1].append(r1)
00064 r2 = self.linear_regression(self.ws,self.s[1],self.t)
00065 self.s[2].pop()
00066 self.s[2].append(r2)
00067 r3 = self.linear_regression(self.ws,self.s[2],self.t)
00068 self.s[3].pop()
00069 self.s[3].append(r3)
00070 self.remove_tails()
00071 if r1<-self.b:
00072 self.prev_value = -1
00073 return -1
00074 elif r1>self.b:
00075 self.prev_value = +1
00076 return +1
00077 elif ~(( (r2>-self.b)&(r2<self.b) ) & ( (r3>-self.b)&(r3<self.b) )):
00078 self.prev_value = 0
00079 return 0
00080 else:
00081 return self.prev_value
00082
00083
00084 def remove_tails(self):
00085 i = 0
00086 while( i< (len(self.s[0])-self.n) ):
00087 self.s[0].pop(0)
00088 self.s[1].pop(0)
00089 self.s[2].pop(0)
00090 self.s[3].pop(0)
00091 self.t.pop(0)
00092
00093 def linear_regression(self, ws,s,t):
00094 Sum_xy = 0.0
00095 Sum_x = 0.0
00096 Sum_y = 0.0
00097 Sum_xx = 0.0
00098 last_indx = len(t) - 1
00099 i = last_indx
00100 n = 0
00101 while (i >-1) & ( (t[last_indx]-t[i]) < ws ):
00102 Sum_xy = Sum_xy + t[i] * s[i]
00103 Sum_x = Sum_x + t[i]
00104 Sum_y = Sum_y + s[i]
00105 Sum_xx = Sum_xx + t[i] * t[i]
00106 i = i - 1
00107 n = n + 1
00108
00109
00110 self.n = n
00111 if (n * Sum_xx - (Sum_x * Sum_x)) <> 0 :
00112 slope = (n*Sum_xy - Sum_x * Sum_y)/(n * Sum_xx - (Sum_x * Sum_x))
00113 return slope
00114 else:
00115 return 0
00116
00117
00118 class Qualitative_Observer(object):
00119
00120 def __init__(self):
00121 rospy.init_node('QObs', anonymous=True)
00122 self.caller_id = '/script'
00123 self.m = xmlrpclib.ServerProxy(os.environ['ROS_MASTER_URI'])
00124 self.param_field = rospy.get_param('~field', 'pose.pose.position.x')
00125 self.param_topic = rospy.get_param('~topic', '/pose')
00126 self.param_ws = rospy.get_param('~ws', 1000)
00127 self.param_b = rospy.get_param('~b', 0.0000005)
00128 self.ws = float(self.param_ws)/1000.0
00129 self.topic = ""
00130 self.topic_type = ""
00131 self.data = None
00132 self.queu = [[0.0 for i in xrange(100)],[0.0 for i in xrange(100)]]
00133 self.pub = rospy.Publisher('/observations', Observations)
00134 self.started = True
00135 self.curr_t = None
00136 self.prev_t = time.time()
00137 self.regression = Regression(self.ws,self.param_b)
00138 thread.start_new_thread(self.check_thread,(self.param_topic,2))
00139
00140
00141 def start(self):
00142 self.params = []
00143 field=self.param_field
00144 i=field.count('.')
00145 for p in xrange(i):
00146 i=field.find('.')
00147 self.params.append(field[0:i])
00148 field = field[i+1:len(field)]
00149 self.params.append(field)
00150 pubcode, statusMessage, topicList = self.m.getPublishedTopics(self.caller_id, "")
00151 topic_found = False
00152 for item in topicList:
00153 if item[0] == self.param_topic:
00154 self.topic = item[0]
00155 self.topic_type = item[1]
00156 topic_found = True
00157 if topic_found == True:
00158 msg_class = roslib.message.get_message_class(self.topic_type)
00159 else:
00160 self.report_error()
00161 rospy.Subscriber(self.topic, msg_class, self.call_back)
00162 rospy.spin()
00163
00164 def call_back(self,data):
00165 self.curr_t = time.time() - self.prev_t
00166 self.extract_data(data)
00167 Trend = self.regression.find(self.data,self.curr_t)
00168 self.make_output(Trend)
00169
00170 def extract_data(self,data):
00171 self.data = data
00172 c = 0
00173 while (c < len(self.params)):
00174 for f in self.data.__slots__:
00175 if f == self.params[c]:
00176 self.data = getattr(self.data, f)
00177 break
00178 c = c + 1
00179 def make_output(self, Trend):
00180 if self.topic[0] == '/':
00181 self.topic = self.topic[1:len(self.topic)]
00182 obs_msg = []
00183 if Trend == +1 :
00184 rospy.loginfo('1');
00185 obs_msg.append('inc('+self.topic+'_'+self.param_field+')')
00186 self.pub.publish(Observations(time.time(),obs_msg))
00187
00188 elif Trend == -1 :
00189 rospy.loginfo('-1');
00190 obs_msg.append('dec('+self.topic+'_'+self.param_field+')')
00191 self.pub.publish(Observations(time.time(),obs_msg))
00192 else:
00193 rospy.loginfo('0');
00194 obs_msg.append('con('+self.topic+'_'+self.param_field+')')
00195 self.pub.publish(Observations(time.time(),obs_msg))
00196
00197 def check_thread(self,string,sleeptime,*args):
00198 while True:
00199 t = 0
00200 m = xmlrpclib.ServerProxy(os.environ['ROS_MASTER_URI'])
00201 pubcode, statusMessage, topicList = m.getPublishedTopics(self.caller_id, "")
00202 for item in topicList:
00203 if item[0] == string:
00204 t = 1
00205 break
00206 if t == 0:
00207 t = 1
00208 print "Topic:[" +string+ "] does not exist."
00209 time.sleep(sleeptime)
00210
00211 def report_error(self):
00212 print 'rosrun tug_ist_diagnosis_observers QObs.py <Topic> <Field_variable> <WindowSize>'
00213 print 'e.g rosrun tug_ist_diagnosis_observers QObs.py _topic:=/odom _field:=pose.pose.position.x _ws:=1000 _b:=0.0005'
00214 sys.exit(os.EX_USAGE)
00215
00216 if __name__=="__main__":
00217 QObs = Qualitative_Observer()
00218 QObs.start()
00219
00220