def __init__(
self,
leftEncoder,
rightEncoder,
leftSignAdjust,
rightSignAdjust,
rollover
):
self.leftEncoderId = leftEncoder
self.rightEncoderId = rightEncoder
self.leftSignAdjust = leftSignAdjust
self.rightSignAdjust = rightSignAdjust
self.rollover = rollover
self.leftEncoder = Int16(0)
self.rightEncoder = Int16(0)
self.encoder = Encoder()
self.encoder.setOnAttachHandler(self.encoderAttached)
self.encoder.setOnDetachHandler(self.encoderDetached)
self.encoder.setOnErrorhandler(self.encoderError)
self.encoder.setOnInputChangeHandler(self.encoderInputChanged)
self.encoder.setOnPositionChangeHandler(self.encoderPositionChange)
try:
rospy.logdebug('openPhidget()')
self.encoder.openPhidget()
except PhidgetException, e:
rospy.logerror("openPhidget() failed")
rospy.logerror("code: %d" % e.code)
rospy.logerror("message", e.message)
raise
def AttachEncoder(databasepath, serialNumber):
def onAttachHandler(event):
logString = "Encoder Attached " + str(event.device.getSerialNum())
#print(logString)
DisplayAttachedDeviceInfo(event.device)
def onDetachHandler(event):
logString = "Encoder Detached " + str(event.device.getSerialNum())
#print(logString)
DisplayDetachedDeviceInfo(event.device)
event.device.closePhidget()
def onErrorHandler(event):
logString = "Encoder Error " + str(event.device.getSerialNum()) + ", Error: " + event.description
print(logString)
DisplayErrorDeviceInfo(event.device)
def onServerConnectHandler(event):
logString = "Encoder Server Connect " + str(event.device.getSerialNum())
#print(logString)
def onServerDisconnectHandler(event):
logString = "Encoder Server Disconnect " + str(event.device.getSerialNum())
#print(logString)
def inputChangeHandler(event):
logString = "Encoder Input Changed"
#print(logString)
try:
conn = sqlite3.connect(databasepath)
conn.execute("INSERT INTO ENCODER_INPUTCHANGE VALUES(NULL, DateTime('now'), ?, ?, ?)",
(event.device.getSerialNum(), event.index, int(event.state)))
conn.commit()
conn.close()
except sqlite3.Error as e:
print "An error occurred:", e.args[0]
def positionChangeHandler(event):
logString = "Encoder Position Changed"
#print(logString)
try:
conn = sqlite3.connect(databasepath)
conn.execute("INSERT INTO ENCODER_POSITIONCHANGE VALUES(NULL, DateTime('now'), ?, ?, ?)",
(event.device.getSerialNum(), event.index, event.position))
conn.commit()
conn.close()
except sqlite3.Error as e:
print "An error occurred:", e.args[0]
try:
p = Encoder()
p.setOnAttachHandler(onAttachHandler)
p.setOnDetachHandler(onDetachHandler)
p.setOnErrorhandler(onErrorHandler)
p.setOnServerConnectHandler(onServerConnectHandler)
p.setOnServerDisconnectHandler(onServerDisconnectHandler)
p.setOnInputChangeHandler(inputChangeHandler)
p.setOnPositionChangeHandler(positionChangeHandler)
p.openPhidget(serialNumber)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting...")
exit(1)
# else:
# if abs(desiredVelocity2 - abs(sum(velocityMeasurements2) / len(velocityMeasurements2))) > desiredVelocity2 / 500:
# stepper2.setVelocityLimit(0, min(capVelocityAt, stepper2.getVelocityLimit(0) + (desiredVelocity2 - abs(sum(velocityMeasurements2) / len(velocityMeasurements2)))))
# #target = stepper.getTargetPosition(0)
# #stepper.setTargetPosition(0, 0)
# #stepper.setTargetPosition(0, target)
# stepper2.setTargetPosition(0, stepper2.getTargetPosition(0) + 1)
# print("Stepper 2 velocity %i" % stepperVelocity)
# print("Velocity 2 adjusted to %i" % stepper2.getVelocityLimit(0))
# print("Stepper 2 mean measured velocity %i" % (sum(velocityMeasurements2) / len(velocityMeasurements2)))
# timer2 = timeDelayBetweenAdjustments
# velocityMeasurements2 = []
#Create an encoder object
try:
encoder = Encoder()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
#Create a stepper object
try:
stepper1 = Stepper()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
#Create a stepper object
try:
def mcVelocityChanged( e):
return
if len(sys.argv) < 5:
print "usage:", sys.argv[0], "motor velocity pulses encoder sign"
sys.exit(1)
whichMotor = int(sys.argv[1])
velocity = float(sys.argv[2])
pulsesWanted = int(sys.argv[3])
whichEncoder = int(sys.argv[4])
encoderSign = int(sys.argv[5])
encoder = Encoder()
encoder.setOnAttachHandler(encoderAttached)
encoder.setOnDetachHandler(encoderDetached)
encoder.setOnErrorhandler(encoderError)
encoder.setOnInputChangeHandler(encoderInputChanged)
encoder.setOnPositionChangeHandler(encoderPositionChange)
try:
encoder.openPhidget()
except PhidgetException, e:
raise
try:
encoder.waitForAttach(10000)
def initMotorAndEncoderBoards():
global motorControl, motorControlRight, rightWheels, phidget1065, encoders, leftEncoderPosition, rightEncoderPosition, motors_inverted, encoders_inverted
try:
motorControl = MotorControl()
except:
rospy.logerr("Unable to connect to Phidget HC Motor Control")
return
try:
motorControl.setOnAttachHandler(mcAttached)
motorControl.setOnErrorhandler(mcError)
motorControl.setOnVelocityChangeHandler(mcVelocityChanged)
motorControl.openPhidget()
#attach the board
motorControl.waitForAttach(10000)
"""use the function getMotorCount() to know how many motors the Phidget board can take
if the result is more than 1, we have a 1064 board and we take care of both motors with one motorControl variable. We need to handle the Phidget encoder board that is
separated from the phidget 1064.
if the result is equal to 1, we have two phidget 1065 boards. The one with serial number that is the lowest will be the left will, the other the right weel. The encoder has to be handled
in this file as it is part of the 1065 board.
"""
if motorControl.getMotorCount() == 1:
phidget1065 = True
rightWheels = 0
motorControlRight = MotorControl()
motorControlRight.setOnAttachHandler(mcAttached)
motorControlRight.setOnErrorhandler(mcError)
motorControlRight.setOnVelocityChangeHandler(mcVelocityChanged)
if motorControl.getSerialNum() > motorControlRight.getSerialNum():
""" As a rule, we need the serial number of the left board to be lower than for the right board. This is for consistancy for all the robots
"""
motorControlTemp = motorControl
motorControl = motorControlRight
motorControlRight = motorControlTemp
#Set up the encoders handler
motorControl.setOnPositionUpdateHandler(leftEncoderUpdated)
motorControlRight.setOnPositionUpdateHandler(rightEncoderUpdated)
#attach the board
motorControlRight.openPhidget()
motorControlRight.waitForAttach(10000)
# we have a motor controller board that control 2 motors but doesn't get any encoder input, so we need to initialize the encoder board.
else:
encoders = Encoder()
encoders.setOnPositionChangeHandler(encoderBoardPositionChange)
encoders.openPhidget()
encoders.waitForAttach(10000)
# some robots have the left and right encoders switched by mistake
if(motors_inverted or encoders_inverted):
leftEncoderPosition = 1;
rightEncoderPosition = 0;
encoders.setEnabled(leftEncoderPosition, True)
encoders.setEnabled(rightEncoderPosition, True)
except PhidgetException as e:
motorsError = 1
encodersError = 1
rospy.logerr("Unable to initialize the motors and encoders board: %i: %s", e.code, e.details)
return
except:
motorsError = 1
encodersError = 1
rospy.logerr("Unable to register the motors and encoders board")
return
if motorControl.isAttached():
rospy.loginfo("Device: %s, Serial: %d, Version: %d",motorControl.getDeviceName(),motorControl.getSerialNum(),motorControl.getDeviceVersion())
if phidget1065 == True:
if motorControlRight.isAttached():
rospy.loginfo("Device: %s, Serial: %d, Version: %d",motorControlRight.getDeviceName(),motorControlRight.getSerialNum(),motorControlRight.getDeviceVersion())
else:
rospy.loginfo("Device: %s, Serial: %d, Version: %d",encoders.getDeviceName(),encoders.getSerialNum(),encoders.getDeviceVersion())
if stop_when_obstacle:
timer = Timer(1.0, checkEncoders)
timer.start()
return
def initMotorAndEncoderBoards():
global motorControl, motorControlRight, rightWheels, phidget1065, encoders, leftEncoderPosition, rightEncoderPosition, motors_inverted, encoders_inverted
try:
motorControl = MotorControl()
except:
rospy.logerr("Unable to connect to Phidget HC Motor Control")
return
try:
motorControl.setOnAttachHandler(mcAttached)
motorControl.setOnErrorhandler(mcError)
motorControl.setOnVelocityChangeHandler(mcVelocityChanged)
motorControl.openPhidget()
#attach the board
motorControl.waitForAttach(10000)
"""use the function getMotorCount() to know how many motors the Phidget board can take
if the result is more than 1, we have a 1064 board and we take care of both motors with one motorControl variable. We need to handle the Phidget encoder board that is
separated from the phidget 1064.
if the result is equal to 1, we have two phidget 1065 boards. The one with serial number that is the lowest will be the left will, the other the right weel. The encoder has to be handled
in this file as it is part of the 1065 board.
"""
if motorControl.getMotorCount() == 1:
phidget1065 = True
rightWheels = 0
motorControlRight = MotorControl()
motorControlRight.setOnAttachHandler(mcAttached)
motorControlRight.setOnErrorhandler(mcError)
motorControlRight.setOnVelocityChangeHandler(mcVelocityChanged)
if motorControl.getSerialNum() > motorControlRight.getSerialNum():
""" As a rule, we need the serial number of the left board to be lower than for the right board. This is for consistancy for all the robots
"""
motorControlTemp = motorControl
motorControl = motorControlRight
motorControlRight = motorControlTemp
#Set up the encoders handler
motorControl.setOnPositionUpdateHandler(leftEncoderUpdated)
motorControlRight.setOnPositionUpdateHandler(rightEncoderUpdated)
#attach the board
motorControlRight.openPhidget()
motorControlRight.waitForAttach(10000)
# we have a motor controller board that control 2 motors but doesn't get any encoder input, so we need to initialize the encoder board.
else:
encoders = Encoder()
encoders.setOnPositionChangeHandler(encoderBoardPositionChange)
encoders.openPhidget()
encoders.waitForAttach(10000)
# some robots have the left and right encoders switched by mistake
if(motors_inverted or encoders_inverted):
leftEncoderPosition = 1;
rightEncoderPosition = 0;
encoders.setEnabled(leftEncoderPosition, True)
encoders.setEnabled(rightEncoderPosition, True)
except PhidgetException as e:
motorsError = 1
encodersError = 1
rospy.logerr("Unable to initialize the motors and encoders board: %i: %s", e.code, e.details)
return
except:
motorsError = 1
encodersError = 1
rospy.logerr("Unable to register the motors and encoders board")
return
if motorControl.isAttached():
rospy.loginfo("Device: %s, Serial: %d, Version: %d",motorControl.getDeviceName(),motorControl.getSerialNum(),motorControl.getDeviceVersion())
if phidget1065 == True:
if motorControlRight.isAttached():
rospy.loginfo("Device: %s, Serial: %d, Version: %d",motorControlRight.getDeviceName(),motorControlRight.getSerialNum(),motorControlRight.getDeviceVersion())
else:
rospy.loginfo("Device: %s, Serial: %d, Version: %d",encoders.getDeviceName(),encoders.getSerialNum(),encoders.getDeviceVersion())
if stop_when_obstacle:
timer = Timer(1.0, checkEncoders)
timer.start()
return
__author__ = 'Adam Stelmack'
__version__ = '2.1.8'
__date__ = 'May 17 2010'
#Basic imports
from ctypes import *
import sys
#Phidget specific imports
from Phidgets.PhidgetException import PhidgetErrorCodes, PhidgetException
from Phidgets.Events.Events import AttachEventArgs, DetachEventArgs, ErrorEventArgs, EncoderPositionChangeEventArgs, InputChangeEventArgs
from Phidgets.Devices.Encoder import Encoder
#Create an accelerometer object
try:
encoder = Encoder()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
#Information Display Function
def displayDeviceInfo():
print("|------------|----------------------------------|--------------|------------|")
print("|- Attached -|- Type -|- Serial No. -|- Version -|")
print("|------------|----------------------------------|--------------|------------|")
print("|- %8s -|- %30s -|- %10d -|- %8d -|" % (encoder.isAttached(), encoder.getDeviceName(), encoder.getSerialNum(), encoder.getDeviceVersion()))
print("|------------|----------------------------------|--------------|------------|")
#Event Handler Callback Functions
def encoderAttached(e):
interfaceKit = InterfaceKit()
interfaceKit.setOnAttachHandler(interfaceKitAttachedEvent)
interfaceKit.setOnDetachHandler(interfaceKitDetachedEvent)
interfaceKit.setOnInputChangeHandler(interfaceKitInputChanged)
interfaceKit.openPhidget()
# Attach InterfaceKit phidget
try:
interfaceKit.waitForAttach(1000)
except PhidgetException as e:
print(
"Cannot connect to InterfaceKit (for buttons). Please ensure all devices are plugged in."
)
# Encoder object
encoder = Encoder()
encoder.setOnAttachHandler(encoderAttachedEvent)
encoder.setOnDetachHandler(encoderDetachedEvent)
encoder.setOnPositionChangeHandler(encoderPositionChanged)
encoder.openPhidget()
# Attach encoder object
try:
encoder.waitForAttach(1000)
except PhidgetException as e:
print(
"Cannot connect to Encoder. Please ensure all devices are plugged in.")
#=======================================================================================
# Main loop
class PhidgetEncoder:
'''Monitor the pose of each encoders'''
def __init__(self):
self.period = rospy.get_param('~period', 0.02)
self.minPubPeriod = rospy.get_param('~min_pub_period', None)
if self.minPubPeriod < self.period * 2:
rospy.loginfo(
"Warning: you attempted to set the min_pub_period" +
" to a smaller value (%.3f)" % (self.minPubPeriod) +
" than twice the period (2 * %.3f)." % (self.period) +
" This is not allowed, hence I am setting the min_pub_period" +
" to %.3f." % (self.period * 2))
self.minPubPeriod = self.period * 2
# encoders are identified by their index (i.e. on which port they are
# plugged on the phidget)
self.left = 0
self.right = 1
self.countBufs = {self.left: CountBuffer(), self.right: CountBuffer()}
self.lastPub = None
self._mutex = threading.RLock()
self.initPhidget()
self.encoder.setEnabled(self.left, True)
self.encoder.setEnabled(self.right, True)
self.pub = rospy.Publisher('encoder_counts', EncoderCounts)
# diagnostics
self.diag_updater = DIAG.Updater()
self.diag_updater.setHardwareID('none')
f = {'min': 1.0 / self.minPubPeriod}
fs_params = DIAG.FrequencyStatusParam(f, 0.25, 1)
self.pub_diag = DIAG.HeaderlessTopicDiagnostic('encoder_counts',
self.diag_updater,
fs_params)
#TODO: add a diagnostic task to monitor the connection with the phidget
self.forcePub(None)
def initPhidget(self):
'''Connects to the phidget and init communication.'''
#Create an encoder object (from the Phidget library)
try:
self.encoder = Encoder()
except RuntimeError as e:
rospy.logerr("Runtime exception: %s. Exiting..." % e.details)
exit(1)
#Set the callback
try:
self.encoder.setOnPositionChangeHandler(self.encoderPositionChange)
except PhidgetException as e:
err(e)
rospy.loginfo("Opening phidget object....")
try:
self.encoder.openPhidget()
except PhidgetException as e:
err(e)
rospy.loginfo("Waiting for attach....")
try:
self.encoder.waitForAttach(10000)
except PhidgetException as e:
rospy.logerr("Phidget error %i: %s" % (e.code, e.details))
try:
self.encoder.closePhidget()
except PhidgetException as e:
err(e)
exit(1)
def closePhidget(self):
rospy.loginfo("Closing...")
try:
self.encoder.closePhidget()
except PhidgetException as e:
err(e)
def encoderPositionChange(self, e):
'''A callback function called whenever the position changed'''
#rospy.loginfo("Encoder %i: Encoder %i -- Change: %i -- Time: %i -- Position: %i"
#% ( e.device.getSerialNum(), e.index, e.positionChange, e.time,
#self.encoder.getPosition(e.index)) )
with self._mutex:
if e.index in self.countBufs.keys():
self.countBufs[e.index].add(e)
dts = [b.dt for b in self.countBufs.values()]
if min(dts) >= self.period:
#if self.countBufs[self.left].n != self.countBufs[self.right].n:
#rospy.loginfo("encoders: time criteria met, but not count criteria")
dt = sum(dts) / len(dts)
#rospy.loginfo("Publishing")
self.publish(dt)
def publish(self, dt):
encodersMsg = EncoderCounts()
self.lastPub = rospy.Time.now()
encodersMsg.stamp = self.lastPub # i.e. now()
encodersMsg.dt = dt
with self._mutex:
encodersMsg.d_left = self.countBufs[self.left].get_delta_rev()
encodersMsg.d_right = -self.countBufs[self.right].get_delta_rev()
self.countBufs[self.left].reset()
self.countBufs[self.right].reset()
self.pub.publish(encodersMsg)
self.pub_diag.tick()
self.diag_updater.update()
def forcePub(self, dummy):
''' We want to publish messages even if the vehicle is not moving. '''
if self.minPubPeriod:
if self.lastPub is None:
self.lastPub = rospy.Time.now()
else:
dt = (rospy.Time.now() - self.lastPub).to_sec()
if dt > self.minPubPeriod:
self.publish(dt)
self.timer = rospy.Timer(rospy.Duration(self.minPubPeriod),
self.forcePub, True)
def home(e):
#if the home switch is activated,
#e.setPosition(0,0)
if __name__ == '__main__':
#Main Program Code
#Create an accelerometer object
try:
encoder = Encoder()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
try:
#logging example, uncomment to generate a log file
#encoder.enableLogging(PhidgetLogLevel.PHIDGET_LOG_VERBOSE, "phidgetlog.log")
encoder.setOnAttachHandler(encoderAttached)
encoder.setOnDetachHandler(encoderDetached)
encoder.setOnErrorhandler(encoderError)
encoder.setOnInputChangeHandler(encoderInputChange)
encoder.setOnPositionChangeHandler(encoderPositionChange)
except PhidgetException as e:
print("Phidget Error %i: %s" % (e.code, e.details))
exit(1)
print("Opening phidget object....")
try:
encoder.openPhidget()
except PhidgetException as e:
print("Phidget Error %i: %s" % (e.code, e.details))
exit(1)
print("Waiting for attach....")
try:
encoder.waitForAttach(10000)
except PhidgetException as e:
print("Phidget Error %i: %s" % (e.code, e.details))
try:
encoder.closePhidget()
except PhidgetException as e:
print("Phidget Error %i: %s" % (e.code, e.details))
exit(1)
exit(1)
else:
displayDeviceInfo()
print("Press Enter to quit....")
chr = sys.stdin.read(1)
print("Closing...")
try:
encoder.closePhidget()
except PhidgetException as e:
print("Phidget Error %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Done.")
exit(0)
class PhidgetNode(object):
"""Node for polling InterfaceKit, Encoder Board, and Wheatstone Bridge"""
def __init__(self):
"""Open InterfaceKit and Encoder devices and initialize ros node"""
rospy.init_node("phidgets_node")
#rospy.init_node("phidgets_node", log_level=rospy.DEBUG)
# Call base class initializer, which starts a ros node with a name and log_level
# It then opens and attaches Phidget device
self.interfaceKit = InterfaceKit()
self.encoder = Encoder()
# self.bridge = Bridge()
# initialize this to nan to indicate we haven't homed the location
self.sled_pos = float('nan') # position of sled in millimeters
self.encoder_rear_offset = 0
self.encoder_front_offset = 0
# Open the devices and wait for them to attach
self._attachPhidget(self.interfaceKit, "Interface Kit")
self._attachPhidget(self.encoder, "Encoder board")
# self._attachPhidget(self.bridge, "Wheatstone Bridge")
self.params = rospy.get_param("/phidgets")
self.sled_params = self.params["sled"]
self.sensor_pub = rospy.Publisher("/actuator_states/raw/",
ActuatorStates,
queue_size=10)
self.sensor_processed_pub = rospy.Publisher("/actuator_states/proc",
ActuatorStatesProcessed,
queue_size=10)
self.limit_sub = rospy.Subscriber("/pololu/limit_switch",
LimitSwitch,
self.limit_callback,
queue_size=10)
self.sled_is_homed = False
# enable both the sled encoders
self.encoder.setEnabled(self.sled_params["encoder_index"]["left"],
True)
self.encoder.setEnabled(self.sled_params["encoder_index"]["right"],
True)
# Display info of the devices
self._displayDeviceInfo(self.encoder)
self.displayInterfaceKitInfo()
# self.displayBridgeInfo()
def _attachPhidget(self, phidget, name):
"""Open and wait for the Phidget object to attach"""
try:
phidget.openPhidget()
rospy.loginfo("Waiting for %s to attach....", name)
phidget.waitForAttach(10000) # 10 s
except:
rospy.logerr("%s did not attach!", name)
def _displayDeviceInfo(self, phidget):
"""Display relevant info about device"""
rospy.logdebug("Attached: %s", phidget.isAttached())
rospy.logdebug("Type: %s", phidget.getDeviceName())
rospy.logdebug("Serial No.: %s", phidget.getSerialNum())
rospy.logdebug("Version: %s", phidget.getDeviceVersion())
def displayInterfaceKitInfo(self):
"""Display relevant info about interface kit"""
self._displayDeviceInfo(self.interfaceKit)
rospy.logdebug("Number of Digital Inputs: %i",
self.interfaceKit.getInputCount())
rospy.logdebug("Number of Digital Outputs: %i",
self.interfaceKit.getOutputCount())
rospy.logdebug("Number of Sensor Inputs: %i",
self.interfaceKit.getSensorCount())
rospy.logdebug("Min data rate: %i, Max data rate: %i",
self.interfaceKit.getDataRateMin(0),
self.interfaceKit.getDataRateMax(0))
def displayBridgeInfo(self):
"""Display relevant info about wheatstone bridge"""
self._displayDeviceInfo(self.bridge)
rospy.logdebug("Number of bridge inputs: %i",
self.bridge.getInputCount())
rospy.logdebug("Data Rate Max: %d", self.bridge.getDataRateMax())
rospy.logdebug("Data Rate Min: %d", self.bridge.getDataRateMin())
rospy.logdebug("Input Value Max: %d", self.bridge.getBridgeMax(0))
rospy.logdebug("Input Value Min: %d", self.bridge.getBridgeMin(0))
def limit_callback(self, limit_switch):
"""Callback for the current state of the switches. If this is the first
time they have been pressed, set flag to indicate the sled has been homed"""
if limit_switch.rear and not self.sled_is_homed:
self.sled_pos = self.sled_params["rear_pos"]
self.encoder_rear_offset = self.pollEncoders()
self.sled_is_homed = True
#elif limit_switch.front:
# self.sled_pos = self.sled_params["front_pos"]
# self.encoder_front_offset = self.pollEncoders()
# self.sled_is_homed = True
def pollEncoders(self):
"""Return averaged value of sled left and right encoder values"""
sled_left_ticks = self.encoder.getPosition(
self.sled_params["encoder_index"]["left"])
sled_right_ticks = self.encoder.getPosition(
self.sled_params["encoder_index"]["right"])
average = (-sled_left_ticks + sled_right_ticks) / 2
rospy.logdebug("left right tick average: %.2f", average)
return average
def pollLinearActuator(self, name):
"""Poll the sensor for its raw value. Then convert that value
to the actual position in millimeters. Return as a SensorValuePair"""
device = self.params[name]
index = device["sensor_index"]
min_pot_val = device["min"]
max_pot_val = device["max"]
physical_length = device["length"]
# Create objects to fill and later send over message
raw_val = self.interfaceKit.getSensorValue(index)
# Convert the potentiometer value to millimeter position
pos = (raw_val - min_pot_val) * (physical_length /
(max_pot_val - min_pot_val))
return pos
def sendProcessedMessage(self, actuator_states):
"""Process the interface sensor message to average and get the positions
of the motors"""
proc = ActuatorStatesProcessed()
proc.header = actuator_states.header
proc.arm = (actuator_states.arm_left + actuator_states.arm_right) / 2
proc.bucket = (actuator_states.bucket_left +
actuator_states.bucket_right) / 2
proc.sled = actuator_states.sled
self.sensor_processed_pub.publish(proc)
def pollSensors(self):
"""Poll and publish all the sensor values as ActuatorStates message"""
ticks_per_mm = self.sled_params["ticks_per_mm"]
actuator_states = ActuatorStates(
) # create object to store message components to send on topic
header = Header()
actuator_states.header.stamp = rospy.Time.now()
actuator_states.arm_left = self.pollLinearActuator("arm_left")
actuator_states.arm_right = self.pollLinearActuator("arm_right")
actuator_states.bucket_left = self.pollLinearActuator("bucket_left")
actuator_states.bucket_right = self.pollLinearActuator("bucket_right")
sled_left_ticks = self.encoder.getPosition(
self.sled_params["encoder_index"]["left"])
sled_right_ticks = self.encoder.getPosition(
self.sled_params["encoder_index"]["right"])
rospy.logdebug("left ticks = %d", sled_left_ticks)
rospy.logdebug("right ticks = %d", sled_right_ticks)
if self.sled_is_homed:
rospy.logdebug("left pos = %.2f", sled_left_ticks / ticks_per_mm)
rospy.logdebug("right pos = %.2f", sled_right_ticks / ticks_per_mm)
sled_pos = (self.pollEncoders() -
self.encoder_rear_offset) / ticks_per_mm
else:
sled_pos = float(
'nan'
) # set this so everybody knows we haven't homed the sled yet
actuator_states.sled = sled_pos
# TODO: add sled_pos to position when that is setup
# actuator_states.sled = self.interfaceKit.getSensorValue(self.params["sled"]["sensor_index"])
self.sensor_pub.publish(actuator_states)
self.sendProcessedMessage(actuator_states)
def run(self):
"""Run the main ros loop"""
rospy.loginfo("Starting phidgets node loop")
r_time = rospy.Rate(10) #10 Hz looping
while not rospy.is_shutdown():
self.pollSensors()
r_time.sleep()
import csv
# Device specific imports
from Phidgets.PhidgetException import PhidgetErrorCodes, PhidgetException
from Phidgets.Events.Events import *
from Phidgets.Manager import Manager
from Phidgets.Devices.MotorControl import MotorControl
from Phidgets.Devices.Encoder import Encoder
from Phidgets.Phidget import PhidgetLogLevel
"""
Step 1 - Create device objects(s) [I'm skipping the event handlers & their callbacks for simplicity]
"""
try:
motor = MotorControl()
encoder = Encoder()
torque_sensor = AnalogInput()
except RuntimeError as error:
print ('Python Runtime Exception relating to motor object: %s' % error.details)
exit(1)
"""
Step 2 - Open device(s) using the object created above
"""
try:
motor.openPhidget()
encoder.openPhidget()
torque_sensor.openPhidget()
except PhidgetException as error:
print ('Device error %i, %s' % (error.code, error.details))
exit(1)
def AttachEncoder(databasepath, serialNumber):
def onAttachHandler(event):
logString = "Encoder Attached " + str(event.device.getSerialNum())
#print(logString)
DisplayAttachedDeviceInfo(event.device)
def onDetachHandler(event):
logString = "Encoder Detached " + str(event.device.getSerialNum())
#print(logString)
DisplayDetachedDeviceInfo(event.device)
event.device.closePhidget()
def onErrorHandler(event):
logString = "Encoder Error " + str(
event.device.getSerialNum()) + ", Error: " + event.description
print(logString)
DisplayErrorDeviceInfo(event.device)
def onServerConnectHandler(event):
logString = "Encoder Server Connect " + str(
event.device.getSerialNum())
#print(logString)
def onServerDisconnectHandler(event):
logString = "Encoder Server Disconnect " + str(
event.device.getSerialNum())
#print(logString)
def inputChangeHandler(event):
logString = "Encoder Input Changed"
#print(logString)
try:
conn = sqlite3.connect(databasepath)
conn.execute(
"INSERT INTO ENCODER_INPUTCHANGE VALUES(NULL, DateTime('now'), ?, ?, ?)",
(event.device.getSerialNum(), event.index, int(event.state)))
conn.commit()
conn.close()
except sqlite3.Error as e:
print "An error occurred:", e.args[0]
def positionChangeHandler(event):
logString = "Encoder Position Changed"
#print(logString)
try:
conn = sqlite3.connect(databasepath)
conn.execute(
"INSERT INTO ENCODER_POSITIONCHANGE VALUES(NULL, DateTime('now'), ?, ?, ?)",
(event.device.getSerialNum(), event.index, event.position))
conn.commit()
conn.close()
except sqlite3.Error as e:
print "An error occurred:", e.args[0]
try:
p = Encoder()
p.setOnAttachHandler(onAttachHandler)
p.setOnDetachHandler(onDetachHandler)
p.setOnErrorhandler(onErrorHandler)
p.setOnServerConnectHandler(onServerConnectHandler)
p.setOnServerDisconnectHandler(onServerDisconnectHandler)
p.setOnInputChangeHandler(inputChangeHandler)
p.setOnPositionChangeHandler(positionChangeHandler)
p.openPhidget(serialNumber)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting...")
exit(1)
def __init__(self):
rospy.loginfo("Initializing PhidgetEncoders")
self.leftFrontEncoder = 1
self.rightFrontEncoder = 0
self.leftRearEncoder = 3
self.rightRearEncoder = 2
self.leftSignAdjust = 1 # forward is positive
self.rightSignAdjust = -1 # forward is negative
self.roverHasntMoved = True
self.useCalculatedCovariances = False
self.driveWheelRadius = 0.054
self.wheelSeparation = 0.277
self.pulsesPerRevolution = 4331 # wheel revolution
self.wheelsConstant = 2 * pi * self.driveWheelRadius / self.wheelSeparation
self.pulsesConstant = (pi / self.pulsesPerRevolution) * self.driveWheelRadius
#
# the initialCovariance is used before the rover has moved,
# because it's position and orientation are known perfectly.
# once it has moved, the defaultCovariance is used, until
# enough data points have been received to estimate a reasonable
# covariance matrix.
#
self.initialCovariance = [0.0001, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0001, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0001, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0001, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0001, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0001]
#
# the rover is not able to move along the Z axis, so the Z value
# of zero will always be "perfect". the rover is also incapable
# of rotating about either the X or the Y axis, so those
# zeroes will also always be "perfect".
#
self.defaultCovariance = [1000.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 1000.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0001, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0001, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0001, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 1000.0]
#
# the rover begins with the origins and axes of the odom and
# base_link frames aligned.
#
self.previousX = 0
self.previousY = 0
self.previousLeftFrontPosition = 0
self.previousLeftRearPosition = 0
self.previousRightFrontPosition = 0
self.previousRightRearPosition = 0
#
# the rover begins oriented along the positive X of both the
# odom and base_link frames.
#
self.previousTheta = 0
#
# publish the Odometry message to describe the current position
# and orientation of the origin of the base_link frame.
#
self.encoder = Encoder()
self.odometryMessage = Odometry()
self.odometryMessage.header.frame_id = 'base_link'
self.odometryMessage.child_frame_id = 'base_link'
if self.useCalculatedCovariances:
self.poseCovariance, self.poseSampleList, self.numberOfPoseSamples = calcCovariance(
None,
[0.0,
0.0,
0.0,
0.0,
0.0,
0.0],
0,
100
)
self.twistCovariance, self.twistSampleList, self.numberOfTwistSamples = calcCovariance(
None,
[0.0,
0.0,
0.0,
0.0,
0.0,
0.0],
0,
100
)
#
# the rover is incapable of translating along the Z axis,
# so it will always be zero
#
self.odometryMessage.pose.pose.position.z = 0
self.odometryMessage.pose.covariance = self.initialCovariance
self.odometryMessage.twist.covariance = self.initialCovariance
#
# robot_pose_ekf subscribes (via remapping) to wheel_odom,
# to get 2D position and orientation data. the z, roll and pitch
# are ignored by robot_pose_ekf.
#
self.encoderPublisher = rospy.Publisher('wheel_odom', Odometry)
self.encoder.setOnAttachHandler(self.encoderAttached)
self.encoder.setOnDetachHandler(self.encoderDetached)
self.encoder.setOnErrorhandler(self.encoderError)
self.encoder.setOnInputChangeHandler(self.encoderInputChanged)
self.encoder.setOnPositionChangeHandler(self.encoderPositionChange)
try:
rospy.logdebug('openPhidget()')
self.encoder.openPhidget()
except PhidgetException, e:
rospy.logerror("openPhidget() failed")
rospy.logerror("code: %d" % e.code)
rospy.logerror("message", e.message)
raise
