From ff9480304c5464ff480b9eba5d5c527e9a29c185 Mon Sep 17 00:00:00 2001 From: Hercogs Date: Sat, 3 Feb 2024 17:07:02 +0000 Subject: [PATCH 1/4] feat: improved table detection node --- .../launch/table_detection.launch.py | 38 + .../__pycache__/__init__.cpython-310.pyc | Bin 0 -> 159 bytes .../__pycache__/action.cpython-310.pyc | Bin 0 -> 1631 bytes .../__pycache__/main_node.cpython-310.pyc | Bin 0 -> 4636 bytes .../table_detection.cpython-310.pyc | Bin 0 -> 11282 bytes .../__pycache__/tf2_listener.cpython-310.pyc | Bin 0 -> 3616 bytes robot_firmware/robot_firmware/action.py | 66 ++ robot_firmware/robot_firmware/main_node.py | 223 +++++- .../robot_firmware/table_detection.py | 660 ++++++++++++++++++ .../robot_firmware/tf2_listener copy 1.py | 146 ++++ .../robot_firmware/tf2_listener copy.py | 146 ++++ robot_firmware/robot_firmware/tf2_listener.py | 174 +++++ .../robot_firmware/tf2_listener_working.py | 42 ++ robot_firmware/setup.py | 7 +- robot_firmware/src/table_detection.py | 222 ------ 15 files changed, 1471 insertions(+), 253 deletions(-) create mode 100644 robot_firmware/launch/table_detection.launch.py create mode 100644 robot_firmware/robot_firmware/__pycache__/__init__.cpython-310.pyc create mode 100644 robot_firmware/robot_firmware/__pycache__/action.cpython-310.pyc create mode 100644 robot_firmware/robot_firmware/__pycache__/main_node.cpython-310.pyc create mode 100644 robot_firmware/robot_firmware/__pycache__/table_detection.cpython-310.pyc create mode 100644 robot_firmware/robot_firmware/__pycache__/tf2_listener.cpython-310.pyc create mode 100644 robot_firmware/robot_firmware/action.py create mode 100644 robot_firmware/robot_firmware/table_detection.py create mode 100644 robot_firmware/robot_firmware/tf2_listener copy 1.py create mode 100644 robot_firmware/robot_firmware/tf2_listener copy.py create mode 100644 robot_firmware/robot_firmware/tf2_listener.py create mode 100644 robot_firmware/robot_firmware/tf2_listener_working.py delete mode 100644 robot_firmware/src/table_detection.py diff --git a/robot_firmware/launch/table_detection.launch.py b/robot_firmware/launch/table_detection.launch.py new file mode 100644 index 0000000..cd90daf --- /dev/null +++ b/robot_firmware/launch/table_detection.launch.py @@ -0,0 +1,38 @@ +import os +from launch import LaunchDescription +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + +def generate_launch_description(): + + package_name = "robot_firmware" + + + # RVIZ configuration file + rviz_file = "rvis.rviz" + rviz_config_dir = os.path.join(get_package_share_directory(package_name), "rviz", rviz_file) + rviz_config_dir = "/home/user/ros2_ws/src/rvis.rviz" + + return LaunchDescription([ + + Node( + package=package_name, + executable='table_detection', + name='table_detection', + output='screen', + parameters=[{'use_sim_time': True}], + ), + + Node( + package="rviz2", + executable="rviz2", + output="screen", + parameters=[{ + "use_sim_time": True, + }], + arguments=[ + "-d ", rviz_config_dir + ] + ) + + ]) diff --git a/robot_firmware/robot_firmware/__pycache__/__init__.cpython-310.pyc b/robot_firmware/robot_firmware/__pycache__/__init__.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..ed51346a20b258073c8093e7743fb73e428682f2 GIT binary patch literal 159 zcmd1j<>g`kg8fPdQ$h4&5P=LBfgA@QE@lA|DGb33nv8xc8Hzx{2;!HMenx(7s(xv4 zYLR|Xez8$}d9i*{X=Y9eke8HS5}%e?lv|!yl#0%ckI&4@EQycTE2zB1VUwGmQks)$ N2Qs^u2}rOo007A_CYk^M literal 0 HcmV?d00001 diff --git a/robot_firmware/robot_firmware/__pycache__/action.cpython-310.pyc b/robot_firmware/robot_firmware/__pycache__/action.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..6ecf5c3d8c3168bf978740473d61d48e885eeddc GIT binary patch literal 1631 zcmZux&2HQ_5GE-~tCh7ICu!QEMGF@#&@KY3f%eivQKSe0_t5xI=cEfEi`-o~l(ZqK zICj^kHIP%@VSUU~^a=U`y!PZ*Xb+I7}xDR4N$-H086I5;+(A$tzK` zrcs4^XuAb#Lw^N|KpEyJBd^f}bIi$QoCQ4K^b%$CHKHgBkI*Qz_m0YHF2%O11>U;U z={@M~Zv71igO+FoH(p_k3|Sx(aF8d|giC<-O}IcT;t1}hhqhnkWzA{`+Ya(BXlB+? z4~un>Z0e)z*-*E0p=``pS(k<}Mc_H*GHQ2)k6EzIAr-pmAka)#z{8sops=5j zChL!4iqpT(Km9L;<4q8S3u4n8h^{||u73uxKr3{01Lp#t5cSaluYxP6sGBw7pmvZN zx(ct*1>wPhun6|`{66BrKEK=5MOjbxrcEx#yCyl*dYCzWQVoJPdg*JlQcyq2xLG-K8P=z*9-PqG@Q3%0Dfw}|t+8y^Oo8>i^ zLVW@=>fVjG@3aAkkD+TfKg3ZK<00S|_aht=iqF6N9~9qD`zUhdQRkIl%qEOg4R58> zQ^sBZhq@J?F8`h#tm^id_fjuMHikRph3nUmt+;W(V$h>t$^ld)>HwK(_BsUwJUcpcIvib z#uHwTfEuch^D~#N(s<{TbJoT==N-MMo|mpG8cLK4VM3}cGbL;ohA?pV!6_;55-?pI?|@7}oMi>Iwkx+%wf<-oIh zx48x!H$qMFLg>vo&-4e~xed>|pM&n`ey3a8c&|MO9URUz^_dgzQtUM!SNua)C_g#| W=np}}5D$D_?gq)%@gTwUpML?lkfL7z literal 0 HcmV?d00001 diff --git a/robot_firmware/robot_firmware/__pycache__/main_node.cpython-310.pyc b/robot_firmware/robot_firmware/__pycache__/main_node.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..c44a8f35c9484e791e6d584cbb1c148e662d548c GIT binary patch literal 4636 zcmbVP&5t8T74Pb9+wJz}e1_eSO^0O_3}lww2oi0gB%9gUC6FCLW+EgDsnz2ud)m|O zcB$F7To9b#knnrewr6JfIAE(^y{dZk z_1>@EdzI8`u7TgLKKx8PylfbMqRRQ7jmjJN(m#T5gR{tJQ)xylHrpm6-ioZ)Zrhr- zqe5J47d2miOsQScd@*w3a=WbgQsl;!c180}RE=xxn&!(+qihsJ(ZR;UPBcox2VxzyouIRKPbQ-wBv$T2GTh#kBH)5=JQSS~rlF4a zM?og!0DJViGKqaLiiA|f-6R(Njh5Y`BO1mtZ%i5cx1T@zi@*H%H&_1g+2?teVW?6@ zXE-PRhtGO1>fhgAmraPt21sWnKP#;%ll}mVFxre8ZSw=8%Q)laBd2X~i`$Qkw#^H? zh_b*-+(B97W$vOZ@rrP|Ca?0^BfDMxz_5+ByJfWM%DEqe1F}Jf%@#vRd;uS=n+`xS z!vG&cw*d9Qc5gi}ngHfL62G zk->|PskP2appE$q4$Z&~?5n(~$}u+P53n<3jIaJA2(xg|^SX)j2DxnsEMAs)X%@(A zZEa1}f*dBk*6~#5e|;UMt9PbG_IqUIyW<5qwMUcZ!-1owDx{+!4C?rPI0!S}mt`DW zm8Z)AN(C&H3FY=g=0{1d2VpxLbQ86-kL|rT$ifJ?yGaNKxJiGqNK?3h#=PH&LRIYm zSefv%Fvjq*F3_rf>Tat>X`zcdpAF@H3`u?ZJKi97l^sbLmA+O6M zz3K0#*LOx?#8KNxGQS(jct4Qh$?9COYr_M13Cmu?mo9=-nPV1N-E>*qTD6L7v5;$u z@Ub>i%LSm&AxrqyX^K|?8<-eCApmd&+OZe{4?}w=Nl=wx)$k#))-kPRy*( zFYa0L`K-i?6Z0eEWAkVV1hkZh$E*%_c=<8=sd-3atgPI3G3GWT+=+F5%*5ezFQ+q{|8bEzY?Y&>n8u;YdXc!@8NWY*-1 zI3ZmGdt&Po+WLgJj8B~KmzaI&-%hwZTd%dUc~>q#lalV=sU{}4@he-I7=yy>Z7Izx@9UzVoBajcow0#z%2{ z;7@PXcr^#62PDHad)*+61i$5tFB9f*9rJdzOl6>>m+nd3%xW~ z8(kM~n51we$zV30H-7Gah~4r?6RidLHB6|AonX)rk-SQkc!lTC`U_E^YgR?O};_gb0DwaOJ4?YS&=!` zWdPFUac5g3kb1QZ=qPT(!jR^k>)r(gEnIair-)p=XhKWoFi)g!8Y9k;x3+??pp zrGrahbG*DIGLO7`7({bKC@Tiebr4>~?qKv6QAl!ci1L&UAUJeofN&pjRi-G=fim}$ zh3lw_uzH;KbdZRL$|eLTXJ#739x8UADv6|vqpz)EW5xC*l0rk(Q<1%uZ|5D1Ynizr ztJ5v!aJpzTwTv-3QsE49{QVrWrL=p86=Az&Sn=%NG{?`(ZSMTwBgq6KJyX#k1d#KYAW4UoKRn>@A&13Ipy4}!cCagIBFB#L8 zr4!?@G%3klv^uNEs*#)SVz%;p4vdjodh{}6or#0&!`+mx;k?=l%NB9pB=T*L@wInF zM5Q^=75oiNqfP=!M*fn8QdsABwQKmxvDH&ycDmpFm^h^9U zTTY&<+4Pu@-@>BuSrFBr)Xs;EAx!a0l~Xa`evhPHB`NZ-tx~R48m3?qTu5>S<=_DV zBqjXHf?q$Ae^Qea5q^zNCtCZIvY;Id49DasqaE0x}c-C(VLNm5RD#FK8NScA)~yN zTe-$4bJdRZlTr_c73fvvx7cRut1_mxs@}|6o}@{-hRV&G=y@bbRjQsh@aYp>(y{dW zM&-HL{HcL>geP)0=!kSSqkaJ`^aOIROdim^NY_B}u-eF%J!^GnxodcG-SpFtT$$fV z;y4&!Cu{kuX8s)0&$B$oTFLi!@s*e6PpU@VH&a`h>v}Qk`pDk~DQXthZ}+!^RJr*-bUnuGc^O4U<}4-Ry#|7KNl&Ae_pCHo7E!T@uSzf_PXs|d;Qv{*R$UBW|goR$l6iJC`+ZMZ*{xJ z{g}B`ZO?RWCyT}*58`AI5rRZPGl~!_ND=%9T7(ou0U;7a2n6{-Awf`t5)%?4B?uH2 z%y&*z_w>wq5kb(cx^?ckuXE2m_nybC&e&L5!|(H_zv6xDJ(~6{%8dRjWS+s}|1lDw z3B9Q;(c5V1En~^h2{xN%%UZG+)|-i?1lO~dY`m>zvgIr}T$X62TIr>9E3=eoWtXyy zvzueB+)^%XJH9lIHpynbHL*0oIHy@?O)gC`oN5+ZQ%h3}r<>EQnWY(BdqEQ!k-epf ztY>VROS2*`#zgLxzBK3U6XVwn`F?M1Q$rj53P0tEJD7>KSk^;pu_HXZQ!iG0PhP23 z+JtO*c$;D7XF9%jC8)GkJrU-w%1Ya>b!2ORNnE|z@B`GJTI*HiV7CX3)5LO?nmKiRah7YB%V}Dv>|>GvkcE4#3lvbfGO7AJJ;M(1mf!Su%wwtXtZW zB@)8MJ0X(7!P^#dA}hwgs-ys`L>`dirNo3N+|oAnr8MA4QDis+cuGt&oCQ21W*Hv) zh-PU^xhqrRT3*nB!pu`jXUTooN1CEyNn#I943R> z8CQ1vus9UtXCaKDTJa&fp|LE(d_2f%ce&Z{uX$3^T7@Zp)$@c4Y-m5*t<_LF71Z42 z2%H#p_ab(K*G6durRab&GKDTA9WddH$Kva`6&8*m%`(IDh_Rp^nV7<@n{I z-)eI?bOyOFACnp}73HDR@P{0>e6Lx14V&9Vvx@J#?rYkQ-+t;^r{$gMVw_W8#~Jsg ze`>kgXbR+LC2I}Yx>=Fl-s~PxIJvqZr$F~C9{(XES>4hN{3*5HuIrI!3Avs+A7%C+xnKVjdLw9x2$cg zZzz2wv10cj&MQg(Sl|eA6WdBy%5YP{M4DFmR)$*8ra&(S&O^IziDW+^oW3nm{iI0u zogllZ^;60bnd_GP74$ZyER_)1+s6Bhe!8DgX`$g%ePQiZS=FCB8Q&FKWmA+%F_FD7Eu*UsO+XLCNRQ=D5%s*!JQPq72}e54Y;B^)y^q9 zDpkohm(nKQno%h_K7!dTLrn5P!j$_@VC*7uV9-Ck(dTF@=H%3XlbC&$IWpZ(L|lmJ zoB^HRdrMAyeeLzpIp1p@qhsz+y3@P=B~Uy2lTnL4ldtOj0koX^AFMQ1cK6({F8i)q zuj=yCfab4jfX>E}@fK^28qd9|uO0orSa-Aq>c+J4^SCMrO zXA`vvQ_`zN;2knLoS)xhxj_jFysZxVR zDK*+97(=B_Evg&qx4&M93A?k8*7=KM2QoFgI4#@))&Ftjb0^2yGeFHCui?O#0nw6>2e3k!W5Kq<*YcCAbC)VNC{M zq7qcvXF{{G?1wfu(yn@P zf!Y}Br$b}oG^i!Hk)+C^GbSq&QmTnu_xPI0sT2v?dLuY(Y2P5qs1>@L8 zT2C7`TBR{!Gzw@M872K)XV$hxTjo%IKx^+f3)e0K_2gTaCA$n)(!)tStcQOU5{R=` z^Wo)?M*$OC3v|-9mti#bp`lj{7^pC{Sz{WkkqMKGF;+jJ4APhQ&ue{0nJN*(DY65gEtAl@Y0!{Rj!Hp0 zTjU+xh8Z2xHTEXDa&Mw5i0Qf^#@~{zTO)LR{C`DPeuS9+J{Ot9{P+ zC1VX{5bSZQ>D65p{jw1nCm#v*#-7~}rZ13LTkW(~Fa8bnN1ab=y|ecas)3102J6WS z%?>uEe-Xf-{T(7Iv^t{G3KKN)Bl0w17(UHsT>bXWYhQNjvCd_7RW08Q_Y{P6$!nkt!(H3+>3I=hC zQZ|UwXpy0|B0Z-;HIlZ6L2RJ_GAzLrU@yajez`E<3t`mstOh410W|l&oOljGdW5#8PM_aQ=p>UKd#DZ z`nrx0u3?0id4v;jU&m?1qIFOwh9gASXoM5)e&*rPm3eh^WxqOs_lcGL%*zKcBAdxP z_6ZuBMgSil!t*emlc4Mr=y{N(Wu3Y52-f=%&KlWa%%2RNS^=8v=yz1lheSLI;r@KSI8tPZRD)dT8C-RhrH=T;6u zElxq2r*DHpICIY9c@*n$j@RQjug7ERv6Xq44JEwyZ|g;^|9FhKZ-{w9omY=PruEOO zN7WN}KMK`*SUs+u5Ho6;G^m)pjnm>)W32_M4y*Gb0q6ZuO1E+DFeP(4l&Et&=c9OI zhW#fYSrG-0Fs9(C7;_)V9Ahr1C*wL##+avL%zb!AbuOx>)YCA8QZR}Z)CF}hhM!Rv zk$ZX`fg1G`jH*j;IW?D0^PO|Oyl?Ss>>b6fko8RcpWa3O*Y7RJ_d+4m%kTTtSN`SL z*B9!C$R|7a53!r3eP@AnGpp-Jk)A|#j?Y#B?D~=0@(dooh-46>6*^+GI^wcqgP6TD z_pl^(#rJkh39k8~SLd-8%Lxh=lEz1{FzvdnjzA0=aMpDZoo+@Y@?98OUZrHm&S!{> z{m>coSp%&;g`FIY^sbZu;p28XrKacmC5~76G7s$4p0oJVrK^{oeewBSW*1GC9x@Dm zNIgMlkaatf86^nz4)Q5pb@m5SJV4Y#459}c?y%Pu;kdurM1;7} zj-tqZI8|#j;eN+icX`A08$ARKyml9nRPuk#rq_nG3rjRKdyUo5B;w3gWnDfG^25Ue z|Il6bf}5V#c2_%aR}tOrNYQAc2VXu-^CSy)M511###W%jl*!2dO?h&G#kqb)rY zm7irNmYq)7qq-@K>+*~MR@P&%KwxmQgHhphyF^Twqna#r(PVM~$K}av=E)&GLUH=_ zUAcJTV^NuCRO;bm%fMy13e!=Bd547;G1?)Jk<*k8*Egz7(v)Ck@qQ0;k-yROY6wSo^>B=XL&I#2 zv$9dY78Q?2)prmnMA9(`HQm43@%;uZ(aypRnCOS;A<_@^l`tDEbQJP}XESI@9kDk9 z&w^HdBTUl^E(XFOVUpkmybfK2b}R^XECShh@gwmdKaT=3NL}nBes9pGs7EJKo4B6# zJ>2F9ram*A2*0}MY%kUjej};+V8?ogJ%W^Esa<5 zc;otmbI7Bd9-_EF{k&3$=J*PtGFBe-3ZR8zLDY-Id5R)A`aZpw&Ku~^V2R*d0%e5=F0b96Vh6OPS9lE?0Hm9*60M_wQuZ|n&U?kN$3!L z35AoQEV8|u3uF?t*endUpL-LBh3ZOSf zCd-Ph^uSo*3pQm8Arla;+9>pQ8-c&uxI}x8)_%a!g5-*`@wBe>5pL_vKweUi6@=g@ z)`Y7S11Tg0Rx_{Pd$jjr4f}}?;yxaMM22qaH#KFgWLC0t?Z)q1jjTgwkxf)sHp%GN z@e#10*e_Nkx*lM2A?tU#vWg%u-xXxy92ep2T8AErN|J=?bc0^;xlXq!IC?(deXG>7 zDAM0Eo-CD(#YM=`AsT3^+38&GuDZi|ZmrvFh6!11;yU!FsET!!V%U!8!qnhs^W_+l zb5vXyN&-H4@Sv@AvLhfIf?~t!G71I)Gz@x6io}I_5E|b4h&8h3 zyuE}<1(=imVF0+G#i>MbYI<;n{W$M9I4P~WI1^tn{-c=a?YoEg#4-M=i7o~|y4X(Q zVX^rd01_IZ!?B@TSiO%(Zl6Lci2xfvARL_TO$yXpqT^%*2LRkU6G4-MYxH3P=W#fY z;Ndd}Wfu`b!i{QT%SNQv!X2ms_d69NRSF>hpy$P?lGl%`T@Hpwo7Wz{IGiRxy+M2Z~>K;qQC579g|Cdg8kLI3*vY z2r8|(rozd36_e&S5=7U z;5#bh@No#|po6%~So$9{Eu(k- z9;_KS5kptzO|B}o9ven~N&|is38DeSqB8F03AIYe8YNSd9H!(5C1jzqrH8HJfg9krau=ajL>m1d=uCm$i_CPllWav=2FR z&IRp5=|*~=F$bx55RZ*#s-UxM?h0F&XiN{U^g93qtSE#|F}}b+#9F5>=wOlP_#DW< zbzt;mow1dm&whZh`K@my`NJVC%JUww_jt88vmIsy?$c#wgG;dtKSFhx<2KQaYe(+gHn=@+w11vf zKaYCkP^95d@C6VIfB6PCMsN$Yz~KN{T<|klX*NMl()W21?aA%-+K57s73rTOTbI}t z$t>F`WPB()Dk|8(0KgXeemzW|{qcsl?9 literal 0 HcmV?d00001 diff --git a/robot_firmware/robot_firmware/__pycache__/tf2_listener.cpython-310.pyc b/robot_firmware/robot_firmware/__pycache__/tf2_listener.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..ab7aa9a35e229ee61b7b8f8d92973cf1dc466783 GIT binary patch literal 3616 zcmcf@--{c^d1iOCTCG+;?oM|;$9Ale1}d7`#DxY-$(cHFpd_Z$2T2#oc26_XN!}mw z%&gBRq_l8`JQ?!Zx&hsz|AUr7DDxm!u23L(6Ni%2~*o|9BJ7_0s!CJB&tQ%Y{-bgw@ zXZihRu=yV09B-$5E!f?R z3s~KEavAASmI7RRKT<;8>qod@i7@M{ALKfZ;`s30NBuZgqle-)?DnGm{r6;+Pk?gs zgFM!w-F+z{F8I5TL_gP=?Ab;AXPFZBbd*d4FWS2@O4T5f$qG~1Js2sC3?Af>7BU4X z!hy_^P~@?YeH8WV&t7iS8vqCrP)-8t6EdKja_g}h*xcsqF$oy2a0k}PCxnr}xkq}{ z!rL9Z50{A)_|h{>5w5^M-s)E{X>vps!10(;qOFB}OlBlx099sW%x2`+g1s{%i>h|H z#qDDo_BA;3IGm2_+CKtAKS68*s5}m|#z;|9 z6CpTa3x7$WM~PvHOW2S1q=ST&6787<(Qg)xf!}-v{-s1bfVCFFNu*R5J|kb>vJ;k~!p}=_PQ}#xYqm zxj&;%32)3Sn47cOQ}UT(!l1fnafZ-`^7i%+jyguF-cX(@q zEa6RrFOXw8uK>l?3cI2_w%{!mZ^KL0o-)2Zb8!8P;oJ3vnR7(>2E6e1ygI9nx5EzK zm^q#0$t41~i{UG28-mW9&KTe5ke4YyJC_!hXVuepbPy`Fr!K?&n?~2GJ8%WQXTJjN zT;W^CbkRcV7%NkVut2Z0z^C-^>bJ7N-~T^0fLDg6R$wpd7_6}UEv)bb zSm7dCVd;mj@>kGCr#F7HO)#xqx;w3zVCzDjDOzQ^Fex=2rT2^0M2b*Hd$E8D6G4_* zgXkNU3G-xU2pdBF{L!rv0JvZo&yo{N1dwQ}N zWq)}=QD5%NQGGCa1SzxpsEVj8*VE4JERT6N&2(3zJ4)Z| zPVH+q2GiOb-6$Q#;%0X#`d@dJz_Xoc`{yF+?+f0Ar*?-~6w4Mq1VlW2_3(%0_m`ci zzamD$>E<1%5|H3|H_0B3(jiK)d#86*UPcZMGz5JS7nO+|L3P61CAaWkGtRR6`6N6Q zJ{;sR(8+#0IV_lYIon0mxO~}VNXT_WcJS-<)2aiVg_vLuQh2D^I2#V3nlMQ8DHO6; zhzWEfk=}=z@jz;*Tq|Y4;}DEZ15?mP1wAY(k&eb4szm+^_yP*F_Ax@zG@WL?3IP`S@`(X$jUtFG;T5W2mM2!@UWj_EO z7l+{)rF{oZr75^r)YoAs7bX|#yW40piP&>R7F`X;Z5ZmOFljQTGiZdLV2QDyj8bi5 z5BG#ZLbn!dX1YFrDwyhFb<9c^Ra2Nrxi?)LGd(l}zdFGl0dhn=&67}@CRp`c`6Ixh zLyfc%OjdF4Mx2K!G~VFJlTli@YCqRJJ4g$k3k8kep*ii@@*^bgl%dOo7JUql@>5qV zJM1&MLA79vjM4=_stuD5-s4%|S715U@~PZI8ZZb_fnr~WZSP_!r)MJe8cHTH=@iWp zaV~0$mJ+GTWC$*;uE8Yn8A<(8jWclW9KJp=~{le-m^{Vkc#CN~vmmsNBWrweyqFlKbP*58uYzt`LPD agnx_@7|f>~(3eZw7WuW;avL5*+ bool: + # return True if SUCCESS else False + status = self.navigator.spin(spin_dist=yaw) + return status - self.get_logger().info('Goal completed') def main(args=None): rclpy.init(args=args) - simple_action_server = SimpleActionServer() - rclpy.spin(simple_action_server) + main_node = MainNode() + + executor = MultiThreadedExecutor(num_threads=4) + executor.add_node(main_node) + + executor.spin() + executor.shutdown() + + rclpy.spin(main_node) rclpy.shutdown() if __name__ == '__main__': diff --git a/robot_firmware/robot_firmware/table_detection.py b/robot_firmware/robot_firmware/table_detection.py new file mode 100644 index 0000000..4b434a0 --- /dev/null +++ b/robot_firmware/robot_firmware/table_detection.py @@ -0,0 +1,660 @@ + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import LaserScan +import math, time +from statistics import mean + +from geometry_msgs.msg import PoseStamped, TransformStamped, Twist +from tf_transformations import quaternion_from_euler +import tf2_ros + +from rclpy.action import ActionServer + +from robot_firmware_interfaces.action import AproachTable # import the action message +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rclpy.executors import MultiThreadedExecutor + +# ros2 run robot_firmware table_detection --ros-args -p use_sim_time:=true + +# ros2 action send_goal /aproach_table robot_firmware_interfaces/action/AproachTable "dummy_aproach: true" + + +class TableDetectionNode(Node): + def __init__(self): + super().__init__('table_detection_node') + self.laser_scan_subscription = self.create_subscription( + LaserScan, + 'scan', + self.scan_callback, + 3 + ) + self.laser_scan_subscription # prevent unused variable warning + self.br = tf2_ros.TransformBroadcaster(self) + + # Create Twist publisher + self.speed_pub = self.create_publisher(Twist, 'diffbot_base_controller/cmd_vel_unstamped', 3) + + # Create tf2 listener + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) + + # Create action server for approaching table + self.action_server = ActionServer( + self, + AproachTable, + 'aproach_table', + self.execute_action_callback, + callback_group=MutuallyExclusiveCallbackGroup() + ) + + # Timer to publish tf to approach table + self.timer_tf = self.create_timer(0.1, self.timer_tf_clb, callback_group=MutuallyExclusiveCallbackGroup()) + + + + self.publish_table_tf = False # defult False + self.is_table = False + + + def execute_action_callback(self, goal_handle): + # Enable table tf publishing + self.publish_table_tf = True + + self.create_rate(1/2.0).sleep() # Sleep 2 sec + + # 0 - success, 1 - table not found, 2 - failed approach + result = AproachTable.Result() + + # self.publish_table_tf = False + # goal_handle.succeed() + # return result + + dummy_aproach = goal_handle.request.dummy_aproach + + if not self.is_table: + print("Table not found") + result.result = 1 + result.msg = "Table not found" + goal_handle.succeed() + self.publish_table_tf = False + return result + + # try to reach pre_table_link + target_frame = 'robot_base_link' + ref_frame = 'pre_table_frame' + scale_forward_speed = 0.15 + scale_rotation = 0.30 + + distance = 99.0 + # angle = 0.0 + + msg = Twist() + while(distance > 0.03): + x1, y1 = self.read_tf(target_frame=target_frame, ref_frame=ref_frame) + if x1 == None: + continue + distance = math.sqrt(x1**2 + y1**2) + angle = math.atan2(y1, x1) + + print(f'Dst: {distance:.2f}, angle: {angle:.2f}') + + if abs(angle) > 0.20: # 13 degree + msg.linear.x = 0.0 + msg.angular.z = 0.20 * angle / abs(angle) + else: + msg.linear.x = scale_forward_speed + msg.angular.z = scale_rotation * angle / abs(angle) + + self.speed_pub.publish(msg) + + time.sleep(0.1) + + print("Reached pre table") + # Stop robot + msg.linear.x = 0.0 + msg.angular.z = 0.0 + self.speed_pub.publish(msg) + #self.create_rate(1.0).sleep() + + + # Turn to "back_table_frame" + #distance = 99.0 + angle = 99.0 + + target_frame = 'robot_base_link' + ref_frame = 'back_table_frame' + + msg = Twist() + while(abs(angle) > 0.087): # 5 degrees + x1, y1 = self.read_tf(target_frame=target_frame, ref_frame=ref_frame) + if x1 == None: + continue + # distance = math.sqrt(x1**2 + y1**2) + angle = math.atan2(y1, x1) + + # print(f'Dst: {distance:.2f}, angle: {angle:.2f}') + + msg.angular.z = 0.20 * angle / abs(angle) + + self.speed_pub.publish(msg) + + time.sleep(0.1) + + print("Turned to back table link") + # Stop robot + msg.linear.x = 0.0 + msg.angular.z = 0.0 + self.speed_pub.publish(msg) + #self.create_rate(1.0).sleep() + + # CHeck if there is still table + if not self.is_table: + print("Table not found after in fro of table") + result.result = 1 + result.msg = "Table not found" + goal_handle.succeed() + self.publish_table_tf = False + return result + + # Table is found + if dummy_aproach: + print("Table on robot - dummy approach") + result.result = 0 + result.msg = "Table on robot - dummy approach" + goal_handle.succeed() + self.publish_table_tf = False + return result + + # Go to "back_table_frame" + distance = 99.0 + #angle = 99.0 + + target_frame = 'robot_base_link' + ref_frame = 'back_table_frame' + + msg = Twist() + while(distance > 0.4): + # Stop publish whgen uder, so no mistkaes + if distance < 0.8: + self.publish_table_tf = False + x1, y1 = self.read_tf(target_frame=target_frame, ref_frame=ref_frame) + if x1 == None: + continue + distance = math.sqrt(x1**2 + y1**2) + angle = math.atan2(y1, x1) + + print(f'Dst: {distance:.2f}, angle: {angle:.2f}') + + if abs(angle) > 0.20: # 13 degree + msg.linear.x = 0.0 + msg.angular.z = 0.20 * angle / abs(angle) + else: + msg.linear.x = scale_forward_speed + msg.angular.z = scale_rotation * angle / abs(angle) + + self.speed_pub.publish(msg) + + time.sleep(0.1) + + + print("Robot is under table") + + + # self.get_logger().error(f"Dummy approach is not implemented yet") + # Stop robot + msg.linear.x = 0.0 + msg.angular.z = 0.0 + self.speed_pub.publish(msg) + + result.result = 0 + result.msg = "Robot is under table" + goal_handle.succeed() + self.publish_table_tf = False + return result + + + def timer_tf_clb(self): + # NOTE - timer execution might be longer than callback interval + + if not self.is_table: + return + + # Is table + frame_name = [f"leg_{i}" for i in range(4)] + + # Get first leg pos + x1, y1 = self.read_tf(target_frame="odom", ref_frame="leg_0") #map + x2, y2 = self.read_tf(target_frame="odom", ref_frame="leg_1") #map + + if x1 == None or x2 == None: + return + + dy = y2 - y1 + dx = x2 - x1 + if dy == 0: + dy = 1e-6 + if dx == 0: + dx = 1e-6 + + x_mid = (x1 + x2)/2 + y_mid = (y1 + y2)/2 + slope = math.atan2(dy, dx) + self.publish_tf(x_mid, y_mid, theta=slope, parent_frame = "odom", frame_name="front_table_frame") #map + + # Get second leg pos + x1, y1 = self.read_tf(target_frame="odom", ref_frame="leg_2") #map + x2, y2 = self.read_tf(target_frame="odom", ref_frame="leg_3") #map + + if x1 == None or x2 == None: + return -1 + + dy = y2 - y1 + dx = x2 - x1 + if dy == 0: + dy = 1e-6 + if dx == 0: + dx = 1e-6 + + x_mid = (x1 + x2)/2 + y_mid = (y1 + y2)/2 + slope = math.atan2(dy, dx) + self.publish_tf(x_mid, y_mid, theta=slope, parent_frame = "odom", frame_name="back_table_frame") #map + + # Publish pre table frame + x1, y1 = self.read_tf(target_frame="front_table_frame", ref_frame="robot_base_link") + if x1 == None: + return + # Publish extra frame for robot before going under the table + d_y = 0.4 + y = -d_y if y1 <= 0 else d_y + + self.publish_tf(0.0, y, theta=0.0, parent_frame = "front_table_frame", frame_name="pre_table_frame") + + + + + def scan_callback(self, msg): + + if not self.publish_table_tf: + self.is_table = False + return + # Get the range values from the laser scan message + ranges = msg.ranges + + # Define the minimum and maximum distances for table legs + min_range = 0.1 # Minimum range value for table detection + max_range = 2.5 # Maximum range value for table detection + + max_two_point_dist = 0.07 # Maximum distance between 2 laser reading to be in 1 group + min_group_size = 4 + + table_leg_size = 0.22 + + table_x_size = 0.66 + table_y_size = 0.75 + table_diagnol = math.sqrt(table_x_size**2 + table_y_size**2) + table_size_tolerance = 0.1 + + #group = [] + prev_range = msg.ranges[0] + scans_too_big = 0 + + # Filter parameters + min_distance = 0.1 # Minimum distance to keep + max_distance = 2.5 # Maximum distance to keep + + # Create a new LaserScan message for filtered data + filtered_msg = LaserScan() + filtered_msg.header = msg.header + filtered_msg.angle_min = msg.angle_min + filtered_msg.angle_max = msg.angle_max + filtered_msg.angle_increment = msg.angle_increment + filtered_msg.time_increment = msg.time_increment + filtered_msg.scan_time = msg.scan_time + filtered_msg.range_min = msg.range_min + filtered_msg.range_max = msg.range_max + + # Filter the ranges based on distance + filtered_ranges = [] + for range in msg.ranges: + if min_distance <= range <= max_distance: + filtered_ranges.append(range) + else: + filtered_ranges.append(0.0) # Set invalid ranges to 0.0 + + # Update the filtered ranges in the new LaserScan message + filtered_msg.ranges = filtered_ranges + + + # Split in groups or objects + splitted_groups = [] + self.split_in_groups(filtered_msg, splitted_groups, max_two_point_dist) + + + # Remove too small groups/noise + filtered_groups_by_size = [] + self.filter_groups_by_size(splitted_groups, filtered_groups_by_size, min_group_size) + + + # Filter groups by shape + filtered_groups = [] + for i, group in enumerate(filtered_groups_by_size): + #print(list(zip(*group))) + if len(list(zip(*group))) == 0: + continue + min_dist = min(list(zip(*group))[0]) + max_dist = max(list(zip(*group))[0]) + delta_dist = abs(max_dist - min_dist) + start_angle = list(zip(*group))[1][0] + start_dist = list(zip(*group))[0][0] + end_angle = list(zip(*group))[1][-1] + end_dist = list(zip(*group))[0][-1] + dist_between_start_end = self.distance_between_polar_coordinates(start_dist, start_angle, end_dist, end_angle) + + #print(f"Delta {delta_dist:.2f} {dist_between_start_end:.2f}") + + if dist_between_start_end <= table_leg_size and \ + delta_dist <= table_leg_size: + filtered_groups.append(group) + + + table = False + class TableLeg: + distance = 0 + angle = 0 + + front_left_leg = TableLeg() + back_left_leg = TableLeg() + back_right_leg = TableLeg() + front_right_leg = TableLeg() + + if len(filtered_groups) < 4: + print("Too less groups") + + # Calculate distacne betwen groups + for i, group in enumerate(filtered_groups): + # Check if there are at least 3 legs left + if not len(filtered_groups) - i >= 3: + #print("NO TABLE") + break + if len(list(zip(*group))) == 0: + continue + first_leg = TableLeg() + first_leg.distance = mean(list(zip(*group))[0]) + first_leg.angle = mean(list(zip(*group))[1]) + possible_table_leg_distances = [] + table_legs = [] + table_legs.append(first_leg) + + for j, group_target in enumerate(filtered_groups): + if j <= i: + continue + possible_leg = TableLeg() + possible_leg.distance = mean(list(zip(*group_target))[0]) + possible_leg.angle = mean(list(zip(*group_target))[1]) + dist_between_start_end = self.distance_between_polar_coordinates( + first_leg.distance, + first_leg.angle, + possible_leg.distance, + possible_leg.angle) + possible_table_leg_distances.append(dist_between_start_end) + table_legs.append(possible_leg) + + + # Analyze distances + #print(dst) + #print([f"{i:.2f}" for i in possible_table_leg_distances]) + + #print(len(possible_table_leg_distances), len(table_legs)) + + leg_x = False + leg_y = False + leg_middle = False + leg_idx = [] + for i, d in enumerate(possible_table_leg_distances): + x_size = abs(d - table_x_size) + y_size = abs(d - table_y_size) + diagnol_size = abs(d - table_diagnol) + + #print("Table errors:") + #print(x_size, y_size, diagnol_size) + + if x_size < table_size_tolerance and \ + y_size < table_size_tolerance: + if x_size < y_size and not leg_x: + #print(f"leg_x 1: {x_size}") + leg_x = True + leg_idx.append(i) + elif not leg_y: + #print(f"leg_y 1: {y_size}") + leg_y = True + leg_idx.append(i) + elif x_size < table_size_tolerance and not leg_x: + #print(f"leg_x 2: {x_size}") + leg_x = True + leg_idx.append(i) + elif y_size < table_size_tolerance and not leg_y: + #print(f"leg_y 2: {y_size}") + leg_y = True + leg_idx.append(i) + if diagnol_size < table_size_tolerance and \ + (leg_x or leg_y): + #print("leg_mid") + leg_middle = True + leg_idx.append(i) + if leg_x and leg_y and leg_middle: + break + + if leg_x and leg_y and leg_middle: + table = True + #print([f"{d:.2f} " for d in possible_table_leg_distances]) + break + if table: + #self.get_logger().info("TABLE") + # info = f"{i}" for i in leg_idx + #self.get_logger().info("".join(map(str, leg_idx))) + front_left_leg = table_legs[0] + back_left_leg = table_legs[leg_idx[0]+1] + back_right_leg = table_legs[leg_idx[1]+1] + front_right_leg = table_legs[leg_idx[2]+1] + else: + self.get_logger().info("NO TABLE") + + if table: + legs = [front_left_leg, back_left_leg, back_right_leg, front_right_leg] + # Sort legs by length to robot + legs.sort(key=lambda x:x.distance) + #self.get_logger().info("__________") + legs_x = 0 + legs_y = 0 + for i, leg in enumerate(legs): + x = leg.distance * math.cos(leg.angle) + y = leg.distance * math.sin(leg.angle) + legs_x += x + legs_y +=y + self.publish_tf(x, y, frame_name=f"leg_{i}") + # self.get_logger().info(f"angle: {leg.angle:.2f}") + # self.get_logger().info(f"dist: {leg.distance:.2f}") + + + # mean_angle = mean([i.angle for i in legs]) + # mean_distance = mean([i.distance for i in legs]) + # x = mean_distance * math.cos(mean_angle) + # y = mean_distance * math.sin(mean_angle) + + self.publish_tf(legs_x/4, legs_y/4) + + legs.sort(key=lambda x:x.distance) + + first_leg = legs[0] + second_leg = legs[1] + first_leg_x = first_leg.distance * math.cos(first_leg.angle) + first_leg_y = first_leg.distance * math.sin(first_leg.angle) + second_leg_x = second_leg.distance * math.cos(second_leg.angle) + second_leg_y = second_leg.distance * math.sin(second_leg.angle) + + x_mid = (first_leg_x + second_leg_x)/2 + y_mid = (first_leg_y + second_leg_y)/2 + slope = second_leg_y - first_leg_y / second_leg_x - first_leg_x + #self.publish_tf(x_mid, y_mid, theta=slope, frame_name="pre_table_frame") + + self.is_table = True + + else: + self.is_table = False + + + + def publish_tf(self, x, y, theta=0, parent_frame = "robot_front_laser_base_link", frame_name = "table_frame"): + # TF broadcaster + transfor_msg = TransformStamped() + # transfor_msg.header.stamp = self.get_clock().now().to_msg() + transfor_msg.header.stamp = self.get_clock().now().to_msg() + # xx = int(transfor_msg.header.stamp.sec) + # self.get_logger().info(f"{transfor_msg.header.stamp}") + transfor_msg.header.frame_id = parent_frame + transfor_msg.child_frame_id = frame_name + + # Find middle point + transfor_msg.transform.translation.x = x + transfor_msg.transform.translation.y = y + transfor_msg.transform.translation.z = 0.0 + + # Rotation + q = quaternion_from_euler(0, 0, theta) + + transfor_msg.transform.rotation.x = q[0] + transfor_msg.transform.rotation.y = q[1] + transfor_msg.transform.rotation.z = q[2] + transfor_msg.transform.rotation.w = q[3] + + self.br.sendTransform(transfor_msg) + + + def read_tf(self, target_frame, ref_frame, fixed_frame="odom"): + try: + t = self.tf_buffer.lookup_transform_full( + target_frame=target_frame, + target_time=rclpy.time.Time(), + source_frame=ref_frame, + source_time=rclpy.time.Time(), + fixed_frame=fixed_frame, + timeout=rclpy.duration.Duration(seconds=0.1) + ) + except tf2_ros.TransformException as ex: + self.get_logger().warn( + f'Could not transform {target_frame} to {ref_frame}: {ex}') + return (None, None) + + return (t.transform.translation.x, t.transform.translation.y) + distance = math.sqrt( + t.transform.translation.x ** 2 + + t.transform.translation.y ** 2) + + angle = math.atan2( + t.transform.translation.y, + t.transform.translation.x) + + + @staticmethod + def split_in_groups(msg, groups, max_two_point_dist): + prev_range = msg.ranges[0] + prev_idx = msg.angle_min + + max_two_point_dist = max_two_point_dist + group = [] + + # Iterate through the laser scan readings + for i in range(1, len(msg.ranges)): + if msg.ranges[i] == 0.0: + continue + curr_range = msg.ranges[i] + curr_idx = msg.angle_min + msg.angle_increment * i + + # Check if the current reading is part of the same object + d = TableDetectionNode.distance_between_polar_coordinates( + prev_range, + prev_idx, + curr_range, + curr_idx + ) + if abs(d) < max_two_point_dist: + group.append((curr_range, curr_idx)) + else: + # Add the current group to the list of groups + if len(group) >= 0: + groups.append(group) + + # Start a new group with the current reading + group = [(curr_range, curr_idx)] + + # Update the previous range + prev_range = curr_range + prev_idx = curr_idx + + # Add the last group to the list of groups + if len(group) >= 0: + groups.append(group) + + @staticmethod + def filter_groups_by_size(in_groups, out_groups, min_group_size): + # remove too small group by thershold value + for i, group in enumerate(in_groups): + if len(group) >= min_group_size: + out_groups.append(group) + + @staticmethod + def merge_groups_by_size(in_groups, out_groups, max_two_point_dist): + for i, group in enumerate(in_groups): + if i == 0: + out_groups.append(group) + continue + start_angle = min(list(zip(*group))[1]) + start_dist = list(zip(*group))[0][0] + # Debug + #print(f"Idx {i}, group len: {len(group)}") + + end_angle = max(list(zip(*out_groups[-1]))[1]) + end_dist = list(zip(*out_groups[-1]))[0][-1] + dist_between_start_end = TableDetectionNode.distance_between_polar_coordinates(start_dist, start_angle, end_dist, end_angle) + + if dist_between_start_end <= max_two_point_dist+0.01: + #print("Append to prev") + + out_groups[-1].extend(group) + else: + out_groups.append(group) + + @staticmethod + def distance_between_polar_coordinates(r1, theta1, r2, theta2): + # Convert polar coordinates to Cartesian coordinates + x1 = r1 * math.cos(theta1) + y1 = r1 * math.sin(theta1) + x2 = r2 * math.cos(theta2) + y2 = r2 * math.sin(theta2) + + # Calculate the distance between the Cartesian coordinates + distance = math.sqrt((x2 - x1)**2 + (y2 - y1)**2) + + return distance + + + + + +def main(args=None): + rclpy.init(args=args) + table_detection_node = TableDetectionNode() + + executor = MultiThreadedExecutor(num_threads=4) + executor.add_node(table_detection_node) + + executor.spin() + executor.shutdown() + + table_detection_node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/robot_firmware/robot_firmware/tf2_listener copy 1.py b/robot_firmware/robot_firmware/tf2_listener copy 1.py new file mode 100644 index 0000000..eda41bb --- /dev/null +++ b/robot_firmware/robot_firmware/tf2_listener copy 1.py @@ -0,0 +1,146 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import LaserScan +import math +from statistics import mean +import time + +from geometry_msgs.msg import PoseStamped, TransformStamped, Twist +from tf_transformations import quaternion_from_euler +import tf2_ros + +class TfListener(Node): + def __init__(self): + super().__init__('tf_listener') + + # Create tf2 listener + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) + + # Create Twist publisher + self.speed_pub = self.create_publisher(Twist, 'diffbot_base_controller/cmd_vel_unstamped', 3) + + #self.timer = self.create_timer(1, self.timer_clb) + + def timer_clb(self): + #self.timer.cancel() + print("timer clb") + + # Try to find transform + target_frame = 'robot_base_link' + ref_frame = 'pre_table_frame' + + scale_forward_speed = 0.1 + scale_rotation = 0.8 + + distance = 99.0 + angle = 0.0 + + msg = Twist() + + #rclpy.spin_once(self.tf_listener) + + while(distance > 0.1): + try: + # t = self.tf_buffer.lookup_transform_full( + # #print(self.get_clock().now()) + # target_frame=target_frame, + # target_time=rclpy.duration.Duration(seconds=0.0), + # source_frame=ref_frame, + # source_time=rclpy.duration.Duration(seconds=0.0), + # fixed_frame="odom", + # timeout=rclpy.duration.Duration(seconds=0.1) + # ) + #clk = self.get_clock().now() + #clk.nanoseconds=0 + t = self.tf_buffer.lookup_transform( + #print(self.get_clock().now()) + target_frame=target_frame, + source_frame=ref_frame, + time=rclpy.time.Time(), + timeout=rclpy.duration.Duration(seconds=1.1) + ) + except tf2_ros.TransformException as ex: + self.get_logger().error( + f'Could not transform {target_frame} to {ref_frame}: {ex}') + time.sleep(0.1) + # TODO + continue + + #print(self.get_clock().now().to_msg().seconds) + print(t.transform.translation.x) + print(t.transform.translation.y) + + distance = math.sqrt( + t.transform.translation.x ** 2 + + t.transform.translation.y ** 2) + + angle = math.atan2( + t.transform.translation.y, + t.transform.translation.x) + + print(f'Dst: {distance:.2f}, angle: {angle:.2f}') + + if abs(angle) > 0.35: # 20 degree + msg.linear.x = 0.0 + msg.angular.z = 0.20 * angle / abs(angle) + else: + pass + #msg.linear.x = 0.25 + #msg.angular.z = scale_rotation * angle + + #self.speed_pub.publish(msg) + + time.sleep(0.1) + rclpy.spin_once(self.tf_listener) + + + + + print("Reached goal") + + self.get_logger().info(f'Finished moving to TF') + msg.linear.x = 0.0 + msg.angular.z = 0.0 + self.speed_pub.publish(msg) + self.create_rate(1.0).sleep() + + return + + + # Try to find transform + target_frame = 'robot_base_link' + ref_frame = 'table_frame' + try: + t = self.tf_buffer.lookup_transform( + target_frame, + ref_frame, + time=rclpy.time.Time()) + except tf2_ros.TransformException as ex: + self.get_logger().info( + f'Could not transform {target_frame} to {ref_frame}: {ex}') + + distance = math.sqrt( + t.transform.translation.x ** 2 + + t.transform.translation.y ** 2) + + angle = math.atan2( + t.transform.translation.y, + t.transform.translation.x) + + print(f'Dst: {distance}') + +def main(args=None): + rclpy.init(args=args) + table_detection_node = TfListener() + + rclpy.spin_once(table_detection_node) + + table_detection_node.timer_clb() + + #rclpy.spin(table_detection_node) + table_detection_node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/robot_firmware/robot_firmware/tf2_listener copy.py b/robot_firmware/robot_firmware/tf2_listener copy.py new file mode 100644 index 0000000..eda41bb --- /dev/null +++ b/robot_firmware/robot_firmware/tf2_listener copy.py @@ -0,0 +1,146 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import LaserScan +import math +from statistics import mean +import time + +from geometry_msgs.msg import PoseStamped, TransformStamped, Twist +from tf_transformations import quaternion_from_euler +import tf2_ros + +class TfListener(Node): + def __init__(self): + super().__init__('tf_listener') + + # Create tf2 listener + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) + + # Create Twist publisher + self.speed_pub = self.create_publisher(Twist, 'diffbot_base_controller/cmd_vel_unstamped', 3) + + #self.timer = self.create_timer(1, self.timer_clb) + + def timer_clb(self): + #self.timer.cancel() + print("timer clb") + + # Try to find transform + target_frame = 'robot_base_link' + ref_frame = 'pre_table_frame' + + scale_forward_speed = 0.1 + scale_rotation = 0.8 + + distance = 99.0 + angle = 0.0 + + msg = Twist() + + #rclpy.spin_once(self.tf_listener) + + while(distance > 0.1): + try: + # t = self.tf_buffer.lookup_transform_full( + # #print(self.get_clock().now()) + # target_frame=target_frame, + # target_time=rclpy.duration.Duration(seconds=0.0), + # source_frame=ref_frame, + # source_time=rclpy.duration.Duration(seconds=0.0), + # fixed_frame="odom", + # timeout=rclpy.duration.Duration(seconds=0.1) + # ) + #clk = self.get_clock().now() + #clk.nanoseconds=0 + t = self.tf_buffer.lookup_transform( + #print(self.get_clock().now()) + target_frame=target_frame, + source_frame=ref_frame, + time=rclpy.time.Time(), + timeout=rclpy.duration.Duration(seconds=1.1) + ) + except tf2_ros.TransformException as ex: + self.get_logger().error( + f'Could not transform {target_frame} to {ref_frame}: {ex}') + time.sleep(0.1) + # TODO + continue + + #print(self.get_clock().now().to_msg().seconds) + print(t.transform.translation.x) + print(t.transform.translation.y) + + distance = math.sqrt( + t.transform.translation.x ** 2 + + t.transform.translation.y ** 2) + + angle = math.atan2( + t.transform.translation.y, + t.transform.translation.x) + + print(f'Dst: {distance:.2f}, angle: {angle:.2f}') + + if abs(angle) > 0.35: # 20 degree + msg.linear.x = 0.0 + msg.angular.z = 0.20 * angle / abs(angle) + else: + pass + #msg.linear.x = 0.25 + #msg.angular.z = scale_rotation * angle + + #self.speed_pub.publish(msg) + + time.sleep(0.1) + rclpy.spin_once(self.tf_listener) + + + + + print("Reached goal") + + self.get_logger().info(f'Finished moving to TF') + msg.linear.x = 0.0 + msg.angular.z = 0.0 + self.speed_pub.publish(msg) + self.create_rate(1.0).sleep() + + return + + + # Try to find transform + target_frame = 'robot_base_link' + ref_frame = 'table_frame' + try: + t = self.tf_buffer.lookup_transform( + target_frame, + ref_frame, + time=rclpy.time.Time()) + except tf2_ros.TransformException as ex: + self.get_logger().info( + f'Could not transform {target_frame} to {ref_frame}: {ex}') + + distance = math.sqrt( + t.transform.translation.x ** 2 + + t.transform.translation.y ** 2) + + angle = math.atan2( + t.transform.translation.y, + t.transform.translation.x) + + print(f'Dst: {distance}') + +def main(args=None): + rclpy.init(args=args) + table_detection_node = TfListener() + + rclpy.spin_once(table_detection_node) + + table_detection_node.timer_clb() + + #rclpy.spin(table_detection_node) + table_detection_node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/robot_firmware/robot_firmware/tf2_listener.py b/robot_firmware/robot_firmware/tf2_listener.py new file mode 100644 index 0000000..e85a2eb --- /dev/null +++ b/robot_firmware/robot_firmware/tf2_listener.py @@ -0,0 +1,174 @@ +import rclpy +from rclpy.node import Node +from rclpy.duration import Duration +from sensor_msgs.msg import LaserScan +import math +from statistics import mean +import time + +# ros2 run robot_firmware tf2_listener --ros-args -p use_sim_time:=true + +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rclpy.executors import MultiThreadedExecutor + +from geometry_msgs.msg import PoseStamped, TransformStamped, Twist +from tf_transformations import quaternion_from_euler +import tf2_ros + +class TfListener(Node): + def __init__(self): + super().__init__('tf_listener') + + # Create tf2 listener + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) + + # Create Twist publisher + self.speed_pub = self.create_publisher(Twist, 'diffbot_base_controller/cmd_vel_unstamped', 3) + + self.timer = self.create_timer(1.0, self.timer_clb, callback_group=MutuallyExclusiveCallbackGroup()) + self.timer1 = self.create_timer(0.1, self.timer_clb1) + + def timer_clb1(self): + pass + + + def timer_clb(self): + self.timer.cancel() + print("timer clb") + + # Try to find transform + target_frame = 'robot_base_link' + ref_frame = 'pre_table_frame' + + # target_frame = 'robot_base_link' + # ref_frame = 'odom' + + scale_forward_speed = 0.1 + scale_rotation = 0.8 + + distance = 99.0 + angle = 0.0 + + msg = Twist() + + #rclpy.spin_once(self.tf_listener) + + while(distance > 0.1): + try: + t = self.tf_buffer.lookup_transform_full( + target_frame=target_frame, + target_time=rclpy.time.Time(), + source_frame=ref_frame, + source_time=rclpy.time.Time(), + fixed_frame='odom', + timeout=rclpy.duration.Duration(seconds=1) + ) + # t = self.tf_buffer.lookup_transform( + # target_frame=target_frame, + # source_frame=ref_frame, + # time=rclpy.time.Time(), # rclpy.time.Duration(seconds=5.0) + # #timeout=rclpy.duration.Duration(seconds=1.1) + # ) + except tf2_ros.TransformException as ex: + self.get_logger().warn( + f'Could not transform {target_frame} to {ref_frame}: {ex}') + time.sleep(0.1) + # TODO + continue + distance = math.sqrt( + t.transform.translation.x ** 2 + + t.transform.translation.y ** 2) + + angle = math.atan2( + t.transform.translation.y, + t.transform.translation.x) + + print(f'Dst: {distance:.2f}, angle: {angle:.2f}') + + if abs(angle) > 0.20: # 13 degree + msg.linear.x = 0.0 + msg.angular.z = 0.20 * angle / abs(angle) + else: + msg.linear.x = 0.1 + msg.angular.z = scale_rotation * 0.20 * angle / abs(angle) + + self.speed_pub.publish(msg) + + time.sleep(0.05) + + + + print("Reached pre goal") + msg.linear.x = 0.0 + msg.angular.z = 0.0 + self.speed_pub.publish(msg) + self.create_rate(1.0).sleep() + + distance = 99.0 + ref_frame = 'table_frame' + + while(distance > 0.1): + try: + t = self.tf_buffer.lookup_transform_full( + target_frame=target_frame, + target_time=rclpy.time.Time(), + source_frame=ref_frame, + source_time=rclpy.time.Time(), + fixed_frame='odom', + timeout=rclpy.duration.Duration(seconds=1) + ) + except tf2_ros.TransformException as ex: + self.get_logger().warn( + f'Could not transform {target_frame} to {ref_frame}: {ex}') + time.sleep(0.1) + # TODO + continue + distance = math.sqrt( + t.transform.translation.x ** 2 + + t.transform.translation.y ** 2) + + angle = math.atan2( + t.transform.translation.y, + t.transform.translation.x) + + print(f'Dst: {distance:.2f}, angle: {angle:.2f}') + + if abs(angle) > 0.20: # 13 degree + msg.linear.x = 0.0 + msg.angular.z = 0.10 * angle / abs(angle) + else: + msg.linear.x = 0.15 + msg.angular.z = scale_rotation * 0.20 * angle / abs(angle) + + self.speed_pub.publish(msg) + + time.sleep(0.05) + + print("Reached goal") + + + self.get_logger().info(f'Finished moving to TF') + msg.linear.x = 0.0 + msg.angular.z = 0.0 + self.speed_pub.publish(msg) + self.create_rate(1.0).sleep() + + return + + +def main(args=None): + rclpy.init(args=args) + table_detection_node = TfListener() + + executor = MultiThreadedExecutor(num_threads=4) + executor.add_node(table_detection_node) + + executor.spin() + executor.shutdown() + + table_detection_node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/robot_firmware/robot_firmware/tf2_listener_working.py b/robot_firmware/robot_firmware/tf2_listener_working.py new file mode 100644 index 0000000..7f2cb28 --- /dev/null +++ b/robot_firmware/robot_firmware/tf2_listener_working.py @@ -0,0 +1,42 @@ + +import rclpy +from rclpy.node import Node +import tf2_ros +from geometry_msgs.msg import TransformStamped + +class TransformListener(Node): + def __init__(self): + super().__init__('transform_listener') + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) + + def get_latest_transform(self, target_frame, source_frame): + try: + # Get the latest transform from the buffer + transform = self.tf_buffer.lookup_transform(target_frame, source_frame, rclpy.time.Time()) + return transform + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: + self.get_logger().warn(str(e)) + return None + +def main(args=None): + rclpy.init(args=args) + transform_listener = TransformListener() + + # Specify the target and source frames + target_frame = 'robot_base_link' + source_frame = 'pre_table_frame' + + while rclpy.ok(): + # Get the latest transform + latest_transform = transform_listener.get_latest_transform(target_frame, source_frame) + if latest_transform: + # Print the transform + print(latest_transform.transform.translation) + rclpy.spin_once(transform_listener) + + transform_listener.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/robot_firmware/setup.py b/robot_firmware/setup.py index 1152a11..d81dde2 100644 --- a/robot_firmware/setup.py +++ b/robot_firmware/setup.py @@ -1,4 +1,6 @@ from setuptools import setup +import os +from glob import glob package_name = 'robot_firmware' @@ -10,6 +12,7 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name), glob('launch/*.launch.py')), ], install_requires=['setuptools'], zip_safe=True, @@ -21,7 +24,9 @@ entry_points={ 'console_scripts': [ "main_node = robot_firmware.main_node:main", - "table_detection = robot_firmware.table_detection:main" + "table_detection = robot_firmware.table_detection:main", + "tf2_listener = robot_firmware.tf2_listener:main", + "action = robot_firmware.action:main", ], }, ) diff --git a/robot_firmware/src/table_detection.py b/robot_firmware/src/table_detection.py deleted file mode 100644 index de16d8f..0000000 --- a/robot_firmware/src/table_detection.py +++ /dev/null @@ -1,222 +0,0 @@ - -import rclpy -from rclpy.node import Node -from sensor_msgs.msg import LaserScan -import math -from statistics import mean - -class TableDetectionNode(Node): - def __init__(self): - super().__init__('table_detection_node') - self.laser_scan_subscription = self.create_subscription( - LaserScan, - 'scan', - self.scan_callback, - 5 - ) - self.laser_scan_subscription # prevent unused variable warning - - self.groups = [] - - @staticmethod - def distance_between_polar_coordinates(r1, theta1, r2, theta2): - # Convert polar coordinates to Cartesian coordinates - x1 = r1 * math.cos(theta1) - y1 = r1 * math.sin(theta1) - x2 = r2 * math.cos(theta2) - y2 = r2 * math.sin(theta2) - - # Calculate the distance between the Cartesian coordinates - distance = math.sqrt((x2 - x1)**2 + (y2 - y1)**2) - - return distance - - def scan_callback(self, msg): - # Get the range values from the laser scan message - ranges = msg.ranges - - # print(msg) - - # print("#"*10) - # print([f"{i:.2f}" for i in ranges]) - # return - - # Define the minimum and maximum distances for table legs - min_range = 0.2 # Minimum range value for table detection - max_range = 2.5 # Maximum range value for table detection - - max_two_point_dist = 0.03 # Maximum distance between 2 laser reading to be in 1 group - min_group_size = 3 - max_missing_reading = 1 - - table_leg_size = 0.20 - - table_x_size = 0.7 - table_y_size = 0.6 - table_diagnol = math.sqrt(0.7**2 + 0.6**2) - table_size_tolerance = 0.1 - - self.groups.clear() - - group = [] - prev_range = msg.ranges[0] - - scans_too_big = 0 - - # Iterate through the laser scan readings - for i in range(1, len(msg.ranges)): - curr_range = msg.ranges[i] - curr_idx = msg.angle_min + msg.angle_increment * i - - # Check if the current reading is part of the same object - if abs(curr_range - prev_range) < max_two_point_dist: - group.append((curr_range, curr_idx)) - else: - # Add the current group to the list of groups - #if len(group) >= min_group_size: - self.groups.append(group) - - # Start a new group with the current reading - group = [(curr_range, curr_idx)] - - # Update the previous range - prev_range = curr_range - - # Add the last group to the list of groups - if len(group) >= 0: - self.groups.append(group) - - # Print the groups - #print(f"Total {len(self.groups)} groups detected") - - for i, group in enumerate(self.groups): - if len(list(zip(*group))) == 0: - continue - #self.get_logger().info(f'Group {i+1}: {len(group)}, start idx: {group[0][1]*180/3.14:.2f}, end idx: {group[-1][1]*180/3.14:.2f},dist: {mean(list(zip(*group))[0])}') - - # remove too small group - big_groups = [] - for i, group in enumerate(self.groups): - if len(group) >= min_group_size: - big_groups.append(group) - - #print(f"Total {len(big_groups)} groups detected after removing small ones") - - # Merge close groups - merge_groups = [] - for i, group in enumerate(big_groups): - #print(list(zip(*group))) - if i == 0: - merge_groups.append(group) - continue - start_angle = min(list(zip(*group))[1]) - start_dist = list(zip(*group))[0][0] - end_angle = max(list(zip(*merge_groups[-1]))[1]) - end_dist = list(zip(*merge_groups[-1]))[0][-1] - dist_between_start_end = self.distance_between_polar_coordinates(start_dist, start_angle, end_dist, end_angle) - - if dist_between_start_end <= max_two_point_dist+0.01: - #print("Append to prev") - - merge_groups[-1].extend(group) - else: - merge_groups.append(group) - - #print(f"Total {len(merge_groups)} groups detected after merging") - - filtered_groups = [] - for i, group in enumerate(merge_groups): - #print(list(zip(*group))) - if len(list(zip(*group))) == 0: - continue - min_dist = min(list(zip(*group))[0]) - max_dist = max(list(zip(*group))[0]) - delta_dist = abs(max_dist - min_dist) - start_angle = min(list(zip(*group))[1]) - start_dist = list(zip(*group))[0][0] - end_angle = max(list(zip(*group))[1]) - end_dist = list(zip(*group))[0][-1] - dist_between_start_end = self.distance_between_polar_coordinates(start_dist, start_angle, end_dist, end_angle) - - if dist_between_start_end <= table_leg_size and \ - len(group) >= min_group_size and \ - delta_dist <= table_leg_size and \ - min_dist >= min_range and \ - max_dist <= max_range: - filtered_groups.append(group) - - #print(f"Total {len(filtered_groups)} groups detected after filtering v0") - - - # for i, group in enumerate(filtered_groups): - # self.get_logger().info(f'Group {i+1}: {len(group)}, start idx: {group[0][1]*180/3.14:.2f}, end idx: {group[-1][1]*180/3.14:.2f},dist: {mean(list(zip(*group))[0])}') - - table = False - # Calculate distacne betwen groups - for i, group in enumerate(filtered_groups): - # Check if there are at least 3 legs left - if not len(filtered_groups) - i >= 3: - #print("NO TABLE") - break - if len(list(zip(*group))) == 0: - continue - current_group_dist = mean(list(zip(*group))[0]) - current_group_angle = mean(list(zip(*group))[1]) - dst = [] - - for j, group_target in enumerate(filtered_groups): - if j <= i: - continue - target_group_dist = mean(list(zip(*group_target))[0]) - target_group_angle = mean(list(zip(*group_target))[1]) - dist_between_start_end = self.distance_between_polar_coordinates(current_group_dist, current_group_angle, target_group_dist, target_group_angle) - dst.append(dist_between_start_end) - - # Analyze distances - #print(dst) - #print([f"{i:.2f}" for i in dst]) - - leg_x = False - leg_y = False - leg_middle = False - for d in dst: - x_size = abs(d - table_x_size) - y_size = abs(d - table_y_size) - diagnol_size = abs(d - table_diagnol) - if x_size < table_size_tolerance and \ - y_size < table_size_tolerance: - if x_size < y_size: - #print("leg_x") - leg_x = True - else: - #print("leg_y") - leg_y = True - elif x_size < table_size_tolerance: - leg_x = True - elif y_size < table_size_tolerance: - leg_y = True - if diagnol_size < table_size_tolerance and \ - (leg_x or leg_y): - #print("leg_mid") - leg_middle = True - - if leg_x and leg_y and leg_middle: - table = True - if table: - self.get_logger().info("TABLE") - else: - self.get_logger().info("NO TABLE") - - - - - -def main(args=None): - rclpy.init(args=args) - table_detection_node = TableDetectionNode() - rclpy.spin(table_detection_node) - table_detection_node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() From d7ed0750b96042061ce79bbebdd3f34704acab51 Mon Sep 17 00:00:00 2001 From: Hercogs Date: Tue, 6 Feb 2024 13:32:10 +0000 Subject: [PATCH 2/4] fix: changed robot firmware action names --- robot_firmware_interfaces/CMakeLists.txt | 2 +- robot_firmware_interfaces/action/AproachTable.action | 6 ++++++ robot_firmware_interfaces/action/Empty.action | 6 ------ 3 files changed, 7 insertions(+), 7 deletions(-) create mode 100644 robot_firmware_interfaces/action/AproachTable.action delete mode 100644 robot_firmware_interfaces/action/Empty.action diff --git a/robot_firmware_interfaces/CMakeLists.txt b/robot_firmware_interfaces/CMakeLists.txt index ec6e574..d475e80 100644 --- a/robot_firmware_interfaces/CMakeLists.txt +++ b/robot_firmware_interfaces/CMakeLists.txt @@ -14,7 +14,7 @@ find_package(rosidl_default_generators REQUIRED) find_package(action_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} - "action/Empty.action" + "action/AproachTable.action" ) diff --git a/robot_firmware_interfaces/action/AproachTable.action b/robot_firmware_interfaces/action/AproachTable.action new file mode 100644 index 0000000..c91c22b --- /dev/null +++ b/robot_firmware_interfaces/action/AproachTable.action @@ -0,0 +1,6 @@ +bool dummy_aproach # If true, then only check table existence (for simulation) +--- +int32 result # 0 - success, 1 - table not found, 2 - failed approach +string msg +--- +string state \ No newline at end of file diff --git a/robot_firmware_interfaces/action/Empty.action b/robot_firmware_interfaces/action/Empty.action deleted file mode 100644 index 14d9d49..0000000 --- a/robot_firmware_interfaces/action/Empty.action +++ /dev/null @@ -1,6 +0,0 @@ -#int32 order ---- -bool result -string msg ---- -#int32[] partial_sequence \ No newline at end of file From df34e2579db1d7ecef5f023c4361444424eb359f Mon Sep 17 00:00:00 2001 From: Hercogs Date: Tue, 6 Feb 2024 13:32:50 +0000 Subject: [PATCH 3/4] fix: updated localization and path planer parameters --- cartographer_slam/maps/sim_map.pgm | Bin 73569 -> 73615 bytes cartographer_slam/maps/sim_map_orig.pgm | Bin 0 -> 73569 bytes localization_server/config/amcl_config.yaml | 9 +++++---- path_planner_server/config/controller.yaml | 2 +- 4 files changed, 6 insertions(+), 5 deletions(-) create mode 100644 cartographer_slam/maps/sim_map_orig.pgm diff --git a/cartographer_slam/maps/sim_map.pgm b/cartographer_slam/maps/sim_map.pgm index 29a55257e9de2ba7e8eed8b7dcf12df452f47911..0eb678dae2ee29f520a3d2b81bb2385c87818547 100644 GIT binary patch delta 2145 zcmd6nOH30{6owsAsC_zR3PnK}gaieFK4_H$3W*DA3_9YX2~jMBiXblwC1QlKS490y z0F4n`s8Nj6SsA0!wL2HaM2)x-S4v`ZVQ9T=A?=hqz=DO7G@U!&fBtjMeSLkc7=NRv z73}IF{*cexGT?3FTZj0=M;Ziv(Ch2(=}W+d^1!{L{Ui;F1K%-GEidpGOE9PZn3af2dW%`CDC8`%;4wN8C^bN+J%t6%y=mdP zIVF}`p%gvFG)CTtU=5>9>OHd3{9|}bEpnhSHz|TFPB>|pY>=u;maXcXn9hBbCm|_= zm5MegT;PRr$lyxWnQm0=4J)qKnULuGpYuXKJ4r`sTu zbct-yEZN#cPc%u6SSt2U@G2(v<{lGz%WA-~CN%BcNq#!2j_gh&viDu4!nlbOyg`ra zrRD2rhSp(DQf)du@3%|aSSs!(dwX|0M+V-O`?i#e_sj*UJI>ONpN&SGuue+Kov{o= zzfaV0jD{>xCOl^qxN94f))Q6D*Kl}HHAw?Lm>maoNlOu3{#Zziq=xv3R}4rh30=F0 zdHW;Ew}(=LxUdh(ngew3w2TTapxNo5Mf89z&A`<<+giAw)nmjpB@5Ox?I-sX8x5h~ zRiWt-M2=3$T^5$^SU6E+OJ;L*5Ju?AIrMSqV0GIu{aqQ&WLl+?|L`_gT02Q+28CVn%x^%ZzK@S zZWFca$bUj;Q;kaZ2m&oa;_};W%-wIXM3h DR+!R9 delta 1125 zcmb_bUuaTs6uwuk6Ls!x&N=Nm6(1@aq02^WOcOTHm(_wO7BPkBp>5iQA{II3dW&h^ z^*vbjVvj2~6zQc0H=?%?Wbc7{ir#vNUIx;+*FQN}QPl6@-tYU)_dECe&hOmJE_YDj zRCD7c?^`XT7^h+6Rc&NnnOFxTjT&=0E%L$;f+Zu88cj>v#h#thGmZ9QvIi#CibA?? z^O8mJiu!vfLO{|Spu`4DE?KdiHXz>n9ZzGt!X@Dpq!kO|emCldX-I;N{4<1|gnt(c zz}f$Sd~Wjn8J@K)BO{D-Tw68j(AH#I@| z9(VFB_>b}@fumdu_2At_pR(ucv=KYu=Sn@l##=4#mfX|kl;^Kph4`6A@mh|=A(p{< zXc1!QHR@V5U1Tp;tul?WCp^ree~^A5hn~iTsdf~;@pCHV5rHSXhWbJQmBbPasS)+t zJIKVExb+p9FS>HlHzKQ@cX#YxaWciBf01gZQZaUU7129kNY3j#ou$3xQ*>g<%>1)k hI<|_~VY*Q~xsSW4X9piMtA3 zRZrwqM3p9+Y?wJodKFRObH+Nas>d_<)8Df6s(Svpil{1cR(-CDL?zFv)a#W+ugV7~ zQROt;bmOS=8?z9y3$o#c26M-8BdoK{YhT|9mkTsM^S?#oP+jz^F=MG4oca7HPN!MCIbi zl|+@(aMMKP;>ndnmD6z3MCIbim9ePI0NFYR9M0~H06K38oVK>k|%ITUD7q=L%xppx}?j7EK-DkBo7E3wAps0CT%w%61)G<(B!Oozw zscDxQVWFzMYg1Vtx$aUWKGRoFeO3^7mxT(v{YW#Sg6R_#QNaXYLwePTQOSFA3Ku?G zNW!h#)G*$wbcoS*T(xOel!cAqYoCZ*!R4;APFd-FH-wLXxyo#t`k2@AsltaTfJVT=L=qMdgDIA_aR306rBPxZ%6Nt*A zqjW^2a7=<)%y+_(sH6fXjzPPPmlryr^pF$R?ZZGn75@$OL9Do+-)=}NTtDOA{IhJv>T6G6hmYXl;XVHS#64fHG!5|Km zJSjUa?UE@5Zs&?aC1D9sVU9sm7$>pf06YPzpg$vuo2zDDgh?y)ZKib{|G=I1reP#1 zPU~Rj?C4cs4OW^(dQ}-KY&HN_G5~LnLp2Yph|1`s1ynIpvr#uF`(Uq{>PS=>i`ffM zx@l7Hm~x)tZ^GzM<}_ZlBPzdW%|qF06;UZToHy4t!Ik{l5k@^Z= zMO2_O(5paD=_He01;QdK5Y#EXDy>VW3vS6ihQl$Fd>31OK470`l%?WtK#eM>dS!KA za$c7xRPxNBgT_y5p)eXq!m=@%wt4pC-tzzI#dKTys8R=R-V?O~8%9-Y&YpR1pvuSJ z83y2Jd<0sg(`!%D*5UGi6!OA|Afu+k3&!? zU%`b{bk^Q1W8sJah||%Qesi_$T=MURw4T%Lp_A#ktt$=9G@86b?@yDvyrR5tYK>2}I@5Q97bhI6Q%4-|<@PrmoNu9kW?WH3sm~5M<;O1>zv3x!$nVv#p-h`0siU%Oy04YC3%zSS>eny+w<$!bIwoia8b}D%jnw zfhuHnC#qX{m2@}U-W7&d@G7F3z38kAqO!=$@`-AO#L6Hli_9#asAfp445G5g%<^N_ zHEn*sv4BNX1rS>#Dw|EAA*uq1EfST@rqB>o0mK%G%4Sn&h^hc$i$rC!DKs&t8b7e< znGsdtJGMwvHk(32R0R-QBr2Otp&_aQh%LsTl3lU(FQS6`kf?|XCIB1qa2Z7vGyb@> zj(>;Y3U5Gq6*~T+ejJ8g1&=_YA}W{wY)G#nDwqIlNUypVs*o?uUL#s3Dl`U-SuM-Y zZ?}ID72JnJMN}{W*pOaDR4@V9FvhE{@$HU~xau0HLVox83QNGCT~MOB)oPhu1-}4^ zil}e`uw~3SAg6Us>o5V>kX}VpFag+*UPV+e0oagUMN}{W*pOaDR4@V9kX}VpFag+* zUPV+e0od>gUs;IR+PlPo4N<{6dqhQ4I04u)gbg{@a84XXRD;KCo+`QYh-Q>|8-GD*J-!7%#E0ph&sG$fQe5h;+UsxkcYv XIU{y!8f-iXkN^pg011%5`vm?6K|HPn literal 0 HcmV?d00001 diff --git a/localization_server/config/amcl_config.yaml b/localization_server/config/amcl_config.yaml index 09f0e04..27208e1 100644 --- a/localization_server/config/amcl_config.yaml +++ b/localization_server/config/amcl_config.yaml @@ -6,7 +6,7 @@ amcl: alpha3: 0.2 alpha4: 0.2 alpha5: 0.2 - base_frame_id: "robot_base_footprint" + base_frame_id: "robot_base_link" beam_skip_distance: 0.5 beam_skip_error_threshold: 0.9 beam_skip_threshold: 0.3 @@ -15,7 +15,7 @@ amcl: lambda_short: 0.1 laser_likelihood_max_dist: 2.0 laser_max_range: 100.0 - laser_min_range: -1.0 + laser_min_range: 0.2 laser_model_type: "likelihood_field" max_beams: 60 max_particles: 8000 @@ -32,12 +32,13 @@ amcl: tf_broadcast: true transform_tolerance: 1.0 update_min_a: 0.2 - update_min_d: 0.25 + update_min_d: 0.2 z_hit: 0.5 z_max: 0.05 z_rand: 0.5 z_short: 0.05 - set_initial_pose: true + scan_topic: scan + set_initial_pose: false initial_pose: x: 0.0 y: 0.0 diff --git a/path_planner_server/config/controller.yaml b/path_planner_server/config/controller.yaml index 36a40c6..472c904 100644 --- a/path_planner_server/config/controller.yaml +++ b/path_planner_server/config/controller.yaml @@ -30,7 +30,7 @@ controller_server: min_vel_y: 0.0 max_vel_x: 0.20 max_vel_y: 0.0 - max_vel_theta: 0.5 + max_vel_theta: 0.3 min_speed_xy: 0.0 max_speed_xy: 0.25 min_speed_theta: 0.0 From f8edd91521d113324405d027927e90ec75e2419f Mon Sep 17 00:00:00 2001 From: Hercogs Date: Sun, 11 Feb 2024 11:18:38 +0000 Subject: [PATCH 4/4] feat: added gitignore --- .gitignore | 1 + .../__pycache__/__init__.cpython-310.pyc | Bin 159 -> 0 bytes .../__pycache__/action.cpython-310.pyc | Bin 1631 -> 0 bytes .../__pycache__/main_node.cpython-310.pyc | Bin 4636 -> 0 bytes .../__pycache__/table_detection.cpython-310.pyc | Bin 11282 -> 0 bytes .../__pycache__/tf2_listener.cpython-310.pyc | Bin 3616 -> 0 bytes .../robot_firmware/table_detection.py | 7 ++++--- 7 files changed, 5 insertions(+), 3 deletions(-) create mode 100644 .gitignore delete mode 100644 robot_firmware/robot_firmware/__pycache__/__init__.cpython-310.pyc delete mode 100644 robot_firmware/robot_firmware/__pycache__/action.cpython-310.pyc delete mode 100644 robot_firmware/robot_firmware/__pycache__/main_node.cpython-310.pyc delete mode 100644 robot_firmware/robot_firmware/__pycache__/table_detection.cpython-310.pyc delete mode 100644 robot_firmware/robot_firmware/__pycache__/tf2_listener.cpython-310.pyc diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c18dd8d --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +__pycache__/ diff --git a/robot_firmware/robot_firmware/__pycache__/__init__.cpython-310.pyc b/robot_firmware/robot_firmware/__pycache__/__init__.cpython-310.pyc deleted file mode 100644 index ed51346a20b258073c8093e7743fb73e428682f2..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 159 zcmd1j<>g`kg8fPdQ$h4&5P=LBfgA@QE@lA|DGb33nv8xc8Hzx{2;!HMenx(7s(xv4 zYLR|Xez8$}d9i*{X=Y9eke8HS5}%e?lv|!yl#0%ckI&4@EQycTE2zB1VUwGmQks)$ N2Qs^u2}rOo007A_CYk^M diff --git a/robot_firmware/robot_firmware/__pycache__/action.cpython-310.pyc b/robot_firmware/robot_firmware/__pycache__/action.cpython-310.pyc deleted file mode 100644 index 6ecf5c3d8c3168bf978740473d61d48e885eeddc..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1631 zcmZux&2HQ_5GE-~tCh7ICu!QEMGF@#&@KY3f%eivQKSe0_t5xI=cEfEi`-o~l(ZqK zICj^kHIP%@VSUU~^a=U`y!PZ*Xb+I7}xDR4N$-H086I5;+(A$tzK` zrcs4^XuAb#Lw^N|KpEyJBd^f}bIi$QoCQ4K^b%$CHKHgBkI*Qz_m0YHF2%O11>U;U z={@M~Zv71igO+FoH(p_k3|Sx(aF8d|giC<-O}IcT;t1}hhqhnkWzA{`+Ya(BXlB+? z4~un>Z0e)z*-*E0p=``pS(k<}Mc_H*GHQ2)k6EzIAr-pmAka)#z{8sops=5j zChL!4iqpT(Km9L;<4q8S3u4n8h^{||u73uxKr3{01Lp#t5cSaluYxP6sGBw7pmvZN zx(ct*1>wPhun6|`{66BrKEK=5MOjbxrcEx#yCyl*dYCzWQVoJPdg*JlQcyq2xLG-K8P=z*9-PqG@Q3%0Dfw}|t+8y^Oo8>i^ zLVW@=>fVjG@3aAkkD+TfKg3ZK<00S|_aht=iqF6N9~9qD`zUhdQRkIl%qEOg4R58> zQ^sBZhq@J?F8`h#tm^id_fjuMHikRph3nUmt+;W(V$h>t$^ld)>HwK(_BsUwJUcpcIvib z#uHwTfEuch^D~#N(s<{TbJoT==N-MMo|mpG8cLK4VM3}cGbL;ohA?pV!6_;55-?pI?|@7}oMi>Iwkx+%wf<-oIh zx48x!H$qMFLg>vo&-4e~xed>|pM&n`ey3a8c&|MO9URUz^_dgzQtUM!SNua)C_g#| W=np}}5D$D_?gq)%@gTwUpML?lkfL7z diff --git a/robot_firmware/robot_firmware/__pycache__/main_node.cpython-310.pyc b/robot_firmware/robot_firmware/__pycache__/main_node.cpython-310.pyc deleted file mode 100644 index c44a8f35c9484e791e6d584cbb1c148e662d548c..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 4636 zcmbVP&5t8T74Pb9+wJz}e1_eSO^0O_3}lww2oi0gB%9gUC6FCLW+EgDsnz2ud)m|O zcB$F7To9b#knnrewr6JfIAE(^y{dZk z_1>@EdzI8`u7TgLKKx8PylfbMqRRQ7jmjJN(m#T5gR{tJQ)xylHrpm6-ioZ)Zrhr- zqe5J47d2miOsQScd@*w3a=WbgQsl;!c180}RE=xxn&!(+qihsJ(ZR;UPBcox2VxzyouIRKPbQ-wBv$T2GTh#kBH)5=JQSS~rlF4a zM?og!0DJViGKqaLiiA|f-6R(Njh5Y`BO1mtZ%i5cx1T@zi@*H%H&_1g+2?teVW?6@ zXE-PRhtGO1>fhgAmraPt21sWnKP#;%ll}mVFxre8ZSw=8%Q)laBd2X~i`$Qkw#^H? zh_b*-+(B97W$vOZ@rrP|Ca?0^BfDMxz_5+ByJfWM%DEqe1F}Jf%@#vRd;uS=n+`xS z!vG&cw*d9Qc5gi}ngHfL62G zk->|PskP2appE$q4$Z&~?5n(~$}u+P53n<3jIaJA2(xg|^SX)j2DxnsEMAs)X%@(A zZEa1}f*dBk*6~#5e|;UMt9PbG_IqUIyW<5qwMUcZ!-1owDx{+!4C?rPI0!S}mt`DW zm8Z)AN(C&H3FY=g=0{1d2VpxLbQ86-kL|rT$ifJ?yGaNKxJiGqNK?3h#=PH&LRIYm zSefv%Fvjq*F3_rf>Tat>X`zcdpAF@H3`u?ZJKi97l^sbLmA+O6M zz3K0#*LOx?#8KNxGQS(jct4Qh$?9COYr_M13Cmu?mo9=-nPV1N-E>*qTD6L7v5;$u z@Ub>i%LSm&AxrqyX^K|?8<-eCApmd&+OZe{4?}w=Nl=wx)$k#))-kPRy*( zFYa0L`K-i?6Z0eEWAkVV1hkZh$E*%_c=<8=sd-3atgPI3G3GWT+=+F5%*5ezFQ+q{|8bEzY?Y&>n8u;YdXc!@8NWY*-1 zI3ZmGdt&Po+WLgJj8B~KmzaI&-%hwZTd%dUc~>q#lalV=sU{}4@he-I7=yy>Z7Izx@9UzVoBajcow0#z%2{ z;7@PXcr^#62PDHad)*+61i$5tFB9f*9rJdzOl6>>m+nd3%xW~ z8(kM~n51we$zV30H-7Gah~4r?6RidLHB6|AonX)rk-SQkc!lTC`U_E^YgR?O};_gb0DwaOJ4?YS&=!` zWdPFUac5g3kb1QZ=qPT(!jR^k>)r(gEnIair-)p=XhKWoFi)g!8Y9k;x3+??pp zrGrahbG*DIGLO7`7({bKC@Tiebr4>~?qKv6QAl!ci1L&UAUJeofN&pjRi-G=fim}$ zh3lw_uzH;KbdZRL$|eLTXJ#739x8UADv6|vqpz)EW5xC*l0rk(Q<1%uZ|5D1Ynizr ztJ5v!aJpzTwTv-3QsE49{QVrWrL=p86=Az&Sn=%NG{?`(ZSMTwBgq6KJyX#k1d#KYAW4UoKRn>@A&13Ipy4}!cCagIBFB#L8 zr4!?@G%3klv^uNEs*#)SVz%;p4vdjodh{}6or#0&!`+mx;k?=l%NB9pB=T*L@wInF zM5Q^=75oiNqfP=!M*fn8QdsABwQKmxvDH&ycDmpFm^h^9U zTTY&<+4Pu@-@>BuSrFBr)Xs;EAx!a0l~Xa`evhPHB`NZ-tx~R48m3?qTu5>S<=_DV zBqjXHf?q$Ae^Qea5q^zNCtCZIvY;Id49DasqaE0x}c-C(VLNm5RD#FK8NScA)~yN zTe-$4bJdRZlTr_c73fvvx7cRut1_mxs@}|6o}@{-hRV&G=y@bbRjQsh@aYp>(y{dW zM&-HL{HcL>geP)0=!kSSqkaJ`^aOIROdim^NY_B}u-eF%J!^GnxodcG-SpFtT$$fV z;y4&!Cu{kuX8s)0&$B$oTFLi!@s*e6PpU@VH&a`h>v}Qk`pDk~DQXthZ}+!^RJr*-bUnuGc^O4U<}4-Ry#|7KNl&Ae_pCHo7E!T@uSzf_PXs|d;Qv{*R$UBW|goR$l6iJC`+ZMZ*{xJ z{g}B`ZO?RWCyT}*58`AI5rRZPGl~!_ND=%9T7(ou0U;7a2n6{-Awf`t5)%?4B?uH2 z%y&*z_w>wq5kb(cx^?ckuXE2m_nybC&e&L5!|(H_zv6xDJ(~6{%8dRjWS+s}|1lDw z3B9Q;(c5V1En~^h2{xN%%UZG+)|-i?1lO~dY`m>zvgIr}T$X62TIr>9E3=eoWtXyy zvzueB+)^%XJH9lIHpynbHL*0oIHy@?O)gC`oN5+ZQ%h3}r<>EQnWY(BdqEQ!k-epf ztY>VROS2*`#zgLxzBK3U6XVwn`F?M1Q$rj53P0tEJD7>KSk^;pu_HXZQ!iG0PhP23 z+JtO*c$;D7XF9%jC8)GkJrU-w%1Ya>b!2ORNnE|z@B`GJTI*HiV7CX3)5LO?nmKiRah7YB%V}Dv>|>GvkcE4#3lvbfGO7AJJ;M(1mf!Su%wwtXtZW zB@)8MJ0X(7!P^#dA}hwgs-ys`L>`dirNo3N+|oAnr8MA4QDis+cuGt&oCQ21W*Hv) zh-PU^xhqrRT3*nB!pu`jXUTooN1CEyNn#I943R> z8CQ1vus9UtXCaKDTJa&fp|LE(d_2f%ce&Z{uX$3^T7@Zp)$@c4Y-m5*t<_LF71Z42 z2%H#p_ab(K*G6durRab&GKDTA9WddH$Kva`6&8*m%`(IDh_Rp^nV7<@n{I z-)eI?bOyOFACnp}73HDR@P{0>e6Lx14V&9Vvx@J#?rYkQ-+t;^r{$gMVw_W8#~Jsg ze`>kgXbR+LC2I}Yx>=Fl-s~PxIJvqZr$F~C9{(XES>4hN{3*5HuIrI!3Avs+A7%C+xnKVjdLw9x2$cg zZzz2wv10cj&MQg(Sl|eA6WdBy%5YP{M4DFmR)$*8ra&(S&O^IziDW+^oW3nm{iI0u zogllZ^;60bnd_GP74$ZyER_)1+s6Bhe!8DgX`$g%ePQiZS=FCB8Q&FKWmA+%F_FD7Eu*UsO+XLCNRQ=D5%s*!JQPq72}e54Y;B^)y^q9 zDpkohm(nKQno%h_K7!dTLrn5P!j$_@VC*7uV9-Ck(dTF@=H%3XlbC&$IWpZ(L|lmJ zoB^HRdrMAyeeLzpIp1p@qhsz+y3@P=B~Uy2lTnL4ldtOj0koX^AFMQ1cK6({F8i)q zuj=yCfab4jfX>E}@fK^28qd9|uO0orSa-Aq>c+J4^SCMrO zXA`vvQ_`zN;2knLoS)xhxj_jFysZxVR zDK*+97(=B_Evg&qx4&M93A?k8*7=KM2QoFgI4#@))&Ftjb0^2yGeFHCui?O#0nw6>2e3k!W5Kq<*YcCAbC)VNC{M zq7qcvXF{{G?1wfu(yn@P zf!Y}Br$b}oG^i!Hk)+C^GbSq&QmTnu_xPI0sT2v?dLuY(Y2P5qs1>@L8 zT2C7`TBR{!Gzw@M872K)XV$hxTjo%IKx^+f3)e0K_2gTaCA$n)(!)tStcQOU5{R=` z^Wo)?M*$OC3v|-9mti#bp`lj{7^pC{Sz{WkkqMKGF;+jJ4APhQ&ue{0nJN*(DY65gEtAl@Y0!{Rj!Hp0 zTjU+xh8Z2xHTEXDa&Mw5i0Qf^#@~{zTO)LR{C`DPeuS9+J{Ot9{P+ zC1VX{5bSZQ>D65p{jw1nCm#v*#-7~}rZ13LTkW(~Fa8bnN1ab=y|ecas)3102J6WS z%?>uEe-Xf-{T(7Iv^t{G3KKN)Bl0w17(UHsT>bXWYhQNjvCd_7RW08Q_Y{P6$!nkt!(H3+>3I=hC zQZ|UwXpy0|B0Z-;HIlZ6L2RJ_GAzLrU@yajez`E<3t`mstOh410W|l&oOljGdW5#8PM_aQ=p>UKd#DZ z`nrx0u3?0id4v;jU&m?1qIFOwh9gASXoM5)e&*rPm3eh^WxqOs_lcGL%*zKcBAdxP z_6ZuBMgSil!t*emlc4Mr=y{N(Wu3Y52-f=%&KlWa%%2RNS^=8v=yz1lheSLI;r@KSI8tPZRD)dT8C-RhrH=T;6u zElxq2r*DHpICIY9c@*n$j@RQjug7ERv6Xq44JEwyZ|g;^|9FhKZ-{w9omY=PruEOO zN7WN}KMK`*SUs+u5Ho6;G^m)pjnm>)W32_M4y*Gb0q6ZuO1E+DFeP(4l&Et&=c9OI zhW#fYSrG-0Fs9(C7;_)V9Ahr1C*wL##+avL%zb!AbuOx>)YCA8QZR}Z)CF}hhM!Rv zk$ZX`fg1G`jH*j;IW?D0^PO|Oyl?Ss>>b6fko8RcpWa3O*Y7RJ_d+4m%kTTtSN`SL z*B9!C$R|7a53!r3eP@AnGpp-Jk)A|#j?Y#B?D~=0@(dooh-46>6*^+GI^wcqgP6TD z_pl^(#rJkh39k8~SLd-8%Lxh=lEz1{FzvdnjzA0=aMpDZoo+@Y@?98OUZrHm&S!{> z{m>coSp%&;g`FIY^sbZu;p28XrKacmC5~76G7s$4p0oJVrK^{oeewBSW*1GC9x@Dm zNIgMlkaatf86^nz4)Q5pb@m5SJV4Y#459}c?y%Pu;kdurM1;7} zj-tqZI8|#j;eN+icX`A08$ARKyml9nRPuk#rq_nG3rjRKdyUo5B;w3gWnDfG^25Ue z|Il6bf}5V#c2_%aR}tOrNYQAc2VXu-^CSy)M511###W%jl*!2dO?h&G#kqb)rY zm7irNmYq)7qq-@K>+*~MR@P&%KwxmQgHhphyF^Twqna#r(PVM~$K}av=E)&GLUH=_ zUAcJTV^NuCRO;bm%fMy13e!=Bd547;G1?)Jk<*k8*Egz7(v)Ck@qQ0;k-yROY6wSo^>B=XL&I#2 zv$9dY78Q?2)prmnMA9(`HQm43@%;uZ(aypRnCOS;A<_@^l`tDEbQJP}XESI@9kDk9 z&w^HdBTUl^E(XFOVUpkmybfK2b}R^XECShh@gwmdKaT=3NL}nBes9pGs7EJKo4B6# zJ>2F9ram*A2*0}MY%kUjej};+V8?ogJ%W^Esa<5 zc;otmbI7Bd9-_EF{k&3$=J*PtGFBe-3ZR8zLDY-Id5R)A`aZpw&Ku~^V2R*d0%e5=F0b96Vh6OPS9lE?0Hm9*60M_wQuZ|n&U?kN$3!L z35AoQEV8|u3uF?t*endUpL-LBh3ZOSf zCd-Ph^uSo*3pQm8Arla;+9>pQ8-c&uxI}x8)_%a!g5-*`@wBe>5pL_vKweUi6@=g@ z)`Y7S11Tg0Rx_{Pd$jjr4f}}?;yxaMM22qaH#KFgWLC0t?Z)q1jjTgwkxf)sHp%GN z@e#10*e_Nkx*lM2A?tU#vWg%u-xXxy92ep2T8AErN|J=?bc0^;xlXq!IC?(deXG>7 zDAM0Eo-CD(#YM=`AsT3^+38&GuDZi|ZmrvFh6!11;yU!FsET!!V%U!8!qnhs^W_+l zb5vXyN&-H4@Sv@AvLhfIf?~t!G71I)Gz@x6io}I_5E|b4h&8h3 zyuE}<1(=imVF0+G#i>MbYI<;n{W$M9I4P~WI1^tn{-c=a?YoEg#4-M=i7o~|y4X(Q zVX^rd01_IZ!?B@TSiO%(Zl6Lci2xfvARL_TO$yXpqT^%*2LRkU6G4-MYxH3P=W#fY z;Ndd}Wfu`b!i{QT%SNQv!X2ms_d69NRSF>hpy$P?lGl%`T@Hpwo7Wz{IGiRxy+M2Z~>K;qQC579g|Cdg8kLI3*vY z2r8|(rozd36_e&S5=7U z;5#bh@No#|po6%~So$9{Eu(k- z9;_KS5kptzO|B}o9ven~N&|is38DeSqB8F03AIYe8YNSd9H!(5C1jzqrH8HJfg9krau=ajL>m1d=uCm$i_CPllWav=2FR z&IRp5=|*~=F$bx55RZ*#s-UxM?h0F&XiN{U^g93qtSE#|F}}b+#9F5>=wOlP_#DW< zbzt;mow1dm&whZh`K@my`NJVC%JUww_jt88vmIsy?$c#wgG;dtKSFhx<2KQaYe(+gHn=@+w11vf zKaYCkP^95d@C6VIfB6PCMsN$Yz~KN{T<|klX*NMl()W21?aA%-+K57s73rTOTbI}t z$t>F`WPB()Dk|8(0KgXeemzW|{qcsl?9 diff --git a/robot_firmware/robot_firmware/__pycache__/tf2_listener.cpython-310.pyc b/robot_firmware/robot_firmware/__pycache__/tf2_listener.cpython-310.pyc deleted file mode 100644 index ab7aa9a35e229ee61b7b8f8d92973cf1dc466783..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 3616 zcmcf@--{c^d1iOCTCG+;?oM|;$9Ale1}d7`#DxY-$(cHFpd_Z$2T2#oc26_XN!}mw z%&gBRq_l8`JQ?!Zx&hsz|AUr7DDxm!u23L(6Ni%2~*o|9BJ7_0s!CJB&tQ%Y{-bgw@ zXZihRu=yV09B-$5E!f?R z3s~KEavAASmI7RRKT<;8>qod@i7@M{ALKfZ;`s30NBuZgqle-)?DnGm{r6;+Pk?gs zgFM!w-F+z{F8I5TL_gP=?Ab;AXPFZBbd*d4FWS2@O4T5f$qG~1Js2sC3?Af>7BU4X z!hy_^P~@?YeH8WV&t7iS8vqCrP)-8t6EdKja_g}h*xcsqF$oy2a0k}PCxnr}xkq}{ z!rL9Z50{A)_|h{>5w5^M-s)E{X>vps!10(;qOFB}OlBlx099sW%x2`+g1s{%i>h|H z#qDDo_BA;3IGm2_+CKtAKS68*s5}m|#z;|9 z6CpTa3x7$WM~PvHOW2S1q=ST&6787<(Qg)xf!}-v{-s1bfVCFFNu*R5J|kb>vJ;k~!p}=_PQ}#xYqm zxj&;%32)3Sn47cOQ}UT(!l1fnafZ-`^7i%+jyguF-cX(@q zEa6RrFOXw8uK>l?3cI2_w%{!mZ^KL0o-)2Zb8!8P;oJ3vnR7(>2E6e1ygI9nx5EzK zm^q#0$t41~i{UG28-mW9&KTe5ke4YyJC_!hXVuepbPy`Fr!K?&n?~2GJ8%WQXTJjN zT;W^CbkRcV7%NkVut2Z0z^C-^>bJ7N-~T^0fLDg6R$wpd7_6}UEv)bb zSm7dCVd;mj@>kGCr#F7HO)#xqx;w3zVCzDjDOzQ^Fex=2rT2^0M2b*Hd$E8D6G4_* zgXkNU3G-xU2pdBF{L!rv0JvZo&yo{N1dwQ}N zWq)}=QD5%NQGGCa1SzxpsEVj8*VE4JERT6N&2(3zJ4)Z| zPVH+q2GiOb-6$Q#;%0X#`d@dJz_Xoc`{yF+?+f0Ar*?-~6w4Mq1VlW2_3(%0_m`ci zzamD$>E<1%5|H3|H_0B3(jiK)d#86*UPcZMGz5JS7nO+|L3P61CAaWkGtRR6`6N6Q zJ{;sR(8+#0IV_lYIon0mxO~}VNXT_WcJS-<)2aiVg_vLuQh2D^I2#V3nlMQ8DHO6; zhzWEfk=}=z@jz;*Tq|Y4;}DEZ15?mP1wAY(k&eb4szm+^_yP*F_Ax@zG@WL?3IP`S@`(X$jUtFG;T5W2mM2!@UWj_EO z7l+{)rF{oZr75^r)YoAs7bX|#yW40piP&>R7F`X;Z5ZmOFljQTGiZdLV2QDyj8bi5 z5BG#ZLbn!dX1YFrDwyhFb<9c^Ra2Nrxi?)LGd(l}zdFGl0dhn=&67}@CRp`c`6Ixh zLyfc%OjdF4Mx2K!G~VFJlTli@YCqRJJ4g$k3k8kep*ii@@*^bgl%dOo7JUql@>5qV zJM1&MLA79vjM4=_stuD5-s4%|S715U@~PZI8ZZb_fnr~WZSP_!r)MJe8cHTH=@iWp zaV~0$mJ+GTWC$*;uE8Yn8A<(8jWclW9KJp=~{le-m^{Vkc#CN~vmmsNBWrweyqFlKbP*58uYzt`LPD agnx_@7|f>~(3eZw7WuW;avL5*+ 0.20: # 13 degree msg.linear.x = 0.0 - msg.angular.z = 0.20 * angle / abs(angle) + msg.angular.z = 0.25 * angle / abs(angle) else: msg.linear.x = scale_forward_speed - msg.angular.z = scale_rotation * angle / abs(angle) + sign = angle / abs(angle) + msg.angular.z = sign * scale_rotation * abs(angle) self.speed_pub.publish(msg)