From 0c0764387b4a32e72c33bc934cc29c4900275534 Mon Sep 17 00:00:00 2001 From: cesar Date: Mon, 14 Mar 2022 16:09:30 -0300 Subject: [PATCH] added service node to keep track of desired waypoints --- .gitignore | 3 +- CMakeLists.txt | 10 ++-- config/Ctrl_param.yaml | 4 +- config/noCtrl_param.yaml | 1 + frames.gv | 9 ++++ frames.pdf | Bin 0 -> 17277 bytes launch/klausen_dampen.launch | 5 +- launch/mocap_sim.launch | 36 +++++++++++++++ launch/takeoff_noCtrl.launch | 5 +- src/LinkState.py | 5 +- src/MoCap_Localization_noTether.py | 36 +++++++++++---- src/klausen_control.py | 8 ++-- src/offb_node.cpp | 2 +- src/path_follow.cpp | 10 ++-- src/ref_signalGen.py | 27 +++++++---- src/step.py | 6 +-- src/wpoint_tracker.py | 71 +++++++++++++++++++++++++++++ srv/WaypointTrack.srv | 4 ++ 18 files changed, 200 insertions(+), 42 deletions(-) create mode 100644 frames.gv create mode 100644 frames.pdf create mode 100644 launch/mocap_sim.launch create mode 100755 src/wpoint_tracker.py create mode 100644 srv/WaypointTrack.srv diff --git a/.gitignore b/.gitignore index acf4bf1..eae16d7 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ - +launch/debug.launch src/MoCap_*.py +*.rviz diff --git a/CMakeLists.txt b/CMakeLists.txt index 3c7b76b..17af69a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,6 +10,7 @@ project(oscillation_ctrl) find_package(catkin REQUIRED COMPONENTS geometry_msgs mavros + message_generation message_runtime roscpp ) @@ -56,11 +57,10 @@ add_message_files( ) ## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) + add_service_files( + FILES + WaypointTrack.srv + ) ## Generate actions in the 'action' folder # add_action_files( diff --git a/config/Ctrl_param.yaml b/config/Ctrl_param.yaml index b49f087..96d5581 100644 --- a/config/Ctrl_param.yaml +++ b/config/Ctrl_param.yaml @@ -1,3 +1,5 @@ # Ros param when using Klausen Ctrl -wait: 52 \ No newline at end of file +wait: 52 +waypoints: {x: 0.0, y: 0.0, z: 3.0} + diff --git a/config/noCtrl_param.yaml b/config/noCtrl_param.yaml index fa41ec4..92b4b98 100644 --- a/config/noCtrl_param.yaml +++ b/config/noCtrl_param.yaml @@ -1,3 +1,4 @@ # Ros param when not using Klausen Ctrl wait: 55 +waypoints: {x: 0.0, y: 0.0, z: 3.0} diff --git a/frames.gv b/frames.gv new file mode 100644 index 0000000..3ab6af0 --- /dev/null +++ b/frames.gv @@ -0,0 +1,9 @@ +digraph G { +"local_origin" -> "local_origin_ned"[label="Broadcaster: /mavros\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 124.428 sec old)\nBuffer length: 0.000 sec\n"]; +"fcu" -> "fcu_frd"[label="Broadcaster: /mavros\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 124.428 sec old)\nBuffer length: 0.000 sec\n"]; +"fcu_frd" -> "fcu"[label="Broadcaster: /mavros\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 124.428 sec old)\nBuffer length: 0.000 sec\n"]; +edge [style=invis]; + subgraph cluster_legend { style=bold; color=black; label ="view_frames Result"; +"Recorded at time: 124.428"[ shape=plaintext ] ; + }->"local_origin"; +} \ No newline at end of file diff --git a/frames.pdf b/frames.pdf new file mode 100644 index 0000000000000000000000000000000000000000..455ad028ba64a6d682a8402d8681eabe597188bc GIT binary patch literal 17277 zcma*P1ymf%_J<3EV8PuP9D=*MySux)y9Wsp+#xswg1fuBySux~gPeQr|J-}uTJLo; zT~oi=RlBTrF|*d!#Il0IRCLr#P{cJSrB6_d09t^JzBv>pCxAxM$lAow6u|T@$wL7E z02*O4OGhL7_ta9)(MZt9z{b!BiklnC!O`AG&kD*l?L<||28#v0mRcAHjyn|e65>EQ#wh1#^F~T_&sf+wLgC?v z^h=KD)!0f+>%HmCP0OX&7K_3(x1raam<~CGxsA`1v`^PI)9b798MmoL$?Dh2#|YRU zA9A#GLW-A#c-=eu-(=_(zrSRi@yL88<#4jAsigQ4HLG&Ca?U7^(Hd?T zEJN6{v9v#Vj#j1>A=tGdYJJ}i=e#x8L*l?AkA7XB&Do5j)t;TAjHh<>?Kf9A-fY=S z*%2`#zo{aE+{~y_%hXgin!+c5_<(Bx4lRD>jz`Xbacr1m7)v~f8IzQr8AC)7Ce~4j zz%=xR2ZP35Ppb`*iu+j6NJDuHjBQV{BZzXxdcwp!t)VY%4O53Q?lN7w|FaX%C(Ksd ztX=qU8Vn2pU&{=mCEB40IAy{I7(A9srAqd5xnq}> zf$LEj`wOX6NaioXJd7K&Bt5hgusl|=$F*_7eHAI$wP#mrP1N+2&+`wDJG^Ccl~00m z2>O0bkzo&+xB((CGo&${$=wR`QgZ<&cA_tk#9;V@hm=Vc-v zcZra3WQrUNCtw9?uwvrx+SB}K!gVTT|5o^L*-LUjpX(oq2`rz zDQ4HrchZ$_f*`HnO*C~plL+R`>KZT_{$i`F<``s_Z)t`ig#^s`5 zAEhrrLm`j5bxJU}nt}0XF+iZ+QK98h(yIvB&d7$yUy`N!x+E#m)z~{t<2PxK0te!9 zmTo79f9D$jnPcklg~}oMN`!zYo?12-Nr6kFNM_fE#!U;n5#KRKq@r8d4>*%f@HVO( zzRU&|PJm4acTh{BAnnV8Q%G*%A6i!?%=5{0F+S)^(iVTqyY|VR)Nyk)Qn+>%5?YBZ zm|Q-5lWR{voV#qTI|+?jEKE*EJXm1!0&c9$3?w_o>0<~ajS|oq=B>Y)#5?4v)k~U$ zGO{-OS5SI?_YuxMV%SIe9>wVBnCU+ZKC%q|j&^_In~JL?{(4rU6w}5$k0rW-^LZ7@h;H*IMVsGPQ`;WH#<>Qa~cV`sdJ=^P9JJ^2sGI0A_76UliI~o0*6L_~0G;%gG zFp?MH{}BJxRe2)^8z*}MBL~38u*v_UXDsje|JpQI|J|(rw@u??VE@`(=m2zV3=AJ$ z{w2@@=;)c~+5Vw00GJq={&SycQc_G?T4V}oYEr0gvhe2s_HRNt4QfJKUJ39JwRLfE zfxunHnLPGtt-9lWNq@_KDiu+QkW+HtwippvZTQveA*X04o|!Btc@p6lbL%`RX*pc- z3?w9>VSx4{tmSt$S0-mH)cEq}^HB#vS-B#0_9dXVB@ocBPCt?QCs!li)+Bn;e1%8P z0_&r%o?w-}p!tS;X9H@7)!kY*+1lRQ{#0I7dHL*-)q=#+K0L6B?k7IH+R-&Ix!jqI zcRnWq^gAUC4Cm9Qtu<&YJUqPi`K>hsUsiZSGi&gluW)`YvnZwpXS!di9Z-QnTBewp z6G(s@u;9SIz_<5LBbz=3e`}0}8OH>enHe0c>z+aVM9F!QiNE7tNLtmYhc5(@=jHf0 zJ4{3b6#SJdujo!h6!-IvR>ohVF2E^J>Pu(#ZZV@a>(*U9q= z#!szn_^y88ntnhgq-Z~*=6F86g~HFz1LY@>i~wtGl-ehR()@;RYyg#fnM){$;%f%#}x!_l{=_ z#5ZVRboIgOOM6Gp0{FSNGr}t^5IBeL=WX3O~b9t>$^pzi_ENaLemrn}E9L$^G~ zZKYQ_jCUL`j+NCcFR7buD%Er#YR@C*AgY{x(ziZHoGLTEPb2Dw=QW%Eo2NH zXa#MdcZ}_CqQf_(X8JZ)+;u(-%bGx_(QOk?uoG?YV@*G=*Y;cZgWBNFJS2B$y6;Li zFZ=HKNN%NlRQm5)d-iRvwc7@C_M?t^^&jExxxcRStmBP5f3gQs9ex951ExfM^F2GD zdIMf-s(t|X9J6?0G4vR-xP!aTn0v)L*IGDuzhGRym&DuiX}^2bd<9;+YJC0Fy65zS zRoUjU{XqH*N0Cy`+Va%(Fm{&UlKG02s@9qb4j9KV^*={cu!AX_lzd%e`~cp>qM2UX-I<8;IdQVR6}6|f(1s!w%t z%<4##G;)T4v<{Ms+6dAz#*ONXzvf-9^O^-!$)6uh9Dt*4auL7V)#2K> zxWKp0#Gp%i%1T1B!b~j(X321u*;@Iv@4B5jjmpl#&_r|D z2o!=L%Ohcr$psq7kWaMR3Qu!Z%}knKoW2afA~#*!^w=CM>n5f4Cy`|IubY<%!s<`8 zWiV7q31H1yP{5%Yi(h;py;`t?B2!09wRxp1acG&M%dZQqhP-6D;EVG;D!1c+$$35J zR!7&RxeBol)XN~=wD4Os;7pl(6;GOVyo=-ySsbfx0e2fIgjI|OZdfUoRtByymU4{QqFp?mzlSBrw2xOU%ZS!j^GIW zSJmmL_bl^hva?bE$z*o-HdrI-lDDDV5>s^+3-#k;U(8R6?GSGhHUa6Ljn}Fw6-92R z>Y|qI#?}*G1~gecA}8>HinHGCiH_Vi`=5$T=c5td4AGFMQjHni(3kRGMNlCL_=Bb^XZJ0R6w@2=~ z>)Y`VJ$gsF%KmJdd-KyDm$uyEcGFOF>k+@O4r~pa zC%4Y-6*UbF)=SXXa1A?96{WxVWAB@ivPge}IKZ!I<(a$&-N%_iB2$Sj0?_4c3qL@n zr0Q7QHSpMF1;^-`xfkTZE11H+W%#*f>3@!T2&4+ZVpaZ`o~QR#+R zkP75eIh;7_R5=bwHepYgUZY+>uDe#7uCmk{)M?G89;@9oc9xi8E34POMP)8a+LgxU z4EtBokWoy;T1v~ja2mm|#B)7%R#;k1EP!_z4AL;|I+bKB!ixs68Z=d!5@BPV1;vmn=s+bVqZRXqX2Db_`C4 zH%AEg*wISRp%=6K3cQ!(c#|XD%{@!1CunLIQRX+seWF@SHY6p(c7_zc?>NwJo3BO{W0boKaXop zsxYZ==aph^&mm=bfyU)F+q(NhViga0KfoT$gbArlN)59t^{ncr?9~ioq81v2`N?_$ z1_8i}P@kDWTi^KSC1H)9*M$+UB#Nzc7%K}(+Y^E4^A_saNs91=2q=^I%(==p2e}n} zYvg-LLNpkwpv48qBMjjifzIYicasC+q8u&Gv}|lao8oTR_EFKB^ki_~_%_Zf*>klm zb33PGW|kr~1yuGO4?R>#Io&YC*7~3>?*aakFS-qSIBZ?sl0O$OqTN^Q>Vv$vXyE!| zBbZ%~Lkv>}%wh1xWM>y@!+&V%n@o}=m?%cAF;;nC%~q1z zRcnd%E2<>29Vb?E-EZb8+ zRi}|-!!Fn}IQj)S*3u?Tuig3@>!w76^ByBRz|hPsK!S)T_q0B7!9hWjB=J|TL#kL& zGN*0=>zlCQK1x-?P=beYPTpG&Sm1Yjy(Dg~l2M_>ISi%a@6F{CT~^^Zt+FsWKk{p| z0kJwN6(EeyXycszw1dK24z zU*@8mNJb+Ml+3dW`~Zp6Wy{o6{S#+Ce7jkCw9q2UaKApPc8<)NC{_#m+jU{R5SVWr zV2Lc*%Z*x(6rb{|@dP!e>m!__$agdj-RZMi({qFhw$EtMd)4AG^W;J9>cKD7LdsBa z6wf1{Onxc{JzSX}85-tAgP2cGs<@M=4mlmbF=#l%rh<@G`eO3eOo!tx&!edsW9L_M zRJT7S1Z3-Plab|5&z6V9I$S4teSs5!Xbd_WBsW6Sz-atuqxBpv<8%TI^;lMPQh&Vi!?JUHSN1E`f2OIydCl9y2RGg zPs`!{!oKLLJ;X_615Cd2k8I=p?8r^a3cePLRtx}*alm>hWj+x3# zus{*;%pGola2koii{PE6KuP8>P#l$BuHA`h+0k@jd0PgjC|QXYp0u8378&^95fZ_K z-5ZCSPpuEu$67LPhGNtiMxJ*eUFk5=y=|{GDF^n!u)ak!ozv0MYcfu|av*W;xlIE> zZKSFagy+nrK&q!w7w@ZziJ@{m%}QK3&&r)}W7vus_TN%3M#1qVTeKy+W^?~0CMahH zIU0$^4bj3Na@yrxP1YU2OsRs_rhFaA)^h#Csx~swuqL!a1|^y8j-7%o4t*uSir4zI zNxrvum3Ypxqbv0welLH^A5<+#a(TClrF&=l$eaGcJB`o>qJwG%ewIM3{RIsATG+Kf zTNdbaCFFdar*=G{gtUuqC#Fn<6)Lxun# zZ5LeWa0W0Uu%hPpd43MyV)^WB$zw#!-2;0j@9ta(y{NT3fT0~k$@RG;?c1m?31j$o z;~Q1E9{o*hX17Ij9?@aYzNBtT{kB;i6JNc8D*bDgiF$i2Q>k8}!d z5%Nz{=b$Bf2a$EdivW3538l^Z5_ArpVi0k~hg8Jux`>YB!KcI(tvb(#*YKxIpK<{T*X^Hqj}w^AqR04^hIH z?=KDPHm-NOgtreXi2wke5?M9xR60(MPcsa;vx-~#1dTT7M6lIRm=X%ngGn*Jm35e7 zy`91~R%{;_8xJewX$Z5Zo22gIWS&sI3p9lbMkH5pnVQ86&}jXf_9Z1+@bJO4>J$p} zEF$>*cHNXD7cj7UZ-Ny!zZpFA7^rPV{A(w=+fb3=JKM>O8J~a;pv9T=F6@^E_4BUz znLkAwE^Won*PHmss|5hj?`1RJZi8JSXgjPNLxZh;(Tr?3?W1tEgu`k` zJoY26a0P@N?4M;LX%l8byPSo2J~`zJ6(()84!BCbC=+lv(}QWZT(TsSvg^2H{c+4n137vT=cX zu%vGU@;UXwKTHKpDSyzHGgv+YpKBy~hN zqoJNA`aMey;pr;LHk7GS#iupi+ggKZt!Zr*H!-iWvfy2ygKhfOUpJi{!3i>HyI;C7iwT~bSH-+;4?!`YlvMAIpVl;v5+B>zzu z=Gf(z!|`vjl}s942lR@zZ*G=s(e845a|M)s@K9mjA|@KfN=xzCo#=ko-^Ek(HDi5p zEi-Pkabp#E_NkJ~|E6%3;tnpRp(zqAydo3lh(KRvHobREHL}A{3f^X+h?n%b zisES`0mULf5^%{!GBY&jOnlE~T32N*6m6a>n9)#MEn!(>-zjwva5bB%E6>hpStI3P z97;lZ13sjH)wGHL#s>=>@D%A4q2j2T zT54qUsN6BN8Wm!4cOpB2A#(}A{{9@BYG8?p(Z2hAulKrT9V7sRc@z=aex7aH`7zm& z&|9JSiVCPC@s0ARx+~p*UOY)AM{lK8T;n7)Q%34#Wg?ouQ|+4_8OFWHup94vA(Hyx zq5NTZ6x%7M5C5G^HfvUP_F|YpOti37zsgmxyrrc{PO%_9w+gC!C?olc#Bj4a2Fzz? zLxd!;(94d4Thb+(;z^X^ItGPQ)4Ff#IWJ6s`~|BH`^eATVc^7!Ugx!QfH|vnIMgI- zKwo~jkH|KBjiqUrMHsoZw`_V_Ye&V6z+z*i(Sr~z+Ys;2W$U1#KdE;1`DKPiMvlH% z#;x^3ETzUd+H4Q1Gi9TW_z{)XWUya2Q^pVF&rxmWPS+b%jn!L4J|rvf#t1aTjh?^8 z&g@R$4*7AOMPq@(AN0Bno3)=7XcMumr{h_L_AOTXwodZBplWP`CD$n(2%{uIu`rBE zlx*Bdq84lgfp-n+=ZhNm2EgNrD>`%ZgusZ~YP#y#^CP&rJ_{ir^2X4@rudQD$#8kI~wr^DWgGG<{z{dXm76vza^$;MfBh zc6!okzDC8LXGuwJxAF#n@8rCWsVrt z(>a%Pr+C=OD>$(yc7G(i7UNdyOhpiUgyr0?J^QiIbiyN>glD=i9J8hEks}N3MqR?4 zuS!sojOw&)h`4o*(4UBfwgY3gO&jth#48UoRq3;&IbObT`>&od|FkYQ@QJY+d~>JF zOXpl39KY%7^v5I=$(59yn|b1WaPQc$4byBCt#hqcug~}DI`uCC_7hp*_oa@cMt4k zAbzY2W*WPa>$j7Q+Eak${MLfOI<})aKf%9Ga)p_}C0q}dMPbKp=jLlPnYcKCzq(V6 z!Sh$M>{GbuuvvGNsbw?RE36_U#Ip$y-IfZ8jIPz7XSR`z&f2{?ykNC86pE@!n%7(I zFUG}M1g4b#*bp=%479I{#h0#=jZhUCO=iM*4ZS}Vp?qgWdN*vYj>aF z)ao~bu<^04K4ae{Jtzcjcf?G;_z1i6IlvVCB8K1&QShCHaw=X9v~x9k3lNyLZvOe2 zaR_wRXVoJ03D|m#m3bsX9^p)B7!7?!(?H;oiQWrV1}iK|9t=Fk5=_tXOM`}aJPL$d89TqErRGtE-=wbJx%~1TfpRh{EH%pP5hk zWO8&!CkK>4Gc88okk$z{!bs}LZ7Iwj7-l?(g~`-@R80@baN89~Qw~iOqRoq2Qz)xw zvCq1&4Z-V+?QiO2sqPhN?f;@@WW3EcU$5B%S&6t!lLP9?;8!fLau5Y4G{5NO{%uM0 zyfUz!<<4QJeg&JjX3lwULt(z*nn7L>7y`Ne)y2=f&9=yOc=sBYHh||yZb%pNT$AKR zL0*<@Y^r#4ggYa>cL!!vJw@>kBkmM-72pyB*!Zq_?P4M!gt)o{J~z-oh-!eO zMZ|PYh!dg~K-e z&ho(m&@9eUy-wCw_M}Ze>P}h7y^&Fg$x^)>+`TiBO74@bYG{*GLW$#LKDSM73UXvq zDa=y>dX8mM#x^`&M56-n$nweN0r=q)1Gy>_Ki4ajw&3&KuT_5<8l&;thnb?NI58gK z2*<*U!vR=U%K77sQ)?THu40{`amsF*9#LvnN$S*)a}sh~eB6Ul*}m$rNeu&OBrSEp zZUhuNpd@rT+)VwF@MC5tqI6N<{hK#Wj1Bj{7tFf7|(C|-8hffOQ;e$lXHYC*5ocC`mXlq^T&pEaAL0HHRbMr zKx@kVn7kAfNf^v?Yv_fKxFro zkQJnTx4FbR*=r7XXJNC?gtTnG#iOXB9#?#yvo046P66-nc?q99LGZ#Mv2F`q7LNAs z^p*Q*3v&lK&&kQ*vJNSw9D;RdPzu*6%iuWI(yJV%vtGJ0%_<2c>FXA0Q60_1Cm4h+4-m{nD1Gc zCG4whU$A9h$lFh*Ur3j&rLuhE2oFF5vB0BJU!MQ5f8cK9A*sZkojPi znLJX~1~%Zqkl3*dsmdH?a2Z75Myw}*Z;_^}vX`J+l|;vmHRQ75v_}K^E|^>1glF*Z zI6kdBhkk56f*6@Csrg0=H`v5JJ|=GiMZ+Jx{?J32j+l@`!{_MfL2I)L{hH2hsRdfQ z@fYbE|5|;T0p^%8Ks7EAHcRUJ7cIRZPsJxknDpTeUfS>Bjt25E8NL>9MNz|xmI@A~ zK*xcYmyb_e=JMCOSs4RZkO-zb4fzh)7(!-PJIbYpvQf|Op_I^AnMM0Ey=q3q{_2|g zHL%%JnsTldCmvsVROzI_c9b$Ej-8DdTn?cydu>#~yrM-4Yt0JF8!u6e!)FUJ*WzTH za!+VUxrCgR%Ck8yo8d;PiL#|_@Nw9gCNYCc5$zuyNgr!b^uqcP(A!$T3iG)AH&DtU z$56YRy8}hK%U`h*s8lyjzbM^6r|Ji%OX;l?khTn{#0~K(lKTA6WTHu*R!6z6fwpi< z&Z=(i{ZZm|jGAm0q@^@k^%Z@PY-Ck7bv^q8V!c3`WWV zSwAzlP=VKRtX{pGrA95NzUxq*qtjkoUHzpsCj6wD#>W#rRJ@)n5QM~9UqNn{n@}X% z2%Nt~w2!>qOCpYuLf#m&I0^?1alUp-x3K#dk6`K?w{*dkvFUXG;boS72FLuZ2cm}n zuXLGai_pmb%u+n6ts)rQ0d43D!>^X!{4sKKfty04m#*8&wt-7wiD(bKs!ty0Tr=Dk z6Tj|SYBg<|2f zP^!%?5=s@wq$Qf+TPXAB=)q%r-zUr~bDQ9F84?R9r&kGr&MYO_E%|QDHlgI9VPxL; zgaiV8iFFu&ot1;YsA^lnU(W~xVT8;`eKKpyr1s^sG}Nbx%VK!HSnLOhX> z+pML-kU9&k09tn!871+x?l-u3H2i!~$wks><8~6lXakTZ7Iw}F3DdN)mkWiSz{Mii z=YVa}(XQ|WL>TWkJNbj?R#qGvEos~%J?#0lsMFvUgj(?v;=MKf`1D+%S+|>QRUi8j zJQG^*&zoEwvjAFp%twQZ&ZimTEv-64)V|-7?7_Jg1jChXagB&FZneX!3h}IhpW=uW zL54;!5Wg8wy%dl8K6{;p=x`{QLc~=QWN3hZTi!LasydgOfZLwKa)im+L2%*vQ5WaW z9w_APqdh7ytjmt_?}^Fh8iwnZbvm_aDt=4X@&s5E=ggoGaKw?ll*+_&=57)Rnw}&{ zTFlUv2+$95d0n2T8LHP(VFGEdhK^U2?>B*`ycu=5EQ633JqO%Cdh%m+JFTq?DRQW( z4vHxA-IDt$g`2tzd4$>ye1S`68vrHy{8^frzL-`qdmWsNGrAgqp{aU0|%v6B-=V^3%@IVxv&L4doNeF=RVpi*9XvfK%qdAV;ZlNbT<^U)KXStQljV`B7#lS-$t zB1PWlo^eCX39bROG5ssHx_CAwc$F=3IoZKpT{1ezfQ7U%P)Z>a$s&60g3m`BDmgWX zsJv63@yBa*KnP2Vz!2s=38MPbgh((;`bgwdp%=00Sz%pBZL9}S!!knhmU*+ zS#~c-3Y#SHl%%4E%oh(E#`mI@6pEBCUDni>YRzRi&0y)xbNQJm6FF4XYlV$Wn)rIJ zpfIH!U~ZdP&qfA`#M7*m+#UFgW-_$6X}6+GDS0~UDEUZ^nIm-y&rJokwb9N_3 z>pWKG(_kY?&~%)7a=_(PN?B)-#6L4g@r%Bux3bnnaN$87d8bw_RG5k>zZgUmi7VCn zMwm6HhMkMmJeBT;73Go$xO|8`M42cZXl}QeszT4iF)n zO@{bUB*b6s%bMlwbogKsE6@iTMWLY_^jirF59>IqODDOAhg&e)$LG~e{f>%S_)MOf z6r3Q2tFOwH*{ESwNKQyVK1+?VFcmEDVjq1`oWBA_4^le)1BTe#=fYQCHNZ8tuhv$R z0quo$q%t6wcKcRhr@Nb;trU}pvtgnLJZNw-WL(a2AZjw{1odZW2_2!x?D*_eRaNq) zNj4-%J-vjZ|82$!DL)_cSi7etc2YOFD8r+02fL91I)q&3q2*Y1jZsCS3T#~F%`p?D2=(NFUAlxYhR3?1Hin&EgIPu3@nnNZ;m-|Q zM9>QIFk``Fg?%wu(qA=@{oSU+a~`mPd~<%=xrAp&(oC)CZ12}KhVgtu-&-eTTZrQy(}eGa}ayz5Dk%y@(uyRmomNab)oIC$h-@vy>+ahXA{`&8g`TaKszL`Bulb zAjbLqlMsi5wPB#BToa2^#TvL0WA$YDjT&Usv!HE7wLN#P)gC z3p*ydbleZU;2`;A+oO>-a`nWNucR=Un_~}@9(_GKiRt3DX#&0A#*89C)Q;vR7=urO zex&Yy7pCaN=22Bzk_kR2YMEZ26$T{5kQLS%oBLPOw@a9k@0!U6c4gI>qjS;R5 z?k=-VjEh@5RG3q{d(o{avPUJs2uzDUQW3w>_W)+cl6?d0_StA4<|A=>ttM_3jYH6)RbvQV+NP~d2H&XeN}51gQ5u&<^k&cH2(9SZxCPT4kS(nY2l z!(=uuScHAC$D)%38?khH9!^xJ#*R3^*wCnnB}W;xIzNg6vQj4E!42h~9zng1v%Kf{ z&qQ0C14&j>4wt)s7Y2yr`lB2L`k$(W&AQJ6kSrXnmL#LTrt##CFYgGAk=t*mik!Qd z_~FNN9+;K}d^QGgHakG}<}+!op1Zh%oUO1i=_k81_Js>1C&Mf1am@!$icF5Kz|C!-sTzy2MrzS=L?OBfa5oX(r2 zf8I0D-rKf5;`%LMi?78y@fVu1Zp@iebL2Hom=0N>*~Z#)E=aG!yqAcMIx-C^49zGf zfog4;AG`&!Tj!;en%rjW+wPnWOR>so*6Kkw%lhe{8w#cyA`Yc^f&S3_uN zlEN|Ry4TZ=a<$jmG+A@`pbAFr7pJV0 z(i(k5HJAj?#(tr_-=nOkRiSemj3C!DR|gkoKthZ8q7|hwen|{`Vg#|@t^ zfRp|Cx!`wNbrR$P(DOd{;G&G1Y`zn!B>n0n(tQ{ACNaHj;RV@>>T^nZ4po~7f{u|O z#C3M-=Z>LjcLk=~1x+EH3TJmQ7`^KnqG0l5xrxb<-eD*UkmH1V4@NT zZb?#QZqt5^$1*=H3zX`w~o4hy`~E! zBBlm)`z5Yt3A#lx4v>#lAp{m$o5@PE<6`2od@|#?d1pHenr?ds7o3Lad>qR&QFW4b zuGd4paF7DWf=(_K7p=*UU-?pJzqT4OY_;*{yG-H_ruTK|1k?@0KyAfg<08;}UIoiX zelp4qQlNMtixwcJ^P`FmI&7D&03@i64fE5azpfQPlCks76;igl0GOt2r`~_@ZfQna zayt&0Yvyn%K}bND)F*lHQblsqst?UVPqsX%1BRTfvWTyUd}{=>02R(vbCBJ>ZN{ zCvyvl&fleZ`(i5vr=<-}b_zyhpKQNJm)~hj_zuE9MDX!m>`4673J(dEk%-G%9u*># zrF2{otDxxei$kjCcdbOqDp#%(u_m2|{jhi&waV)l^ri*q?09}{wQofG2~9T@RdS-0 z{N7uO*1e=)J;f8&-i6tyjMZvUYpmEd9bd z-RqfiK9ujC8~_uUr&O!85}1oQ2rA`8ftZj1>N`5{38d*+H-k_yQNS-3+U?1uNs2-X zswHr>ygIBqg%UR2q%<;!4on%fPWgse+%MR|5Tgy}R>aP!&3zMNJ@4N-W8PPlzZKGJ zjb1;y?O1_;QWHvIbhg@R{gjSSaN7Y!6CUXzf#+PS)${^8!vGhOax39dw)p9whFPR2 z3)>FR86Qt*`#?HXoMC5-U9)}EuCdsovZRjN6#`zIUFbn3q)B|YRf~Ndy`QVA;ZGEO zEi#8F5{joVM>j>`uUXm_v%7hSnS~ThYP0V>fG6i*no{$N*hcdizePbm-=l58PH zwnM>ryLB(C_aJdYeo7=D{gMcl3G8*cSg{d(9ln7piEN%A>li;PG?J3QwqF6&g)-jo zWpQW1JWg%OF0PaW^7Q>bD|;B2y-8_T_RP(?qeo28k(tWLFnS}T_<6=3mn?3IO-+Z2 z{o%U5K?&UEMK=~;W@2eGTQ3jzhPrI$YiYXee&Cc%iuH>aZ;h04z&3j7I^}2u#$(m> z?R+kNsOcHrYyRB!y*F>7x-;<*yJra@Jc-Cw6e-DTd2o|mHxxk_Y*{XS5)7E?zMk;N z@>DNluB2I&d}yolLA=BoRI=@dSn+;n$2=N#dWQl&=I}bCcL|lCI!s#1QniooC}W1J zWbJGTxz7NiTk&=%OEIR@nm^8S$0P0Chzq7TXoVh4$%*c`<)oZBRooOiyD~;aY$)2# z$2^}9R@SNnEkA+4cwV3m4IQ%b7khc#l66N#K$3wjSN=>y2{#4r&Roe4MRN`0XuDT2 z(|FaA4eKGp73dHy+pt2Hr1}ii&iNk&u!}67E`_u?u6FVy6g$uyEBD8U#wc>7=asE( zMr;k6Tb6y=VH{1VcBo{GCkLU@3y8lzv7OYC%bqVpUjSXP*%6v?KMO5S+$;(uD{CEL zBP)r{pY6kssx&Sfh!+STqu6P^^e%V%2GmqDD7Adq!#!zk30h@XvRNE6Lzm63SI1?U1%{&lJl6%gXrBchau26Y zj5mhM^63#l6K>0#oLb-D-Q#!M13fCT_*VqKulhFidl+#0giZ(My&xF0 zY-2qWSEcSpU;SujR4pIcA5m>s_=IUBJgEBUhb*8ax>{DMR%Hb)_G|=py?{5IaUUP|9LXPluJLju{K$iAwF=W4G$$?2;+D}C|Q>W0k+4xJu<-05(}&+IZqFR=~h46-nP znpJV@+YUG$GGNM?HW1;c*!4b`Mnf(JuuhL!=EarvS32|4s0QI5Wv)nZmZOj!SBJwV zN(;Ndsa8_l;~6uT7EyaIpNty)6yV`U3tOr-#nX8UbF-45N)rMhj01Ft8)oXg_&o65 zzkPn?!PnLQ#At6{TgPNyDSU1Z;-7Ys{{DLcK1k==XG$Z|+Yh+yKT*36^6PKZj)8@i z?Qi^z?SCOLx(}}H{~ubcB|l`|MGN12MByk2*AtoBPoBhUM#FZ^H|6b1_*+4M5UYo6 z`>d%LlKfit%$11&vr2=@DznZk$>}DeuRKTXP9Q8E2o=|-~u+C>rJ{yk46RcqeCi@K>_ytp1CR5-z#n@-ut3vvo(vB zj=32p+KmS_95JO~_ug0!o=G%!lurhp+9Z?to-_#L;nC6xXk;!2yuW@QMOZ(1=aBXI zn9K@Po^`n-DlNI@&oZISJO=wW&}svhp|ilS!fYw&(4y_~9tka>+lF5!?$dl0StrB( z5@}?4=-*IIT1_#*TwfRM;pNLLx|Vg88wK5J=v#m$6P2cI?+!xJG44LWzPz(dy@WCx zbk#l;QXt?>`ao4qg`WJ|kolZt^K5p#jF^-Y>%#Hgh4jt3ovyQ@UsnRmuqtN6-yq0< z_KSkCA2%AtzLPz@`a;LjXrRMrGqsW2P)7eZ=uqn>&WPB;!K*LpSY3<%JKKMz@L$u- z!1iyjlI{az{2K}U3q<@NB?XNf4D8Kp9c}FY@O>YWw4T*F2`Qs0kyLcdtNmDL^6NPm{h>$yzdCPb@8Bq4s%H;i zp#M{sq~5>sbo74-|2eYnfa|~SDk1B4M&HcZ1VE!=X3b~qVD`80ry>ElNH=gGjp0Qh+Qt$kR~v#|a79aXw7JrxcNLlFV0nE%F!}Rz0q5S=x zmGvVe{B{1P)!*u0>0fm-GJcS*AJ>okNBYsezuWWCwh!x%l;t1o{LudN<=xjm+y8ev zKI-`c)V_nUHcHlJACEKud^GEy^ZLJQ`2*Z__%ntQW`=)8@T17T(OjqZrA7A-K9=sE z=&!83jiHml`(qTy4D`(GZ2)xCbWGImw-K4CqoXZ5%{ycKuA12E*_xUeI8fWzn~;B~x z$)D=}bm3pL@&7T(uJ%U8P_zI#Mkv~U9{^@1CI%*eG2kza?tQM_aq5o)VEr$RmX3k- zef9sBM$hs-_5Z2S($f8JS$d}Tsr?VTcSM`^z4`y4(b6%~{*N6KBjfw*{D&PK6C?9~ z`=DcDd+))2+p#gdFPi@>%gn;~zGVJOV`chBKOF7#-tT?;KM%-IFmpHh7^@F@S>DFx rebPV1{I3m7%-Z;UZa*ge&r))5)U$W|GrDxlY;?3x#Kb}}!chMY?kN*2 literal 0 HcmV?d00001 diff --git a/launch/klausen_dampen.launch b/launch/klausen_dampen.launch index 5ee05bb..ac9b4e6 100644 --- a/launch/klausen_dampen.launch +++ b/launch/klausen_dampen.launch @@ -10,6 +10,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo + @@ -59,5 +60,5 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo - - \ No newline at end of file + + diff --git a/launch/mocap_sim.launch b/launch/mocap_sim.launch new file mode 100644 index 0000000..0cc5597 --- /dev/null +++ b/launch/mocap_sim.launch @@ -0,0 +1,36 @@ + + + + + + + + + + + + + diff --git a/launch/takeoff_noCtrl.launch b/launch/takeoff_noCtrl.launch index 87404cf..56ecfe7 100644 --- a/launch/takeoff_noCtrl.launch +++ b/launch/takeoff_noCtrl.launch @@ -1,7 +1,7 @@ - + @@ -24,4 +24,7 @@ name="test_node" launch-prefix="xterm -e" /> + + + diff --git a/src/LinkState.py b/src/LinkState.py index 3012bd9..7e6c6ef 100755 --- a/src/LinkState.py +++ b/src/LinkState.py @@ -11,6 +11,7 @@ from tf.transformations import * from offboard_ex.msg import tethered_status from geometry_msgs.msg import Pose from gazebo_msgs.srv import GetLinkState +from oscillation_ctrl.srv import WaypointTrack from std_msgs.msg import Bool class Main: @@ -55,11 +56,7 @@ class Main: # variables for message gen self.status = tethered_status() self.status.drone_id = 'spiri_with_tether::spiri::base' - self.status.drone_pos = Pose() self.status.pload_id = 'spiri_with_tether::mass::payload' - self.status.pload_pos = Pose() - self.status.phi = 0.0 - self.status.theta = 0.0 # service(s) self.service1 = '/gazebo/get_link_state' diff --git a/src/MoCap_Localization_noTether.py b/src/MoCap_Localization_noTether.py index 8c207fb..a38ac54 100755 --- a/src/MoCap_Localization_noTether.py +++ b/src/MoCap_Localization_noTether.py @@ -41,8 +41,19 @@ class Main: ### -*-*-*- END -*-*-*- ### # initialize variables - self.buff_pose = PoseStamped() + #self.buff_pose1 = PoseStamped() self.drone_pose = PoseStamped() + self.buff_pose = PoseStamped() + + # Debugging +# self.buff_pose.pose.position.y = 0 +# self.buff_pose.pose.position.x = 0 +# self.buff_pose.pose.position.z = 0 +# self.buff_pose.pose.orientation.y = 0 +# self.buff_pose.pose.orientation.x = 0 +# self.buff_pose.pose.orientation.z = 0 +# self.buff_pose.pose.orientation.w = 1 +# self.buff_pose.header.frame_id = 'map' # Max dot values to prevent 'blowup' self.phidot_max = 3.0 @@ -110,15 +121,18 @@ class Main: ''' Transforms mocap reading to proper coordinate frame ''' - self.drone_pose.pose.position.x = self.buff_pose.pose.position.y - self.drone_pose.pose.position.y = self.buff_pose.pose.position.x - self.drone_pose.pose.position.z = -self.buff_pose.pose.position.z + + self.drone_pose = self.buff_pose + self.drone_pose.header.frame_id = "map" +# self.drone_pose.pose.position.x = self.buff_pose.pose.position.y +# self.drone_pose.pose.position.y = self.buff_pose.pose.position.x +# self.drone_pose.pose.position.z = -self.buff_pose.pose.position.z # Keep the w same and change x, y, and z as above. - self.drone_pose.pose.orientation.x = self.buff_pose.pose.orientation.y - self.drone_pose.pose.orientation.y = self.buff_pose.pose.orientation.x - self.drone_pose.pose.orientation.z = -self.buff_pose.pose.orientation.z - self.drone_pose.pose.orientation.w = self.buff_pose.pose.orientation.w +# self.drone_pose.pose.orientation.x = self.buff_pose.pose.orientation.y +# self.drone_pose.pose.orientation.y = self.buff_pose.pose.orientation.x +# self.drone_pose.pose.orientation.z = -self.buff_pose.pose.orientation.z +# self.drone_pose.pose.orientation.w = self.buff_pose.pose.orientation.w # def path_follow(self,path_timer): # now = rospy.get_time() @@ -138,6 +152,12 @@ class Main: def publisher(self,pub_timer): self.FRD_Transform() self.pose_pub.publish(self.drone_pose) + print "\n" + rospy.loginfo("") + print "drone pos.x: " + str(round(self.drone_pose.pose.position.x,2)) + print "drone pos.y: " + str(round(self.drone_pose.pose.position.y,2)) + print "drone pos.z: " + str(round(self.drone_pose.pose.position.z,2)) + if __name__=="__main__": diff --git a/src/klausen_control.py b/src/klausen_control.py index 28199d8..0f1d61c 100755 --- a/src/klausen_control.py +++ b/src/klausen_control.py @@ -75,7 +75,7 @@ class Main: self.K1 = np.identity(3) self.K2 = np.identity(5) - # Values which require updates *_p = prior + # Values which require updates. *_p = prior self.z1_p = np.zeros([3,1]) self.a45_0 = np.zeros(2) self.alpha = np.zeros([5,1]) @@ -308,9 +308,9 @@ class Main: # Attitude control self.quaternion.header.stamp = rospy.Time.now() - self.quaternion.pose.position.x = 0 - self.quaternion.pose.position.y = 0 - self.quaternion.pose.position.z = 5.0 +# self.quaternion.pose.position.x = 0 +# self.quaternion.pose.position.y = 0 +# self.quaternion.pose.position.z = 5.0 self.quaternion.pose.orientation.x = q[0] self.quaternion.pose.orientation.y = q[1] self.quaternion.pose.orientation.z = q[2] diff --git a/src/offb_node.cpp b/src/offb_node.cpp index 9e2f61c..9398b68 100644 --- a/src/offb_node.cpp +++ b/src/offb_node.cpp @@ -115,7 +115,7 @@ int main(int argc, char **argv) // Populate pose msg pose.pose.position.x = 0; pose.pose.position.y = 0; - pose.pose.position.z = 5.0; + pose.pose.position.z = 1.0; pose.pose.orientation.x = q_init.x; pose.pose.orientation.y = q_init.y; pose.pose.orientation.z = q_init.z; diff --git a/src/path_follow.cpp b/src/path_follow.cpp index d3d4a58..fe2afea 100644 --- a/src/path_follow.cpp +++ b/src/path_follow.cpp @@ -144,10 +144,14 @@ int main(int argc, char **argv) attitude.header.frame_id = "map"; // NED Ref Frame(?) #TODO attitude.type_mask = 1|2|4; // Ignore body rates + // Retrieve parameters from Ctrl_param.yaml file + /*std::vector waypoints; + nh.getParam("sim/waypoints",waypoints); + // Need to send pose commands until throttle has been established - buff_pose.pose.position.x = 0.0; - buff_pose.pose.position.y = 0.0; - buff_pose.pose.position.z = 4.5; + buff_pose.pose.position.x = waypoints(1); + buff_pose.pose.position.y = waypoints(2); + buff_pose.pose.position.z = waypoints(3);*/ // Desired mode is offboard mavros_msgs::SetMode offb_set_mode; diff --git a/src/ref_signalGen.py b/src/ref_signalGen.py index 2fa367d..889a931 100755 --- a/src/ref_signalGen.py +++ b/src/ref_signalGen.py @@ -15,7 +15,7 @@ from oscillation_ctrl.msg import tethered_status, RefSignal from controller_msgs.msg import FlatTarget -from geometry_msgs.msg import Pose, Vector3, PoseStamped +from geometry_msgs.msg import Pose, Vector3, PoseStamped, Point from geometry_msgs.msg import TwistStamped from sensor_msgs.msg import Imu @@ -58,6 +58,7 @@ class Main: self.has_run = 0 # Bool to keep track of first run instance self.drone_id = '' self.pload_id = '' + self.pl_pos = Pose() self.dr_pos = Pose() self.dr_vel = self.vel_data.twist.linear @@ -76,7 +77,7 @@ class Main: self.EPS_I = np.zeros(9) # Epsilon shapefilter # Constants for smooth path generation - self.w_tune = 3.0 # 3.13 works well? ######################################################################### + self.w_tune = 3.13 # 3.13 works well? ######################################################################### self.epsilon = 0.7 # Damping ratio self.wn = math.sqrt(9.81/self.tetherL) self.wd = self.wn*math.sqrt(1 - self.epsilon**2) @@ -94,7 +95,15 @@ class Main: self.max_array = np.array([[self.vmax],[self.amax],[self.jmax]]).T # Desired position array - self.xd = np.array([[0],[0],[5.0]]) + #if rospy.has_param('sim/waypoints'): + # self.xd = rospy.get_param('sim/waypoints') # waypoints + #elif rospy.get_time() - self.tstart >= 3.0: + # self.xd = np.array([[0],[0],[5.0]]) # make our own if there are no waypoints + + self.xd = Point() + self.get_xd = rospy.ServiceProxy('status/waypoint_tracker',WaypointTrack) + + self.des_waypoints = True # True = changing waypoints # Initial conditions: [pos, vel, acc, jerk] @@ -148,7 +157,7 @@ class Main: rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb) rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb) rospy.Subscriber('/mavros/state', State, self.state_cb) - rospy.Subscriber('/reference/waypoints',PoseStamped, self.waypoints_cb) + #rospy.Subscriber('/reference/waypoints',Position, self.waypoints_cb) ### Change to position # --------------------------------------------------------------------------------# # PUBLISHERS @@ -220,15 +229,15 @@ class Main: except ValueError: pass - def waypoints_cb(self,msg): + def waypoints_srv_cb(self): + rospy.wait_for_service('status/waypoint_tracker') try: - if self.des_waypoints == True: - self.xd[0] = msg.pose.position.x - self.xd[1] = msg.pose.position.y - #self.xd[2] = msg.pose.position.z + self.xd = self.get_xd(True) +# if self.des_waypoints == True: except ValueError: pass + # --------------------------------------------------------------------------------# # ALGORITHM # --------------------------------------------------------------------------------# diff --git a/src/step.py b/src/step.py index d90c4fb..e229107 100755 --- a/src/step.py +++ b/src/step.py @@ -4,7 +4,7 @@ ### Generate step input in x or y direction import rospy, tf import time -from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import Point from std_msgs.msg import Bool class Main: @@ -12,7 +12,7 @@ class Main: def __init__(self): # variable(s) - self.Point = PoseStamped() + self.Point = Point() # Init x, y, & z coordinates self.Point.pose.position.x = 1 self.Point.pose.position.y = 0 @@ -24,7 +24,7 @@ class Main: rospy.Subscriber('/status/path_follow',Bool, self.wait_cb) # publisher(s), with specified topic, message type, and queue_size - self.pub_step = rospy.Publisher('/reference/waypoints', PoseStamped, queue_size = 5) + self.pub_step = rospy.Publisher('/reference/waypoints', Point, queue_size = 5) # rate(s) pub_rate = 1 # rate for the publisher method, specified in Hz diff --git a/src/wpoint_tracker.py b/src/wpoint_tracker.py new file mode 100755 index 0000000..0a7dcd4 --- /dev/null +++ b/src/wpoint_tracker.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python2.7 + +### Cesar Rodriguez March 2022 +### Reads waypoints from .yaml file and keeps track of them + +import rospy, tf +import rosservice +import time +from geometry_msgs.msg import Point,Pose +from oscillation_ctrl.srv import WaypointTrack,WaypointTrackResponse + +class Main: + + def __init__(self): + + # Variables needed for testing start + self.tstart = rospy.get_time() # Keep track of the start time + while self.tstart == 0.0: # Need to make sure get_rostime works + self.tstart = rospy.get_time() + + # initialize variables + self.xd = Point() # used to in response + self.wpoints = [] # used to save waypoint from yaml file + self.resp = WaypointTrackResponse() + self.param_exists = False # check in case param does not exist + + while self.param_exists == False: + if rospy.has_param('sim/waypoints'): + self.wpoints = rospy.get_param('sim/waypoints') # get waypoints + self.xd.x = self.wpoints['x'] + self.xd.y = self.wpoints['y'] + self.xd.z = self.wpoints['z'] + self.param_exists = True + + elif rospy.get_time() - self.tstart >= 3.0: + self.xd.x = 0.0 + self.xd.y = 0.0 + self.xd.z = 5.0 + + # service(s) + + + # subscriber(s) + rospy.Subscriber('/reference/waypoints',Point, self.waypoints_cb) + + # publisher(s) + + + def waypoint_relay(self,req): + self.resp.xd = self.xd + return self.resp + + # Change desired position if there is a change + def waypoints_cb(self,msg): + try: + self.xd = msg + except ValueError or TypeError: + pass + +if __name__=="__main__": + + # Initiate ROS node + rospy.init_node('waypoints_server',anonymous=False) + try: + obj = Main() # create class object + s = rospy.Service('status/waypoint_tracker',WaypointTrack,obj.waypoint_relay) + rospy.spin() # loop until shutdown signal + + except rospy.ROSInterruptException: + pass + diff --git a/srv/WaypointTrack.srv b/srv/WaypointTrack.srv new file mode 100644 index 0000000..7352ec2 --- /dev/null +++ b/srv/WaypointTrack.srv @@ -0,0 +1,4 @@ +bool query # bool to request waypoints +--- +geometry_msgs/Point xd # waypoints +