00001
00002
00003 """
00004 io.py - ROS wrappers for ArbotiX I/O
00005 Copyright (c) 2010-2011 Vanadium Labs LLC. All right reserved.
00006
00007 Redistribution and use in source and binary forms, with or without
00008 modification, are permitted provided that the following conditions are met:
00009 * Redistributions of source code must retain the above copyright
00010 notice, this list of conditions and the following disclaimer.
00011 * Redistributions in binary form must reproduce the above copyright
00012 notice, this list of conditions and the following disclaimer in the
00013 documentation and/or other materials provided with the distribution.
00014 * Neither the name of Vanadium Labs LLC nor the names of its
00015 contributors may be used to endorse or promote products derived
00016 from this software without specific prior written permission.
00017
00018 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00019 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00020 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00021 DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
00022 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00023 LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
00024 OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00025 LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00026 OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00027 ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00028 """
00029
00030 import rospy
00031 from arbotix_msgs.msg import *
00032
00033 class DigitalServo:
00034 """ Class for a digital output. """
00035 def __init__(self, name, pin, value, rate, device):
00036 self.device = device
00037 self.value = value
00038 self.direction = 0
00039 self.pin = pin
00040 self.device.setDigital(self.pin, self.value, self.direction)
00041 rospy.Subscriber('~'+name, Digital, self.stateCb)
00042 self.t_delta = rospy.Duration(1.0/rate)
00043 self.t_next = rospy.Time.now() + self.t_delta
00044 def stateCb(self, msg):
00045 self.value = msg.value
00046 self.direction = msg.direction
00047 def update(self):
00048 if rospy.Time.now() > self.t_next:
00049 self.device.setDigital(self.pin, self.value, self.direction)
00050 self.t_next = rospy.Time.now() + self.t_delta
00051
00052 class DigitalSensor:
00053 """ Class for a digital input. """
00054 def __init__(self, name, pin, value, rate, device):
00055 self.device = device
00056 self.pin = pin
00057 self.device.setDigital(pin, value, 0)
00058 self.pub = rospy.Publisher('~'+name, Digital, queue_size=5)
00059 self.t_delta = rospy.Duration(1.0/rate)
00060 self.t_next = rospy.Time.now() + self.t_delta
00061 def update(self):
00062 if rospy.Time.now() > self.t_next:
00063 msg = Digital()
00064 msg.header.stamp = rospy.Time.now()
00065 msg.value = self.device.getDigital(self.pin)
00066 self.pub.publish(msg)
00067 self.t_next = rospy.Time.now() + self.t_delta
00068
00069 class AnalogSensor:
00070 """ Class for an analog input. """
00071 def __init__(self, name, pin, value, rate, leng, device):
00072 self.device = device
00073 self.pin = pin
00074 self.device.setDigital(pin, value, 0)
00075 self.pub = rospy.Publisher('~'+name, Analog, queue_size=5)
00076 self.t_delta = rospy.Duration(1.0/rate)
00077 self.t_next = rospy.Time.now() + self.t_delta
00078 self.leng = leng
00079 def update(self):
00080 if rospy.Time.now() > self.t_next:
00081 msg = Analog()
00082 msg.header.stamp = rospy.Time.now()
00083 msg.value = self.device.getAnalog(self.pin, self.leng)
00084 if msg.value >= 0:
00085 self.pub.publish(msg)
00086 self.t_next = rospy.Time.now() + self.t_delta
00087