arduino_sensors.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004     Sensor class for the arudino_python package
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('ros_arduino_python')
00023 import rospy
00024 from sensor_msgs.msg import Range
00025 from ros_arduino_msgs.msg import *
00026 
00027 LOW = 0
00028 HIGH = 1
00029 
00030 INPUT = 0
00031 OUTPUT = 1
00032     
00033 class MessageType:
00034     ANALOG = 0
00035     DIGITAL = 1
00036     RANGE = 2
00037     FLOAT = 3
00038     INT = 4
00039     BOOL = 5
00040     
00041 class Sensor(object):
00042     def __init__(self, controller, name, pin, rate, frame_id, direction="input", **kwargs):
00043         self.controller = controller
00044         self.name = name
00045         self.pin = pin
00046         self.rate = rate
00047         self.direction = direction
00048 
00049         self.frame_id = frame_id
00050         self.value = None
00051         
00052         self.t_delta = rospy.Duration(1.0 / self.rate)
00053         self.t_next = rospy.Time.now() + self.t_delta
00054     
00055     def poll(self):
00056         now = rospy.Time.now()
00057         if now > self.t_next:
00058             if self.direction == "input":
00059                 try:
00060                     self.value = self.read_value()
00061                 except:
00062                     return
00063             else:
00064                 try:
00065                     self.ack = self.write_value()
00066                 except:
00067                     return          
00068     
00069             # For range sensors, assign the value to the range message field
00070             if self.message_type == MessageType.RANGE:
00071                 self.msg.range = self.value
00072             else:
00073                 self.msg.value = self.value
00074 
00075             # Add a timestamp and publish the message
00076             self.msg.header.stamp = rospy.Time.now()
00077             self.pub.publish(self.msg)
00078             
00079             self.t_next = now + self.t_delta
00080     
00081 class AnalogSensor(Sensor):
00082     def __init__(self, *args, **kwargs):
00083         super(AnalogSensor, self).__init__(*args, **kwargs)
00084                 
00085         self.message_type = MessageType.ANALOG
00086         
00087         self.msg = Analog()
00088         self.msg.header.frame_id = self.frame_id
00089         
00090         self.pub = rospy.Publisher("~sensor/" + self.name, Analog)
00091         
00092         if self.direction == "output":
00093             self.controller.pin_mode(self.pin, OUTPUT)
00094         else:
00095             self.controller.pin_mode(self.pin, INPUT)
00096 
00097         self.value = LOW
00098         
00099     def read_value(self):
00100         return self.controller.analog_read(self.pin)
00101     
00102     def write_value(self, value):
00103         return self.controller.analog_write(self.pin, value)
00104     
00105 class AnalogFloatSensor(Sensor):
00106     def __init__(self, *args, **kwargs):
00107         super(AnalogFloatSensor, self).__init__(*args, **kwargs)
00108                 
00109         self.message_type = MessageType.ANALOG
00110         
00111         self.msg = AnalogFloat()
00112         self.msg.header.frame_id = self.frame_id
00113         
00114         self.pub = rospy.Publisher("~sensor/" + self.name, AnalogFloat)
00115         
00116         if self.direction == "output":
00117             self.controller.pin_mode(self.pin, OUTPUT)
00118         else:
00119             self.controller.pin_mode(self.pin, INPUT)
00120 
00121         self.value = LOW
00122         
00123     def read_value(self):
00124         return self.controller.analog_read(self.pin)
00125     
00126     def write_value(self, value):
00127         return self.controller.analog_write(self.pin, value)
00128     
00129         
00130 class DigitalSensor(Sensor):
00131     def __init__(self, *args, **kwargs):
00132         super(DigitalSensor, self).__init__(*args, **kwargs)
00133         
00134         self.message_type = MessageType.BOOL
00135         
00136         self.msg = Digital()
00137         self.msg.header.frame_id = self.frame_id
00138         
00139         self.pub = rospy.Publisher("~sensor/" + self.name, Digital)
00140         
00141         if self.direction == "output":
00142             self.controller.pin_mode(self.pin, OUTPUT)
00143         else:
00144             self.controller.pin_mode(self.pin, INPUT)
00145 
00146         self.value = LOW
00147         
00148     def read_value(self):
00149         return self.controller.digital_read(self.pin)
00150     
00151     def write_value(self):
00152         # Alternate HIGH/LOW when writing at a fixed rate
00153         self.value = not self.value
00154         return self.controller.digital_write(self.pin, self.value)
00155     
00156     
00157 class RangeSensor(Sensor):
00158     def __init__(self, *args, **kwargs):
00159         super(RangeSensor, self).__init__(*args, **kwargs)
00160         
00161         self.message_type = MessageType.RANGE
00162         
00163         self.msg = Range()
00164         self.msg.header.frame_id = self.frame_id
00165         
00166         self.pub = rospy.Publisher("~sensor/" + self.name, Range)
00167         
00168     def read_value(self):
00169         self.msg.header.stamp = rospy.Time.now()
00170 
00171         
00172 class SonarSensor(RangeSensor):
00173     def __init__(self, *args, **kwargs):
00174         super(SonarSensor, self).__init__(*args, **kwargs)
00175         
00176         self.msg.radiation_type = Range.ULTRASOUND
00177         
00178         
00179 class IRSensor(RangeSensor):
00180     def __init__(self, *args, **kwargs):
00181         super(IRSensor, self).__init__(*args, **kwargs)
00182         
00183         self.msg.radiation_type = Range.INFRARED
00184         
00185 class Ping(SonarSensor):
00186     def __init__(self,*args, **kwargs):
00187         super(Ping, self).__init__(*args, **kwargs)
00188                 
00189         self.msg.field_of_view = 0.785398163
00190         self.msg.min_range = 0.02
00191         self.msg.max_range = 3.0
00192         
00193     def read_value(self):
00194         # The Arduino Ping code returns the distance in centimeters
00195         cm = self.controller.ping(self.pin)
00196         
00197         # Convert it to meters for ROS
00198         distance = cm / 100.0
00199         
00200         return distance
00201     
00202         
00203 class GP2D12(IRSensor):
00204     def __init__(self, *args, **kwargs):
00205         super(GP2D12, self).__init__(*args, **kwargs)
00206         
00207         self.msg.field_of_view = 0.001
00208         self.msg.min_range = 0.10
00209         self.msg.max_range = 0.80
00210         
00211     def read_value(self):
00212         value = self.controller.analog_read(self.pin)
00213         
00214         if value <= 3.0:
00215             return self.msg.max_range
00216         
00217         try:
00218             distance = (6787.0 / (float(value) - 3.0)) - 4.0
00219         except:
00220             return self.msg.max_range
00221             
00222         # Convert to meters
00223         distance /= 100.0
00224         
00225         # If we get a spurious reading, set it to the max_range
00226         if distance > self.msg.max_range: distance = self.msg.max_range
00227         if distance < self.msg.min_range: distance = self.msg.max_range
00228         
00229         return distance
00230     
00231 class PololuMotorCurrent(AnalogFloatSensor):
00232     def __init__(self, *args, **kwargs):
00233         super(PololuMotorCurrent, self).__init__(*args, **kwargs)
00234         
00235     def read_value(self):
00236         # From the Pololu source code
00237         milliamps = self.controller.analog_read(self.pin) * 34
00238         return milliamps / 1000.0
00239     
00240 class PhidgetsVoltage(AnalogFloatSensor):
00241     def __init__(self, *args, **kwargs):
00242         super(PhidgetsVoltage, self).__init__(*args, **kwargs)
00243         
00244     def read_value(self):
00245         # From the Phidgets documentation
00246         voltage = 0.06 * (self.controller.analog_read(self.pin) - 500.)
00247         return voltage
00248     
00249 class PhidgetsCurrent(AnalogFloatSensor):
00250     def __init__(self, *args, **kwargs):
00251         super(PhidgetsCurrent, self).__init__(*args, **kwargs)
00252         
00253     def read_value(self):
00254         # From the Phidgets documentation for the 20 amp DC sensor
00255         current = 0.05 * (self.controller.analog_read(self.pin) - 500.)
00256         return current
00257     
00258 class MaxEZ1Sensor(SonarSensor):
00259     def __init__(self, *args, **kwargs):
00260         super(MaxEZ1Sensor, self).__init__(*args, **kwargs)
00261         
00262         self.trigger_pin = kwargs['trigger_pin']
00263         self.output_pin = kwargs['output_pin']
00264         
00265         self.msg.field_of_view = 0.785398163
00266         self.msg.min_range = 0.02
00267         self.msg.max_range = 3.0
00268         
00269     def read_value(self):
00270         return self.controller.get_MaxEZ1(self.trigger_pin, self.output_pin)
00271 
00272             
00273 if __name__ == '__main__':
00274     myController = Controller()
00275     mySensor = SonarSensor(myController, "My Sonar", type=Type.PING, pin=0, rate=10)
00276             


ros_arduino_python
Author(s): Patrick Goebel
autogenerated on Wed Aug 26 2015 15:57:04