Go to the documentation of this file.00001
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