tray_monitor.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 import roslib; roslib.load_manifest('cob_tray_monitor')
00003 import rospy
00004 import math
00005 from std_msgs.msg import *
00006 from sensor_msgs.msg import *
00007 from cob_srvs.srv import *
00008 from collections import deque
00009 
00010 class Monitor():
00011     
00012     def __init__(self):        
00013         self.pub = rospy.Publisher("occupied", Bool)
00014         rospy.Subscriber("/tray_sensors/range_0", Range, self.range1_callback)
00015         rospy.Subscriber("/tray_sensors/range_1", Range, self.range2_callback)
00016         rospy.Subscriber("/tray_sensors/range_2", Range, self.range3_callback)
00017         rospy.Subscriber("/tray_sensors/range_3", Range, self.range4_callback)
00018         rospy.Service('occupied', Trigger, self.srv_callback)
00019         self.range1 = Range()
00020         self.queue1 = deque([1]) # init with arbitrary high value
00021         self.avg1 = sum(self.queue1)/len(self.queue1)
00022         self.range2 = Range()
00023         self.queue2 = deque([1])
00024         self.avg2 = sum(self.queue2)/len(self.queue2)
00025         self.range3 = Range()
00026         self.queue3 = deque([1])
00027         self.avg3 = sum(self.queue3)/len(self.queue3)
00028         self.range4 = Range()
00029         self.queue4 = deque([1])
00030         self.avg4 = sum(self.queue4)/len(self.queue4)
00031         self.occupied = False
00032         self.queueLen = 5 # lower: higher sensitivity, but also higher noise
00033         
00034         if not rospy.has_param('distance_limit'):
00035             rospy.logerr("no distance_limit specified")
00036             exit(1)
00037 
00038         self.limit = rospy.get_param('distance_limit')
00039         print self.limit
00040     
00041     def srv_callback(self,req):
00042         res = TriggerResponse()
00043         # res.success.data = self.occupied
00044         averages = [self.avg1, self.avg2, self.avg3, self.avg4]
00045         res.success.data = any(map(lambda x: x <= self.limit, averages))
00046         return res
00047 
00048     def range1_callback(self,msg):
00049         self.range1 = msg
00050         self.queue1.append(msg.range)
00051         if len(self.queue1)>=self.queueLen:
00052             self.queue1.popleft()
00053         self.avg1 = sum(self.queue1)/len(self.queue1)
00054     
00055     def range2_callback(self,msg):
00056         self.range2 = msg
00057         self.queue2.append(msg.range)
00058         if len(self.queue2)>=self.queueLen:
00059             self.queue2.popleft()
00060         self.avg2 = sum(self.queue2)/len(self.queue2)
00061     
00062     def range3_callback(self,msg):
00063         self.range3 = msg
00064         self.queue3.append(msg.range)
00065         if len(self.queue3)>=self.queueLen:
00066             self.queue3.popleft()
00067         self.avg3 = sum(self.queue3)/len(self.queue3)
00068     
00069     def range4_callback(self,msg):
00070         self.range4 = msg
00071         self.queue4.append(msg.range)
00072         if len(self.queue4)>=self.queueLen:
00073             self.queue4.popleft()
00074         self.avg4 = sum(self.queue4)/len(self.queue4)
00075 
00076     def publish_state(self):
00077         msg = Bool()
00078         # msg.data = False;
00079         averages = [self.avg1, self.avg2, self.avg3, self.avg4]
00080         # print ', '.join(map(str,averages))
00081         msg.data = any(map(lambda x: x <= self.limit, averages))
00082         # if self.range1.range <= self.limit:
00083         #     msg.data = True;
00084         # if self.range2.range <= self.limit:
00085         #     msg.data = True;
00086         # if self.range3.range <= self.limit:
00087         #     msg.data = True;
00088         # if self.range4.range <= self.limit:
00089         #     msg.data = True;
00090         #print msg.data
00091         self.pub.publish(msg)
00092 
00093 if __name__ == "__main__":
00094     rospy.init_node('tactile_sensors')
00095     rospy.sleep(0.5)
00096     
00097     m = Monitor()
00098     rospy.loginfo("tray monitor running")
00099     
00100     r = rospy.Rate(10)
00101     while not rospy.is_shutdown():
00102         m.publish_state()
00103         r.sleep()


cob_tray_monitor
Author(s): Florian Weisshardt
autogenerated on Wed Aug 26 2015 11:00:57