diff --git a/.gitignore b/.gitignore
new file mode 100755
index 0000000000000000000000000000000000000000..0d20b6487c61e7d1bde93acf4a14b7a89083a16d
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1 @@
+*.pyc
diff --git a/nodes/mecanum_drive_controller b/nodes/mecanum_drive_controller
index f6be0a78e765fef099ae7a81f6e0acbd48379d1f..9ff7d5768b7d71d61ae8665a9bf44812401efcad 100755
--- a/nodes/mecanum_drive_controller
+++ b/nodes/mecanum_drive_controller
@@ -3,7 +3,7 @@ from __future__ import division
import rospy
from geometry_msgs.msg import Twist
-from std_msgs.msg import Int16, Int16MultiArray
+from std_msgs.msg import Int16MultiArray
from mecanum_drive import controller
diff --git a/nodes/mecanum_drive_odometry b/nodes/mecanum_drive_odometry
index a7546bff3ad6a0c36901263ffb9640580be93ecc..10623a7a6213e6b3bae8dd6966a26c1e89c1faa6 100755
--- a/nodes/mecanum_drive_odometry
+++ b/nodes/mecanum_drive_odometry
@@ -5,7 +5,7 @@ import rospy
from geometry_msgs.msg import Quaternion
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
-from std_msgs.msg import Int32
+from std_msgs.msg import Int16MultiArray
from geometry_msgs.msg import PoseWithCovarianceStamped
from tf.broadcaster import TransformBroadcaster
from tf.transformations import quaternion_from_euler, euler_from_quaternion
@@ -40,8 +40,7 @@ class OdometryNode:
self.nodeName = rospy.get_name()
rospy.loginfo("{0} started".format(self.nodeName))
- rospy.Subscriber("lwheel_ticks", Int32, self.leftCallback)
- rospy.Subscriber("rwheel_ticks", Int32, self.rightCallback)
+ rospy.Subscriber("wheel_ticks", Int16MultiArray, self.ticksCallback)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.on_initial_pose)
@@ -110,11 +109,9 @@ class OdometryNode:
rospy.loginfo('Setting initial pose to %s', pose)
self.odometry.setPose(pose)
- def leftCallback(self, msg):
- self.odometry.updateLeftWheel(msg.data)
+ def ticksCallback(self, msg):
+ self.odometry.updateWheels(msg.data[0],msg.data[1],msg.data[2],msg.data[3])
- def rightCallback(self, msg):
- self.odometry.updateRightWheel(msg.data)
if __name__ == '__main__':
diff --git a/src/mecanum_drive/controller.pyc b/src/mecanum_drive/controller.pyc
deleted file mode 100644
index bf6c72d36464af76c9709cda0465ab1b6a10f0cd..0000000000000000000000000000000000000000
Binary files a/src/mecanum_drive/controller.pyc and /dev/null differ
diff --git a/src/mecanum_drive/encoder.pyc b/src/mecanum_drive/encoder.pyc
deleted file mode 100644
index 5b0ef9b08957448661a87ccd075d53fc9a4f8427..0000000000000000000000000000000000000000
Binary files a/src/mecanum_drive/encoder.pyc and /dev/null differ
diff --git a/src/mecanum_drive/odometry.py b/src/mecanum_drive/odometry.py
index bfb621d90cb6d17816ebac43a8054d6b58b14ac8..24a55c5c00f092370c1497d075d1a8dca639d002 100755
--- a/src/mecanum_drive/odometry.py
+++ b/src/mecanum_drive/odometry.py
@@ -9,8 +9,10 @@ class Odometry:
"""
def __init__(self):
- self.leftEncoder = Encoder()
- self.rightEncoder = Encoder()
+ self.frontLeftEncoder = Encoder()
+ self.frontRightEncoder = Encoder()
+ self.rearLeftEncoder = Encoder()
+ self.rearRightEncoder = Encoder()
self.pose = Pose()
self.lastTime = 0
@@ -24,53 +26,39 @@ class Odometry:
self.ticksPerMeter = ticks
def setEncoderRange(self, low, high):
- self.leftEncoder.setRange(low, high)
- self.rightEncoder.setRange(low, high)
+ self.frontLeftEncoder.setRange(low, high)
+ self.frontRightEncoder.setRange(low, high)
+ self.rearLeftEncoder.setRange(low, high)
+ self.rearRightEncoder.setRange(low, high)
def setTime(self, newTime):
self.lastTime = newTime
- def updateLeftWheel(self, newCount):
- self.leftEncoder.update(newCount)
-
- def updateRightWheel(self, newCount):
- self.rightEncoder.update(newCount)
+ def updateWheels(self, fl, fr, rl, rr):
+ self.frontLeftEncoder.update(fl)
+ self.frontRightEncoder.update(fr)
+ self.rearLeftEncoder.update(rl)
+ self.rearRightEncoder.update(rr)
def updatePose(self, newTime):
"""Updates the pose based on the accumulated encoder ticks
- of the two wheels. See https://chess.eecs.berkeley.edu/eecs149/documentation/differentialDrive.pdf
- for details.
+ of the four mecanum wheels.
"""
- leftTravel = self.leftEncoder.getDelta() / self.ticksPerMeter
- rightTravel = self.rightEncoder.getDelta() / self.ticksPerMeter
+ frontLeftTravel = self.frontLeftEncoder.getDelta() / self.ticksPerMeter
+ frontRightTravel = self.frontRightEncoder.getDelta() / self.ticksPerMeter
+ rearLeftTravel = self.rearLeftEncoder.getDelta() / self.ticksPerMeter
+ rearRightTravel = self.rearRightEncoder.getDelta() / self.ticksPerMeter
deltaTime = newTime - self.lastTime
- deltaTravel = (rightTravel + leftTravel) / 2
- deltaTheta = (rightTravel - leftTravel) / self.wheelSeparation
-
- if rightTravel == leftTravel:
- deltaX = leftTravel*cos(self.pose.theta)
- deltaY = leftTravel*sin(self.pose.theta)
- else:
- radius = deltaTravel / deltaTheta
-
- # Find the instantaneous center of curvature (ICC).
- iccX = self.pose.x - radius*sin(self.pose.theta)
- iccY = self.pose.y + radius*cos(self.pose.theta)
-
- deltaX = cos(deltaTheta)*(self.pose.x - iccX) \
- - sin(deltaTheta)*(self.pose.y - iccY) \
- + iccX - self.pose.x
-
- deltaY = sin(deltaTheta)*(self.pose.x - iccX) \
- + cos(deltaTheta)*(self.pose.y - iccY) \
- + iccY - self.pose.y
+ deltaXTravel = (frontLeftTravel + frontRightTravel + rearLeftTravel + rearRightTravel) / 4
+ deltaYTravel = (-frontLeftTravel + frontRightTravel + rearLeftTravel - rearRightTravel) / 4
+ deltaTheta = (-frontLeftTravel + frontRightTravel - rearLeftTravel + rearRightTravel) / (4 * self.wheelSeparation + self.wheelSeparationLength)
- self.pose.x += deltaX
- self.pose.y += deltaY
+ self.pose.x += deltaXTravel
+ self.pose.y += deltaYTravel
self.pose.theta = (self.pose.theta + deltaTheta) % (2*pi)
- self.pose.xVel = deltaTravel / deltaTime if deltaTime > 0 else 0.
- self.pose.yVel = 0
+ self.pose.xVel = deltaXTravel / deltaTime if deltaTime > 0 else 0.
+ self.pose.yVel = deltaYTravel / deltaTime if deltaTime > 0 else 0.
self.pose.thetaVel = deltaTheta / deltaTime if deltaTime > 0 else 0.
self.lastTime = newTime
diff --git a/src/mecanum_drive/odometry.pyc b/src/mecanum_drive/odometry.pyc
deleted file mode 100644
index 905fe8fea72ed0ae9e457a22a2305313fa948a06..0000000000000000000000000000000000000000
Binary files a/src/mecanum_drive/odometry.pyc and /dev/null differ
diff --git a/src/mecanum_drive/pose.pyc b/src/mecanum_drive/pose.pyc
deleted file mode 100644
index 01e65ddc0f3632bb9b99143f2296020fce3c8e1f..0000000000000000000000000000000000000000
Binary files a/src/mecanum_drive/pose.pyc and /dev/null differ