From 71679686d13adbd4474826db78ab8a42fe932d20 Mon Sep 17 00:00:00 2001
From: daviddudas <david.dudas@outlook.com>
Date: Wed, 15 Jan 2020 17:53:07 +0100
Subject: [PATCH] Mecanum odometry added

---
 .gitignore                       |   1 +
 nodes/mecanum_drive_controller   |   2 +-
 nodes/mecanum_drive_odometry     |  11 ++----
 src/mecanum_drive/controller.pyc | Bin 2858 -> 0 bytes
 src/mecanum_drive/encoder.pyc    | Bin 2416 -> 0 bytes
 src/mecanum_drive/odometry.py    |  62 +++++++++++++------------------
 src/mecanum_drive/odometry.pyc   | Bin 3929 -> 0 bytes
 src/mecanum_drive/pose.pyc       | Bin 1015 -> 0 bytes
 8 files changed, 31 insertions(+), 45 deletions(-)
 create mode 100755 .gitignore
 delete mode 100644 src/mecanum_drive/controller.pyc
 delete mode 100644 src/mecanum_drive/encoder.pyc
 delete mode 100644 src/mecanum_drive/odometry.pyc
 delete mode 100644 src/mecanum_drive/pose.pyc

diff --git a/.gitignore b/.gitignore
new file mode 100755
index 0000000..0d20b64
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1 @@
+*.pyc
diff --git a/nodes/mecanum_drive_controller b/nodes/mecanum_drive_controller
index f6be0a7..9ff7d57 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 a7546bf..10623a7 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
GIT binary patch
literal 0
HcmV?d00001

literal 2858
zcmZSn%*(a)nQUA#0~9bbFfceMFfbHbF)%QsFfgPrGUPBY<T5fuF)}cwFfpXCGo&yx
zv@kF<GcZI(F)^gDFr;uWq_Dz;m>E*o7=kr885kHc|NsC0UxSf>fuV#0WLjofW^rbI
zUI`Ba1A}jVNq&)Yer|4JUJBfDkWwd*<>Cws45<ttJ5v}KK(;V~%#UJb2nyC<Vqjn>
zc4S~+@W{_eDOSh@8KjV$pI1_ppQDfrF|k-7Ex$-1Q6VKWEiJVuHLoNyF-JG0D6=e8
zp(sBozeJBqK|w(w8Dt*_gN$?r*{cC^CL==)14BI6>mUXrC=5UhCK!Vm6fh8n_+^5^
zO@jsGM@|L?hP0ymyb_<(w2~4o7%wO@J);C107a>ZMPNCQN)QJu1qmRK3B{>7X&@Ic
zfQe#v1_lQGjQreG{glMA%oP3P#FFgHy!i5B{o<lz{oK^##Jtknc(AXqh(JRmC$&hg
zpb~6-e0*kJW=VWJJ177_5yHU835F2ogGA!v^AdAY<Kw}ukB`sIPbtj-aY0(*<5TjJ
z<Ku%sY_J0n1T~!+1d0$)Dl7&C2^$j|6WB4%kO<C6Ey7GK*0@qj3KKZlut1Y+6e}pX
zwlFY6u|ZR86gwzIv4H}_hJk^>CAB2AC^s`N6_%`u3sO^4iWN%o6%vz^^K%PwGK(`n
z$ttxtvnVwMl=d?7^Kd05UX;YdzyM1z0_qG5kVprq$W5#O=Y?R9Rbb&Dkb7tpxS+@r
z0(lpd<QbTgL1E0zz`y{q5}X4rGlCO-Eek^m6GJU4LkT#i*Rp}~L>41M4I@Jh6N5wz
z3quVngLn-ygNTGk4I4u|6N<bzlDs%ro*Ab+L_Z6PJhJ%^{j3Z%Yz$dUV3HY3vVcie
zhBOuicMp)W7#Ok`8ANK?8EPPwu`!6$K*cy<-r!`YVP~k}V34Tc1iKl`5eIRY8A{l}
zCStRr8R9$+hHMswqIO1*u^a@{OV@INjAH@0nTx@a2^2zX3@lm93^iab*b^*SEKn}k
zH!NAKP%bM&9s@%OC&+{>CWdB45F?%mCc+F8VTOsYz(iPJBCIeGaL5H~fO3>y`mC8C
zu+u($Nr5fb^~DG5HMkfU7{FPs1e6#{GLy5515%58K^Y+kltzL;8iPQ|B#0GM$ni5U
zFqCJcrsf2v79<uWmVnB0kdoZQiXa}40uU!NuSA0zWH2aMfs6Z`%)HdZq6lyzM-qrc
z2qfmEm*ylEK?KAY7#PAmTwQ(QgIxn0gB(LV{r%#@JzYXPN<dWuhLn%1pL>W$3CKD~
zmIW7hAm0QfmZX+&f^uSJT3QImdnKSe08wQG){&N&T#{czn+ylGIz6=n?5$#u+lviA
zH3lmiBO4PZlL8|cGRZRYGKw&AGV(I=vS~4DF!F<;5R(ui1SW%`0*1l)6ck&KR1d1D
zK!ppa-1LJKAbboA4B&baT$F&z&tiCV(a59VQYEz{95rn?FfcF_gB;GlNTMG=i3!OM
zpv0GxnVVTcBj*c%9GY6<i(DEzgT%;kJScY191lv2#k6%iIC;ZzuN%Vg8lb8@2uy%-
zLJ&C5gK|NTG$@nGf(UsKp$JMhpyWv#cLssV1W?*621PEYYUgC-)Z}F5WcEW?2x*Lf
woERUUR$5Y8lo}r&1acF&lmZ)pq!=VY&DICF+^~VPd+b0t3fu}2U=rpA0Gk<du>b%7

diff --git a/src/mecanum_drive/encoder.pyc b/src/mecanum_drive/encoder.pyc
deleted file mode 100644
index 5b0ef9b08957448661a87ccd075d53fc9a4f8427..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 2416
zcmZSn%*$nAC?1#000qnp3=9qm3=GBU3=9k@3=AoZ3^@!8xr_`^j0}t^ObjWk3@OYE
zEes6J3=EM`ObjV348a;~3=9mJ|NsC0uffQ`z)->g(wbS8S)7@lSHjM~z~GvfoS%|f
z1h)jF$_ZqNF9QQZDg(&u6b1&6X^bGFqnH^|n7|AcsAW;CAd6ZU7^2ufE&wsu8B*B5
zYB(5D*ue}=hM-^#kln?(3=9mu`FWWo`9;MFi3-J;dFeT+3gsE8sW}R%5Vt5K=A|ek
zCMTEXmgXduq!ufrq~??)DwJd<XBTrRC@6q5C8sKsWTYzOBo>z_lw{_Dd6f#~sYR&@
zMX80Qsl_F!DSBYV$siwrFevz(K|a%BU|^_WV8{Z8U<wmMGZO<Nh|dV}Q8Ob0V+j*T
zKAssA$iW&c3=9l@nG6m8|7ZTMXJE(##Rgaq9GJzaB|(XK>8T~0pn%THEOE{+%_}Kk
zWnf@%OUx-wE#U%*7YC)5r4|*Zra)o=WMpw_PMQYTED%xb!oa|wpOK%Ns-KcrmYJfT
zoLG{bnHOJPtY2J|te=~joS0Xd8=q2?S(b`L1R8RB1(je&#m8snWtPOpb22b66oXO#
z1ET;VFJm$&2w)f-6t1A4U}C6YWROT<WDu`mV2Eb`@j$`B0E%gm6lMk~us9=<I12+p
zoCy+2AP@Uxg2Fjd4&>t$5aGtaz@P!L5F8#w@Nmk>FAvEmN-fR+hfYRjdIp3GRvQFz
za0y6PPJVd_D5yXRXb@CEObiSRe25?tWRze`28qEiIEchRIffCOAYefg&j|4|NSz-f
zJ%E^CPlD4b*wvu4sR0s)cnFjZic1oUAPIyP-T?)fFvvTgFk@ikgm?xd3c_H|%ml?Z
zIO&68n2{lyg`vm-7O_kW*$hZrW^j7QVqlP{Wn##41%*HsB;!C77QpmDjDd5R86-f4
zL-p~(R6+PP3=Aw;EDSYF4CYMnEKs?4aB&t4&KrIjpp+B@ik2WykOgrvFfasxijE*&
zSZaa<C`c?XwH%V)K&mqHl8aJvQ}aq_5V$3vL|Iypl30=|4hmwB`xqD%nZ!YuiI<5V
z64;>V24QeugWOxg2nsh;4}vmD5XhH^)B)jw3Ied(saG0sgB+V$0xN!0KwOXm85qIk
zB8UmXVE2NohQwSgBdBtLmIe$AS&R%t3ZS^+4z6Kj2u@*O2nIVHDeZ$J2jUBmdEgiV
zi-8Pf029>n6e#A?Q%hVx)dHw4ECwYMP=e=V<YZ)J0{aXU$na#Z1S*V}K`{X-^vyut
zWCWFOU=|ao;5UPo^pLd12}*S!V<ANn$VNy?1dD+T2YV6hF)%?r-+`=7Pc8Au%*`w*
z)(1rx$aV%sW=1YXE=CQI1E7U`USe))d_1@ejgQaGPbtj-aY40ue0)lNa(sLchz&00
zf<R#rBn0vmsE7y>0kK3u848~0sOya&P#A#<qGACC1_m}}HYQG1PF7A9PF_xCP7Xh?
uW#F)dIzO$nq_ikCK0XNK9<XMJ5)c>6qFw-lg3|^PTy`K=7K2i(05bqjnET)W

diff --git a/src/mecanum_drive/odometry.py b/src/mecanum_drive/odometry.py
index bfb621d..24a55c5 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
GIT binary patch
literal 0
HcmV?d00001

literal 3929
zcmZSn%*%EAtZZB|0~9bbFfceMFfbH@L{b<SQWzO>7#MOH8KM{&7(sj{h8!k_TxNzS
zW`<lAhA0+>TvmoCR)`!kLk=55E;~aMJA}`|ki)@{%gGSM$-tPx%8<grkiy2$!obkX
zzz`Y5#gM|z5Ujz;z`&6C|NsC08jK7K3?&>43=AomWtqj9`FR>3$CNNJFfbHkmVnsB
znR#G3Ilma9nw^1x!8I>AKP9yYBEZ7Hzz~pMoC-F_KP5jmwWO#L<Q1^@KsGpmJeR@1
zz>vxS@>2=}LlhIpmn{q+FQqVn+!w{dkiraRurj2ufEjEIDXd@yJJb(R93c0Dba8?_
z24ZkAq;P`Oa5JQEff+muLBSf_3=9m#aSRL$-l?et#R?@wiOJat`DqFz8L0}%rA0-l
zc_j)3`Nf$fnfZANiFqjsWvMy&$(bdUAVrB>3JMAeMfpkjB?_g*nR)37DVb?$sUS_6
zi8%@>MVV!(dSErlAm4#7D8QUSzBB{{C^)oh7#QLi8A{-6CWaDbP%MDNSwOxAF<2Q=
z7#M;zK-&E>LE)vr#=yW31Pam;kRNhV(@LO$UBbh_z)+N#o&ggGVg=a+Qd$5D{hY+&
zl90^YR7AuSr{<(-fDHx_#jXqt4Eh=QxvBaoiDj87`pJnU*_nCq<;D8NMalZPsmY0X
zrMdB7A7T;7heobmK_xh{<Kr{)GE3s)K`t!@r6LAK9xzM>c?gEV;lj(nz)-`;0P-^f
z#Lu8$^Mg2)pMim)JR>zVCpfhru_zG~Dc~Rp0vTMw1&T|A1jrv?A5qaWLSQRO!cpBI
z2oeSPf`O4l4}pAJl9`-c9FSV%n_7}ugybD?tbydfo&Xb6^oan-&8a0JAPZoYi-5$S
z{s3iZ7zRg-7ARsE7_t}`vKYZJ#Kh3d#K2g?z>vj+Dg;ickXQqm5Ckd+z{#LEwInDp
zFFiE~lmx&92qfY_s&ewnOF&69BQre%>{l>BMGu40PH`$cPk`JZ4)QO^QU*q05@Rk1
zl-Q6W3*^VV)N*j1qD8_3nO_WwuG~~<kgK7w1fpOV97~`m1VsrfmYN|oLohi1_(2>D
zGOGkse3TZXB$lK?1i?876fb$H<<9w~c_rZR2NP6|1c<FZscDdGBuA-tK#mTQWME)G
zcqB*)#HB?%fU*t513{n?7F-Z0qWJ(+{(@2!I70p~GJpy*NVaK)u$dU7Yni~J%%Gfu
zB+3lVSquzWEDRF0EDSZw3^hy);wg*_(zUEmo&+d*SsA2j*`Na1ObkVnLF!7_z-F>D
zWN|PwGct(OvV%oA(L^~I@=h_-urk!JF-X_KCAk=CI2lUdT!;x0wOlaG++cY$8^mk5
z89=tOf$Rghi=82hi=l>#K|%y%6tXZkLk%~Bgos297lSxlr9>?|*iK}HF#T*G2R4Ii
zf-H81EU13*8g96LZU%{34ygNBK=!gTm@~!0T+9N}!T}QDhN{Wp02v7i%@Q645iy7>
z;yD;<SQt{67_wOyincO>!i9xFI*%FD$bhQg1xZ8Md`PMvGuE(yxTvc6p-~vm0&1QF
zYk-RJ;sXo}458rMRt#=hfGT2zq{QOX6ovdeut;KZa%pa9PGU)Fib5)+)uaHf8Nlra
zP&)(KDk&+?R{+<6#d-?Csi_JXB_##LR{HwM8L7p^da0?&#d=ApMcJu2sg-)EDW&=#
z5knJ8{gnLV(%jU%5^$}lkK9mk0k=~MQqtg7rR5hXq@<Q4X66(_+AkpcGeHFnxb2Y+
z%2^=nry&4pg@6kbP&Q0YEpbWBDM<_h3xh<!<zx`3nhOGz@<E)SrU)oVN-|PQ5`#cS
zmVl~?3NTq21Y(2hfr_xy95AC2!~huu))AJPqXEia;EE#%RDgr4eNgi!q$sg0H3!@p
z0=Iske2|SPAV-19X0R-T2T{!hYSMybLGA&UieTvo1TzxMEJ{qtEG-7RC^I=Z0>+30
zmylqBip3<zRgh8!)LvErl`mi`Sj8B{7<rla7zLSl7{wSRnYfr07!_C?SR9yy7=;++
z7?l`#7?qgdtxZs9f=eM#41rQGs0|4cgtVu?u>r0bz`+T&0!&cRD<GTGQ%gYp@CNw<
z6yu~elOXN{=>)s81Y~AjYB|VEuyer#mEBwnakC%7%^ILo3~tB7$LA&HrpCvElU{s$
zZhlH>4u}hKSA2X*esX+#5IB*7+8jYbpe!N=A|yZrsQM0)0kQNzgg%G>bq2tmKoHdP
zP7tW72jOCnBiNYPm^j%unK_v`S->PSr#dIIAH-sCBLI~6;^WgwOG=AU<Ku%s`5c_u
za}!H4!1)uLYr)yMM3jMn0i`pb2Q9lGJrj`EQKdlzAGkjPb```{kPToK_52KuCL2h^
T*ntu}xYr@TD8negD8K{&^4(ZH

diff --git a/src/mecanum_drive/pose.pyc b/src/mecanum_drive/pose.pyc
deleted file mode 100644
index 01e65ddc0f3632bb9b99143f2296020fce3c8e1f..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 1015
zcmZSn%*$nAC?1#000qnp3=9qm3=GBU3=9k@3=AoZ3^@!8xr_`^j0}t^ObjWk3@OYE
zEes6J3=EM`ObjV348a;~3=9mJ|NsC0uffQ`z)->g(wbS8S)7@lSHi-;zz~pMoC>!A
zq{InigA@Y;Ln;Hv;1mXsi6GM$!3<`GpkNIq1_p*?kYW%9ahyS#tUzWnGSo0I#Dg6N
zVlaYS31Tq87|bvR3yi@E@(jdKzf6!{!HzBgnOy-UD@#~GZqG<9Nd&vOA}lor%%}u0
zKwc^V$%UonKs*WZS8-}ing-Y~5K-*Nz`&rNk)NBYpORRXnWCSZSdyKY7hhhiUtE-|
zpPQPTm{*z`pHh@rmWoBBAip?Oub>hfdhzj@d6^~g@gOG^^MDLz<b*?bpn#kJ4iq<#
z%NZCl*+Aiv1r83g8U}_eCWaIy1~7{m6l-7>3n<FKELKotf>~?~DQpa8&5R7e8te=V
z41PhN;0^*Mg&>d}K_F)Yfg(8wBn?(m0+KB*DI%a06uv>Az@vIFfm{+FUtCfYAJ50Y
zz)%cw9s{EaqX?r0$j#7T%uCEojgK$kVqjp1kI&6dDa}cZj}PKxU|<O1W?*1IxC+do
zP7DTtJPi&WkVDv*I63{mrfY!2piW6EEh#MmIRzBAU=0u@ATF3i9jAap!3N@QJCL)A
JK`|@91OQV*%BKJT

-- 
GitLab