MObs.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 ##
00004 # MObs.py is a Multiple Observer.
00005 # It needs three parameters Required frequency, frequency deviation and window size.
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 # The Multiple Observer observers two topics(Triggering and Triggered)
00024 # and provides /observations topic compatible for our Model Based Diagnosis.
00025 # It needs three parameters Trigerring topic, trigerred topic and time (ms).
00026 
00027 import roslib; roslib.load_manifest('tug_ist_diagnosis_observers')
00028 import rospy
00029 import sys
00030 import xmlrpclib
00031 import os
00032 from std_msgs.msg import String
00033 from tug_ist_diagnosis_msgs.msg import Observations
00034 import time
00035 import thread
00036 
00037 class Multiple_Observer_Triggered(object):
00038 
00039     def __init__(self):
00040                                         rospy.init_node('Mobs_Node', anonymous=True)
00041                                         self.m = xmlrpclib.ServerProxy(os.environ['ROS_MASTER_URI'])
00042                                         self.caller_id = '/script'
00043                                         self.topic_in = ""
00044                                         self.topic_type_in = ""
00045                                         self.topic_out = ""
00046                                         self.topic_type_out = ""
00047                                         self.triggered = False
00048                                         self.triggering = False
00049                                         self.time_triggered = None
00050                                         self.time_triggering = None
00051                                         self.started = False
00052                                         self.pub = rospy.Publisher('/observations', Observations)
00053                                         self.param_in_topic = rospy.get_param('~in_topic', '/Topic1')
00054                                         self.param_out_topic = rospy.get_param('~out_topic', '/Topic2')
00055                                         self.param_tm =  rospy.get_param('~tim', 500)
00056                                         self.req_delta_t = float(self.param_tm)/1000.0
00057                                         thread.start_new_thread(self.check_topic,(self.param_in_topic,2))
00058                                         
00059     def start(self):
00060                                 pubcode, statusMessage, topicList = self.m.getPublishedTopics(self.caller_id, "")
00061                                 if self.param_in_topic[0] != '/':
00062                                         self.param_in_topic = "/%s" % (self.param_in_topic)
00063                                 if self.param_out_topic[0] != '/':
00064                                         self.param_out_topic = "/%s" % (self.param_out_topic)
00065                                 top_in_found = False
00066                                 top_out_found = False
00067                                 for item in topicList:
00068                                                 if item[0] == self.param_in_topic:
00069                                                         self.topic_in = item[0]
00070                                                         self.topic_type_in = item[1]
00071                                                         top_in_found = True
00072                                                 if item[0] == self.param_out_topic:
00073                                                         self.topic_out = item[0]
00074                                                         self.topic_type_out = item[1]
00075                                                         top_out_found = True
00076                                 if top_in_found == True:
00077                                                 msg_class_in = roslib.message.get_message_class(self.topic_type_in)
00078                                 else:
00079                                                 self.report_error()
00080                                 if top_in_found == True:
00081                                                 msg_class_out = roslib.message.get_message_class(self.topic_type_out)
00082                                 else:
00083                                                 self.report_error()
00084                                 rospy.Subscriber(self.topic_in, msg_class_in, self.callback_Triggering)
00085                                 rospy.Subscriber(self.topic_out, msg_class_out, self.callback_Triggered)
00086                                 rospy.spin()
00087                                         
00088         
00089     def callback_Triggering(self,data):
00090                                                 self.time_triggering = time.time()
00091                                                 self.triggering = True
00092                                                 
00093                                                 
00094                                 
00095     def callback_Triggered(self,data):
00096                                                 topic = self.param_out_topic
00097                                                 if topic[0] == '/':
00098                                                         topic = topic[1:len(topic)]
00099                                                 self.time_triggered = time.time()
00100                                                 self.triggered = True
00101                                                 obs_msg = []
00102                                                 if self.triggering:
00103                                                                 self.triggering = False
00104                                                                 diff_t = self.time_triggered - self.time_triggering
00105                                                                 if diff_t <= self.req_delta_t:
00106                                                                                         obs_msg.append('ok('+topic+'_Triggered)')
00107                                                                                         print "\nIn Time Triggered after seconds : ", diff_t
00108                                                                                         print 'ok('+topic+'_Triggered)'
00109                                                                                         self.pub.publish(Observations(time.time(),obs_msg))
00110                                                                 else:
00111                                                                                         obs_msg.append('~ok('+topic+'_Triggered)')
00112                                                                                         self.pub.publish(Observations(time.time(),obs_msg))
00113                                                                                         print "Late Triggered after seconds : ", diff_t
00114                                                                                         print '~ok('+topic+'_Triggered)'
00115                                                                 self.triggered = True
00116                                                 else:
00117                                                                 print '~ok('+topic+'_Triggered)'
00118                                                                 obs_msg.append('~ok('+topic+'_Triggered)')
00119                                                                 self.pub.publish(Observations(time.time(),obs_msg))
00120                                                 
00121 
00122     def check_topic(self,string,sleeptime,*args):
00123                                 while True:
00124                                                 t1 = 0
00125                                                 t2 = 0
00126                                                 pubcode, statusMessage, topicList = self.m.getPublishedTopics(self.caller_id, "")
00127                                                 for item in topicList:
00128                                                         if item[0] == self.param_in_topic:
00129                                                                         t1 = 1
00130                                                         if item[0] == self.param_out_topic:
00131                                                                         t2 = 1
00132                                                                         
00133                                                 if t1 == 0:
00134                                                                 t1 = 1
00135                                                                 print "Topic:[" +self.param_in_topic+ "] does not exist."
00136                                                 if t2 == 0:
00137                                                                 t2 = 1
00138                                                                 print "Topic:[" +self.param_out_topic+ "] does not exist."
00139                                                                 self.pub.publish(Observations(time.time(),['~Ok('+self.param_out_topic[1:len(self.param_out_topic)]+'_Triggered)']))
00140                                                 time.sleep(sleeptime) #sleep for a specified amount of time.
00141 
00142     def report_error(self):
00143                                 print 'rosrun tug_ist_diagnosis_observers MObs.py <Topic_Triggering> <Topic_ToBeTriggered> <Time_milisec>'
00144                                 print 'e.g rosrun tug_ist_diagnosis_observers MObs.py _in_topic:=Topic1 _out_topic:=Topic2 _tm:=500'
00145                                 sys.exit(os.EX_USAGE)
00146 
00147 
00148 if __name__ == '__main__':
00149                         MObs = Multiple_Observer_Triggered()
00150                         MObs.start()


tug_ist_diagnosis_observers
Author(s): Safdar Zaman, Gerald Steinbauer
autogenerated on Mon Jan 6 2014 11:51:27