From db35c7a2825338518c196aacda948e67ad9f97c7 Mon Sep 17 00:00:00 2001 From: havoc420ubuntu <2993167370@qq.com> Date: Sun, 11 May 2025 15:44:54 +0000 Subject: [PATCH] :tada: --- .../robot_control_cmd_lcmt.cpython-38.pyc | Bin 0 -> 6600 bytes ...robot_control_response_lcmt.cpython-38.pyc | Bin 0 -> 3119 bytes .../robot_control_cmd_lcmt.cpython-38.pyc | Bin 0 -> 6622 bytes ...robot_control_response_lcmt.cpython-38.pyc | Bin 0 -> 3141 bytes base_move/go_straight/main.py | 110 ++++++++++++ .../go_straight/robot_control_cmd_lcmt.py | 149 ++++++++++++++++ .../robot_control_response_lcmt.py | 73 ++++++++ .../robot_control_cmd_lcmt.cpython-38.pyc | Bin 0 -> 6615 bytes ...robot_control_response_lcmt.cpython-38.pyc | Bin 0 -> 3134 bytes base_move/move/main.py | 161 ++++++++++++++++++ base_move/move/robot_control_cmd_lcmt.py | 149 ++++++++++++++++ base_move/move/robot_control_response_lcmt.py | 73 ++++++++ main.py | 113 ++++++++++++ robot_control_cmd_lcmt.py | 149 ++++++++++++++++ robot_control_response_lcmt.py | 73 ++++++++ task_1/__pycache__/task_1.cpython-38.pyc | Bin 0 -> 607 bytes task_1/task_1.py | 39 +++++ utils/image_raw.py | 72 ++++++++ 18 files changed, 1161 insertions(+) create mode 100644 __pycache__/robot_control_cmd_lcmt.cpython-38.pyc create mode 100644 __pycache__/robot_control_response_lcmt.cpython-38.pyc create mode 100644 base_move/go_straight/__pycache__/robot_control_cmd_lcmt.cpython-38.pyc create mode 100644 base_move/go_straight/__pycache__/robot_control_response_lcmt.cpython-38.pyc create mode 100644 base_move/go_straight/main.py create mode 100644 base_move/go_straight/robot_control_cmd_lcmt.py create mode 100644 base_move/go_straight/robot_control_response_lcmt.py create mode 100644 base_move/move/__pycache__/robot_control_cmd_lcmt.cpython-38.pyc create mode 100644 base_move/move/__pycache__/robot_control_response_lcmt.cpython-38.pyc create mode 100644 base_move/move/main.py create mode 100644 base_move/move/robot_control_cmd_lcmt.py create mode 100644 base_move/move/robot_control_response_lcmt.py create mode 100644 main.py create mode 100644 robot_control_cmd_lcmt.py create mode 100644 robot_control_response_lcmt.py create mode 100644 task_1/__pycache__/task_1.cpython-38.pyc create mode 100644 task_1/task_1.py create mode 100644 utils/image_raw.py diff --git a/__pycache__/robot_control_cmd_lcmt.cpython-38.pyc b/__pycache__/robot_control_cmd_lcmt.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..257800f5780fee8dbacbeb013bb38f47e2962a94 GIT binary patch literal 6600 zcmcIo&2t+`74II6W=5Zu6UX^>v`ID_Es665sQ|&7O|qL!)#4DsF05D>Wc65*Eornp z?QBKmg({p}whF2!?v!=(g%ke+cTUuBXWpD8bR_^3>X z!6$>k9kk%D03IuH!HS09!l3PGOJNH~Bt%lAL|SAjnPN<2yYGw`d~+g)SyRk*-$NBH zdhKwxPrq|$paE@M91-)+SeXH01)(il&!SGt528+~T-KdrS;@NF z@@t?^toryANW3r=D6i#Ky;`NJ!uZoteZvbKu^~&U<~KtN)VEwrtk(S!zJuY7>#kbf zjX@Z@76AjG%mI22^=bMZTJR?T!8e&3L9Y7{A22Zz;d7=;VQ%sJY>RMl2y_%@LByjt z8$yq8&i-0bf>AnBi%~jKln4jWbibR@CrWFW5ex#!EK6c!h%;i9ID3K?p^|QlvjL=xT3Vo|JuF7LTYOq(hQuQ0&6)3y9qDsN~mC15nY&{E; zx9hb)mHkF*X%6cH6To5yYu}i%#&v5=sENCHyY81u^b`S)@U?jR%zv?hvHPaOYp-+x9mTfTSA3Iib7gGDI;_Jvta;nqjm_i2ar%&zf?*W*H9K^_%dcAkrRn$B<`eS!`uv zjAbiYuz>l1u8TVRVRzO5bYT5OZ8nP_n|s}t|C zhxs)_B!IIKvSTld#|~y`{Y)oDoN8yDTi%N=t>D>(%yS}RR8@*bsz~#HR z%CVns3?aLtON7kHv-k9UR6bCg7492aCj7tGhNefi69PE~9a_Uh8z% zW;G7hjQuq3b2`loT5gjP?c}m7fU{i_Kfsg`7PkSjTTU_AwPI_$mP%R6MVXg_myT$xce$jOp=PP zTN1h66VxuG+9I}uD&-EGrij1Cj_N+)acx$V1=fk-f6cp>zqWt+U6bunX7~@n{rgwZ zJHvM0!w^n`z6GtdTP5i=RZz55qXo^cj_?{D))9neT>1XqZ46}bD3p_q%&8kw-Myk{&uF2tiK zcX&dClcW-oY)+EB46vouNGFEpF!2a2paYOq&hhr*L41q{(jghrb<=Ir2LnoDaduP^ z-M9_Mbetk}tiABSQ+ZYO@*m&(exB|o@2~1{!O6)s8c`fx?gOR>fM=y>jrgS1+H7nxUPEE z@@RsFS=U9`;57pjVHnFio$Mk35~aKa5T*l#BCXu;RMi*Sb|Q&)9ec< zh6!@gL7qB%!B{XZ8P1GjIVs0+3@42icerUdreo*}DuZz7q2+hbfV@#+ur^9BXyKwgU2%_u?r_$Z!jo=G`eiQ&+?A3HGr^ZN zRj#@ufsG(c__F5eI2UH9vZmTQ8m1b)@?|uTv$&}umC)C2x=J3#Xjy&-a7@=pJonIo zy8yZ-om4$jy=>~)xQ14@p0)?-X(VspH~zoWvhZbOcai=~EB~r=N(u?OEWLHWtuFR5 z`-!zFT|*%;a`W;B^!>*Ken^1IZh4x(j|fb#R&6K0z@)Cu`=vuvLbT7n%=w0-LH17U zX=z+Q=jEyf`4&jKb%Rb0CAq`MD768*ADpfa1G8 z_A)xt>j7lS$RyS%Yff~KIiLvQ+fi*mH3MbvL$yIyq4pe|Pp_<0_speh*K1mlp)#N1 zV}(@2b3anh6%JI5ljN#JI_XB}zFKHn%^1JL+wUAG#(w=ZCdqxf`hQbhd6n3-SPf90dYP`U2P0$}SyMXuf#0C07o~+H*UL8RvC5BUF{5{%nVi7yfzIWO9klObMR60{}JGl762MBb<;RUZ9PpaU=io4O{Sp zRIrRah8_zeEoHKYm&xVieGnp((?|Uf{g=($egoRU%x&b)#tBD%9qF5-s!+# zPyZ3fe@`bR^JB)rzP*>1$%mjF5SbR9Hc9%}XK3AwXwH~3<_#x_jL6g(QG6&PBI-h- zc&PkD;a{K1!$*s~PU!S{(Vs@ChmDpmRaDs;HD3!MwEUGd5C6x<=s23sz8dYHDRgc} VdE*kXsC|XObNFuv^{tV6{2%WWO56Yd literal 0 HcmV?d00001 diff --git a/__pycache__/robot_control_response_lcmt.cpython-38.pyc b/__pycache__/robot_control_response_lcmt.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..2c7702333d072d483fde6dc3d42c251fdda04d17 GIT binary patch literal 3119 zcma)8OK%iM5bo}I?Cir2u!AuKGCTroL~Jl9;$Rst4#X0yl|UX|C2EbQ+q+}$?kqjM zAg>lhipe!n{y^kCa>?Jwf0!I|@*#h~m!zu4Z(D{Lbxl?EW4fxos_t(G1_}h8htjW4 z7YX?Xh5e5Q!UJf@H$ViblJ2mef;DK9RV_-sCoSJ5(q1j7yOiJyy;j3%TG+ctTI>hZ zu!dW8pAcaS=PePA%&l8hPq62t>~WswYKzyT#j0ij^b8Qcu|iMe>rMMKj`SV z8R#es8jUr-CRA>5RZHaey#-MOoh?cS-hmngts@4- z&|6Y<#jrR9-<&utM&Rp-QSlLc^RgiG>$I8|XT%uDMUaai7sXjI4sr?P63DQ2bwHdG z=Rr3JxqCJlu5#u0+kLVuOl7Sd7=Z|Nz#0*A}*DRp=UXcbvsVB#bPtovDy~g zsHN|5o#x`%@TMsr28C=yanwMv}6{DpgqDfN(~QQ2aRQ@ZKz|Yi?Mi1 zbWYgobdId!C{GZu->E2E_P~D_DDXeep(VpWYNSVg^4>sPe`hakm_^N`ccjcxHgPMp zMbx~daHJty6O;A0DJKCfdQu0;>f{M7o@lS7h3Q6==rC@!XU^k4Z6GdTqIl}Ca}n>s3J)ue*7CGTizC$h1;XNnrZ zA>PifP%<7N)IIQauYSl*k9ZlkXQFux7){p68``5ARI?4MPx_RC-#6?YH5}b=`WP`< zDZ2_olsd0fq@}`8rDMD%HE##usuUaw09e#kfT^^|Wh(>_^SC8b4+gyo8XftLsWF^p zEp?7q>t+)!K!1Y1LR$xEfp#ySWMeid#EtDd$Rmi2g%x1HHlpco01clZgf`LM8UV&!tN$5@5y?&BM_BsAY;4x;#Ez`hg2(zH;Obzz{ z04+~LdahFg1twUZYHP#plT}MiX-8P=^cU2)2n&FNT}f>v1F?yr>Xzmt2HQ7>Im#ZiJhXxt4QPVV zHjV~ev4^gK+UCQW>Qf{LiOpL;%B9Q&mw`{+PRqc;cm}iax`((`Us0Z8Ktgbh( z_WgC^@6Ww!|IXTE6KlQE{r7YB4w23Kpo5Bz|ANTp+JTZSouoP4Yy*1ghfGXIOr^md zI4i@BN}^Zt9?VV56^MGorZzoGyC)X?W7Y^^jROx>Yy%7ojwUN`{H%KFA{IwMH5A4g zl!89fgoCt#MzhKmoB6uzWF>Lrm~yBv>goy-4AVZ_49u~M8{(+j6tKofB|r3Z&!Lq^Kq>0R`=o)seN6SKHlfh$$q?Sp*X>|t0Ln(_@pW|AOp))ZRnw@-L?Mx^Sb)mhx$CrGfEs?UI=CA=|3VN>dQEKCMvvAKgI zuu!SHNcN1{3zY>OScDQEh(|}+C_BTvg6Dd+=XuO?OrtgfnPmvir#aYD9CBVg9X{7^ zM%LyRCU%(A+u^b!wm9Ex$4X~4l0~ucdFsYXD>BrX+mG2->P=^!n!&J}GHE-E4#P1D HWs?05iU;1R literal 0 HcmV?d00001 diff --git a/base_move/go_straight/__pycache__/robot_control_cmd_lcmt.cpython-38.pyc b/base_move/go_straight/__pycache__/robot_control_cmd_lcmt.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..7677d1f9d312e629353a39d93e7bfc6da2c5e125 GIT binary patch literal 6622 zcmcIo&2t+`74II6W=5Zu6UX^>yh%12Es32ENCgN^NV1zv)#4DsF05D>Wc65*Eornp z?QBKmg({rXR#6326!$Ib=*yn?AGmX(h9f6e!I6tE@OwRy^=%SJW~!gMU%&3@*Y8^& zWim;I=h2sEDld*R_IDceKMn?W(1Kq8v~q$8USoA$FgW`OYs{O>i>)W+70&3ztWh)T z2F-rL8pclv!)x(E`~ed&VLoTV^sKE|!Nxc)EQ~EZb_9RI^7byi!ItwpwC_GsUhwcy zi4gry27^0j!CwJ9R^Wma48es#+tZf97LG`Wq)3Uh$dogMn89Rz+wNL9wL5WUadDfqbK(Li?##bA=a#wroC&I!!-_I>k~+cakL~ zYi`r8f3e9wp8y2kWNrkx?n8XQ#7KnCnKFgB#qYB%!o?xbVVngK z594eIJ;FKrYe@-)=}0Yx=}1u`97NOkZc3jhtzkwm2qd#CiID-$h*je330jZ?D6>uW zbLR`F}pXJgZ0IgV&J)T z|EafF_T50qB8g&gyf!Q}pM}X=wQ8VBe!aOggIxm?z+wh#U7xVVb*l|1kGpWI=9h}K zU4wANbH74cJE-w=9BjPIJ?<5UR0USZwKi6yDefEv+PTcLET@oj+odHsY{ZG91 z-AO2KZub-YyGs&9J}XHDWt;%H1Ze|=aanAXJxS3-CJ0c}ktqUc0_4|amcSGNGN&CL znt@ka?OBT?l?t7%1`}mKCt%#_yfeK2wRr2yf3bp*`=-NdUv&W;#kSZNe3Nf;Wo*aV ztj#%WeB0dQy2rQUZJ7TKU(Q>S+(8)sTvnAQ$t{FO-LeO_S}d)5!dnL}%(g)Tf)*%w^%t zz!p<{Tf5)2*d|*u(c)-UJKkXr^J|7k0B0kR$6gr6*;mHzqifKsF`LA!GjTSB*~G-z zG-i{SjgQ?qgW1%?wOP!jC$62sEL^v+M|TFt4rXcnR69nTY8Rhd-it4-;Mt2UU0>Cn z5WO3#+UU`{wyO0Ty=pbj<-54bk)Ll2AUmT=gv`ma_z5j|^bLe-lhk6Ip}dlXNYc?E z^N{C(qZS+jV0@m>4ZN+h6I7;8DMyTg^jYi8Ep!x2K=3U?<~Cs*4M!nVZ*grVhA`S( z#kS2hTaU?W3gLT;|AsIYnG8(DTnzB83SFy^>4_k<}DMRg~`#{eEbToP`z9En+GlN5fz>mf`S2+?i}W z(VCH4AaAB`;VU3v^%M&oR~*6FOxY8p$z_=bXS*hV*6&^Inn{g! z^|g{F%WDKK1LUV8;VhsZS{n_m(pqMEgTVmsN_q`fsAWn1a7du?TfluAEudgSe(>yo z%b(#V_+%CDDb0v*_f?#jBo$w`Br?D!sAx#FMQjPR%N;mP5x>oj>OSFdZB~>U)`;PM z&AOL=Yya%KCflWa@gIcy_phUOitXOU5Ke=>1+BH4Md>wEP_R|K3C*tz@fsf15rk%3 z>6JEQP<`q>z}i5UY}{ccH~BH%dUujZQve!VUc&N$XJ4twXDGIY>~-Cda}@6cSBQ0G zxcj!Dn2XUGnXs|EXDmI=$D=5Bcua(oq!N;BPLjR!v8B~WCx+)R@dz!T1CUkD@YcdX ze2fOt0U6SD({0lS14?6YcGMT0xDCg2oFa9sHTS?%^Qz*_e|YZ)^K>`!{;D2Fo>{(! zr8?Kon-Lr2@_JrKQVi~6D9T8>_xuqi108gYiJ3>2b|*)fOru!$TsKU*$oa)ajrwWV z{cNLH>&zrvH>mk4aNR=2b=9+`M-w#6x-N?$|%u|QY8FR)Z!x4mSj!1YS-Q{ptnQz8wko(oS7YNPamCNA33b@))| z4yS!7Jn1&2U-E*$T`9^i6MSA(r3z}eqS^?;gfFYEj&os#>TRmXqhYGQ`v{TE9Rk!@+WX83lWj$*e=jlHH`5)<|WPZ#z*td7{GWig+eInDs(+_o|zu1 zYl1v7S_ud3+e-X_mHn8P{TurqdLMc6!~TK1EZ?cNV>^xv-Kx{)oVs+KbG~z`zM7e7 zFg&aOc%{G8Wb79bjz1BEJ7~ohAc}R_bWU+427D;G9%p}Lqu6KK-)xxoIHMQ6-XIuy z)cb&q#5bf7gRmQZ%#^Q!M@$8}w&QgpCGN9!IDPQxho8lEYpi3X*RxS(vwT!EzU*g3 zyq*nooJ?#!Ol+1WgTYqZ(<5yXt5v+V6%W$kLStn$?yPA}EEKd~<d#U4?EIx>@LE)gRQ$kk6?WJa?_UYVx(*$}}HYlMkfP#Tc&B5^k}jKg^Xb zYrQ11GE-%ZMkT2&U&xKpMvhIM>Y|Wqi797_?=zeBWnmLLDav|oGO3Np(X*cCcAOUn zVsn_=+#CpDHnO*+Eo=DR`11jC>!!4w{T95L@?(xB-b4Ekt+)%K_%@Rj<&KBfq46B` z9Ss}}DHe~Itto$pudp2&6)6V$gN~tPPy9zfLI1grR?LC)*f#rH^aJAhv-r-(Ebb;f zW^GZ5h3loSvf*_@BOQ6Nve?gudJ$-`i#91X7uOOvbeKQXi@h8UGD+6!+r={+zcAh^ z8%u+%uxUOVFJGmd_#hz@YxNb`0g;eMjR=9* z)QM0kmw`z}J#CQ8>@hF>LJ!uD3^W&LNObrslANCy4^O<)H-67fPx+p<=VJUO7|V9p z4}6>Naw~SdJ+{X=qOt35bI0-BV2|R+JMGX=;L6~8lUZ#jfaN9G(^ig?bW|f%a&Myau zeiALOxu^fcU%qiOtHRFBtN2;FX|CW?hCty2iR_beeg_~N5)4`Y3=9lm^K~?bPsZQj z)3?s@$T7#C3OvQkxn&8AMVJ-e5oT^102oCUNxQ=x6oe9c+I#K%+s%dLQQ?O%Kj@q@d)=!GWTH^p?-oOwC7xNOhvhg)R;V-UoHKlSh z2`0#OUnMp%*J%9HWUq1|tj4{Cu7ZNgfQ(Q?{5+3-10mJKQ?8(WJ6C)PrpkdsjO==3 z{+{@@w#_B-Kn7?qT#_82)zKPmG|+_6K8=R1xlOJ?#f!0~`6Cg6#Qt@VcB^v1Yv9Xp zGIFqRo}p|!yI44Ev_<6@N}y-FW8Q}D2HMlaS>W@(1(PGkpYqL9n7o5E2_~DNX(`yI z1i?dj%eW)lL0+lgeRj$5q=)TcRqY2<`~LIxf8UMX{NG)l?b8Y1bpQW*^`2w>eH(KNcQN7-^SRG2^XZvI|2b=< zSmVS)725%Wf@9eR4yDb=T%+QsxJ6;?p%m;1%Os=?GFo(=h~>}QK~)kjJf|EQin@87 z2!-jGZ4TyC-(7Cp0aMTh2;PX`tuqt-`K9+nG|BN43un+g7#M(){Gie!&96{x_omlA z);6|%9n&q3oE+!tPOhq$xrwRnNN)R;Erg7toHiFpd?sb7>PuSdBXkz7LW{ExtiuWI zIXe4B$xusKHY5&qH5m}!lJdJrGC1g|OIZwZTcBe`O1hg;2Z^&%;>Msy1zl35G?!_6 zuMnYPYW@ULw&)&}r9*A|xpEm)#pV``ph9KdBXVTaQK&q0P!TF(kciKVd2vxhjVO%# zD2hZBxW;_}SyTwGmo@AuPo?ahPw!itk-dAX3x`Y@9dgwX+q^d%=f+kwQe?UFc^T$w f8#=X>+n=+qGFqxUwM=1mWzr7>pTjYVGAVuq_bBI? literal 0 HcmV?d00001 diff --git a/base_move/go_straight/main.py b/base_move/go_straight/main.py new file mode 100644 index 0000000..980221d --- /dev/null +++ b/base_move/go_straight/main.py @@ -0,0 +1,110 @@ +''' +This demo show the communication interface of MR813 motion control board based on Lcm. +Dependency: +- robot_control_cmd_lcmt.py +- robot_control_response_lcmt.py +''' +import lcm +import sys +import os +import time +from threading import Thread, Lock + +from robot_control_cmd_lcmt import robot_control_cmd_lcmt +from robot_control_response_lcmt import robot_control_response_lcmt + +def main(): + Ctrl = Robot_Ctrl() + Ctrl.run() + msg = robot_control_cmd_lcmt() + try: + print("Recovery stand") + + msg.mode = 12 # Recovery stand + msg.gait_id = 0 + msg.life_count += 1 # Command will take effect when life_count update + Ctrl.Send_cmd(msg) + Ctrl.Wait_finish(12, 0) + + print("Go forward") + + msg.mode = 11 + msg.gait_id = 26 # 26 表示快速 trot 步态 + msg.vel_des = [1.0, 1.0, -1.0] + msg.duration = 2000 + msg.step_height = [0.06, 0.06] + msg.life_count += 1 + Ctrl.Send_cmd(msg) + Ctrl.Wait_finish(11, 26) + + + except KeyboardInterrupt: + pass + Ctrl.quit() + sys.exit() + + +class Robot_Ctrl(object): + def __init__(self): + self.rec_thread = Thread(target=self.rec_responce) + self.send_thread = Thread(target=self.send_publish) + self.lc_r = lcm.LCM("udpm://239.255.76.67:7670?ttl=255") + self.lc_s = lcm.LCM("udpm://239.255.76.67:7671?ttl=255") + self.cmd_msg = robot_control_cmd_lcmt() + self.rec_msg = robot_control_response_lcmt() + self.send_lock = Lock() + self.delay_cnt = 0 + self.mode_ok = 0 + self.gait_ok = 0 + self.runing = 1 + + def run(self): + self.lc_r.subscribe("robot_control_response", self.msg_handler) + self.send_thread.start() + self.rec_thread.start() + + def msg_handler(self, channel, data): + self.rec_msg = robot_control_response_lcmt().decode(data) + if(self.rec_msg.order_process_bar >= 95): + self.mode_ok = self.rec_msg.mode + else: + self.mode_ok = 0 + + def rec_responce(self): + while self.runing: + self.lc_r.handle() + time.sleep( 0.002 ) + + def Wait_finish(self, mode, gait_id): + count = 0 + while self.runing and count < 2000: #10s + if self.mode_ok == mode and self.gait_ok == gait_id: + return True + else: + time.sleep(0.005) + count += 1 + + def send_publish(self): + while self.runing: + self.send_lock.acquire() + if self.delay_cnt > 20: # Heartbeat signal 10HZ, It is used to maintain the heartbeat when life count is not updated + self.lc_s.publish("robot_control_cmd",self.cmd_msg.encode()) + self.delay_cnt = 0 + self.delay_cnt += 1 + self.send_lock.release() + time.sleep( 0.005 ) + + def Send_cmd(self, msg): + self.send_lock.acquire() + self.delay_cnt = 50 + self.cmd_msg = msg + self.send_lock.release() + + def quit(self): + self.runing = 0 + self.rec_thread.join() + self.send_thread.join() + +# Main function +if __name__ == '__main__': + main() diff --git a/base_move/go_straight/robot_control_cmd_lcmt.py b/base_move/go_straight/robot_control_cmd_lcmt.py new file mode 100644 index 0000000..addedac --- /dev/null +++ b/base_move/go_straight/robot_control_cmd_lcmt.py @@ -0,0 +1,149 @@ +# LCM type definitions This file automatically generated by lcm. +try: + import cStringIO.StringIO as BytesIO +except ImportError: + from io import BytesIO +import struct + +class robot_control_cmd_lcmt(object): + __slots__ = ["mode", "gait_id", "contact", "life_count", "vel_des", "rpy_des", "pos_des", "acc_des", "ctrl_point", "foot_pose", "step_height", "value", "duration"] + + __typenames__ = ["int8_t", "int8_t", "int8_t", "int8_t", "float", "float", "float", "float", "float", "float", "float", "int32_t", "int32_t"] + + __dimensions__ = [None, None, None, None, [3], [3], [3], [6], [3], [6], [2], None, None] + + def __init__(self): + self.mode = 0 + self.gait_id = 0 + self.contact = 0 + self.life_count = 0 + self.vel_des = [ 0.0 for dim0 in range(3) ] + self.rpy_des = [ 0.0 for dim0 in range(3) ] + self.pos_des = [ 0.0 for dim0 in range(3) ] + self.acc_des = [ 0.0 for dim0 in range(6) ] + self.ctrl_point = [ 0.0 for dim0 in range(3) ] + self.foot_pose = [ 0.0 for dim0 in range(6) ] + self.step_height = [ 0.0 for dim0 in range(2) ] + self.value = 0 + self.duration = 0 + + def encode(self): + buf = BytesIO() + buf.write(robot_control_cmd_lcmt._get_packed_fingerprint()) + self._encode_one(buf) + return buf.getvalue() + + def _encode_one(self, buf): + buf.write(struct.pack(">bbbb", self.mode, self.gait_id, self.contact, self.life_count)) + buf.write(struct.pack('>3f', *self.vel_des[:3])) + buf.write(struct.pack('>3f', *self.rpy_des[:3])) + buf.write(struct.pack('>3f', *self.pos_des[:3])) + buf.write(struct.pack('>6f', *self.acc_des[:6])) + buf.write(struct.pack('>3f', *self.ctrl_point[:3])) + buf.write(struct.pack('>6f', *self.foot_pose[:6])) + buf.write(struct.pack('>2f', *self.step_height[:2])) + buf.write(struct.pack(">ii", self.value, self.duration)) + + def decode(data): + if hasattr(data, 'read'): + buf = data + else: + buf = BytesIO(data) + if buf.read(8) != robot_control_cmd_lcmt._get_packed_fingerprint(): + raise ValueError("Decode error") + return robot_control_cmd_lcmt._decode_one(buf) + decode = staticmethod(decode) + + def _decode_one(buf): + self = robot_control_cmd_lcmt() + self.mode, self.gait_id, self.contact, self.life_count = struct.unpack(">bbbb", buf.read(4)) + self.vel_des = struct.unpack('>3f', buf.read(12)) + self.rpy_des = struct.unpack('>3f', buf.read(12)) + self.pos_des = struct.unpack('>3f', buf.read(12)) + self.acc_des = struct.unpack('>6f', buf.read(24)) + self.ctrl_point = struct.unpack('>3f', buf.read(12)) + self.foot_pose = struct.unpack('>6f', buf.read(24)) + self.step_height = struct.unpack('>2f', buf.read(8)) + self.value, self.duration = struct.unpack(">ii", buf.read(8)) + return self + _decode_one = staticmethod(_decode_one) + + def _get_hash_recursive(parents): + if robot_control_cmd_lcmt in parents: return 0 + tmphash = (0x476b61e296af96f5) & 0xffffffffffffffff + tmphash = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff + return tmphash + _get_hash_recursive = staticmethod(_get_hash_recursive) + _packed_fingerprint = None + + def _get_packed_fingerprint(): + if robot_control_cmd_lcmt._packed_fingerprint is None: + robot_control_cmd_lcmt._packed_fingerprint = struct.pack(">Q", robot_control_cmd_lcmt._get_hash_recursive([])) + return robot_control_cmd_lcmt._packed_fingerprint + _get_packed_fingerprint = staticmethod(_get_packed_fingerprint) + + def get_hash(self): + """Get the LCM hash of the struct""" + return struct.unpack(">Q", robot_control_cmd_lcmt._get_packed_fingerprint())[0] + +class robot_control_response_lcmt(object): + __slots__ = ["mode", "gait_id", "contact", "order_process_bar", "switch_status", "ori_error", "footpos_error", "motor_error"] + + __typenames__ = ["int8_t", "int8_t", "int8_t", "int8_t", "int8_t", "int8_t", "int16_t", "int32_t"] + + __dimensions__ = [None, None, None, None, None, None, None, [12]] + + def __init__(self): + self.mode = 0 + self.gait_id = 0 + self.contact = 0 + self.order_process_bar = 0 + self.switch_status = 0 + self.ori_error = 0 + self.footpos_error = 0 + self.motor_error = [ 0 for dim0 in range(12) ] + + def encode(self): + buf = BytesIO() + buf.write(robot_control_response_lcmt._get_packed_fingerprint()) + self._encode_one(buf) + return buf.getvalue() + + def _encode_one(self, buf): + buf.write(struct.pack(">bbbbbbh", self.mode, self.gait_id, self.contact, self.order_process_bar, self.switch_status, self.ori_error, self.footpos_error)) + buf.write(struct.pack('>12i', *self.motor_error[:12])) + + def decode(data): + if hasattr(data, 'read'): + buf = data + else: + buf = BytesIO(data) + if buf.read(8) != robot_control_response_lcmt._get_packed_fingerprint(): + raise ValueError("Decode error") + return robot_control_response_lcmt._decode_one(buf) + decode = staticmethod(decode) + + def _decode_one(buf): + self = robot_control_response_lcmt() + self.mode, self.gait_id, self.contact, self.order_process_bar, self.switch_status, self.ori_error, self.footpos_error = struct.unpack(">bbbbbbh", buf.read(8)) + self.motor_error = struct.unpack('>12i', buf.read(48)) + return self + _decode_one = staticmethod(_decode_one) + + def _get_hash_recursive(parents): + if robot_control_response_lcmt in parents: return 0 + tmphash = (0x485da98216eda8c7) & 0xffffffffffffffff + tmphash = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff + return tmphash + _get_hash_recursive = staticmethod(_get_hash_recursive) + _packed_fingerprint = None + + def _get_packed_fingerprint(): + if robot_control_response_lcmt._packed_fingerprint is None: + robot_control_response_lcmt._packed_fingerprint = struct.pack(">Q", robot_control_response_lcmt._get_hash_recursive([])) + return robot_control_response_lcmt._packed_fingerprint + _get_packed_fingerprint = staticmethod(_get_packed_fingerprint) + + def get_hash(self): + """Get the LCM hash of the struct""" + return struct.unpack(">Q", robot_control_response_lcmt._get_packed_fingerprint())[0] diff --git a/base_move/go_straight/robot_control_response_lcmt.py b/base_move/go_straight/robot_control_response_lcmt.py new file mode 100644 index 0000000..cc61b8c --- /dev/null +++ b/base_move/go_straight/robot_control_response_lcmt.py @@ -0,0 +1,73 @@ +"""LCM type definitions +This file automatically generated by lcm. +DO NOT MODIFY BY HAND!!!! +""" + +try: + import cStringIO.StringIO as BytesIO +except ImportError: + from io import BytesIO +import struct + +class robot_control_response_lcmt(object): + __slots__ = ["mode", "gait_id", "contact", "order_process_bar", "switch_status", "ori_error", "footpos_error", "motor_error"] + + __typenames__ = ["int8_t", "int8_t", "int8_t", "int8_t", "int8_t", "int8_t", "int16_t", "int32_t"] + + __dimensions__ = [None, None, None, None, None, None, None, [12]] + + def __init__(self): + self.mode = 0 + self.gait_id = 0 + self.contact = 0 + self.order_process_bar = 0 + self.switch_status = 0 + self.ori_error = 0 + self.footpos_error = 0 + self.motor_error = [ 0 for dim0 in range(12) ] + + def encode(self): + buf = BytesIO() + buf.write(robot_control_response_lcmt._get_packed_fingerprint()) + self._encode_one(buf) + return buf.getvalue() + + def _encode_one(self, buf): + buf.write(struct.pack(">bbbbbbh", self.mode, self.gait_id, self.contact, self.order_process_bar, self.switch_status, self.ori_error, self.footpos_error)) + buf.write(struct.pack('>12i', *self.motor_error[:12])) + + def decode(data): + if hasattr(data, 'read'): + buf = data + else: + buf = BytesIO(data) + if buf.read(8) != robot_control_response_lcmt._get_packed_fingerprint(): + raise ValueError("Decode error") + return robot_control_response_lcmt._decode_one(buf) + decode = staticmethod(decode) + + def _decode_one(buf): + self = robot_control_response_lcmt() + self.mode, self.gait_id, self.contact, self.order_process_bar, self.switch_status, self.ori_error, self.footpos_error = struct.unpack(">bbbbbbh", buf.read(8)) + self.motor_error = struct.unpack('>12i', buf.read(48)) + return self + _decode_one = staticmethod(_decode_one) + + def _get_hash_recursive(parents): + if robot_control_response_lcmt in parents: return 0 + tmphash = (0x485da98216eda8c7) & 0xffffffffffffffff + tmphash = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff + return tmphash + _get_hash_recursive = staticmethod(_get_hash_recursive) + _packed_fingerprint = None + + def _get_packed_fingerprint(): + if robot_control_response_lcmt._packed_fingerprint is None: + robot_control_response_lcmt._packed_fingerprint = struct.pack(">Q", robot_control_response_lcmt._get_hash_recursive([])) + return robot_control_response_lcmt._packed_fingerprint + _get_packed_fingerprint = staticmethod(_get_packed_fingerprint) + + def get_hash(self): + """Get the LCM hash of the struct""" + return struct.unpack(">Q", robot_control_response_lcmt._get_packed_fingerprint())[0] + diff --git a/base_move/move/__pycache__/robot_control_cmd_lcmt.cpython-38.pyc b/base_move/move/__pycache__/robot_control_cmd_lcmt.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..b526274b508007788f564c47bf17cab5daab123d GIT binary patch literal 6615 zcmcIo%X1q^8Sfs=OD|h?9OvzLlWaCx5<3q_1qj}dWH+0t#UX?ZtXLRi^;nWEX|z4< zY{lh;Dx6%Xf+~tTWgUIt#Q(sZ6Ez$;xeAV4e1YHBBUx{gKr&PPsr&1%d;06|txvPr z6vJaZov*w+!Pwtv(EGR;+(8R}2C$hEOz;}3^Mb+Ik62^gWPV~jC9iTuFJ_IJSvP3* zGuAMEOc-9X3f2QA62g4Jgz4MciGqW%C2WjsJ$40u!t%}@zQLCBJaXsggA*`SDX^3@tgEhesY@^lH!b*!#Is`8soG$ zE9NoIV4T4?Bc=*j@s2nL+yrnFz)gtr;sVB#VhR`WZZs={l|Xr=prK??^GbE$)k<|0 zne{;Uky9>Kl~)y!Lj=W=ic+=est@vwhKihLe$5kp7&)@}Jnl4uFzysfCEZDtl&pEp zpbGlrYJg9H#E;UU@|#}8ua+w+vYr)d8-C=94Ovvxpb^=izU5(JwH6fd9Sm<=_tf%U z0>ap{2^at+4$yh1Pt*6%!ao5BzQw#4a@~jcfJu-DUod3~bDQ61+k~?q&_SFH5f9=V z2tCHR2Wv?Q2I)vG2I)vqVjM)%{%%^ID5GIkFbE{OEQygm&WKgw>KL~q3mi=6~pz#m15|7_28Mm zsKID8SZF?vQnzc>P?dsub7=gDCX!St zayuGKlp&pfal8G_@ZQ(vn`i%v6%5@s9bP-Bi|8n}&A#GWe1|JzC(&Xp&SBg;<`&mI zzGJmu_q%*KZ%gt6k@cmlDqoU6h>m$>AO5shTK9#wS|#?Ik{oT?^BX1jZ!c)Tt%689 z*j+=OrDchgjUkrpc)=XzL%Kp@c!H<+=9w|<#q=dQQ|F;R-MnUQ0&Wa!F~xVZ-(5>= zu{9ITLbF>|n?20086pXsg9x5@X`EnxFn$|fgIgXmOR;Fd!)vjnfpE&!n`kKAoEV-REl9yDXEP~UIU3XC3zmbDD{-` zkPnFzCM5JqG9iRiQxsKEzDM*2F*SA;c1*X3sgN8EgVorE%cr?J-g@FSL$^TQOy9y^ z(1~3`$9{H@x3qZ6_#Cdmqh_IjXV1|Tur5%TL;)Ob3_944OOC*$G!7PtwRU@LdIVi& z1YK6+9KF_Uubt31STpw1xX_AB~6ys30wil zPsPGnKtHlK8d{~b%=8BR0pPXt8m>^wlKkO-K;<`p`zBgQ!G`?cxjvUa!%y<@D&AF^ zA>kgVI59~ozHUpTeos)!kZOy>5{i~PaGJvUfF09)!sFWPI4P_V!~dG~uKe2h$+u0m zM~UJ;2>0(_#_uHC`v5~Y4f+_<4l?YQ2+80miIjeN=-gPu{ETv>#m%mcqh0*tSiIaw+zKRjMm76 zjpcn~>2cnQquh}Z5l)gyNU}Le_R_T&Fu<%d|Rll;6Hvq3Je z>xCr6;2wtJjHGkVpJFo9LDv|Yd30!ZbCk(6ign-fqLhbZUu@KhL*Z&bVy2ldkQiUDq|-44UO~({N4K&=*t!Vc$c`@1TW$ z1Q=JPmtEB6$(|Rb7;q-)_TD#J=D=F0;U`fa+af2X@au?aaRk4vm=QJd(gh7$pN)^>mzzvQ%hODIO2g^*{wO9>^)&RDmk$Yd1Y5k6^Sc zzXdp?>m;6gXyIJ|U80UErm1{3^{l0#-7c@4p7I*W8~C;Um(mu#jO;GfpK;}1g-%H! zL6@bs4!PBZPG&!{Hl=IGCkJj`exJVon7|JRP!%rE5cna15!R~h6MI@37twjOszJU3(stdTlS4`FFg8kU!0*zK_UkcE?K&?3w}#eV3Jm0R zed@X}rXHa9u8+Np&iHx&nKCkoHOiWkEo2TTg7{8c8&J(a8GK)D&{n8@SLf3!E0uk7 z>Du+GR%EEmQ+zC+j(P4z3cA9cs&SNDwMa+Z2;El`O{*E>m-*&c#&$sS%Mmy`EFh)hl&^&|9OHFNt7XnQlafj=AjozIy6 zLHbPZmC`p!$!}j?*0rWlp8jKy|DH}t=Esb~eS7DA{{xxRP-XxC literal 0 HcmV?d00001 diff --git a/base_move/move/__pycache__/robot_control_response_lcmt.cpython-38.pyc b/base_move/move/__pycache__/robot_control_response_lcmt.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..d3b0620185f8365077dad44cb84ef18a7d2bc8bc GIT binary patch literal 3134 zcma)8OK%*<5$^7J?CgUSEzx>dwlaQu1BcYZFmzyOMP_UZu$Qo9JJG^MV>sR19ddVP z-P0?B3mO4J|@KU)8=LE?v~ZXz15Ty$DgOx{To`%x*Qe?+OP6xuPRhE74@R7r|QSvhH66RtJaBk zrpKWT)T}!9h;>6Xr(VFfrp~Ji_(p18y@+pJH*|f6ckAk+x&*lixe2+cE~_h$Taa6j zv37Sxy`-)}Hw)b?bhGMZ^$O&3ss+zoE3cY-Ew?hwN7m#6X>>7$>$HShZ0QekrOR3` z$*jy&S))-&YReaLqqLD@lc%~UH`o|{Z+V{-JY=eZr{#evuy z<~BD6LYR&0ZE4FIzBk@KU~b)%wzJ=YH&cGh(ZqXbAEFg^K@{I+vZCDa@H#Y}qrRho zqanrO5wkVr@9-72L!%^+wAe+P6q}1{2^>1iAL>O%&hYrcc<I4YIg# zK^C*eyz~n_SU{IZo0|tt9FJEE*d?tZYg>N`Yya zk8~Mf(8FXf(T|0>MAN*r$ti2YYQih%FUVJ9>nv~Z>1$`%SWQZ4V}}p%glT7C2Mlau zmhU>yZg|^l!$%9yLbMuMgjU~+4(!F)4WVdELowz3OI%?;iyxg|4xs!bT3&Nc|B1hR z<7QTcotszjvv$*5!KVxhYbMBGpPchM0O62e$ogksUPl8qDmRm0f+Y7f)^MYNCXDuJG<3~vat%sdj5W=hL&p!_)M~*+`o2M{&7i$tsHbc`=uuTbqhw_$jIk&iP0ll zlr=jX13mp`Oe`}p^EK* zLBX+X1E5-;E@d&uZGny%Dd|#59VE_5iK~Jh6?8k5(p;wP{ecJ-Q}aiV zvPBoDEFEgw&y~xdDmJ%h1Qjat9+4xXjzZ<3gNjfQgG78@%!`X6YD8h=M^PlAz%}j* z$f81cy{utRc`9Z1e0tyFjO^W8T{vXQ=#Z<9*yg?AI5)Pcks`~T&&x1h+t8`4-2NN; XDx;;!Q_B=~S0?>H@Hrf#D3jt>J= 95: + self.mode_ok = self.rec_msg.mode + else: + self.mode_ok = 0 + + def rec_responce(self): + while self.runing: + self.lc_r.handle() + time.sleep(0.002) + + def Wait_finish(self, mode, gait_id): + count = 0 + while self.runing and count < 2000: # 10s + if self.mode_ok == mode and self.gait_ok == gait_id: + return True + else: + time.sleep(0.005) + count += 1 + print("Wait_finish timeout.") + return False + + def send_publish(self): + while self.runing: + self.send_lock.acquire() + if self.delay_cnt > 20: # Heartbeat signal 10HZ + self.lc_s.publish("robot_control_cmd", self.cmd_msg.encode()) + self.delay_cnt = 0 + self.delay_cnt += 1 + self.send_lock.release() + time.sleep(0.005) + + def Send_cmd(self, msg): + self.send_lock.acquire() + self.delay_cnt = 50 + self.cmd_msg = msg + self.send_lock.release() + + def quit(self): + self.runing = 0 + self.rec_thread.join() + self.send_thread.join() + + +# Main function +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/base_move/move/robot_control_cmd_lcmt.py b/base_move/move/robot_control_cmd_lcmt.py new file mode 100644 index 0000000..addedac --- /dev/null +++ b/base_move/move/robot_control_cmd_lcmt.py @@ -0,0 +1,149 @@ +# LCM type definitions This file automatically generated by lcm. +try: + import cStringIO.StringIO as BytesIO +except ImportError: + from io import BytesIO +import struct + +class robot_control_cmd_lcmt(object): + __slots__ = ["mode", "gait_id", "contact", "life_count", "vel_des", "rpy_des", "pos_des", "acc_des", "ctrl_point", "foot_pose", "step_height", "value", "duration"] + + __typenames__ = ["int8_t", "int8_t", "int8_t", "int8_t", "float", "float", "float", "float", "float", "float", "float", "int32_t", "int32_t"] + + __dimensions__ = [None, None, None, None, [3], [3], [3], [6], [3], [6], [2], None, None] + + def __init__(self): + self.mode = 0 + self.gait_id = 0 + self.contact = 0 + self.life_count = 0 + self.vel_des = [ 0.0 for dim0 in range(3) ] + self.rpy_des = [ 0.0 for dim0 in range(3) ] + self.pos_des = [ 0.0 for dim0 in range(3) ] + self.acc_des = [ 0.0 for dim0 in range(6) ] + self.ctrl_point = [ 0.0 for dim0 in range(3) ] + self.foot_pose = [ 0.0 for dim0 in range(6) ] + self.step_height = [ 0.0 for dim0 in range(2) ] + self.value = 0 + self.duration = 0 + + def encode(self): + buf = BytesIO() + buf.write(robot_control_cmd_lcmt._get_packed_fingerprint()) + self._encode_one(buf) + return buf.getvalue() + + def _encode_one(self, buf): + buf.write(struct.pack(">bbbb", self.mode, self.gait_id, self.contact, self.life_count)) + buf.write(struct.pack('>3f', *self.vel_des[:3])) + buf.write(struct.pack('>3f', *self.rpy_des[:3])) + buf.write(struct.pack('>3f', *self.pos_des[:3])) + buf.write(struct.pack('>6f', *self.acc_des[:6])) + buf.write(struct.pack('>3f', *self.ctrl_point[:3])) + buf.write(struct.pack('>6f', *self.foot_pose[:6])) + buf.write(struct.pack('>2f', *self.step_height[:2])) + buf.write(struct.pack(">ii", self.value, self.duration)) + + def decode(data): + if hasattr(data, 'read'): + buf = data + else: + buf = BytesIO(data) + if buf.read(8) != robot_control_cmd_lcmt._get_packed_fingerprint(): + raise ValueError("Decode error") + return robot_control_cmd_lcmt._decode_one(buf) + decode = staticmethod(decode) + + def _decode_one(buf): + self = robot_control_cmd_lcmt() + self.mode, self.gait_id, self.contact, self.life_count = struct.unpack(">bbbb", buf.read(4)) + self.vel_des = struct.unpack('>3f', buf.read(12)) + self.rpy_des = struct.unpack('>3f', buf.read(12)) + self.pos_des = struct.unpack('>3f', buf.read(12)) + self.acc_des = struct.unpack('>6f', buf.read(24)) + self.ctrl_point = struct.unpack('>3f', buf.read(12)) + self.foot_pose = struct.unpack('>6f', buf.read(24)) + self.step_height = struct.unpack('>2f', buf.read(8)) + self.value, self.duration = struct.unpack(">ii", buf.read(8)) + return self + _decode_one = staticmethod(_decode_one) + + def _get_hash_recursive(parents): + if robot_control_cmd_lcmt in parents: return 0 + tmphash = (0x476b61e296af96f5) & 0xffffffffffffffff + tmphash = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff + return tmphash + _get_hash_recursive = staticmethod(_get_hash_recursive) + _packed_fingerprint = None + + def _get_packed_fingerprint(): + if robot_control_cmd_lcmt._packed_fingerprint is None: + robot_control_cmd_lcmt._packed_fingerprint = struct.pack(">Q", robot_control_cmd_lcmt._get_hash_recursive([])) + return robot_control_cmd_lcmt._packed_fingerprint + _get_packed_fingerprint = staticmethod(_get_packed_fingerprint) + + def get_hash(self): + """Get the LCM hash of the struct""" + return struct.unpack(">Q", robot_control_cmd_lcmt._get_packed_fingerprint())[0] + +class robot_control_response_lcmt(object): + __slots__ = ["mode", "gait_id", "contact", "order_process_bar", "switch_status", "ori_error", "footpos_error", "motor_error"] + + __typenames__ = ["int8_t", "int8_t", "int8_t", "int8_t", "int8_t", "int8_t", "int16_t", "int32_t"] + + __dimensions__ = [None, None, None, None, None, None, None, [12]] + + def __init__(self): + self.mode = 0 + self.gait_id = 0 + self.contact = 0 + self.order_process_bar = 0 + self.switch_status = 0 + self.ori_error = 0 + self.footpos_error = 0 + self.motor_error = [ 0 for dim0 in range(12) ] + + def encode(self): + buf = BytesIO() + buf.write(robot_control_response_lcmt._get_packed_fingerprint()) + self._encode_one(buf) + return buf.getvalue() + + def _encode_one(self, buf): + buf.write(struct.pack(">bbbbbbh", self.mode, self.gait_id, self.contact, self.order_process_bar, self.switch_status, self.ori_error, self.footpos_error)) + buf.write(struct.pack('>12i', *self.motor_error[:12])) + + def decode(data): + if hasattr(data, 'read'): + buf = data + else: + buf = BytesIO(data) + if buf.read(8) != robot_control_response_lcmt._get_packed_fingerprint(): + raise ValueError("Decode error") + return robot_control_response_lcmt._decode_one(buf) + decode = staticmethod(decode) + + def _decode_one(buf): + self = robot_control_response_lcmt() + self.mode, self.gait_id, self.contact, self.order_process_bar, self.switch_status, self.ori_error, self.footpos_error = struct.unpack(">bbbbbbh", buf.read(8)) + self.motor_error = struct.unpack('>12i', buf.read(48)) + return self + _decode_one = staticmethod(_decode_one) + + def _get_hash_recursive(parents): + if robot_control_response_lcmt in parents: return 0 + tmphash = (0x485da98216eda8c7) & 0xffffffffffffffff + tmphash = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff + return tmphash + _get_hash_recursive = staticmethod(_get_hash_recursive) + _packed_fingerprint = None + + def _get_packed_fingerprint(): + if robot_control_response_lcmt._packed_fingerprint is None: + robot_control_response_lcmt._packed_fingerprint = struct.pack(">Q", robot_control_response_lcmt._get_hash_recursive([])) + return robot_control_response_lcmt._packed_fingerprint + _get_packed_fingerprint = staticmethod(_get_packed_fingerprint) + + def get_hash(self): + """Get the LCM hash of the struct""" + return struct.unpack(">Q", robot_control_response_lcmt._get_packed_fingerprint())[0] diff --git a/base_move/move/robot_control_response_lcmt.py b/base_move/move/robot_control_response_lcmt.py new file mode 100644 index 0000000..cc61b8c --- /dev/null +++ b/base_move/move/robot_control_response_lcmt.py @@ -0,0 +1,73 @@ +"""LCM type definitions +This file automatically generated by lcm. +DO NOT MODIFY BY HAND!!!! +""" + +try: + import cStringIO.StringIO as BytesIO +except ImportError: + from io import BytesIO +import struct + +class robot_control_response_lcmt(object): + __slots__ = ["mode", "gait_id", "contact", "order_process_bar", "switch_status", "ori_error", "footpos_error", "motor_error"] + + __typenames__ = ["int8_t", "int8_t", "int8_t", "int8_t", "int8_t", "int8_t", "int16_t", "int32_t"] + + __dimensions__ = [None, None, None, None, None, None, None, [12]] + + def __init__(self): + self.mode = 0 + self.gait_id = 0 + self.contact = 0 + self.order_process_bar = 0 + self.switch_status = 0 + self.ori_error = 0 + self.footpos_error = 0 + self.motor_error = [ 0 for dim0 in range(12) ] + + def encode(self): + buf = BytesIO() + buf.write(robot_control_response_lcmt._get_packed_fingerprint()) + self._encode_one(buf) + return buf.getvalue() + + def _encode_one(self, buf): + buf.write(struct.pack(">bbbbbbh", self.mode, self.gait_id, self.contact, self.order_process_bar, self.switch_status, self.ori_error, self.footpos_error)) + buf.write(struct.pack('>12i', *self.motor_error[:12])) + + def decode(data): + if hasattr(data, 'read'): + buf = data + else: + buf = BytesIO(data) + if buf.read(8) != robot_control_response_lcmt._get_packed_fingerprint(): + raise ValueError("Decode error") + return robot_control_response_lcmt._decode_one(buf) + decode = staticmethod(decode) + + def _decode_one(buf): + self = robot_control_response_lcmt() + self.mode, self.gait_id, self.contact, self.order_process_bar, self.switch_status, self.ori_error, self.footpos_error = struct.unpack(">bbbbbbh", buf.read(8)) + self.motor_error = struct.unpack('>12i', buf.read(48)) + return self + _decode_one = staticmethod(_decode_one) + + def _get_hash_recursive(parents): + if robot_control_response_lcmt in parents: return 0 + tmphash = (0x485da98216eda8c7) & 0xffffffffffffffff + tmphash = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff + return tmphash + _get_hash_recursive = staticmethod(_get_hash_recursive) + _packed_fingerprint = None + + def _get_packed_fingerprint(): + if robot_control_response_lcmt._packed_fingerprint is None: + robot_control_response_lcmt._packed_fingerprint = struct.pack(">Q", robot_control_response_lcmt._get_hash_recursive([])) + return robot_control_response_lcmt._packed_fingerprint + _get_packed_fingerprint = staticmethod(_get_packed_fingerprint) + + def get_hash(self): + """Get the LCM hash of the struct""" + return struct.unpack(">Q", robot_control_response_lcmt._get_packed_fingerprint())[0] + diff --git a/main.py b/main.py new file mode 100644 index 0000000..66abbd8 --- /dev/null +++ b/main.py @@ -0,0 +1,113 @@ +''' +This demo show the communication interface of MR813 motion control board based on Lcm. +Dependency: +- robot_control_cmd_lcmt.py +- robot_control_response_lcmt.py +''' +import lcm +import sys +import os +import time +from threading import Thread, Lock + +# For keyboard input +import tty +import termios +import sys + +from robot_control_cmd_lcmt import robot_control_cmd_lcmt +from robot_control_response_lcmt import robot_control_response_lcmt + +from task_1.task_1 import run_task_1 + +def main(): + Ctrl = Robot_Ctrl() + Ctrl.run() + msg = robot_control_cmd_lcmt() + + try: + print("Recovery stand") + msg.mode = 12 # Recovery stand + msg.gait_id = 0 + msg.life_count += 1 + Ctrl.Send_cmd(msg) + Ctrl.Wait_finish(12, 0) + + # run_task_1(Ctrl, msg) + + time.sleep(100) + + except KeyboardInterrupt: + pass + + Ctrl.quit() + sys.exit() + + +class Robot_Ctrl(object): + def __init__(self): + self.rec_thread = Thread(target=self.rec_responce) + self.send_thread = Thread(target=self.send_publish) + self.lc_r = lcm.LCM("udpm://239.255.76.67:7670?ttl=255") + self.lc_s = lcm.LCM("udpm://239.255.76.67:7671?ttl=255") + self.cmd_msg = robot_control_cmd_lcmt() + self.rec_msg = robot_control_response_lcmt() + self.send_lock = Lock() + self.delay_cnt = 0 + self.mode_ok = 0 + self.gait_ok = 0 + self.runing = 1 + + def run(self): + self.lc_r.subscribe("robot_control_response", self.msg_handler) + self.send_thread.start() + self.rec_thread.start() + + def msg_handler(self, channel, data): + self.rec_msg = robot_control_response_lcmt().decode(data) + if self.rec_msg.order_process_bar >= 95: + self.mode_ok = self.rec_msg.mode + else: + self.mode_ok = 0 + + def rec_responce(self): + while self.runing: + self.lc_r.handle() + time.sleep(0.002) + + def Wait_finish(self, mode, gait_id): + count = 0 + while self.runing and count < 2000: # 10s + if self.mode_ok == mode and self.gait_ok == gait_id: + return True + else: + time.sleep(0.005) + count += 1 + print("Wait_finish timeout.") + return False + + def send_publish(self): + while self.runing: + self.send_lock.acquire() + if self.delay_cnt > 20: # Heartbeat signal 10HZ + self.lc_s.publish("robot_control_cmd", self.cmd_msg.encode()) + self.delay_cnt = 0 + self.delay_cnt += 1 + self.send_lock.release() + time.sleep(0.005) + + def Send_cmd(self, msg): + self.send_lock.acquire() + self.delay_cnt = 50 + self.cmd_msg = msg + self.send_lock.release() + + def quit(self): + self.runing = 0 + self.rec_thread.join() + self.send_thread.join() + + +# Main function +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/robot_control_cmd_lcmt.py b/robot_control_cmd_lcmt.py new file mode 100644 index 0000000..addedac --- /dev/null +++ b/robot_control_cmd_lcmt.py @@ -0,0 +1,149 @@ +# LCM type definitions This file automatically generated by lcm. +try: + import cStringIO.StringIO as BytesIO +except ImportError: + from io import BytesIO +import struct + +class robot_control_cmd_lcmt(object): + __slots__ = ["mode", "gait_id", "contact", "life_count", "vel_des", "rpy_des", "pos_des", "acc_des", "ctrl_point", "foot_pose", "step_height", "value", "duration"] + + __typenames__ = ["int8_t", "int8_t", "int8_t", "int8_t", "float", "float", "float", "float", "float", "float", "float", "int32_t", "int32_t"] + + __dimensions__ = [None, None, None, None, [3], [3], [3], [6], [3], [6], [2], None, None] + + def __init__(self): + self.mode = 0 + self.gait_id = 0 + self.contact = 0 + self.life_count = 0 + self.vel_des = [ 0.0 for dim0 in range(3) ] + self.rpy_des = [ 0.0 for dim0 in range(3) ] + self.pos_des = [ 0.0 for dim0 in range(3) ] + self.acc_des = [ 0.0 for dim0 in range(6) ] + self.ctrl_point = [ 0.0 for dim0 in range(3) ] + self.foot_pose = [ 0.0 for dim0 in range(6) ] + self.step_height = [ 0.0 for dim0 in range(2) ] + self.value = 0 + self.duration = 0 + + def encode(self): + buf = BytesIO() + buf.write(robot_control_cmd_lcmt._get_packed_fingerprint()) + self._encode_one(buf) + return buf.getvalue() + + def _encode_one(self, buf): + buf.write(struct.pack(">bbbb", self.mode, self.gait_id, self.contact, self.life_count)) + buf.write(struct.pack('>3f', *self.vel_des[:3])) + buf.write(struct.pack('>3f', *self.rpy_des[:3])) + buf.write(struct.pack('>3f', *self.pos_des[:3])) + buf.write(struct.pack('>6f', *self.acc_des[:6])) + buf.write(struct.pack('>3f', *self.ctrl_point[:3])) + buf.write(struct.pack('>6f', *self.foot_pose[:6])) + buf.write(struct.pack('>2f', *self.step_height[:2])) + buf.write(struct.pack(">ii", self.value, self.duration)) + + def decode(data): + if hasattr(data, 'read'): + buf = data + else: + buf = BytesIO(data) + if buf.read(8) != robot_control_cmd_lcmt._get_packed_fingerprint(): + raise ValueError("Decode error") + return robot_control_cmd_lcmt._decode_one(buf) + decode = staticmethod(decode) + + def _decode_one(buf): + self = robot_control_cmd_lcmt() + self.mode, self.gait_id, self.contact, self.life_count = struct.unpack(">bbbb", buf.read(4)) + self.vel_des = struct.unpack('>3f', buf.read(12)) + self.rpy_des = struct.unpack('>3f', buf.read(12)) + self.pos_des = struct.unpack('>3f', buf.read(12)) + self.acc_des = struct.unpack('>6f', buf.read(24)) + self.ctrl_point = struct.unpack('>3f', buf.read(12)) + self.foot_pose = struct.unpack('>6f', buf.read(24)) + self.step_height = struct.unpack('>2f', buf.read(8)) + self.value, self.duration = struct.unpack(">ii", buf.read(8)) + return self + _decode_one = staticmethod(_decode_one) + + def _get_hash_recursive(parents): + if robot_control_cmd_lcmt in parents: return 0 + tmphash = (0x476b61e296af96f5) & 0xffffffffffffffff + tmphash = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff + return tmphash + _get_hash_recursive = staticmethod(_get_hash_recursive) + _packed_fingerprint = None + + def _get_packed_fingerprint(): + if robot_control_cmd_lcmt._packed_fingerprint is None: + robot_control_cmd_lcmt._packed_fingerprint = struct.pack(">Q", robot_control_cmd_lcmt._get_hash_recursive([])) + return robot_control_cmd_lcmt._packed_fingerprint + _get_packed_fingerprint = staticmethod(_get_packed_fingerprint) + + def get_hash(self): + """Get the LCM hash of the struct""" + return struct.unpack(">Q", robot_control_cmd_lcmt._get_packed_fingerprint())[0] + +class robot_control_response_lcmt(object): + __slots__ = ["mode", "gait_id", "contact", "order_process_bar", "switch_status", "ori_error", "footpos_error", "motor_error"] + + __typenames__ = ["int8_t", "int8_t", "int8_t", "int8_t", "int8_t", "int8_t", "int16_t", "int32_t"] + + __dimensions__ = [None, None, None, None, None, None, None, [12]] + + def __init__(self): + self.mode = 0 + self.gait_id = 0 + self.contact = 0 + self.order_process_bar = 0 + self.switch_status = 0 + self.ori_error = 0 + self.footpos_error = 0 + self.motor_error = [ 0 for dim0 in range(12) ] + + def encode(self): + buf = BytesIO() + buf.write(robot_control_response_lcmt._get_packed_fingerprint()) + self._encode_one(buf) + return buf.getvalue() + + def _encode_one(self, buf): + buf.write(struct.pack(">bbbbbbh", self.mode, self.gait_id, self.contact, self.order_process_bar, self.switch_status, self.ori_error, self.footpos_error)) + buf.write(struct.pack('>12i', *self.motor_error[:12])) + + def decode(data): + if hasattr(data, 'read'): + buf = data + else: + buf = BytesIO(data) + if buf.read(8) != robot_control_response_lcmt._get_packed_fingerprint(): + raise ValueError("Decode error") + return robot_control_response_lcmt._decode_one(buf) + decode = staticmethod(decode) + + def _decode_one(buf): + self = robot_control_response_lcmt() + self.mode, self.gait_id, self.contact, self.order_process_bar, self.switch_status, self.ori_error, self.footpos_error = struct.unpack(">bbbbbbh", buf.read(8)) + self.motor_error = struct.unpack('>12i', buf.read(48)) + return self + _decode_one = staticmethod(_decode_one) + + def _get_hash_recursive(parents): + if robot_control_response_lcmt in parents: return 0 + tmphash = (0x485da98216eda8c7) & 0xffffffffffffffff + tmphash = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff + return tmphash + _get_hash_recursive = staticmethod(_get_hash_recursive) + _packed_fingerprint = None + + def _get_packed_fingerprint(): + if robot_control_response_lcmt._packed_fingerprint is None: + robot_control_response_lcmt._packed_fingerprint = struct.pack(">Q", robot_control_response_lcmt._get_hash_recursive([])) + return robot_control_response_lcmt._packed_fingerprint + _get_packed_fingerprint = staticmethod(_get_packed_fingerprint) + + def get_hash(self): + """Get the LCM hash of the struct""" + return struct.unpack(">Q", robot_control_response_lcmt._get_packed_fingerprint())[0] diff --git a/robot_control_response_lcmt.py b/robot_control_response_lcmt.py new file mode 100644 index 0000000..cc61b8c --- /dev/null +++ b/robot_control_response_lcmt.py @@ -0,0 +1,73 @@ +"""LCM type definitions +This file automatically generated by lcm. +DO NOT MODIFY BY HAND!!!! +""" + +try: + import cStringIO.StringIO as BytesIO +except ImportError: + from io import BytesIO +import struct + +class robot_control_response_lcmt(object): + __slots__ = ["mode", "gait_id", "contact", "order_process_bar", "switch_status", "ori_error", "footpos_error", "motor_error"] + + __typenames__ = ["int8_t", "int8_t", "int8_t", "int8_t", "int8_t", "int8_t", "int16_t", "int32_t"] + + __dimensions__ = [None, None, None, None, None, None, None, [12]] + + def __init__(self): + self.mode = 0 + self.gait_id = 0 + self.contact = 0 + self.order_process_bar = 0 + self.switch_status = 0 + self.ori_error = 0 + self.footpos_error = 0 + self.motor_error = [ 0 for dim0 in range(12) ] + + def encode(self): + buf = BytesIO() + buf.write(robot_control_response_lcmt._get_packed_fingerprint()) + self._encode_one(buf) + return buf.getvalue() + + def _encode_one(self, buf): + buf.write(struct.pack(">bbbbbbh", self.mode, self.gait_id, self.contact, self.order_process_bar, self.switch_status, self.ori_error, self.footpos_error)) + buf.write(struct.pack('>12i', *self.motor_error[:12])) + + def decode(data): + if hasattr(data, 'read'): + buf = data + else: + buf = BytesIO(data) + if buf.read(8) != robot_control_response_lcmt._get_packed_fingerprint(): + raise ValueError("Decode error") + return robot_control_response_lcmt._decode_one(buf) + decode = staticmethod(decode) + + def _decode_one(buf): + self = robot_control_response_lcmt() + self.mode, self.gait_id, self.contact, self.order_process_bar, self.switch_status, self.ori_error, self.footpos_error = struct.unpack(">bbbbbbh", buf.read(8)) + self.motor_error = struct.unpack('>12i', buf.read(48)) + return self + _decode_one = staticmethod(_decode_one) + + def _get_hash_recursive(parents): + if robot_control_response_lcmt in parents: return 0 + tmphash = (0x485da98216eda8c7) & 0xffffffffffffffff + tmphash = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff + return tmphash + _get_hash_recursive = staticmethod(_get_hash_recursive) + _packed_fingerprint = None + + def _get_packed_fingerprint(): + if robot_control_response_lcmt._packed_fingerprint is None: + robot_control_response_lcmt._packed_fingerprint = struct.pack(">Q", robot_control_response_lcmt._get_hash_recursive([])) + return robot_control_response_lcmt._packed_fingerprint + _get_packed_fingerprint = staticmethod(_get_packed_fingerprint) + + def get_hash(self): + """Get the LCM hash of the struct""" + return struct.unpack(">Q", robot_control_response_lcmt._get_packed_fingerprint())[0] + diff --git a/task_1/__pycache__/task_1.cpython-38.pyc b/task_1/__pycache__/task_1.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..94e0eb901e2e89a069a5c4e606bf0794d14ec6cf GIT binary patch literal 607 zcmaJVw`6?p0X(|kJtzmC$@B&h z0ps2 zD$>LZ_1C1=>2&HQ9k~8Ll#&r1`U~gjm*NqnZ#GZ=?%w?D*Nm#^-L1n*zjRUk+odkf z9scy&v29FCRTvxO6O=Yehsp?rHae1-K&dUqsvH_MDQr_4ITd56(y_5|rVgbTO)8S| zeOaIwUPHM``#vY#@g$dBBem(e`^-u77E5RP-Nt2A z2v-ulfD}3*0>=H2g>0EDW-DZ|_ZGXTT&JG4SFX@9_vf3vqctj6zC~B8GWp3pgLZ!a G_g?{(dyh^4 literal 0 HcmV?d00001 diff --git a/task_1/task_1.py b/task_1/task_1.py new file mode 100644 index 0000000..f5860b9 --- /dev/null +++ b/task_1/task_1.py @@ -0,0 +1,39 @@ +import time + + +def run_task_1(ctrl, msg): + print('Running task 1...') + + # 右前方 + msg.mode = 11 + msg.gait_id = 26 # 26 表示快速 trot 步态 + msg.vel_des = [0.5, 0.5, -1.0] + msg.duration = 1800 + msg.step_height = [0.06, 0.06] + msg.life_count += 1 + ctrl.Send_cmd(msg) + time.sleep(1.8) + + msg.mode = 11 + msg.gait_id = 26 + msg.vel_des = [1, 0, 0] + msg.duration = 200 + msg.life_count += 1 + ctrl.Send_cmd(msg) + time.sleep(0.2) + + # TAG take photo + # while True: + # pass + + # msg.mode = 11 + # msg.gait_id = 26 + # msg.vel_des = [1, 0, 0] + # msg.duration = 1000 + # msg.life_count += 1 + # ctrl.Send_cmd(msg) + # time.sleep(1.0) + + + + diff --git a/utils/image_raw.py b/utils/image_raw.py new file mode 100644 index 0000000..854aea6 --- /dev/null +++ b/utils/image_raw.py @@ -0,0 +1,72 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image +from cv_bridge import CvBridge +import cv2 +from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy + +class ImageSubscriber(Node): + def __init__(self): + super().__init__('image_subscriber') + # 定义 QoS 配置（匹配发布者的可靠性策略） + qos_profile = QoSProfile( + reliability=QoSReliabilityPolicy.BEST_EFFORT, # 或 BEST_EFFORT，取决于发布者 + history=QoSHistoryPolicy.KEEP_LAST, + depth=10 + ) + + self.subscription = self.create_subscription( + Image, + '/rgb_camera/image_raw', + self.image_callback, + qos_profile=qos_profile + ) + self.subscription # 防止未使用变量警告 + self.bridge = CvBridge() + self.cv_image = None # Store latest image + + def image_callback(self, msg): + try: + # 将ROS图像消息转换为OpenCV格式 + self.cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8') + + except Exception as e: + self.get_logger().error('Failed to convert image: %s' % str(e)) + +class ImageProcessor: + def __init__(self): + rclpy.init() + self.image_subscriber = ImageSubscriber() + + def run(self): + try: + rclpy.spin(self.image_subscriber) + except KeyboardInterrupt: + pass + + self.image_subscriber.destroy_node() + rclpy.shutdown() + + def get_current_image(self): + return self.image_subscriber.cv_image + +""" DEBUG """ + +def main(args=None): + rclpy.init(args=args) + image_subscriber = ImageSubscriber() + + try: + rclpy.spin(image_subscriber) + except KeyboardInterrupt: + pass + + # 清理 + image_subscriber.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file