$search
00001 #!/usr/bin/env python 00002 00003 """ 00004 ROS Sensors Class for the cmRobot Element 00005 00006 Created for the Pi Robot Project: http://www.pirobot.org 00007 Copyright (c) 2012 Patrick Goebel. All rights reserved. 00008 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 2 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 at: 00018 00019 http://www.gnu.org/licenses/gpl.html 00020 """ 00021 00022 import roslib; roslib.load_manifest('element') 00023 import rospy 00024 from element.msg import SensorState 00025 import os 00026 import threading 00027 from sensor import Sensor 00028 00029 """ Class to poll sensors attached to the Element """ 00030 class Sensors(): 00031 def __init__(self, controller, thread_queue): 00032 self.controller = controller 00033 00034 self.sensors = list() 00035 00036 try: 00037 self.analog_sensors = rospy.get_param("~analog", dict({})) 00038 for name, params in self.analog_sensors.iteritems(): 00039 self.sensors.append(Sensor(self.controller, name, digital=False, type=params['type'], rate=params['rate'], pin=params['pin'])) 00040 rospy.loginfo(name + " " + str(params)) 00041 except: 00042 pass 00043 00044 try: 00045 self.digital_sensors = rospy.get_param("~digital", dict({})) 00046 for name, params in self.digital_sensors.iteritems(): 00047 self.sensors.append(Sensor(self.controller, name, digital=True, type=params['type'], rate=params['rate'], pin=params['pin'])) 00048 rospy.loginfo(name + " " + str(params)) 00049 except: 00050 pass 00051 00052 try: 00053 self.maxez1_sensors = rospy.get_param("~maxez1", dict({})) 00054 for name, params in self.maxez1_sensors.iteritems(): 00055 self.sensors.append(Sensor(self.controller, name, digital=True, type=params['type'], rate=params['rate'], pin=params['pin'], trigger_pin=params['trigger_pin'], output_pin=params['output_pin'])) 00056 rospy.loginfo(name + " " + str(params)) 00057 except: 00058 pass 00059 00060 def poll_sensors(self, thread_queue, sensor): 00061 t = threading.Timer(sensor.interval, self.poll_sensors, [thread_queue, sensor]) 00062 t.daemon = True 00063 t.start() 00064 thread_queue.put(sensor.poll) 00065 00066 if __name__ == '__main__': 00067 try: 00068 sensors = Sensors() 00069 except: 00070 pass