From 8f3dcfcde0eb2552db9fcce51fb01e7a7de4c350 Mon Sep 17 00:00:00 2001 From: VictorEscribano Date: Mon, 30 Oct 2023 20:52:35 +0100 Subject: [PATCH 01/13] add teleop keyboard --- .../launch/motor_driver.launch.py | 47 ++++++++++++++ serial_motor_demo/motor_driver_launch.png | Bin 0 -> 40981 bytes serial_motor_demo/serial_motor_demo/driver.py | 6 +- .../serial_motor_demo/motor_command_node.py | 60 ++++++++++++++++++ serial_motor_demo/setup.py | 18 ++++-- 5 files changed, 122 insertions(+), 9 deletions(-) create mode 100644 serial_motor_demo/launch/motor_driver.launch.py create mode 100644 serial_motor_demo/motor_driver_launch.png create mode 100644 serial_motor_demo/serial_motor_demo/motor_command_node.py diff --git a/serial_motor_demo/launch/motor_driver.launch.py b/serial_motor_demo/launch/motor_driver.launch.py new file mode 100644 index 0000000..215b101 --- /dev/null +++ b/serial_motor_demo/launch/motor_driver.launch.py @@ -0,0 +1,47 @@ +# my_launch_file.launch.py + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument( + 'speed', default_value='0.5', + description='Speed for teleop_twist_keyboard'), + DeclareLaunchArgument( + 'turn', default_value='1.0', + description='Turn for teleop_twist_keyboard'), + DeclareLaunchArgument( + 'serial_port', default_value='/dev/ttyACM0'), + + + Node( + package='serial_motor_demo', + executable='driver', + name='driver', + output='screen', + parameters=[ + {'serial_port': LaunchConfiguration('serial_port')}, + ], + ), + Node( + package='teleop_twist_keyboard', + executable='teleop_twist_keyboard', + name='teleop_twist_keyboard', + output='screen', + prefix='xterm -e', + parameters=[ + {'speed': LaunchConfiguration('speed')}, + {'turn': LaunchConfiguration('turn')}, + ], + ), + Node( + package='serial_motor_demo', + executable='motor_command_node', + name='motor_command_node', + output='screen', + + ), + ]) diff --git a/serial_motor_demo/motor_driver_launch.png b/serial_motor_demo/motor_driver_launch.png new file mode 100644 index 0000000000000000000000000000000000000000..3e552b0d656352ab602b99cdd24e98f1f69e8336 GIT binary patch literal 40981 zcmce;cU+JC`#%1%5(=4>l!%O`_Ks*siXv%A+I#Op6k3F$p{R(Kq`iwmJMEpc_uln8 zuDCzH@Au#D@B8?9yx;fjZd|YHdOpwRc^=1c9_RH);gS^PR@$v35{Xhq`rH)~X;T6I zK1xo8e^%XpAB=x(F_%`cB9XR#CH}we#i{KFNTfp~nR929>_diH9qg~xzF8Tcy?u$C zaRZ|k{WFf}L+j3n?;@k3S|=NG&EWDmrOF$gVQqDySDJ5`CC6zbI3~Z6zsJzmNiH%jddi6!KZfdhgUA|3K4QTU*=1Vprcd;&Xzp zCjRs1>aNYym($Zc&JJxQ4#Oilw z>9h{DM|qf7LP5bN|8PGB^x(wsul9$K6&!zmH0tDadB}jVWGbtFqcDK(EPNz`bu|i z@8F*w&wf@^%%-J#G|7d&X#Jqbd|p!WB<`%UvvZ*4gJL$9sYqGg(L-OOZc8F63n>7HvIk2 zC*%dH+5Y~!{PI)YXH-{Ldkti27yIf;_ZmpXytlNs=Qm>O`uv;X*S2ljSlgdR#|$kb zWn^Sj)h9L@2YhO=i8&Y|B_(C$etGR`cf7=Rj-3DY?VF(MQktce)r;U@qX6~6$6j6s zS~bnh%|r85jE!S6toz@VhYFoKdGZ1k71hQj@olFmB#s_Es(ajZVX9wH?5m)|MC8`3 zTWPbsz5M+AGHi!Gz7}!%DCI{_?`AvP^n#}6@2#(T9m;y}{Q2{=P0h&?qgK7;JPq>Z z=AVw%%zTNwm&C@+{d_v{F7L;Tj2oLaZJPGzE}!WC_5J`wnTv}{Yr#a*T?b=h7A=9b zYo)R5ywCTvu(R^VGv*hep(c%%mX^;$L*wqPc3WNPsY}olo7D5%evmCc;%^M=J8bNnj*gA3 zd3r>u)V$e7$a(J5&ytcsVLlFy$}X0R^_I79Gcoer*?0Ns)u^njEWzBT{=va9itjJg zHcMg2RK-VH-bO^sFK=4A-6o2A<}&Z)!|GO+7i~K7)*a+G7vH>tUg_Gk0Id3;y6Yi9 z!6w#2mI2z@+TC4UR$CZ0k<>yye?EOs)VX0oD=f)*ZcOXy)vL7*ruzEP3$vpMb945T z^ui4-zkmPULrrZ@`~vYuypS#UPzwh|sIWsdMW%XcMgZqH62NF%wundT^1`&@_3I5s zUH6d84Qgs?`o2EeZMrT!N$vXezHwnqO-%%XO2K%(`{q-OjA!sCaPDuIkkl&K+{b>s zmp^snNFnPO+sTs?4Wh!rExQ^hXt|}w$Hxa6lin{aFSC#LtqY$Pa+-<#oRnnKSIJm1 z+87(lQd?7#mOMK?emU25nfvzlos)LMdzIDRFiLogIqh4!?JsG(4EOi%*>hY(M7_Vi zzbW17ef6(jHbV`ZekKtS5v{r>w$lq==j7y!wb@203;+E2O1tY~T=V9quU@_S+&pYL zykWxz9_u~@A8O9Cj~+du<-B#DYzIAyn3x#5H~rsFP&PS9&d$Q}Wn-I=-AGMUgj8#j z%|MNevZhONeAdK;5|+`vK7Dp?TGGwbW+I}k-FOWqj>vtzL}C5rrORw{~G}l^Du`n3$LsPoKW|@`c-^J%@M#LBxw{CSvdCv15jFW1SZ+UR1hu z>+QnyV6o51b>Wo9Pn@V4Y0WG%n;B|KmPIH$FYNsEW!v`cb)U2fxo>?X_cNoO{V7&fxdXh$l|5y_SJl*v4R%veX?e7k$CcsgV7XFi# zJ95ewhe_QR&!4|6c5!y*Hg4eXVGz?mlBs=2b)+WK&g`riDUIUAix&n6tEL>6OkR_w zNS_0I15^FK0(cP(7cO1mbsslv&2TXIvi1q#$?1`2>}_pRMCbqPj?>7#5zJ?H4Y%k} z+TN~tM9Q~jZoG@fb;;g`hD$P3&?ZAkVgu<5qlZ>sZt_(({0TcUf7&vcYL- z{70FYjgWXURu%^L($Nh?Rjpt96f>hLBJ_vbvQ7s*!!@|;J z#fzYzkF_z%FI9sl3qpK+w)%BwnNuN)7COf z_(6j!J6ERbH3wfPYgA2imy{jQxb%|a%(i_jm8Gw^(iv$;;bq@TOKVikvZHiL0{XG0 z{*f{985eel&V`Spb!|sp$S*mvaqTACl_T8EGh5@IKi`u*@$(e1*Gwk_1#j53+64Ag zgdxk#?K^m!L}LB$@#6*Jp-VJZ-eO5*{RM3XA~Q#GRJZMCQ&Lk436vsVyI|1Ad}2NO z3U`RrD9VdOg@n+_R3qP)HMX~m?IKmS{rU5!5Bs%td(W8Vf9fU~l!pi`l$p6kpc>da z*{@xut$<~u>rxlPU|&^45TDuW(_CE1j?;$AeNtkFH=3Ovvduhs^5l&v zTAVCmO#!;DT)A>%>PDbfh3Mi%-e9?Gr`ft!TzXj|biWXdH7GQV#=OJLsTIwIPnl>u z-bm3qoWqZ?XN}!U_55{<*Q|}j&CTswQBhEdbw~AJeL`3Oi^^LZzJ4>e#o*svUDG3( zqbHG`{e&kT0$>deGpfcf)2pSnl$$*-@Ed--ExZqhy0yDsoq**@-A^PBk+Nuy9!#FD6nSlR;aS zLs@n2tj$P^K=#~kFaJm>2A7(W2m^tVKz3yto8*bf$?0lYb_XZ^KoLA}t3~m_rc|@A zHizDWHiLC?>^XC9Jt%12Viil93;#kNpFjKVn>BtuudHVAK*Md>21!^tnAfBlLAWJP zHPNokp_nCiyy(D<#9I&cvEGRE6J3zT!kMHDrp3SIGOUT#9ZXGbOw#isqA>1y;#mm~ z3WF*mljf8%mL0U*(K0T3_U#+!2{yZCX!udmkG@q+f=lkz>8QrVb6sDi7I*mc;tYqa zOucW)b<17p5?hg&pP$d@e;3RRsC@O>HOGI!6TU9s&=L`VD6ariG0b7kS7>NxX!%S- zQj0t!&H3pr$jR}{w&PJ{k0Wze9j>R`vBT`CtEzi|#;u~-$qF&{{({&+qzmp3ejh)6 z?4k2NASRZ(Z#ehFty{OUMDEKL=q5Qhkw+h8Vmg<*x;*;g_3QZi_cvzG_XHD3QMI;O zaxIMWLSLD)s-|3=>?u8M8V4OHVR2syoJhoc#=FM=TMq*>)erN+##iF)EynTE4yp)s?ird!h+v8_Lh@VI5 zMef_WV@EwvD&9zJ_A@auGz3M73J*Voz;{_3P8GBt3m3E-iKRLs={%Y|s4gZ=EodEm zlzImR#q%;<#q;MMEG;dyoW12)_~zrslkk7jh`6sPvCJ*b3SXy_^|ojqyDwWccU%Q=)2^ z4`H`|e*gZY)wDkE?gpNld3PK)=;1dM6v9hOF9PHAJw6q$aQpV{&|$y?OLkebEqlc!wYpHB z%Wc}iZ_@ZV>YJ$Zzf}25i@NBYTrdSr%aKEe@}y;Dxhidir)v~Fm?Zo;Z3dLO%7sR` zXGKg9_t6gVt)_e{lVYpC9zJ|%Ze&+T$wv*W6krsmncHTWl~U-r9zaX6&2&{I`I>OE>4ZH35eNL7^V{!}X@om;V3 zqu~&sjij=doZ8+vhiRdiIwkuGIVg@CJ^F3J_33Sj1H7d~*ru1RayA} zF2(C6)-Y^qwqwiM0kjW){CH~wSl)d$lbR=t5~YwHvHt1Nr=ZTiJQ&DDV!T51oi(hz}Pv5SQaD>v)h#_~5) z`f8)7NkSNi&=~dVK0%%kAJ4mwO-;Xl(Nk>cO{Y0Ot@FX7G)8r?oLDNrE|%$Ju^|o$ zqi`1WxhLVaJ2mQgb$;$i<(TNmcc&LKwrIT;uVx@ntVq&uavJ#Xo~1XMl($~Z$_KX|ojHV!)jyO<#Vrv}Ze|t_l6HxfWAB|ou#X1tgovsNH1_l<<-K%-t*Ez)|Kc4K3s zt^-h8z$6&1PJnvx=i5Ja`PKXS`2{jT|DygX`@diWPL7AWWD}L9Q2cCHg{=4HhXa-Gk{lKc>Mm> zZQIJ0)2(_35U0u@rYd%;%d@HhuLNuco~z^lbMfev(}|4bZ3sAUolWKQ*9o)AXCP+! zRd3HP{E&Bj`{&PXq8^K_&R3Lmxsy>=7d+TD=;E#43iXI6;4&GXM%*Ap1xo!*Q!U;H z_{yK$j#baBYknilR*ibeY}M2E{ppdi1-o{a`qBCT{RNP8fQ(sP}w%P{k2t0#A zRwi7R?PP~yRKP(oOVbH`Lejz_QHy$iK(GDpN6e+#$Pscud)q3j4`Qu2{1)ftsG)9o zAer*it7+4yR=gIWMAqY{+a3W>ue%J4RGOovcmI84B+d^tN*>ps|FdVU^o7`eZ=}SA z*i}HV(iyPP{mb?u7NJxt4cdM*^_p(k%@_51r%^DC>e`(~vn2s@8s*QNxm$%hoSZ{C zqb%g`?tSf49(w>-pt}k3mSloPc4?Q}>S8;-CtOZ0hHY_ZD`C$ycF^S43=9>r%P3>B z1IGN!(mPRf!0cslhz)+(`anC#R#}(4*zy;L*ctD!^IrSXvt1{9EBO56!$ez`iX`yi zz*Ke|N3tlbR)jt*5C0`e9WJ5-+hp>g7g_kNXl-Bnc<-swiCC2+T9>w&;bxnjQW}~4 zrtjvH!j?-dlk(2ElNli4_N>&!sSXaCb^m;!J0F<46t!#DE|%gj``=rVva|}xfMY>S zq9P+-P6M#TcE9t*ohSpAGo4^(mtS3(%^l3Lg>VtMjc>#9bb?zPn|g+HhWU{y0E~K} znRk6)qXb{?FDTodYTEh{dDIZg7+{;8p8m7E{2F8uUj{KY8A5E_wR`vAaQn1wAk;Zx zUFe`V)eki$4QA(STP^$wTZsfe2wo)O3i+U&Kn9;aRZcW5A*8;vFmXe~@)MbPnI0Ap za7-p(wjQLedr5>zCPq1-E>XL8Xow!5~`lY-ubhl^vIxo6b_cN=_f%XTWW zymZOeG;3Tkl{dcR9i6CiTGjIMfO76<7Pl*^fJ~P{1*4$P1luTUI7C5Z&@<>A9E>se z_3o9dDYB|+0MpaRygc#L9p*T&r!a_69Zn#V3=;ok9G_xaqx$$4 zTGTx&$aUSl%Bx~R_2aHcQI8)#t_~5f;x?#ek_+Z7qgcq+Lix_XkuF}I~C zyZ-9)gropr=W6ftS`{Xlk1rv-VsXm`IL|@c2L7qV?vg-RD>?z7 zx@P4Hrru5Yp+*Y*KTx)!0oXq`E;NHF=uo!Oi#XaF*W&~y4bkcfA{Ul*#p>6uASq`$ z7FR1hJ0^F!expW`t`Cq)sWudG2MSJbA# z`QEUENe817Zi~(B);Ofs2h{4T0%%=_kv2p?u-u+mYnhno?aV_{5FDxufXW4>TJ|UaSIgl+&&(I6Hw}QeG9ERs$8xs?jn>KGYLYWV!%w>xoe4J?+ z8aHA+@kb_UXkjIOnHrk)J8&MEjIlV{wdJYInB#YkYsWFwpx??SIjnP;P4^W={mm54 zp3+x}qvJtAL0$t1NGm?H35A^;CFM8Mlcg_QI9(!SJ5(vQGQPuV6=jj91i_Qoay%)K zfp@uMvsfi$4sM5u+o{?N&SQ6rylEncRnX~^n*q|uSG!XDW17d%8|5WQSgT;;^k}=L zzxB+J3X+I^<7b_oaX@EP(~pfxJ`M@#E5XN4ol3{&FdVpHapUV2j7JB~7r|8hKW#uulQiyl33>{!vTy@tkXibC^kf}w*Jvaj_>e4aRu1~t)_duacF5BamP z^jiXA_NC@7$02lC2c3$GiZp62R>?d&W>6g|bMp9cg&Q}52rNbR4LR>dmVIKnb!pTm zyHO27>;lf{G1jYBX~cmtFqgBfr)d@y6%7(|%W)m|(v*-yC1k8!PGKr21~o0;&T=ZUFN$3 z19*l8ZzSa~LP^qXoPYhHY??FNa_gpAlO*pnIirO$4QNpUArAyB zIabK0&Y&!a2Mww~YsQCkT)N)f_UF#sSDE2fb%LCyn~uJU%fi$Lt3jk`cP>Nrce&rc zOUrzY^~o}87cH6CTW;Yt)2P?80v)s^UKPv{1orv>wD)8b8mnB}zDDYD&I|snrU^-* z?;Bf*#DU;%cI+vikxS3l0Z`&`nla9vu2Wr@C}A&i>^)6^e66c)V>5kzw*3iGd{8%1 z6(H&lwe7thW8zQyg$h_nVu@pBrxUV87u4-qbpn1>S6ff@U6$ETKM5>4aFAFfmc{L! zg;#o=ht=!D?6MPMei1M&0KEDnr1KJgc0!jpx{;4U0wBRYTRX;UaFL&j>zB#$^3sxX zz$56wJ?)HlfBACb2y+1xlGVkRwp zKjN46zzKj6a01Nw7yiW}vz3yv_KU%sr;A!F(37P-B50Wxb%F%(S~MPpy0pk zMzxalM<1rp;2v$ zicEYzA~$j%unk3p8!NUv8Mayze#U)jHg`2UKe+QRU}EP~bDpY_l0=Y!z)bvL_5r2a z5GRW>Ms@I@iS%hEnlR^yN2&)PApnyV8z1@c(9e&LY&)HP7J`rfny7Vp0;15-wIMYY zRk*EKbz!4Z>4YH6Yf*b7x-L5=jd&a|{iJ4A8Ga_f>fOaxw+K#GbZ^r(Z*+`QeFY!| ze7hnBO`t2Wn4Zftd-|H}vfJvcTW~J~hwHvFr=f@576yzjLR_#;BMeL?CY`}8uP>mP z#|60N{mpBCunEo)CAuQAnOMl592AB8mVX{L^ZU_@4A_Q3rVJ`pymAH1&znHj6B(_j zRz(qhqJ}Mik^w6KESr+TkWnivjq~oTlRSI&9;e&N(lL=0;@U0(3|2^D%{)jm-dAI; zsSJSMo3sEB6dmyszEv8;Q(`&vZNH{ozp7GeTHVjtK3PV#I`^1_lDSBXFrr!?9H$;=4o2b03z-X z8mF2XG>z5KP^&BP0`vLY$X=#3OtR??hn{{G1A@qb=` zVcXOvpW}z2M{r@?R)8rSDGGq)XGYu2?mu|I)1VAhjindjd}`YaxQHaGSYw;w&Jlx5 zFoB%<$&eQ>bT*Qa)y*#nq?5%0KM=IRfmq*wL#?~(T|>{z9WcwP>gbY{mi{En{@}p_ zNn>MUcTh*ILXY%Kod_^=Q&Of{Q_VCZ-n}cD`^N5C6n+i$%yN+XHQ*;SU5&* z=7I(A0V=8Bh*J$Ky(vX{k%3E3j{V>i({&HLPAHQZGpJn5~oY$`+4ZKA~ADC)CD`7=P56UA?$ zK@SC-=!?jzU%&p6;QX>~^Cge{thN2;oB8V@3e-(xojZ4~Z!jUZ4l1v4dSV}7wEAdQ zY{^(>p~ws;nk6L}L)c0GNWCzZYYc8ni8^KG%78z0i=(-t77j0)20vx89sK<>J;aYm z-Dxs41B=kYZ^DR#X+0R21utpefuF!jM#Br z-fLlQE9o$pB$EY>uk9kKO^=Hr@WCsiM#MQGv!cxi_7X*VY0_%RFtx?5=A5iotF89g^!xKt^=)^cF*cFSOFSP=HgKNpcBUs-v9y1 z?tPBNAy7Cnk)pkQ8UYO9EyzqJ4kk^hFD=gnJhdWvh1==)!wJ^`Cud^o{VUr&C8T`m zsvse}N1mS^X;lS^xD59|Do|fnvELvBsgLpT*TO_yh$hP>oT$bX0&fvZDsu&P3N+e9 zTseNr0IQuoeHv2#J1EdK^Z8F=@HjfRZ%4y-fSf@KZq=>(G8uZ_DQ0FV$i%Q?R1s+8 z-po5oe!2yuZW};@>492V3D4~&)s$DOEYSxNUFbU^GFi%nHk%UsHiR;Ttf*?Lv~4ZL z+(>nFH}5~IFj*zV(Es86>nnIdMEtTU5}$5aP_60N;*qp=`}TST9QICo!k%V=e;V8g+;-{TmBnMthOU!@0+Ah}(#+o~(kqg;S+ z`4Q$8nJbo-p9N&IlG$7Dq!@jBycc95fXSfrCFcT)v%1;hI~Fcf-#9|>GH{NQLBmG3 zJj9`@mk_>yM=ovi?pyVS07rB=R=WJe;;|Q*amAEsTcLsz86z3gDpb_vM)d=m?)_*M zF|5uN``obHytY|iOy#x~gc80X#G7(L*2H7j501;A&rydonLxv(s}D#PaA0;_JJHK> zoa%c6MWe4fP_qtrXwann`61^Y{zn%;bsK9KQ)nR;1{?_CtphZ&>3*S{lJ{^$9;Dru zPT+8G**o|EcHimp|A-DV6d0muTvMYonX8F*at07YvVPTJA6lNr(@kp&rZ1JFj18ss zfI2&csz$6%&lL)AXhIN0Mtwuv-uEYr8@HuJg0BO{R!OF!xl)6!MRVCAHy@ueN}qxC zB0$tMgp*BD8||fJWF8UZ2_GD%mR=n)Z<5r=ty@aHM&v;FX|od9*H%V9IOr(3-Bm@! z2vEoqM~|Kdj{f=M2dCdOpdqxnB-5OEU3dX5q3iy)7Y-fg*T<_d&+%BvM)gD(`@@Kq z0Or#lplVtLbiL=mfx+&lf4i6gbwg*;P8D)|0rTP7(~ffDz;fu2c!Z=+HCC1oE>Qae zx~WJcxm(m8Xxx9X=#oSe|FVIBL4cXNbzkL~U+?68v8J}Xh4}FS@$vt5@o#XGC$Qp3 zf?}t(3-p=BlBaGvQ+mMRBYn z1EUg!&~~UnYlSZ&JUp-Z7<#)ue*A!kf{3NsI8{D&pMNG9UX5)9Mltd6b*SBh9|9kw zkEcD!#+LHs(Zh$8C_g;TbN^XR2HJCTA@pZ)8yf?_b)H3DBrP%lUVkhi}O3)KEoi+m5!y5ubx?ydW>1+G79f1duf0nxXN=(Z^?B z%E`&mpynZU+ya0v-{tro_7wEjl2?E2p`$ZiiD!V;i4WMbdv{mVTFqM*Y~GfV@R)^v|C^EmfP&W$bj^0c1pxCFM>11_~&ZbaHaT4BJZ^epFP*qgb1^XFc-sQ&v!@Bn)3`fA*{K zp0Rhi6ec>n&sR@R?-U{f5KSG)*12l`ufyo>_yP9hO_3Wp!-`80l^AN3JRs!k=EKIJ$Yms zMG?m-^HWmi&nNNvmft!DTU%TAb9QGJ@#3QQKvxsp$Nv6n?)kaN}wYEnx1{mZ|B;F ztbdvT!*XDr%oG1Hh?Qmv@>(S7B zfSQst^r2kE_u<1Qxw&rJL~evnbN;n!h7%WcEQz;4CNN^XbzkueO&yzOwn9=Ft#z zAZ{nvokAKK3L{d0Ve|0JG4)({FM2$s(ZYzgdqiM7E_XPn@hAMj&ik70Xt_#~<6kAn zQC6zstOUKWM}}#=Hv!aroS*+XT0f@2?;Yy}3%;g&TZ1Ma;i5ve42a{j!6Q$TwUmvY zf@SneReO+mz>yCxhMP}}z<>10~=RnZWQq_!MU?zR@cW&y7uX8SU zESPM_VO)ED66v1#ms90Wvg7lz(6{Ph>FIent#dnAOZ^L3aAk4X!{q<{#}}iGo&P@UjxBEP z-`^|asqX)6G{6GKbI<(Wm&G$BcW(dpcm4YRZ%Z@x^<3hu5jGxlYGu z8UPiw(UaXNQG&sMC`Ld;PEIg*5q|>dxC+>xB3>Mw$VqJ4V7j24lQSHDzngWBrZcbs zNP^I*%yw+Z(_w;@5oI5JwZK%*f8DLUrT@ow`G0K*d=!w7ArcAD`R>au_dhCoJE?K% z&olXyKAXRfa;Id=8UD&U_z}WEn4h12L0Vb?Oda~~2LimIiB#seCdm6oLL|6^<673l z_xE?r(F2Gr_*(7S z%PyMy{d%~MmL$Kz4mfO}0@k#I30S>D)-GMf;X@iJXxTy7X8fin5s}s~9>W?bje8*6 zHLynjverd%;(!>Sbqe3ueRPy~AZ`&}!Zs_^E!1UPhzYro=-R7;!Y7#k?Gh6R=tZe4F$SSl476ouI%FZqV4Zr*fZOB?p6KC5X+3v^0L%yf&h{>j*1C4f40TRT&S#98A+X$-aI4+6PzL zT{IF$#!V0LM(+?k`B(xgs;0sb`7ZeRvu9m(gs&a-;wgG9ga8HpI`}Twv>HOn zp1pgcz9tKEwmkTUiWELQ05PnP0@66CiG*%3N7zM5S zo;iK0yQj!V1v`4<Y$)6Cy6 z89)|yB--J;(DgGw`;yH&&!Xqj9_xm$g3mQ}@DnB=>`Y+`1B4sGH?jPOS&Va#FzOLj zjF!~q;TvD{pQbZ_cY(lZ!c@jJ2!g=^;RBj^RI^!bYl|3e6o$p>4u`*cDsFTWy4cvv zA-2tgHy*;@8%To&7>H=2-%iU2oCwO@vw}6MO}ElRSv?N-r$4dCh?7(+v&>;$m$^<) zbWnIEz?{+VllF&e{vA9yFY{-hC)j{{O8di*boIuKll^C(48x-bP*#%B0=q~(43MxR z!IBN^$2(gQD)j!-Ck~=FpqrcRIDG|(xUqr|FW{!141pwi;fSR76Q*g@ad z>{&!)BopfFD;vI6>u1dZpelUz!WHG^GxjF+jqUL^gq#QcAn)(~lHv}Y;qN#DsG4Hj zcx$90L?EfE_qCwS&(FDz(*uspu!7CMU9h6e7Ewttenl8B4~jU}#Ckn?WMNbfNSLIi zyc0FRE;R#v)|cI*Ig9aaiT_=Cooob&ehHGm=@M8q6I(hyGxG58?6-+SqqoipHeWR* zH@Kwy%XE`!95VLaG0G*tkj=kz63&Vw*KtNM@ch%;ZPLF!yor*>WJ*)t3UL=`6hxM% zAPM!bjKGE057pd161}8oyV16Qlwv;yz7q7>#v3%D7U}qmHTCjv+y1?+aGpctn+?(H zPiE1(;-vB%0hMH&+T2<`iu4HIpnm-BHW(jM&6jRbfJCqaK7Srf^iBm6fI)S4=2&)L zU{y=2v~(uC6b^9z(Jx*H8;#Fhag+@mEE9st)foNX6107-J}%BmIJ-`sI(1YAO)5o9 z5p>%_V8Y-BlMLZ1!0xgXqs7GSvS4lH24zUs?Glj#(X`M>YJ46Ts61+~=H3TtPc%5J zjjUjMf8gmkI1rP-J4kdAl9pPmD*B-W^teKla)`JnlYy!utg)Eh`}!9YJd=?GSkB#+ z2Q|x<;YX?w`rlSU1#VlF*Qg-Q&Mm92E`|!$viKo}H;_?Of>9wl?PUB3vuvU83UPp; z77;=y(FAFQ6OcFw+t{-X$tmRt=do7GX(f3^-7~ zh{1_heUN5SF(P$T1`=p3F;5Z(Q*#C)Xb+k{Gj?4*e6ajpw$WHckFQ$ZZ9$PRZLfL3 z7od2zSax0fx1q_OX}(nQUe&n^^9)nEh9klnBFp z(-#9p_htOYaSjexAqWu*dB-W#1l%#%X7HN2#kak>>L8`GAyK0Gm4}uW+gGnaWp?kn zJBto5G4BB{xH2k6s^?z-Xa0lp2%Y)(sM~5YMLs(5*Y)%pFmyKDnyE|}K)XGj#t%BM z-}oYp->Xlv=n|gcG-={yRnJgDpVmf*7eWE_+Mq=UHG~@C3Ja1GMHpt#!Z*{~i>sy> zb0Se?45u_tL1)H1Q2uZTA&)H13n{wWNDX{q2UwOt}QUz$U)P&CMl7n{XZ- z)C#`+;GlpiON@Y^1pPw2FSRK*vI?a_Z8pH269yq5_-1+mbbmX~`gFm82NQlo#Q((6dgr^C8HXqSqnEzp4hT=PSUX%Us;exJ-e_&Hw zHNX1!9WTw5iNKH1(F!oi6W$xM9mS_;Z!r7=)A~mEyqez`MKj&yHlddPf+_BnBtOZE zSVG_x$K~nzbjD+Uztyx!_x^@Y_HfZmScA`~S3%NV4E5Q!{_ zw3=jGo&ljc6?3J0vMwmqblatpOg*&honv9LD9eEZ{S^GxODay84UYB1I1!Q-(Rny~ z_AHC4xj6?z!}|6@LKlXTb-1HQk)6!Ask*h01;Mn|FaSTD_45JC=0aG9+LWD6YZ#$j9h??pg>Yr+a3TA4yi zI-vn!M2wc-JRHfp*+t>f&Ni~;&K+)*yXA#DhqXv)8^PO^u3wMFB07Q^Ha&u)2|0KG z(?x_2YGVURKn!1pZOVt(SmVU;O&d4bQ^` zuvN^550`ChY$9}XG=~y@{=9%TN8P-ZB=QjSS|XbxAMz|Z5i>S6WZlVpdchdm`!~Cb zeYo$XzZ)ob4?X<|?oNL*oDIOsKRkcP{W0fBasjCnsb0yeTLt8E>Cy+!N*D;JIy#*G8MH!aqgdf`0WL+4Y))jA45D*s5 zYR$snNFEnxA39vaE$NDyF4Hd%5g(?erUb=W?8?KTlqjReR$AGd&NEBljD3cBbCQE& z%xM-2`wkEDQZ|(iae#z=6>2R^_{12e3(_9?I|(ekh`1A+oRPN9bAnZivbrF6GG^4g~37rF6@GQf}L( zsGy(_yi~ZK6_5XF)qNj^YGPe5G60o~drs~_Q~M2mOs&kXQ-HoEaOxy8bMIUFK%+Zt zu>6A;675iEcc!YCo?c}mk@UOa)PctP2HGBB>O^%_fQ>iG2b}_94ul2=7eMMzn(mb)%5@GJ;Jp}@NI6G#8}@k(T> z7STM*WrabYte#PK4@~jD;)}Liv1XkGWqbPc>HTgJiTe!lAcO*fQ)8TQ09K=(#5;FL zNKC}=>+WM0iNKrB%vgsCO)KK~1*2XjbuR(~jc{IjE{tp?!G#G|GBW~S};Wk@SL)Ru6T+f#fos>kde4(Z3MXY?8*ak1(|$NFr29IFlolQtc`oK!+?5L# z9uscBwwh|R;NeI41*0rCe3Gzj2boFpx?_Y7oZBX2Bx7(2gP<{ZEd<);7)~aH4G1we z;0gm7iH5MT)@B1Voy*SYpLHShzdI#pOg(}e!K#V|GBK)+(~beA8mAl#V8IGDKoLz~ zw1$R;)rvL;&+``9W~*&cd}ki-Sr>KU(E6r zZ^?KcGmO2N-{7-1-%=9ZTCA|j52goN-1 z$HlR-x*gxM^})pDBQGvkV7AD(J?9|u7{;pCDdc=D2J@iWwd)>2vjENgvH;hmzJY;) z_I3s^zB!nT#%G3A>4a=4K(_K>G)+Xeai}$Oe^XNvIR(X6pyi!AcPjLn>}@*zR^jjG zoFzX4fAW>C2ge6WsH!%B^4Wl+#92v*brmP{~oS-_@GCt$e|=EGaOMn*>7AP$HUcP}s5 z(1!C5%(p%ut~(usRyxkwMkK4!nYnRoVW$}iyjtPHF9Rc^!u)798zOA% z_hLZ{qKKuov-8Z%%)H6ZxDnav-Ub)k9^g}q`TX8fr%v$;2^E%=`3U%BW(r}Z@nWDV zK5Ea;&7MjpzoCGALaQ`H%#B`r{xdVM5;asRctqdC#l>kHIdKo$aF~N*C!}pK-wn{> ze4+RL#fQ~JUW`(k3kHcv-R}C_IVH6 z;H7%<Wqn_B?Ed2Fg* zkt7aLmT>mK)0L0DQQ5vLAKg<@c!2?TBGCBE+HOLfBlZv}<38v{{*ND9Ky-^RKxX)j z^U>B$EH3U#^f=S5EhFu@e(=UwWMzekI31g_xR7|A0_`~8n_U9~Tk#~=>yMpV5qIa8 z`vwOK3kx^U)6-{hd{BCflqT%5um{@B?VpX9s%D zWo2i7*epPNp5X&@28|m(ZxUOXI|9c5iG-q49H{B~2yLdSjNT)81qJINpyr_uZ{GcL zZ>&Z(EpaH2G!yN|Z=I8ryoIqo4iv~nfe$+`5qq)qyoof^E+!_X4p=!JdU+L=1@l?V zj;QxY@ELP+b9emyT|kx9iZzW@;mRO8gu77MKuJl7$ln9C_B`m_%dY!7vm7QpxX4em z<24T!lRa`RSq{&hVWi_II@TS7gC$GJiHRQuWr@@GQ|jKi!}%EV`-B_Ha#`F*oYEaT ziZQctaw&x@%JmoI7CJ#|a{1T7`LNzTL`2eQYZv! z58x}wF<;y~7I4$~Q8;k&o125Vc-k8qU&Sv<;W?8ZN;92>@1QdJ@{zg4MXLgs4fxx- zW=GqloSlV<+XmnT&Hx@4yF`0;iu%Lrlw=#<9Gf1cHBrcr8F}$;g&De*(V_(-p~=h78f63W801n+29RkC%27C+Dg(Se6L%{kWp=~A&hXIN`zwCT9588aVd2*J?{6=K?AW+@ zVwg?-C*Sx)U!3FROKp^=oTZGo0f2?^KV@eM(Al4-Gwt-p8vhG@;TQ zCXcac=6EB&9OmWS2hTx1m>x+%i^yN1JQyJ3G^{zUmSsT7-d zeqt#`b60(#f@^mcJm7a-;)hVVNlL2tWUs`3>vr`mG7?0w^B{xaV7Ee#-HwPPA#1GZt`|tUtguUcSII{!NI{H4w0)e zLTU%8+dYoYwB_dgaYPPB!5ko6=fJ?@Cr@@#P;-#rDcR}u%`j&@ku{a}zq;u0VB7v% zm^G@bt^EeY9d~=C#QH)qwn5_JMGE2|VX|Kv<{kU7j&%^*ul6UM`)}#!h|>y)LnI08 z%oziN6C`mdsm&MzIIp0v6TO}d=tA2H55jmrgET@Iun#?zHt#B;JSQd9 z1&JQ-mm=C}pKg8IiV1CMKx6VPTh72X@do2)rjx{4T-gGoNzG?U2Z--3=|kIzU$bU4 zzu-gV^$o+=cf!ILpmO=)MI#-#t1E@r`0eWI>QVjC|I+Iw)ZB(-qy4a)Jw(*I?_5tp zzg-+Q;+=c;+{bK6L1m>vS-=$>?`}nLS=ns|g>26u*Cb-PpXumP#*-(v0yBJr;VluR z*tjK)3f+ry#IMuRJ_ZifhgYE4npoH4Po6z<2S4e++5@^2!k*O8(GhN%bev%whC_(L zg3_G`Z(iiPcfSFeIk>oXd&~b@R?*Akj6h0cyrwM=v662Po}`YE5nniRi1@>)i+a1B z2(KF7XU}%SV?sqk^Jr+uIE3F~J)l~t=Vv0wH*em2F^GE$0bT%_GUSLj+4@r?-~HY_ z^Zx3i)`NAFsB`yG?K8^WI-dgy-w1G61e?A&FwoRs{SKUo2d`h#v$Lt+_asihb9ry5 zD+D0KGw9dHA0$2o1_5VGx7g2(F#*o24)K6#%=!Ve}MJ$e3|h|dW87D|_tni?HEgLm<6kl(nI>&b?DiNk1e zj99JXS0z!#Ab{pKHqtZOvm0dG<*yxAP-2X_1I<6b%0^!pz+tQU%v_8>>S#M0-Z|a?`s5M*OHc( z-yh;1eiHs%Ej>MC{riu0GuJga46Mhg_x7gv)6?^@`T5CbAt4mIckh09?l249XwllE z^b^qP(5^=}O-+6Ab~l?~i8nR0xZF!IH&Hb;wNH4L0zW^02UK>63m3Lx_-;Fpl}+%R z%#o+kPr||~oL@daJOA>bj}H+%kT+n0duwXd77RaO6r$&mE=$;D7S^wMN|J*RNk|_*uK%_yq*C z@ROuF@blo^L0nqIa-AfO0uU=OM90vOH*_3$T-Sq)q?vc#`&o|VamV3UPeL#5?PurO zGhmW84Ggwmlx7L8JZD>YsAhBRvysS>C92AK}Qc+Q9z&lNX;@&u40JiIK zf1X^1p%~)%_MW;%L?6gyA*cukiWsm={Ugdf-N*?A4Gq+&XDAMt8D*UqVS7A1SWlFG zh6i_KF7E)wJ#S&b3BEE8iu|EDWcHY-C_Se3c90ICky}=$@X;Mq4qV+6??$_Y2CMy4 zAJxLluotFjJAuq3iAP%bSxoQZjCITbXAjejcy% zif*R6Akqz|98myBH#_p~8m36v+}6?QL>DPJU=^>i@x%5JdrJ4hA%HNRwo=7vD6!Qeg1qiQ1Gn9#$xodSfchwUp731{VF@Skej3G*wPT2kl)i%PG ztf9S!-*M{rJNYnQU??J$4e)wSPyso|nC@;nFjMNY!({C76`rBt;Y7U43uSQwSST^` zQ3<9K0*e z_t!1C;V;~*=rAdPf1drj}6{F@qH(5;OMBPVF6smXzMF1{9~0ZzN}W8yMGTLMQF z5w#iERV~*Qqxi9R-@NH`>Xp093Sy%*LzulKW#{(n-_NyZ{prcutpiS*uT!Bu*aV9^ z>;U~B4lPm)1uXD=$c8WUw0M6mkF+P>%z{}3SqhkuxlAi_y z6shd;99B`=`>~k}Z@@Z2x(N>qUY%l>TzzGZD0l!nn19v2<`*8X_54N`xEcJ#Ix8TL zyZ}C5m)gV6Y|t$`GVfS+i_^~d-Ip(W@T6zJQmn15A!OV^0jzC!2VtQTx`?!l%v}Ii z$nmG9Z?dtnenSQ@UtMu|8613sn|lw8mG>;Xyu6;ic(Dm<`|um#>fH_~eJ?qg2XTOO ztI?SbXj3=@6=Qsd`7-@L=?krG| zsAy|@J+H~MD?cyqPN3Q4%T#FDKY99e9XdXEui0@{C;SD|7+cW--g$OxqFWXZLGs+e zP=pcZwvx$i49Eg*ifd^x;uv=h4XM4kIp6NK+KK3b@&S=XIYU>J5gO>G%DOrah!N9U^!_Naz4)9x@ClV&{^nF-GXXf61`KJ+l`tT)2mmiuZ2qK#4dO z4*>_u^3>mdJ=BP_^z<{hv;Jh6s4GiIp7s-ew){W6z4<#;?fVA2HE2^ABceo7NFfQK z>?ny{%2b3RRAkB&qT0$_LJ=7%nKEb0ka_%Uv%z@E+@TpN>?#*8A3yCf(iREGWoy#)JP`wlce#DEHcOnOev zgXm}@uj%CD_#72iByKr4VEq%23U;|yP*NdfG~wObePUuN=!$?)V1Y3~ItJiF_jV8P z4&obO;%|M||(c7!M_{s94f0|V!O9=V8O2gO()9+LQ>LtbD8jJd1-aiXJUkH2$J zT>N_|A5=8z7j9TuZo@IR)fJzy>-X!ZwpV3P@`x zud1SNXlOvUeSX_fwA|>n&_U3Eexj`idD=PCVR8HRZ9G#LO;29GE`sLabLyod)b|8P z27y;iZSCD>&$eRKtWP`Db4Nl-il!=9hF%ihEmF8TJz7Y;3l}788Tg5M8zt|u25&kx zcip}F*(r~oKlcUo0w|3@oj7u=IXbxkWD;MAF7A6@pXN1vAC`M4*!?K17N2A2uzrj# zFaPxidSkx%I)zM3ni#>20ppR$@9X@Ag%Glx?sg7KkN~k0z_T? z8*o+pqF_BAh(g;hYxA8nV&Jhdx0N0FtTkCf_(`wvh4)6Rx(g0rk#9Z_pe3>!iv4@i z&d$z{p~aytx1|{!?30s|tN8Zq`Lk4Pj;|9ERHyEo5@dTB8(Uhj0-skn0swk~klPWr zKzxCqg2D?HiYjP|@BwLQ>B2lfC#IO322o01BWtm63LMT8xIr1eaj~->$Fgv~h8HW` z9n&kG9f~%d9(y9B?3|9y2}GZ%K}uM-0V((m^c(8Ah|c|e;N|<7y&oa{TA1*1%-<0KEZnwxINdp*Q_7&p+-6rHnpVr z75_(%_5k0%H;?4D2!mp`aCUI;ORw>k7UqF@+lhU!a2ZRxzJgocS^U5ywAZ!hid3<2 zb7Pn}ATJ{mW@=_;H&ckImc6Ig-StbMi@jk1Ml^N`xg*DlCr#tBrKLJTtoWi*@Zot~ zJMpd5Qh((`T z6MVqPzdWRj%uE41TMl)7+!>;&JiXR)pFtib#t`xw1Q>J2Zx-zZ%q&xqD1dkSUSlj4 z+@!)QH@&U`2S2WD+t`9swXVHCo3MS~y9@gI!I&59a1|me^Ho(<>qxbT&uP8d%7Fc> z&%g;E6?$QlI##4Zmo7j}>5Ufk3A_cRGtOX*3gu{zXXYCK+Npz6cDV*m?8l$x8B1F! z5da3+b%lb1z%P;i6{9cOgTl{y1msb;4c?3|u)G)+{P^+XdT%4TY&K3#A&WF19S|w7 z?74IC;x>lOn};eL9345`o*srfu;OQ18eiI_>sPK&06&6XUC`=_V_#6kY?b3$_I#jd=?BgITxSadSfxOmW8+K{K%c4N-9vvB} zKK&jcorb+<7US z7w52t$E5`D^~$S+rS{1M=-)muz^G5k$ju$;I)Goq#$BebpH#GF?SHru@v~M|R zQC)gFFOGmk^(F1Zrx(;Br|nWy;qR)c@pjRqZQDTM;UC(5gP%^wkohql4OX}U>gA_BhT4Nw zL*tX$EMAGd{4^F((PWVh&89briM7e%M8HGbyF;vHfSPM+k0;oc-3-CRJ^TlIut;_y z?tEk+Km^UL+8kva`f#|_V6D{ixS3>O%2?xxhUf_@rd*+bn#Lal;D+})LFr3-oYcmT zwoN;ghk^g~yLTbNYeTGH1;5;{TS-r;tMvAzx1f6r%*9^r7*OUksYy^d*SkNWJ^OUQ_rbGUNeZ((oJhf88*lojX1EyhDH{nbFcW;w8%+BBZ$)M05O! z1D|I-3=KdbA>wk}?L;8YBZKqu@ChGF%i$s*D&H)j@rg32k6HRQ*}JB-(Y&U0sD`QvfL? zT_BUv6+WXP@e5ixZMCg16}?|2L^9{TozcW~<5QR--)0afpW22km>H>_Gju6`sW)%# zZ~F4(N^10k_#Z$%x<8{byli6cip~ph`pMc3mK}NjHak0;<6US zp;`efrlPqyxXmI8$;p#Z8ArZBB`rV;*|^y^KpCAY`fAF>9Lrj6OUwb{{K|Ud-1PLF zJv}{dAg{Tr*hYpvbc+IzIhbOx3W|%T zUtB8-sCQ_v6&#hM@duJ56rR*Q6n*^&mUF+Rt5a`48&?Z}p%x<}H!I`bg~7{yUnvx4 zOkClL?sAQ;7mFb@iBu1l^g5V4jxSP=qEPa{>%vWXR_+7Ad`Lo7{3XXqe^?KcLGZ}x zO7E&|KZ;2>rtWas!-w{DJJ7&2cosfp=|MEOBn%GJczEFbK|P57Y&Y~Ifg;=(j#QcN zmh{?;eb5E(#3zXCt^D}WZ?;R6aT|2j{P;hwv3Euej5i4L)4)NzVE>1x5OTv46wqKx%ZTq2kc9ee6{}Fm0;ehk({EFcSvkwFo@)We*FsIPZt^tBWG#BWT4+ndOwfs8B6%p=h6Y3C8nhFt zctGLYwlf<0kDXLe5l(3YHc$gN@ibXDdUr)2&JQ>Vw{>X*onVO~h2~`gc#*jOGq`RR z506!m#?f80$LLOHXQ064!yVlT0B!j9B*F*C{BD}AuyvS%Yl~FOdhq#{xA!a*~e8KBFN zf54gu%F9OrQ1=14U|$Brsg^2&~ z%=Cv z#;#xVc3lRK%=oQw;bQCp{pxhjW$lI7h1h(!4ruVR4uJMhm&-W|I;JxDF^0|XDcmns zN?vmtsV{I4>Vi`!3|z6gnq(s|uIz{9YTO0yjX9W6SD%e-6l2s(o{zC-Z#li z*Z|abfZraWWyDMSR$duiFn0Cf;o^FXj!oj)7IRMI4+k+MKcf6W2(^#0Kw^{7)M zXKlZ!&D_EqGorB84rwjrtP=+e@;=an8T-ZqlhuU=yvxUz*tofCc_*7*ZD>x#Kb6l= z(WW>9nqt?vcu@^FeONl03W>|(7g)eJ_TTyplxO*LhTu#?sJ(<4P!iev~0S9E)RV zF#NKS)Xb#2rL6PawQ^?(_Oj^OgHTdY7eA#=wolx0@z}Q4=$z+@A^aIp4EDEfnbI${ z3#2@I_UyYux=K9VT6&wmQ3UzIZg%-{sk4_TV_$4+tQzvYD4@H8pP^V;>p-Ev57yMY z;tzmAn+Aj;xd`3Fq-xF*IB_9OPde-4Z$Ly-QB8ON5`E*2zFkYvEX6E}Fn|Xx$h&g# zo-H1~znmLfYZ^IUfo+1!KQxznwu7qb()r-hfvoKbTfA)Mzpv--3a_xV9y0P+37uU| z|7feS;qG6pz>i);87rL`cgGif#mF)`a_xKXYat=P(KZ2 zoy1=nM!n4iT7onL;y!bt^7BWs+vT^R0M*)Z_^rgs664%r9{@;PsH6WO zcvhQ}H3&cay)z*eF^sZ|z3Z+pr%8_I=@#k4)|&dOI`rInA&uu1%Vy)({Cpj}uiOt7 z(*af;_G}XQ%wba){HJC>no6Wm0W1wgp8w^d8_v=YHP;G|HEOGqqoZ_uaj;+JxLi+5 z%kLuP`xrWGEz|10^;>GrA+x97E33_jSJIShAKBffGp;3?vkOX_N`?{$1lX%M&vY!b zLwJ?@K&bO8c7z$rtFD(by_{shC=Fsko1zym1-L4&6z;x#+?)u_DJ*Qi-!yaW2f7Ph z2}OSXyTL*zQ}&A5!)gB-yk%~K)Scn7;(`~72<4>#wm%sAAI9RZ%V zZf_g+&pq|RB71i2x<5PD2gY1Sb^^MMaCZv%ZAyxZEntI>;|Rvk2{W>@lg#4d;nDAF z%b_Dw7?Co-YlseYPoUmDprU*L%19(Yye#!MfR#nALZ#mKoqCxVohIHrO zqPY+Qs^h`hVxjb$}$*#>U%!Ub;9hu+?3_M6wP)sAPCWHuC!K)BTZ zA|AB-9}KEVR(ug)EeI+JtBB-@v4kEy$w}~{hK0_h6~Vn(METiIdaIYQ2GR~N3{2)8 zyNeYCIQ?!c54iv`q|~?$DQRv#m#Eb(bhgD?U8o#W6qMHY@2?-ow(gBu$aX>_Zf2A* zPG2|1BzwT9c-@DTT}PM34}vbV|Dva_{|XN(&uvkX^c}5=1;&QBw$g@Nd{%KOsXNb2dLWn@jRG$M!zXQ=Z zVs`y{U6z=zu#fClXr8H^^dz7@pV0v?Ltd)Bj1g@B@TGbrn)um8Sgkav{iL+=S~n0lWWOJW7Dg)@aUNtU2r$Eu-OgH;+JT-p(v2;yOO~L zfq6M7i!e71iGv4|-#xm(ieWA?Ggic1aef7A*FmoZtaQNHz>{$=g-K7Eoq_0Z2c(0>J*W9YTFn6r4Zb*?&;SW; zDW=k}0pFn#y?s>}Nr`;JIBB;Z3CZ%wKb(@8F~&1McS@@3yacLfJ&9V>iyoFx#s)Wl zYKCe9tbB^NM`dP)Bw~C0VaH&hu<-Esfg36N^l39Hsi~AO=F2zocR)@;P#J8fS}p7G zPd@8_Rx#bs)q!f#lFMs~jLJ~@5u+;^c^4_pK*>iua!8^TkY|W&dgi2_{Ja-qDZz#g zd*EA@I*&6c=)Zlsi_J%dBZU`u_?y{(;2VbX474`n^&isr=NygHdGJyuRu%VJcJ;>f ze`%@&p1pa-J;tE$Y(;5l?6(|Yyf95*l|VVrk#G>l0h}bT11W~a{ajpJ;sFXdGE7rm zwkp6k0a1$9y2T#!FmQGj|MK#^)j`W)_U+zY$PRTx@a=E;x0ACkwU0c>O$JvJWHDCudoN#|Y*xrih;BC6*RcIk~#4?it^HAll+ zFQK5I5Ed3T^(C=DG5H1_$NFe!n?>W+GPDxM7I8^iay?+$?h61?0BjHlMM76|>fflR zLVFwD=Bij)$_4Ef`XqI7+D-lTni?{Ac-Tsp=}3RjeG&RFsC0sT4#H+RtYCVylLWa^ zIOFV5+x~TvVGkZW_}ag7uOI~H zh;10=>Kup_8hP80r5fu7)t5yKe8_~d3=?QN!ggsiS`0R2n_IV(EiK1g8|!0Q_K(lJ z#b~2enO;YKFiEbxbpJqTX!!U5o}0tdw5LyXLL1`L+8sA$A8xM(dUa#4R*Pq?ruL_U z&^+QqHnG`NI6zoAIJ_XB;K%>N=5xLyKj~hQLTSrN*Tzd{Gzx!O=ns|;K>@Vk?0&%( zP6#h5F^>2{x6+T961q8qqAlErvn`chh4AX$LEWT#1-)aBVinJu9e(cy3tjTOY|Dk{ zB9~cuZ0%S70iF^6ht9?z-)=qdamas)>l(B}o#(o<@^dV{y%R3}J1W}s-iaBFS|uSe z1f3_-so?hK>?h8zycBYL_YRES$Z=oGcy7;#c<26$d?yEfXYV6U=nCmYcyo6{QUXxz_l1}y?v=I_8Mo-Jen3HRuJk- zqEu>rzAPjXGP|67cMRFVe--8)+XPq>i zj*7x_PtY6??-4eBgjTOVU~v9CJNBVtFgW3wd@u^0vo2>2NmAdXyL)_y(muz@lcRfD zyXf0>(caMyceZXZ4;C(OY6{9YynDA-Zc7#ZL6|$t7@?av+c+#w{-?EskSo&0#Z{Ff zz35e_*kW06v;%|d)8~>rw5T)KKZh5cp@V80Vc(`?8-`S4Nu~z7;k z@uR6^!}PcG_tLU1GlI!R-YO^DgXL*v|w`!!bS*@Cujo- z7z_dqqLTwN-tgq?c&5skGt3a2CyG!0)uC!VgRqOV*XT*|o$M)LK6wypeg5(|Jmx-o zLzIr-ph`0YuVy7sNP?xFnw@3u0+u8)@ml)Y7g|SSZLQ|>XJ`kTZLj!^p@Y_Ks>A1# zdqNpYru%@$Dzhq@x+-MkF%BfJUP9$^bhn<)kX%HoSJAKSuA>3Gi+?kdjENcVA966m zSMZ_izsAEiG$O@@7;BMb_g<%MieBfQf#Eel-=#{WR$)6AhX+fE)t~|Fg*d{?pywwv zQxKM<9KRu~&5zFJg2@&VO3?L{$nn4#H1E!hD!2`)2Ki;n2aNvuUK-x0A!|0`iW9z~ z;xACCOd}C|l+)LX4y2~0s*fPHQ_}`vDD93P+YXqsLtj4L_TW=?fEx?<-sqS0LU3Zm z9XtV4iTex$D^gu09?|SkNT0}Y+~^v!To)=Dx%-Ug%=}e|duK~PW_!*!JRBl-i!Oe# zvV>j^??Lz~X4efsuHy9FqXhm*u%kmjmkY(pG92q77fwKbu>-s%p=p@@Ub+^f+SDu7 zQiv{O&;J;^ZG3qD{=hx|)G)=6wQzw(Ejm~(C*#S@_HbVqz#m8_fcaD?!PGYs<^3h8c^~Uo7Y8)2IF5lFxSJqy3X{w{curzt@AleMfGz0GQPe zxVbiPCl%hFV?^_L9D8{PlV@#FJT!2Px5qRZcZ`ogSY2JKYFJDfI{j04e|QK~fIl<$ z9#T1e{QkgP5uyRiJfOdh?5%+3=E4NQ@PpUm$kA+(cptZa5mPW73DuJ5cCu@Z%C8G? z-6$ORCe!N|s(Vj$0&I9?g>Tt>)@9m9LEtK}^nY%qNqI;#?qL&;iGPw5CT+DX%VX-hQp59Mu!m{Cof4S*IK|lB zYI%KqZq7_mk5+tfc;=#>o;MJeV}fk%Gn={U9zM#-I!Jgp2)7Bgjz8eUUHG`P9-y%V zl-D;h@(?HZ5Q9UZL*L#}ukM(en2AH%vhVO=-l~4P{W-){+%bR@pEjXIGB#UEKg#I{ zeOHf`4JOdx0dEwxLZ+WkbaR!)q8Sa{Z=%Bv>QmOr7y8GUulvOI?Nf$C9Epn?N&hig z>8RK?W(Ti=Cr%x}VG(EZKV@&$SG~+E$WEC;4shac1MK6GIi=8hhF!P?$ZqjRf*fQp z!*14Bk$x2!_y_8S@I#uQ=^NgywznO`;c*QL{(*ry1vx^l1-Edn?4+v{W=+B+fCmLX z(eE9)ClWWq7ES2pAt1Q|%=8DQsDRnb^z;u1C7ObWydQKKVWI*NrTF~XkR4=#jV&9o zR_ZqlUS~gl{(OTqVo!T*dd7ff9_)9wJfAx4A~yRyj95kg+fMD+Gpx;x=)fy4F~wpp3p z;jy>YvfB+9HWYae*Jqcm%>-H&J!o} z+WmuSVc@N_xc#uYp}y>a1Iz#lz#8#6^iE>{%X43n#&G^|`#FX3Wi{7Je56oT9GDj0PO~8hNKnoYLx#VRl!16@Z@EuASF`A5PS+! zN}t$RetTrZJ!GMHW0bYGB*^=v#(x1QW91C4)BWqKqkMn8f*2{|-Mhg{X+=fuL(Hdl zgW9p5+2U9EIwr<*>m=Tw!dZPiy~hxcKZd``E^usgRPtwg<9EOsJ)J$@lsP8dVQpX_ z*f4wM8DKaN_k2*Uz;h>Gco-V)VA!-tkmLO#9vdH-wriHFMu_elMxPvHZ z5~83<%0yJ~8c#wcLmZE+_H#=#VSJE;Xxk3z`<^AE?SqY{pzsWsO%Ns6AA?m;U(S7y zL`K?O2*Tbk(Fae$u-7Uq#p-{cwF@7>O!~cR36GNymh>D+((Bp8EAyEBrnU7MoTF3r zyBhh?vVG{{cLNI8iG*s|5rTXOmq*rV4e!FtOwx2w$n5f>y#OZbk&Rd=P_vpM$T?C` z7ehI2AE*c0{ztsBPlyuG{D{r3QczUXPqq=YDLfrJ!_+vMHZgXfH)4h|?%qm)MEV$)bC1m%^F;{XeSFiAgw-KFP9%^gf~auvMHkf6L6!^Pdp zLDj!vZeC6MIXe0TC2M+g9my1qEXvWo4M0Sh=#IfPzpk7kO#@Zo`I%BSlBog1VQ_4zOlf{q>MXMK} zI~o21&G)*-#=vsw$2WKf3Z0Y*laGCWctk`nyibylNI_Xv#(?s$Bba(e{p87bx!hm( zReDmN=6WQu7@l-vr2GMLA#>mhh_rPBIC3*WS0-IlD^W|0cl-9xs>{}hGQf=>q(rej9;-s4Zr&j>=>nhPjCvo1jkC?HV5+4`Xsfepc83_MWYUMtIc+9MWky(8aM) z@;IKKefV_y{{g|xHc3Agv~v7>l9>ZTH93VPbp_Sv}>bkLKEqpGcPaso(6iFGc zcP(nuDdcO^HA3oH1FIdZlmY@l-o&EC`MU|r@*zn;QFy5N_7EN$^3HpvKK_pj5i(FE zn~yYdE?~Cw!J+qFL&PyBE{iJ|7dgI$viZ=2^+T*cA%EXA<%Vje9wtJwp^z8xbdt48 zjBsks7r8l1k!q7Ek69WB&Zm9NMt$-le}B_Ju;=4SRvpQ!uO?GQ&r{o>5k{=PmX5+JIG>)DS6Cr^W4?|@JTNy%D18gnY-XSJrUvCQfZu0T#S z4Mo_Xcpns9t~<5yd6~g>;zMvtTk3gD>cJTRg*L+;4S=)NE~*JP!=$dA^IE*^>KY7x ziO@gbS7GA7{%I_k7-&U8cVTa#J+_-Rs1f!lvT9GiYtNL^zF&}*)y&AyM_KrVH}Y~I z0|Rj|o_Zc`%6|JHiVVMYT5m_=XB;FDHb9~LfuuR~+ilo;L|#mqu$2wnK1nEhFnOrL z@?O{QA}mbxU3NBJjKFi3ejN9jP=-n&S*izLko3Dgz0z{t_9`VW^PT0L#i6a#LV$lF z+4dMTv@`92Qpoct((^B4MJS_}CblL};_Q?J7DIEaWSPz*wG|bw%kr>hDA3ANc$3$C9S%;+P)0#sP#t{0ri>UZ+Iuo zrP0doA>M@gmlq1bY8vY4sbaJj)|=md@tya=+bo4LR9D?NS6vQ-8mnSit(!m#{5`t} zu{*N;X>PMKGi;G6{LEYZD*Z|PTYkZGy<~-XF>DGVgM-x_jkKD28p;JWS8tlSRDRfvKBp1>S zCub0p_(kc|q2REvns666xbUuTY=op+5IOckIjvjy<)x)_%8J+V4^2kN2lmIX^3&k^ zC@TBch~(9R6LIFp&8YKknet!)|jtPE!ciBe@l4#Gsy7 z_*Lw-_=F)E?_KDxOWH*9_!Cj;#N2`r@q=_7`hOXQ}ptMl3frT9}M>C z-Y;u6NVYdnv^SXkr(-&XS4kC0KADj$%nw}yC_uAc+!M5fXh0!?ArQTIWEC5VhNs;I zX6<^n6w&sAr;qQGB0wP9y<;K0#%GT#%_Rtj=?>9N^0$Ahil1@hb-XBzCLDnET|j~? z=(C={7!msde6d#29%$={-XUNhW;ZjtT|?p-NcrFRUv@3@BPYfux3wKaiVT4^!Fply z^=BfJnVA`3?|@UvN7jBgp}lMYN93^JyO8SIG>M!WwHe@=9=3=Uz+euM6XC}G+}4(+ zX$Wo)4m-+~D*{N7@CRZ4p}hRn7pan!d7d5e&Ln8}H$j_}bw{L=x+oWnp^9J@pmZkU zJ|JHQpc671PJBBA4_YY{ z{X}`GwSOED;fJ5ukLyl<_f8qxhxSZH{^*nNa344VFx+9WG7?!R5Uj2rV53YSJ)jGA zA{xs(i>5VV7vCi~jU|Y@0t^83NeOri@@=aY(=S>bBbNe#o<^qy@mX1O^X&cnXLt^I zjuYGd=c)*V0iwLUJ@3mc2&(ezN0So zalRuFYk}zqlVv(&wZC!jN%tEGfal+_AaQE(1K|4wpET>>3`xKB^Bn0Ic70p)o7wH~ zIMVQ81pw4TglN%QT=Zq8SPl!9phF3G5O#_tI2YhOO8(G4TDt!vEC$Bb*31BD;-U#{oH629T3L;~PNh<*L?{TXDc>Q3jC061stnnW=DBnC zFffa>T~30mo(w00Go;jnvJQ)?u)!)}KQ3}0q!h{Y$yx^lXpl8-05Em}6NloDdDGjs z5gkvT%hwW{tXp>Pnwu6w6%oz-G?)u6^ER>2}n(G-Jg+!Vu1 z_uwnK44DGt>uZJ7 z!#s&2hMaRY$6di2Ov9HR+sAo)A7^rBB=Zvp=MgiJS~m@1I|Gae7UtKN+FP`V^c}(( z2k=Fkit`fgfpBq8H`ESc4z8$Bw(Kt803PrG9WaRm!DAT!4!l?&s?H=~o#BoC+HI8%-jNDhddCH^xBx2cB0}*x#@M%c`nQbejny>uVj- zBf#6i(;H*;__i7zG!yySPoxL5WBx^4 z+(r^M&T7mTEUc__V1p0o|FKnCKjq6UaYZ@1dAzrTkCv);dQp{VAP^&#jsrCy7VAfg z#8>|%iE_3&^smO9JBqOM6T1ZT?w5f_70Uc>i#w~OY+AF?QrN* z62)M1*L-yg;%|{0+Dr!AZ+EdycL@rvL*XFoTExdf30lU$={z^eA#L5Gu41G10*m6@ zgw!F{M&XdZ_uyRul};R@jN8S7qNZYI_J&9Upb=JG;?Gd|%Q^k>$G6@n=cKtH$f%-% zY6FXBa_6S(W(arCHgch-(5B)HMqdCfEm($(D%?;Wm!*~b^F<`}W$Xkp&Qm+_h`rI% z5D=TVgoF~LA*@JUz#RFv>9B7dA{=V!>L`eZ3vGtGv8dxY(peZ;GF_Wc~#zGi1G)JL=&QqM{7@Nio(QFg&s4B*JvMSd24Gn zr`XH0Esx;kW8zcv$E&qdNQfvL)hKBO9$w9C@!4O1HfOrXsGpUJZ#`TN{7*ZL+ z%JHXOeRFn{8~ZH*xdPUC0C<*2uYkPB+sCJra6KWw4VV^@hXq&|CRb)mjz8H%wuHjB z&j_f{Awnrp<+-#~(oOcgbN&Dp4Bk6#hP5B31(G4B2~UTD${4*ak$r(x`*Y@e2s1rc zbf6>&otqek?w5lQ#n*>H1d0Pg?n1EJhadj~Cq5QvylSy0l2VXhhQfFmGRAf|ZM=Hd z#fs4rp%iEL@&cNXz~Mu9f#^}eh>OO@B&swKPJr=_%C2k`!of%<35%DLY z7UE9)e-lseD8WP8nt_h#c*0szR8(}$ok-`I+XR0$B;1=(!HG1mRS(yNZOtud$_(@a{E0c^Ed6sGVAm6jGxM0$=o+8eca!VZ#l-+^cnO1)3<`e z8p6dS1anq%BaA)mR%)njwtW@fWt(6ai?!NIU#|29sbg7G~)86GH7UPL;<)X;sDLT%lP;W zXkjT=ukOYgw87*_WSXG4U-Nlu%=>%Oh}VD@GWu|~vhrZiC$n-aZV~$4xw*OY&WV1< ze1kyP`FG-H-!6(O=VxVIPf}t@H>qZVb}Tk8kBjflAAWeVe6dBLJFo8j^orqSVj=^Y z?SzyRBIW`#`q9BfPA(cd0u~=5_5r4GetbKUv2=u{zP=wOve-9o=+Uy40hP6Zh=t^C zPruKwkP96rUYs)IsmN5xur>(}-jV~@N+U=(BB8;`O6=eA>>1lg>zD(7?);2_0A7tMe2#~1O!R$-CXy1skhyp7-eYi=dlrWm0PMs;4ZG-J?$Ab3CflP^>gve5 zdZ2epZaXS+$^N_E-Y3GFf6|&PD)4Ss+Vz#K*GtjKT>%kT`TYot$6!>y>-8!t2dkn1 zA+pSH`1vN3ab){fp%_d-S!9A_v41yr1RR94q6+>y8-y3*zK!=3@nBVQ&-2t2Ht!S= zc&QnCE-|KUEVgZoj>PG=9tATYIhpGwzRQM<>4@8&u z9Wya`rT^U>*VS&BqqKBgY8#GIFh+t944_D>le=#%@bVN@AjlY4KS2!;)fQfIO)2Nuejvhoq&6cp)OnfJCehoM>9>y+>&H#RlL80Ss~Hd|>%=6Xd7Z-)woEFXGeEgjeXSy zZ4zP2$7V<*)Ij$sZLx~lu?sd=AQ?6F^)zby{d0-GkAL*>wh%z-+;MGfj!pDyQ5pOZ z1dHr{;(-HJ6@~b|57+)ETwMq$TN^+-tOsO|7>|1xG7D$wL>r~tnC{ykQGABMNBJIM zQr^9rNIN0@>y7x#;osKQHh|iQjgSI%9KwR`z-V+4XqGE%43b-krqwRL%jlVk@)q$( z>K^b2d+2*sKBCE1*l`fY#Y#*$7PUfKI56;(omF$EGvZ}zkyta{naj-Q{+kshL_eGa z!G>C13i#>~?vBUdpsIE>MKFE)B1Rv_6>S#@WhA%{TifY``t4;qBM}g^d_o{-r>rbD z?jR8g=2>h*sLAr|hJ5wO#E5BY5sDPs+}V1NDraRS%rFG z3yv$dBs=Q1^IG?EqdAY$C-H&6vp^uBcvAr{ekQhG)JGH45W*XcdTtAD4gnJ^0E3Zc z&YX_(@p=X}u*5g}0B5`cg{?T(CXy!+Q3dGE8OLZN&PVooZ{>g2T*ZMq3|A{U&4Lsh z6oGX?3?BGZ@p%}VH7@Ifz5PLy$zex-hzD`$F}Uz1i>^@+c)X#8>#?GPTCaQIU4y!L zHnZ7!^=M7rF1(X6x>&|&{K|YOazPkSGu9A0jF~p zPnl?%Ky)d)xfPZ~sO^G>K!LytW#HihctA%|)zGNK4b5{=3A(AJ+Fg!F&gW@Z!B;xd$rq)(3v=RNtZonh3k>O=oM@PuU@ab_r z3A2x~q*-s{e@{W^9gMhe;$x%Jj0|h8G|TcSeH_0d2zDJpk~f7jL^sJ<-?}9Qgc)Wd zOfl;oIfSbZCw~-td8(q%h8Tjx!_@)ODV;w3G3GDEyC(2zbG&PNy??HzKeDc1VQo@I zehy)qhRcRgD3}36liR^To&z4i8SlH6zg z2~iwsiedyysFDo!m-TO9ll^f^?#HbHrOtx)?`^>VD$(U9-^}VT&g!t48`Vx5fy>yi z+^$VhUfylrfgo=oF04zBe~Z;7c@s&H0c^$6Kcj#{$+}{4FB*8+3$n0k#y~bxjk|gl zqu{r17m5BDmIVZ990!AmBnx7N|CqatSc&5)I!o_sgBzfhK|H+=u9N@?kW;pXgu3w^ zyk51wbrF&jyCf!7CK&$PQ=W%_VXE+v+km9xdf#DC>p$A^~@6=3xAmxR8*07IXN%kt%KbxS8w6B%U=FmwYZr-HlMVSZ9Z~e zT0r1=3v`Wkc6LM|OikTSToA54AtB)yYClrT`N?adJpQ@BzAJ!A1Sb8S!TtrO=CxE; z*CTgUSDcXM8dFfP&0~4`p$wnTxx@-+(|yrI>@zCvN?SPaUt{4-kEMhy0dmVqRA%{K z1GnV5AMBsve5#+KgNy7!CRAcC&-U#ThxR@4^SggB}or>h)GG=Oysp`Xly3B9gvWShPGV-t%p^n38?@& z1pf&Bj{o`*)&S3)}G_`TzyA>H3HJHN{QLoz&hJ%@Nd0+f94dxg-Ffov>O&)d%T8mW;)Ap zx#U(6rcYO-wmobWiaRI#U;t5H1bl`xK>hCXwpPzR4vhy;6^XD(0;|WnpYJ}O^XW>4 zcb_AI1ig!oVCX^v)IV7ld~X8l5)UL`rj=;qve?!s@iI;0tem^eKb4XDkd&NU8C4$; z5MW_xSpjObasW+#P*hYP;y5y3l0y&uXZm{8Q#6=>vAXMUOKbpSDJ@l1SNkc9K_@1G zEr+40Mc2j1!=R~x2AJx#q0sFaIEabv#0zls=n0f8%L z8JL+pMnRN)w(W4G6^>0zRATG<+|eQY^lo%f%H)q9A7I59JM?)+gUzlUkfN?`N6NB) zzoW*T;};OPx5|kpwZ?xZISlL>dh3TUWp5>Y{`Som4GVH?s0IaP(|230XLI@Eprnl$ zC$^q;bCbuTc5^><9C5x60G9ezno7pz=U>=VgxOwU$6>%WkPflHq0w&#H?!i+l^@0| zigXTW)RU7x_I}zaz-ObeUrAXCF&>Y(`qBSwUd%(L77!CG& z-8Y?hj6%IFUjNB8b%sSY9Xf}%2Kj8qdLy1=<3=Pa()%9hU$e5RguYl)S9fIEz%Jil zQx(+i7?KJ(A^T6%l1s?@bp|s3%ekGX1-p8p^G8#^G&S)zoEl8T3or2-Ot8mjBmeH* z>w&CXX}X?`qIlv%*J%lIv}cZWZy?^2rT5%K|4oM^8C%QQAncgX=Q>}p`IT~VD){zB zcxrs_-Y_>G*=u+xU^i|-aIL&zq4PH$Iur-P@^OIYFoOMEn(=ls1Mm9p<{BJ9IN4Km zC}<97Y*)^O6rGF>4hTX7DP$}eUU5Y_qH8$G@BXVJ0k!ck3a% z2R`@jCOzw77#JcPcvBLgq%6p`j?ONh7r)Ue^R(J8%+D8i^+nz0W6pM(wQ0j26*Cs* zvVOP!nwZ#bJ2?~kcI&#!H442azxXLiNwwT)-;R%(x24ik)= z|8Ac6pt*MofgIp$wCV{|#VgE~ZElLGJsoSu@TZ##aVxIw@1L%?`Yer{93c^zjF3}f5~X^zfI_Wf44VA zf@qEY-+$;?_5b@nYgjH4LwroB8x^RWNlwRu})ny~ybK|T3zJ>c^aznWgN zZSuUd-hC$y#j`_V8I#I!}vx~l$R{vo)N#KgjY7S@Rsk0SK*T<4m&xeLRC(s I7~lJU0OgiZZ~y=R literal 0 HcmV?d00001 diff --git a/serial_motor_demo/serial_motor_demo/driver.py b/serial_motor_demo/serial_motor_demo/driver.py index 6b2018a..dc2421c 100644 --- a/serial_motor_demo/serial_motor_demo/driver.py +++ b/serial_motor_demo/serial_motor_demo/driver.py @@ -18,17 +18,17 @@ def __init__(self): # Setup parameters - self.declare_parameter('encoder_cpr', value=0) + self.declare_parameter('encoder_cpr', value=17200) if (self.get_parameter('encoder_cpr').value == 0): print("WARNING! ENCODER CPR SET TO 0!!") - self.declare_parameter('loop_rate', value=0) + self.declare_parameter('loop_rate', value=30) if (self.get_parameter('loop_rate').value == 0): print("WARNING! LOOP RATE SET TO 0!!") - self.declare_parameter('serial_port', value="/dev/ttyUSB0") + self.declare_parameter('serial_port', value="/dev/ttyACM0") self.serial_port = self.get_parameter('serial_port').value diff --git a/serial_motor_demo/serial_motor_demo/motor_command_node.py b/serial_motor_demo/serial_motor_demo/motor_command_node.py new file mode 100644 index 0000000..4922063 --- /dev/null +++ b/serial_motor_demo/serial_motor_demo/motor_command_node.py @@ -0,0 +1,60 @@ +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Twist +from serial_motor_demo_msgs.msg import MotorCommand +import math + +class MotorCommandNode(Node): + def __init__(self): + super().__init__('motor_command_node') + + # Create a subscriber to listen to Twist messages + self.create_subscription( + Twist, + 'cmd_vel', # Topic name might be different based on your setup + self.twist_callback, + 10 + ) + + # Create a publisher to send out MotorCommand messages + self.publisher = self.create_publisher(MotorCommand, 'motor_command', 10) + + self.get_logger().info('Motor Command Node Started') + + def twist_callback(self, msg): + try: + # Assuming linear.x controls the speed and angular.z controls the steering + speed = msg.linear.x + rotation = msg.angular.z + + # Convert to motor commands based on your robot's kinematics + # This is a simple differential drive example + wheel_radius = 0.1 # meters, adjust based on your robot + wheel_distance = 0.5 # meters, adjust based on your robot + mot_1_speed = (speed - (rotation * wheel_distance / 2)) / wheel_radius + mot_2_speed = (speed + (rotation * wheel_distance / 2)) / wheel_radius + #convert to rad/s + mot_1_speed = mot_1_speed / (2*math.pi) + mot_2_speed = mot_2_speed / (2*math.pi) + + # Create and publish the motor command + motor_msg = MotorCommand() + motor_msg.is_pwm = False + motor_msg.mot_1_req_rad_sec = mot_1_speed + motor_msg.mot_2_req_rad_sec = mot_2_speed + self.publisher.publish(motor_msg) + + self.get_logger().info(f'Motor Commands sent: mot_1 = {mot_1_speed:.2f} rad/s, mot_2 = {mot_2_speed:.2f} rad/s') + + except Exception as e: + self.get_logger().error(f'Error in twist_callback: {str(e)}') + + +def main(args=None): + rclpy.init(args=args) + node = MotorCommandNode() + rclpy.spin(node) + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/serial_motor_demo/setup.py b/serial_motor_demo/setup.py index d7e142e..ea70ba2 100644 --- a/serial_motor_demo/setup.py +++ b/serial_motor_demo/setup.py @@ -1,4 +1,6 @@ from setuptools import setup +import os +from glob import glob package_name = 'serial_motor_demo' @@ -8,8 +10,10 @@ packages=[package_name], data_files=[ ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), + ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + # Include launch files + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), ], install_requires=['setuptools'], zip_safe=True, @@ -19,9 +23,11 @@ license='TODO: License declaration', tests_require=['pytest'], entry_points={ - 'console_scripts': [ - 'gui = serial_motor_demo.gui:main', - 'driver = serial_motor_demo.driver:main' - ], - }, + 'console_scripts': [ + 'gui = serial_motor_demo.gui:main', + 'driver = serial_motor_demo.driver:main', + 'motor_command_node = serial_motor_demo.motor_command_node:main' + ], +}, + ) From c6d952b542c34aaeb3aa77ccf3efdd5b531cdbab Mon Sep 17 00:00:00 2001 From: VictorEscribano Date: Mon, 30 Oct 2023 21:00:57 +0100 Subject: [PATCH 02/13] setup tools sintax fix --- serial_motor_demo/setup.cfg | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/serial_motor_demo/setup.cfg b/serial_motor_demo/setup.cfg index bd0b17c..eafb207 100644 --- a/serial_motor_demo/setup.cfg +++ b/serial_motor_demo/setup.cfg @@ -1,4 +1,4 @@ [develop] -script-dir=$base/lib/serial_motor_demo +script_dir=$base/lib/serial_motor_demo [install] -install-scripts=$base/lib/serial_motor_demo +install_scripts=$base/lib/serial_motor_demo From 72eeb3f30455071706f2dcded900c67fa6176969 Mon Sep 17 00:00:00 2001 From: VictorEscribano Date: Thu, 16 Nov 2023 21:38:50 +0100 Subject: [PATCH 03/13] change robot parameters for correct vel setpoint --- serial_motor_demo/serial_motor_demo/motor_command_node.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/serial_motor_demo/serial_motor_demo/motor_command_node.py b/serial_motor_demo/serial_motor_demo/motor_command_node.py index 4922063..36571b5 100644 --- a/serial_motor_demo/serial_motor_demo/motor_command_node.py +++ b/serial_motor_demo/serial_motor_demo/motor_command_node.py @@ -29,8 +29,8 @@ def twist_callback(self, msg): # Convert to motor commands based on your robot's kinematics # This is a simple differential drive example - wheel_radius = 0.1 # meters, adjust based on your robot - wheel_distance = 0.5 # meters, adjust based on your robot + wheel_radius = 0.055 # meters, adjust based on your robot + wheel_distance = 0.28 # meters, adjust based on your robot mot_1_speed = (speed - (rotation * wheel_distance / 2)) / wheel_radius mot_2_speed = (speed + (rotation * wheel_distance / 2)) / wheel_radius #convert to rad/s From cecc22e32f6e9a34cb33dc6dbf1f08cb4e569caa Mon Sep 17 00:00:00 2001 From: VictorEscribano Date: Thu, 16 Nov 2023 21:39:28 +0100 Subject: [PATCH 04/13] TODO: wheel odom node --- serial_motor_demo/serial_motor_demo/wheels_odometry.py | 1 + 1 file changed, 1 insertion(+) create mode 100644 serial_motor_demo/serial_motor_demo/wheels_odometry.py diff --git a/serial_motor_demo/serial_motor_demo/wheels_odometry.py b/serial_motor_demo/serial_motor_demo/wheels_odometry.py new file mode 100644 index 0000000..503fa1d --- /dev/null +++ b/serial_motor_demo/serial_motor_demo/wheels_odometry.py @@ -0,0 +1 @@ +#TODO \ No newline at end of file From e23878e4576cae656c3e7953b22c6c9e234b28a6 Mon Sep 17 00:00:00 2001 From: VictorEscribano Date: Sat, 18 Nov 2023 13:35:43 +0100 Subject: [PATCH 05/13] parameter adjustment --- serial_motor_demo/serial_motor_demo/motor_command_node.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/serial_motor_demo/serial_motor_demo/motor_command_node.py b/serial_motor_demo/serial_motor_demo/motor_command_node.py index 36571b5..53920d3 100644 --- a/serial_motor_demo/serial_motor_demo/motor_command_node.py +++ b/serial_motor_demo/serial_motor_demo/motor_command_node.py @@ -29,8 +29,8 @@ def twist_callback(self, msg): # Convert to motor commands based on your robot's kinematics # This is a simple differential drive example - wheel_radius = 0.055 # meters, adjust based on your robot - wheel_distance = 0.28 # meters, adjust based on your robot + wheel_radius = 0.0553 # meters, adjust based on your robot + wheel_distance = 0.284 # meters, adjust based on your robot mot_1_speed = (speed - (rotation * wheel_distance / 2)) / wheel_radius mot_2_speed = (speed + (rotation * wheel_distance / 2)) / wheel_radius #convert to rad/s From b8239e2fc5f54ef90808c52fa91392735d25aab8 Mon Sep 17 00:00:00 2001 From: VictorEscribano Date: Sat, 18 Nov 2023 14:05:10 +0100 Subject: [PATCH 06/13] library of functions for basic ROS2 transformations --- serial_motor_demo/resource/transformations.py | 179 ++++++++++++++++++ 1 file changed, 179 insertions(+) create mode 100644 serial_motor_demo/resource/transformations.py diff --git a/serial_motor_demo/resource/transformations.py b/serial_motor_demo/resource/transformations.py new file mode 100644 index 0000000..542a289 --- /dev/null +++ b/serial_motor_demo/resource/transformations.py @@ -0,0 +1,179 @@ +""" +$$$$$$$\ $$$$$$$$\ $$$$$$\ $$$$$$\ $$$$$$$\ $$$$$$$$\ $$$$$$$\ $$\ +$$ __$$\ $$ _____|$$ __$$\ $$ __$$\ $$ __$$\ $$ _____| $$ __$$\ $$$$ | +$$ | $$ |$$ | $$ / \__|$$ / $$ |$$ | $$ |$$ | $$ | $$ |\_$$ | +$$$$$$$ |$$$$$\ $$ | $$ | $$ |$$$$$$$ |$$$$$\ $$$$$$$ | $$ | +$$ ____/ $$ __| $$ | $$ | $$ |$$ __$$< $$ __| $$ ____/ $$ | +$$ | $$ | $$ | $$\ $$ | $$ |$$ | $$ |$$ | $$ | $$ | +$$ | $$$$$$$$\ \$$$$$$ | $$$$$$ |$$ | $$ |$$$$$$$$\ $$ | $$$$$$\ +\__| \________| \______/ \______/ \__| \__|\________| \__| \______| +""" +# * PECORE - Master en Automática y Control en Robótica * +# * Universitat Politècnica de Catalunya (UPC) * +# * * +# * Participantes: * +# * - Victor Escribano Garcia * +# * - Alejandro Acosta Montilla * +# * * +# * Año: 2023 +# +# * FIND THE ORIGINAL CODE IN THE FOLLOWING GITHUB REPO: https://github.com/VictorEscribano/PECORE/tree/main +""" +Descripcion: Funciones para poder transformar entre matrices de transformacion y TransformStamped de ROS. +""" + +from geometry_msgs.msg import TransformStamped, PoseStamped +import numpy as np +import math + + +# Define a function to calculate the transformation matrix +def Rt2homo_matrix(translation, rotation): + # Reconstruct the transformation matrix + # Create a 4x4 identity matrix + matrix = np.identity(4) + + # Set the translation components in the matrix + matrix[0, 3] = translation.x + matrix[1, 3] = translation.y + matrix[2, 3] = translation.z + + # Extract the rotation components + x = rotation.x + y = rotation.y + z = rotation.z + w = rotation.w + + # Calculate the rotation matrix components + xx = x * x + xy = x * y + xz = x * z + yy = y * y + yz = y * z + zz = z * z + wx = w * x + wy = w * y + wz = w * z + + # Set the rotation components in the matrix + matrix[0, 0] = 1 - 2 * (yy + zz) + matrix[0, 1] = 2 * (xy - wz) + matrix[0, 2] = 2 * (xz + wy) + + matrix[1, 0] = 2 * (xy + wz) + matrix[1, 1] = 1 - 2 * (xx + zz) + matrix[1, 2] = 2 * (yz - wx) + + matrix[2, 0] = 2 * (xz - wy) + matrix[2, 1] = 2 * (yz + wx) + matrix[2, 2] = 1 - 2 * (xx + yy) + + return matrix + + +def homo_matrix2tf(matrix, parent_frame, child_frame, clock_stamp): + transform_stamped = TransformStamped() + transform_stamped.header.stamp = clock_stamp + transform_stamped.header.frame_id = parent_frame + transform_stamped.child_frame_id = child_frame + + # Extract translation from the matrix + translation = np.array([matrix[0, 3], matrix[1, 3], matrix[2, 3]]) + + # Extract rotation (quaternion) from the matrix + trace = matrix[0, 0] + matrix[1, 1] + matrix[2, 2] + if trace > 0: + s = 0.5 / np.sqrt(trace + 1.0) + w = 0.25 / s + x = (matrix[2, 1] - matrix[1, 2]) * s + y = (matrix[0, 2] - matrix[2, 0]) * s + z = (matrix[1, 0] - matrix[0, 1]) * s + elif matrix[0, 0] > matrix[1, 1] and matrix[0, 0] > matrix[2, 2]: + s = 2.0 * np.sqrt(1.0 + matrix[0, 0] - matrix[1, 1] - matrix[2, 2]) + w = (matrix[2, 1] - matrix[1, 2]) / s + x = 0.25 * s + y = (matrix[0, 1] + matrix[1, 0]) / s + z = (matrix[0, 2] + matrix[2, 0]) / s + elif matrix[1, 1] > matrix[2, 2]: + s = 2.0 * np.sqrt(1.0 + matrix[1, 1] - matrix[0, 0] - matrix[2, 2]) + w = (matrix[0, 2] - matrix[2, 0]) / s + x = (matrix[0, 1] + matrix[1, 0]) / s + y = 0.25 * s + z = (matrix[1, 2] + matrix[2, 1]) / s + else: + s = 2.0 * np.sqrt(1.0 + matrix[2, 2] - matrix[0, 0] - matrix[1, 1]) + w = (matrix[1, 0] - matrix[0, 1]) / s + x = (matrix[0, 2] + matrix[2, 0]) / s + y = (matrix[1, 2] + matrix[2, 1]) / s + z = 0.25 * s + + # Set the translation and rotation in the TransformStamped message + transform_stamped.transform.translation.x = translation[0] + transform_stamped.transform.translation.y = translation[1] + transform_stamped.transform.translation.z = translation[2] + transform_stamped.transform.rotation.x = x + transform_stamped.transform.rotation.y = y + transform_stamped.transform.rotation.z = z + transform_stamped.transform.rotation.w = w + + return transform_stamped + +def transform2pose(transform): + pose = PoseStamped() + pose.header = transform.header + pose.pose.position.x = transform.transform.translation.x + pose.pose.position.y = transform.transform.translation.y + pose.pose.position.z = transform.transform.translation.z + pose.pose.orientation = transform.transform.rotation + return pose + +def euler2quaternion(ai, aj, ak): + ai /= 2.0 + aj /= 2.0 + ak /= 2.0 + ci = math.cos(ai) + si = math.sin(ai) + cj = math.cos(aj) + sj = math.sin(aj) + ck = math.cos(ak) + sk = math.sin(ak) + cc = ci*ck + cs = ci*sk + sc = si*ck + ss = si*sk + q = np.empty((4, )) + q[0] = cj*sc - sj*cs + q[1] = cj*ss + sj*cc + q[2] = cj*cs - sj*sc + q[3] = cj*cc + sj*ss + return q + + +def quaternion2euler(x, y, z, w): + """ + Convert a quaternion into euler angles (roll, pitch, yaw) + roll is rotation around x in radians (counterclockwise) + pitch is rotation around y in radians (counterclockwise) + yaw is rotation around z in radians (counterclockwise) + """ + t0 = +2.0 * (w * x + y * z) + t1 = +1.0 - 2.0 * (x * x + y * y) + roll_x = math.atan2(t0, t1) + + t2 = +2.0 * (w * y - z * x) + t2 = +1.0 if t2 > +1.0 else t2 + t2 = -1.0 if t2 < -1.0 else t2 + pitch_y = math.asin(t2) + + t3 = +2.0 * (w * z + x * y) + t4 = +1.0 - 2.0 * (y * y + z * z) + yaw_z = math.atan2(t3, t4) + + return roll_x, pitch_y, yaw_z # in radians + +def normalize_angle(angle): + while angle > math.pi: + angle -= 2.0 * math.pi + while angle < -math.pi: + angle += 2.0 * math.pi + return angle \ No newline at end of file From 7a9bac3e1db2c61f8272a8598b701831127010ce Mon Sep 17 00:00:00 2001 From: VictorEscribano Date: Sat, 18 Nov 2023 14:05:19 +0100 Subject: [PATCH 07/13] add transformations.py --- serial_motor_demo/package.xml | 3 +++ serial_motor_demo/setup.py | 4 +++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/serial_motor_demo/package.xml b/serial_motor_demo/package.xml index 49b03ae..01fbb10 100644 --- a/serial_motor_demo/package.xml +++ b/serial_motor_demo/package.xml @@ -11,6 +11,9 @@ ament_flake8 ament_pep257 python3-pytest + + geometry_msgs + tf2_ros rclpy serial_motor_demo_msgs diff --git a/serial_motor_demo/setup.py b/serial_motor_demo/setup.py index ea70ba2..889a447 100644 --- a/serial_motor_demo/setup.py +++ b/serial_motor_demo/setup.py @@ -8,6 +8,7 @@ name=package_name, version='0.0.0', packages=[package_name], + py_modules=['resource.transformations'], data_files=[ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), @@ -26,7 +27,8 @@ 'console_scripts': [ 'gui = serial_motor_demo.gui:main', 'driver = serial_motor_demo.driver:main', - 'motor_command_node = serial_motor_demo.motor_command_node:main' + 'motor_command_node = serial_motor_demo.motor_command_node:main', + 'wheels_odometry = serial_motor_demo.wheels_odometry:main', ], }, From ae2a44db104c776c615225d9f53426e180464fc0 Mon Sep 17 00:00:00 2001 From: VictorEscribano Date: Sat, 18 Nov 2023 14:05:26 +0100 Subject: [PATCH 08/13] odometry wheel node --- .../serial_motor_demo/wheels_odometry.py | 80 ++++++++++++++++++- 1 file changed, 79 insertions(+), 1 deletion(-) diff --git a/serial_motor_demo/serial_motor_demo/wheels_odometry.py b/serial_motor_demo/serial_motor_demo/wheels_odometry.py index 503fa1d..f86bad2 100644 --- a/serial_motor_demo/serial_motor_demo/wheels_odometry.py +++ b/serial_motor_demo/serial_motor_demo/wheels_odometry.py @@ -1 +1,79 @@ -#TODO \ No newline at end of file +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Twist, Pose, Quaternion, Point, TransformStamped +from nav_msgs.msg import Odometry +from resource.transformations import euler2quaternion +from math import sin, cos +import tf2_ros + +from serial_motor_demo_msgs.msg import WheelVelocities + + +class OdometryNode(Node): + def __init__(self): + super().__init__('odometry_node') + self.subscription = self.create_subscription( + WheelVelocities, 'wheel_velocities', self.listener_callback, 10) + self.publisher = self.create_publisher(Odometry, 'odom', 10) + self.odom_broadcaster = tf2_ros.TransformBroadcaster(self) + self.last_time = self.get_clock().now() + + # Estado inicial del robot + self.x = 0.0 + self.y = 0.0 + self.theta = 0.0 + + # Especificaciones del robot + self.wheel_radius = 0.05 # Metros + self.wheel_base = 0.3 # Metros + + def listener_callback(self, msg): + current_time = self.get_clock().now() + dt = (current_time - self.last_time).nanoseconds / 1e9 + self.last_time = current_time + + # Calcular velocidades lineales de las ruedas + v_left = msg.left_wheel_velocity * self.wheel_radius + v_right = msg.right_wheel_velocity * self.wheel_radius + + # Calcular la velocidad lineal y angular del robot + V = (v_right + v_left) / 2 + omega = (v_right - v_left) / self.wheel_base + + # Actualizar la posición y orientación + self.x += V * cos(self.theta) * dt + self.y += V * sin(self.theta) * dt + self.theta += omega * dt + + # Crear y llenar el mensaje de odometría + odom = Odometry() + odom.header.stamp = current_time.to_msg() + odom.header.frame_id = "odom" + odom.child_frame_id = "base_link" + odom.pose.pose = Pose( + position=Point(x=self.x, y=self.y, z=0.0), + orientation=euler2quaternion(0, 0, self.theta)) + # Aquí puedes agregar la velocidad (Twist) si es necesario + + self.publisher.publish(odom) + + # Enviar transformación + t = TransformStamped() + t.header.stamp = current_time.to_msg() + t.header.frame_id = 'odom' + t.child_frame_id = 'base_link' + t.transform.translation.x = self.x + t.transform.translation.y = self.y + t.transform.rotation = euler2quaternion(0, 0, self.theta) + + self.odom_broadcaster.sendTransform(t) + +def main(args=None): + rclpy.init(args=args) + odometry_node = OdometryNode() + rclpy.spin(odometry_node) + odometry_node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() From 7448243539d9b35b09c34881a200afa6e679e8bc Mon Sep 17 00:00:00 2001 From: VictorEscribano Date: Sat, 18 Nov 2023 14:07:18 +0100 Subject: [PATCH 09/13] change transformations library path --- serial_motor_demo/serial_motor_demo/wheels_odometry.py | 2 +- serial_motor_demo/setup.py | 2 +- serial_motor_demo/{resource => utils}/transformations.py | 0 3 files changed, 2 insertions(+), 2 deletions(-) rename serial_motor_demo/{resource => utils}/transformations.py (100%) diff --git a/serial_motor_demo/serial_motor_demo/wheels_odometry.py b/serial_motor_demo/serial_motor_demo/wheels_odometry.py index f86bad2..bfe2c28 100644 --- a/serial_motor_demo/serial_motor_demo/wheels_odometry.py +++ b/serial_motor_demo/serial_motor_demo/wheels_odometry.py @@ -2,7 +2,7 @@ from rclpy.node import Node from geometry_msgs.msg import Twist, Pose, Quaternion, Point, TransformStamped from nav_msgs.msg import Odometry -from resource.transformations import euler2quaternion +from serial_motor_demo.serial_motor_demo.utils.transformations import euler2quaternion from math import sin, cos import tf2_ros diff --git a/serial_motor_demo/setup.py b/serial_motor_demo/setup.py index 889a447..145f353 100644 --- a/serial_motor_demo/setup.py +++ b/serial_motor_demo/setup.py @@ -8,7 +8,7 @@ name=package_name, version='0.0.0', packages=[package_name], - py_modules=['resource.transformations'], + py_modules=['serial_motor_demo.serial_motor_demo.utils.transformations'], data_files=[ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), diff --git a/serial_motor_demo/resource/transformations.py b/serial_motor_demo/utils/transformations.py similarity index 100% rename from serial_motor_demo/resource/transformations.py rename to serial_motor_demo/utils/transformations.py From f7bd0385c618175f27025eefd072eee150b4df54 Mon Sep 17 00:00:00 2001 From: VictorEscribano Date: Sat, 18 Nov 2023 14:40:44 +0100 Subject: [PATCH 10/13] setup robot parameters --- serial_motor_demo/config/robot_params.yaml | 6 ++++ .../launch/motor_driver.launch.py | 22 ++++++++++++-- .../serial_motor_demo/motor_command_node.py | 6 ++-- .../serial_motor_demo/wheels_odometry.py | 29 ++++++++++++------- 4 files changed, 49 insertions(+), 14 deletions(-) create mode 100644 serial_motor_demo/config/robot_params.yaml diff --git a/serial_motor_demo/config/robot_params.yaml b/serial_motor_demo/config/robot_params.yaml new file mode 100644 index 0000000..3faf33f --- /dev/null +++ b/serial_motor_demo/config/robot_params.yaml @@ -0,0 +1,6 @@ +# robot_params.yaml +wheel_odometry_node: + ros__parameters: + wheel_radius: 0.0553 # Meters + wheel_base: 0.284 # Meters + # Add other parameters as needed diff --git a/serial_motor_demo/launch/motor_driver.launch.py b/serial_motor_demo/launch/motor_driver.launch.py index 215b101..48585c3 100644 --- a/serial_motor_demo/launch/motor_driver.launch.py +++ b/serial_motor_demo/launch/motor_driver.launch.py @@ -4,14 +4,23 @@ from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os def generate_launch_description(): + + config = os.path.join( + get_package_share_directory('serial_motor_demo'), + 'config', + 'robot_params.yaml' + ) + return LaunchDescription([ DeclareLaunchArgument( - 'speed', default_value='0.5', + 'speed', default_value='0.3', description='Speed for teleop_twist_keyboard'), DeclareLaunchArgument( - 'turn', default_value='1.0', + 'turn', default_value='0.5', description='Turn for teleop_twist_keyboard'), DeclareLaunchArgument( 'serial_port', default_value='/dev/ttyACM0'), @@ -42,6 +51,15 @@ def generate_launch_description(): executable='motor_command_node', name='motor_command_node', output='screen', + parameters=[config], ), + + Node( + package='serial_motor_demo', + executable='wheels_odometry', + name='wheel_odometry_node', + output='screen', + parameters=[config], + ), ]) diff --git a/serial_motor_demo/serial_motor_demo/motor_command_node.py b/serial_motor_demo/serial_motor_demo/motor_command_node.py index 53920d3..313c0a9 100644 --- a/serial_motor_demo/serial_motor_demo/motor_command_node.py +++ b/serial_motor_demo/serial_motor_demo/motor_command_node.py @@ -29,8 +29,10 @@ def twist_callback(self, msg): # Convert to motor commands based on your robot's kinematics # This is a simple differential drive example - wheel_radius = 0.0553 # meters, adjust based on your robot - wheel_distance = 0.284 # meters, adjust based on your robot + # Especificaciones del robot + wheel_radius = self.declare_parameter('wheel_radius', 0.0553).value + wheel_distance = self.declare_parameter('wheel_base', 0.284).value + mot_1_speed = (speed - (rotation * wheel_distance / 2)) / wheel_radius mot_2_speed = (speed + (rotation * wheel_distance / 2)) / wheel_radius #convert to rad/s diff --git a/serial_motor_demo/serial_motor_demo/wheels_odometry.py b/serial_motor_demo/serial_motor_demo/wheels_odometry.py index bfe2c28..7f8fa37 100644 --- a/serial_motor_demo/serial_motor_demo/wheels_odometry.py +++ b/serial_motor_demo/serial_motor_demo/wheels_odometry.py @@ -2,19 +2,19 @@ from rclpy.node import Node from geometry_msgs.msg import Twist, Pose, Quaternion, Point, TransformStamped from nav_msgs.msg import Odometry -from serial_motor_demo.serial_motor_demo.utils.transformations import euler2quaternion +from utils.transformations import euler2quaternion from math import sin, cos import tf2_ros -from serial_motor_demo_msgs.msg import WheelVelocities +from serial_motor_demo_msgs.msg import MotorVels class OdometryNode(Node): def __init__(self): super().__init__('odometry_node') self.subscription = self.create_subscription( - WheelVelocities, 'wheel_velocities', self.listener_callback, 10) - self.publisher = self.create_publisher(Odometry, 'odom', 10) + MotorVels, 'motor_vels', self.listener_callback, 10) + self.publisher = self.create_publisher(Odometry, 'wheel_odom', 10) self.odom_broadcaster = tf2_ros.TransformBroadcaster(self) self.last_time = self.get_clock().now() @@ -24,17 +24,18 @@ def __init__(self): self.theta = 0.0 # Especificaciones del robot - self.wheel_radius = 0.05 # Metros - self.wheel_base = 0.3 # Metros + self.wheel_radius = self.declare_parameter('wheel_radius', 0.0553).value + self.wheel_base = self.declare_parameter('wheel_base', 0.284).value def listener_callback(self, msg): + self.get_logger().info('I heard: "%s"' % msg) current_time = self.get_clock().now() dt = (current_time - self.last_time).nanoseconds / 1e9 self.last_time = current_time # Calcular velocidades lineales de las ruedas - v_left = msg.left_wheel_velocity * self.wheel_radius - v_right = msg.right_wheel_velocity * self.wheel_radius + v_left = msg.mot_1_rad_sec * self.wheel_radius + v_right = msg.mot_2_rad_sec * self.wheel_radius # Calcular la velocidad lineal y angular del robot V = (v_right + v_left) / 2 @@ -50,9 +51,12 @@ def listener_callback(self, msg): odom.header.stamp = current_time.to_msg() odom.header.frame_id = "odom" odom.child_frame_id = "base_link" + orientation=euler2quaternion(0, 0, self.theta) odom.pose.pose = Pose( position=Point(x=self.x, y=self.y, z=0.0), - orientation=euler2quaternion(0, 0, self.theta)) + orientation=Quaternion(x=orientation[0], y=orientation[1], z=orientation[2], w=orientation[3]) + ) + # Aquí puedes agregar la velocidad (Twist) si es necesario self.publisher.publish(odom) @@ -64,7 +68,12 @@ def listener_callback(self, msg): t.child_frame_id = 'base_link' t.transform.translation.x = self.x t.transform.translation.y = self.y - t.transform.rotation = euler2quaternion(0, 0, self.theta) + quaternion = euler2quaternion(0, 0, self.theta) + # Use the quaternion for rotation + t.transform.rotation.x = quaternion[0] + t.transform.rotation.y = quaternion[1] + t.transform.rotation.z = quaternion[2] + t.transform.rotation.w = quaternion[3] self.odom_broadcaster.sendTransform(t) From 3dc645c4a393969ddc37515f053b501e9d4ed559 Mon Sep 17 00:00:00 2001 From: VictorEscribano Date: Sat, 18 Nov 2023 14:40:52 +0100 Subject: [PATCH 11/13] update setup --- serial_motor_demo/setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/serial_motor_demo/setup.py b/serial_motor_demo/setup.py index 145f353..99a78de 100644 --- a/serial_motor_demo/setup.py +++ b/serial_motor_demo/setup.py @@ -8,7 +8,7 @@ name=package_name, version='0.0.0', packages=[package_name], - py_modules=['serial_motor_demo.serial_motor_demo.utils.transformations'], + py_modules=['utils.transformations'], data_files=[ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), From aca64ff2c95f39c24bc475828cdc4509c8472bc0 Mon Sep 17 00:00:00 2001 From: VictorEscribano Date: Sat, 18 Nov 2023 14:40:56 +0100 Subject: [PATCH 12/13] init --- serial_motor_demo/utils/__init__.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 serial_motor_demo/utils/__init__.py diff --git a/serial_motor_demo/utils/__init__.py b/serial_motor_demo/utils/__init__.py new file mode 100644 index 0000000..e69de29 From 3ed03fccc56fa67d5924bd2a33df37045bbc5ffc Mon Sep 17 00:00:00 2001 From: VictorEscribano Date: Sun, 24 Mar 2024 13:53:17 +0100 Subject: [PATCH 13/13] cambios xd --- serial_motor_demo/config/robot_params.yaml | 7 ++++- .../launch/motor_driver.launch.py | 2 +- .../serial_motor_demo/motor_command_node.py | 29 ++++++++++++------- .../serial_motor_demo/wheels_odometry.py | 3 +- serial_motor_demo/setup.py | 1 + 5 files changed, 27 insertions(+), 15 deletions(-) diff --git a/serial_motor_demo/config/robot_params.yaml b/serial_motor_demo/config/robot_params.yaml index 3faf33f..e048698 100644 --- a/serial_motor_demo/config/robot_params.yaml +++ b/serial_motor_demo/config/robot_params.yaml @@ -1,6 +1,11 @@ # robot_params.yaml wheel_odometry_node: ros__parameters: - wheel_radius: 0.0553 # Meters + wheel_radius: 0.058 # Meters wheel_base: 0.284 # Meters # Add other parameters as needed + +motor_command_node: + ros__parameters: + wheel_radius: 0.058 # Meters + wheel_base: 0.284 # Meters diff --git a/serial_motor_demo/launch/motor_driver.launch.py b/serial_motor_demo/launch/motor_driver.launch.py index 48585c3..77136ec 100644 --- a/serial_motor_demo/launch/motor_driver.launch.py +++ b/serial_motor_demo/launch/motor_driver.launch.py @@ -23,7 +23,7 @@ def generate_launch_description(): 'turn', default_value='0.5', description='Turn for teleop_twist_keyboard'), DeclareLaunchArgument( - 'serial_port', default_value='/dev/ttyACM0'), + 'serial_port', default_value='/dev/ttyUSB0'), Node( diff --git a/serial_motor_demo/serial_motor_demo/motor_command_node.py b/serial_motor_demo/serial_motor_demo/motor_command_node.py index 313c0a9..b0759fa 100644 --- a/serial_motor_demo/serial_motor_demo/motor_command_node.py +++ b/serial_motor_demo/serial_motor_demo/motor_command_node.py @@ -18,6 +18,15 @@ def __init__(self): # Create a publisher to send out MotorCommand messages self.publisher = self.create_publisher(MotorCommand, 'motor_command', 10) + + # Especificaciones del robot + self.declare_parameter('wheel_radius', 0.0553, ignore_override=True) + self.declare_parameter('wheel_base', 0.284, ignore_override=True) + + # Retrieve parameter values + self.wheel_radius = self.get_parameter('wheel_radius').value + self.wheel_base = self.get_parameter('wheel_base').value + self.get_logger().info('Motor Command Node Started') @@ -27,17 +36,14 @@ def twist_callback(self, msg): speed = msg.linear.x rotation = msg.angular.z - # Convert to motor commands based on your robot's kinematics - # This is a simple differential drive example - # Especificaciones del robot - wheel_radius = self.declare_parameter('wheel_radius', 0.0553).value - wheel_distance = self.declare_parameter('wheel_base', 0.284).value - - mot_1_speed = (speed - (rotation * wheel_distance / 2)) / wheel_radius - mot_2_speed = (speed + (rotation * wheel_distance / 2)) / wheel_radius - #convert to rad/s - mot_1_speed = mot_1_speed / (2*math.pi) - mot_2_speed = mot_2_speed / (2*math.pi) + + + mot_1_speed = (speed + (rotation * self.wheel_base / 2)) / self.wheel_radius + mot_2_speed = (speed - (rotation * self.wheel_base / 2)) / self.wheel_radius + + # Convert to rad/s + mot_1_speed = mot_1_speed / (2 * math.pi) + mot_2_speed = mot_2_speed / (2 * math.pi) # Create and publish the motor command motor_msg = MotorCommand() @@ -52,6 +58,7 @@ def twist_callback(self, msg): self.get_logger().error(f'Error in twist_callback: {str(e)}') + def main(args=None): rclpy.init(args=args) node = MotorCommandNode() diff --git a/serial_motor_demo/serial_motor_demo/wheels_odometry.py b/serial_motor_demo/serial_motor_demo/wheels_odometry.py index 7f8fa37..cf49c47 100644 --- a/serial_motor_demo/serial_motor_demo/wheels_odometry.py +++ b/serial_motor_demo/serial_motor_demo/wheels_odometry.py @@ -11,7 +11,7 @@ class OdometryNode(Node): def __init__(self): - super().__init__('odometry_node') + super().__init__('wheel_odometry_node') self.subscription = self.create_subscription( MotorVels, 'motor_vels', self.listener_callback, 10) self.publisher = self.create_publisher(Odometry, 'wheel_odom', 10) @@ -28,7 +28,6 @@ def __init__(self): self.wheel_base = self.declare_parameter('wheel_base', 0.284).value def listener_callback(self, msg): - self.get_logger().info('I heard: "%s"' % msg) current_time = self.get_clock().now() dt = (current_time - self.last_time).nanoseconds / 1e9 self.last_time = current_time diff --git a/serial_motor_demo/setup.py b/serial_motor_demo/setup.py index 99a78de..5691f03 100644 --- a/serial_motor_demo/setup.py +++ b/serial_motor_demo/setup.py @@ -15,6 +15,7 @@ ('share/' + package_name, ['package.xml']), # Include launch files (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), ], install_requires=['setuptools'], zip_safe=True,