ros_arduino_python: ros_arduino_python: arduino

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="/base_link", 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     def read_value(self):
00093         return self.controller.analog_read(self.pin)
00094     
00095     def write_value(self, value):
00096         return self.controller.analog_write(self.pin, value)
00097     
00098 class AnalogFloatSensor(Sensor):
00099     def __init__(self, *args, **kwargs):
00100         super(AnalogFloatSensor, self).__init__(*args, **kwargs)
00101                 
00102         self.message_type = MessageType.ANALOG
00103         
00104         self.msg = AnalogFloat()
00105         self.msg.header.frame_id = self.frame_id
00106         
00107         self.pub = rospy.Publisher("~sensor/" + self.name, AnalogFloat)
00108     
00109         
00110 class DigitalSensor(Sensor):
00111     def __init__(self, *args, **kwargs):
00112         super(DigitalSensor, self).__init__(*args, **kwargs)
00113         
00114         self.message_type = MessageType.BOOL
00115         
00116         self.msg = Digital()
00117         self.msg.header.frame_id = self.frame_id
00118         
00119         self.pub = rospy.Publisher("~sensor/" + self.name, Digital)
00120         
00121         if self.direction == "output":
00122             self.controller.pin_mode(self.pin, OUTPUT)
00123         else:
00124             self.controller.pin_mode(self.pin, INPUT)
00125 
00126         self.value = LOW
00127         
00128     def read_value(self):
00129         return self.controller.digital_read(self.pin)
00130     
00131     def write_value(self):
00132         # Alternate HIGH/LOW when writing at a fixed rate
00133         self.value = not self.value
00134         return self.controller.digital_write(self.pin, self.value)
00135     
00136     
00137 class RangeSensor(Sensor):
00138     def __init__(self, *args, **kwargs):
00139         super(RangeSensor, self).__init__(*args, **kwargs)
00140         
00141         self.message_type = MessageType.RANGE
00142         
00143         self.msg = Range()
00144         self.msg.header.frame_id = self.frame_id
00145         
00146         self.pub = rospy.Publisher("~sensor/" + self.name, Range)
00147         
00148     def read_value(self):
00149         self.msg.header.stamp = rospy.Time.now()
00150 
00151         
00152 class SonarSensor(RangeSensor):
00153     def __init__(self, *args, **kwargs):
00154         super(SonarSensor, self).__init__(*args, **kwargs)
00155         
00156         self.msg.radiation_type = Range.ULTRASOUND
00157         
00158         
00159 class IRSensor(RangeSensor):
00160     def __init__(self, *args, **kwargs):
00161         super(IRSensor, self).__init__(*args, **kwargs)
00162         
00163         self.msg.radiation_type = Range.INFRARED
00164         
00165 class Ping(SonarSensor):
00166     def __init__(self,*args, **kwargs):
00167         super(Ping, self).__init__(*args, **kwargs)
00168                 
00169         self.msg.field_of_view = 0.785398163
00170         self.msg.min_range = 0.02
00171         self.msg.max_range = 3.0
00172         
00173     def read_value(self):
00174         # The Arduino Ping code returns the distance in centimeters
00175         cm = self.controller.ping(self.pin)
00176         
00177         # Convert it to meters for ROS
00178         distance = cm / 100.0
00179         
00180         return distance
00181     
00182         
00183 class GP2D12(IRSensor):
00184     def __init__(self, *args, **kwargs):
00185         super(GP2D12, self).__init__(*args, **kwargs)
00186         
00187         self.msg.field_of_view = 0.001
00188         self.msg.min_range = 0.10
00189         self.msg.max_range = 0.80
00190         
00191     def read_value(self):
00192         value = self.controller.analog_read(self.pin)
00193         
00194         if value <= 3.0:
00195             return self.msg.max_range
00196         
00197         try:
00198             distance = (6787.0 / (float(value) - 3.0)) - 4.0
00199         except:
00200             return self.msg.max_range
00201             
00202         # Convert to meters
00203         distance /= 100.0
00204         
00205         # If we get a spurious reading, set it to the max_range
00206         if distance > self.msg.max_range: distance = self.msg.max_range
00207         if distance < self.msg.min_range: distance = self.msg.max_range
00208         
00209         return distance
00210     
00211 class PololuMotorCurrent(AnalogFloatSensor):
00212     def __init__(self, *args, **kwargs):
00213         super(PololuMotorCurrent, self).__init__(*args, **kwargs)
00214         
00215     def read_value(self):
00216         # From the Pololu source code
00217         milliamps = self.controller.analog_read(self.pin) * 34
00218         return milliamps / 1000.0
00219     
00220 class PhidgetsVoltage(AnalogFloatSensor):
00221     def __init__(self, *args, **kwargs):
00222         super(PhidgetsVoltage, self).__init__(*args, **kwargs)
00223         
00224     def read_value(self):
00225         # From the Phidgets documentation
00226         voltage = 0.06 * (self.controller.analog_read(self.pin) - 500.)
00227         return voltage
00228     
00229 class PhidgetsCurrent(AnalogFloatSensor):
00230     def __init__(self, *args, **kwargs):
00231         super(PhidgetsCurrent, self).__init__(*args, **kwargs)
00232         
00233     def read_value(self):
00234         # From the Phidgets documentation for the 20 amp DC sensor
00235         current = 0.05 * (self.controller.analog_read(self.pin) - 500.)
00236         return current
00237     
00238 class MaxEZ1Sensor(SonarSensor):
00239     def __init__(self, *args, **kwargs):
00240         super(MaxEZ1Sensor, self).__init__(*args, **kwargs)
00241         
00242         self.trigger_pin = kwargs['trigger_pin']
00243         self.output_pin = kwargs['output_pin']
00244         
00245         self.msg.field_of_view = 0.785398163
00246         self.msg.min_range = 0.02
00247         self.msg.max_range = 3.0
00248         
00249     def read_value(self):
00250         return self.controller.get_MaxEZ1(self.trigger_pin, self.output_pin)
00251 
00252             
00253 if __name__ == '__main__':
00254     myController = Controller()
00255     mySensor = SonarSensor(myController, "My Sonar", type=Type.PING, pin=0, rate=10)
00256
arduino_sensors.py
