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 sensor import Sensor
00025
00026 """ Class to poll sensors attached to the Element """
00027 class Sensors():
00028 def __init__(self, controller):
00029 self.controller = controller
00030
00031 self.sensors = list()
00032
00033 try:
00034 self.analog_sensors = rospy.get_param("~analog", dict({}))
00035 for name, params in self.analog_sensors.iteritems():
00036 self.sensors.append(Sensor(self.controller, name, digital=False, type=params['type'], rate=params['rate'], pin=params['pin']))
00037 rospy.loginfo(name + " " + str(params))
00038 except:
00039 pass
00040
00041 try:
00042 self.digital_sensors = rospy.get_param("~digital", dict({}))
00043 for name, params in self.digital_sensors.iteritems():
00044 self.sensors.append(Sensor(self.controller, name, digital=True, type=params['type'], rate=params['rate'], pin=params['pin']))
00045 rospy.loginfo(name + " " + str(params))
00046 except:
00047 pass
00048
00049 try:
00050 self.maxez1_sensors = rospy.get_param("~maxez1", dict({}))
00051 for name, params in self.maxez1_sensors.iteritems():
00052 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']))
00053 rospy.loginfo(name + " " + str(params))
00054 except:
00055 pass
00056
00057 if __name__ == '__main__':
00058 try:
00059 sensors = Sensors()
00060 except:
00061 pass