class PhidgetEncoders:
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
try:
rospy.logdebug('waitForAttach()')
self.encoder.waitForAttach(10000)
except PhidgetException, e:
rospy.logerror("waitForAttach() failed")
rospy.logerror("code: %d" % e.code)
rospy.logerror("message", e.message)
raise
encoder = Encoder()
except RuntimeError as e:
g.log.write("Runtime Exception: %s\n" % e.details)
g.log.write("Exiting....\n")
#print("Runtime Exception: %s" % e.details)
#print("Exiting....")
exit(1)
#Main Program Code
try:
#logging example, uncomment to generate a log file
#encoder.enableLogging(PhidgetLogLevel.PHIDGET_LOG_VERBOSE, "phidgetlog.log")
encoder.setOnAttachHandler(g.encoderAttached)
encoder.setOnDetachHandler(g.encoderDetached)
encoder.setOnErrorhandler(g.encoderError)
encoder.setOnInputChangeHandler(g.encoderInputChange)
encoder.setOnPositionChangeHandler(g.encoderPositionChange)
except PhidgetException as e:
g.log.write("Phidget Error %i: %s\n" % (e.code, e.details))
#print("Phidget Error %i: %s" % (e.code, e.details))
exit(1)
g.log.write("Opening phidget object....\n")
#print("Opening phidget object....")
try:
encoder.openPhidget()
except PhidgetException as e:
g.log.write("Phidget Error %i: %s\n" % (e.code, e.details))
#print("Phidget Error %i: %s" % (e.code, e.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)
stepper2 = Stepper()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
#Main Program Code
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....")
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)
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)
class PhidgetEncoders:
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
try:
rospy.logdebug('waitForAttach()')
self.encoder.waitForAttach(10000)
except PhidgetException, e:
rospy.logerror("waitForAttach() failed")
rospy.logerror("code: %d" % e.code)
rospy.logerror("message", e.message)
raise
