From 1e8f0d054a7d70256fae738bce836a3de5ac579e Mon Sep 17 00:00:00 2001 From: isvogor Date: Fri, 12 May 2017 10:46:39 -0400 Subject: [PATCH] local pos --- include/roscontroller.h | 8 ++ script/testsolo.bdb | Bin 68524 -> 65144 bytes script/testsolo.bo | Bin 3809 -> 3809 bytes script/update_sim/testflockfev5.bzz | 208 ---------------------------- script/update_sim/testflockfev6.bzz | 208 ---------------------------- script/update_sim/testflockfev7.bzz | 208 ---------------------------- src/buzz_update.cpp | 88 ++++++------ src/buzz_utility.cpp | 52 +++++-- src/roscontroller.cpp | 183 +++++++++--------------- 9 files changed, 155 insertions(+), 800 deletions(-) delete mode 100644 script/update_sim/testflockfev5.bzz delete mode 100644 script/update_sim/testflockfev6.bzz delete mode 100644 script/update_sim/testflockfev7.bzz diff --git a/include/roscontroller.h b/include/roscontroller.h index 6dc1859..4169307 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -110,6 +110,11 @@ private: ros::Subscriber obstacle_sub; ros::Subscriber Robot_id_sub; ros::Subscriber relative_altitude_sub; + + ros::Subscriber local_pos_sub; + double local_pos_new[3]; + + ros::ServiceClient stream_client; int setpoint_counter; @@ -182,6 +187,7 @@ private: void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg); void users_pos(const rosbuzz::neigh_pos msg); + /*current relative altitude callback*/ void current_rel_alt(const std_msgs::Float64::ConstPtr& msg); @@ -209,6 +215,8 @@ private: /*Robot independent subscribers*/ void Subscribe(ros::NodeHandle& n_c); + void local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose); + //void WaypointMissionSetup(float lat, float lng, float alt); void fc_command_setup(); diff --git a/script/testsolo.bdb b/script/testsolo.bdb index e3f057f93e9187ddb79beb03aa2646dcb0363b36..d25ea48913a362e840bf458b3b0f8a48eb19684a 100644 GIT binary patch literal 65144 zcmb_kXPB1NwH*|}4pBj|z~BsnQ(yq4C>ErMjbcZy3Ns@J44^V#03#Y1%%y`0m;fde z9ZrgFeFeC?%4oP^XHpCd%e4?z0Y3n_d)f6 zIXU%n{&V`3FRsARYiqXSv9#cAIaMhTqn?~I9@92LxI49>4K`3$|O+Pdz=XOD8?m~L^ z@SL2-1Oe`aV~cJj61!0lPI4jbIWi|_yCC4taBMy`QG8?VV)-^+@vVt?5o`OV;#=d4 zw&ynZFH4TeXnP2_daT%VlTTabMk>~`+iXa$0I5)X>EZIJ?2w$CazQxWg;WcqVS;d& z3+W{E&x3+MhS~De7sHf3Wu2=}z~FlEqLC|}?hf|tQkCxM@}eB|sSpGtI2>D6w*#qC z5Pb8k2s3wrAhA9+pPDD*%&i!YTarZA4d9v;7kfH-pi43yF9cF2@!WS0{1ZH$Do8%|4FKh42$F9v zd|Gt`#(Fq5_Em$&j|hVAR=8O47@!WHj!``JAx&{8v`=7rn#93dF181Hp$9G%TTanf z+tXDKoGKW$+SUU#(x|LC=z%mU>+dLN8kO}p_(qjQRcg!Ao8a4a$u#~9$A&aj@hxlJ zkB0%@2xL`R6Jc8-o;Q`xSiZdjTfPzCO%`i=BnDy{l{FejtRf?@)!+cd`eMn24e1`R z{tiL#-60<;*8Ar5UJ&#J@rhH(7Tr3SQz`lOvlA3^$%a@4{msF?Mv8siU6rm<>^s4Q zv>Ql;f^dNg=|&KxLJ)kn;0_>F3X+eS8Ea;4W0XgIO# zTMyfTVgn+EWAox&n4c~PxCFw9^GeyH%qRA@|Yn=x$h3eNWenL0WSD(Y)DfTSADF%RB_d}F+T;Q zG)iig(mwzVC${ciDxUK$jcxU+Lz<6S zpGLKO3a(PC`}$`c$ndm85C9sE&8Ox{8}Tl+A>9R}t>TMM`Snw3hZ&D$^%|vi7*DM2 zvB^53MyVa2cKs;RL%IrEE#DUSzR&#@ z_dCC9#5f8kmO;mXLA)D$cWKXLY9ZMrk+m9E43{>7eu6_m9KQ2*m9@sMejAA{xYD1a_*2ArE*we8L+5!V=5EL#Pn->L+ z{&`1nrmxG3*HDy7LGV@j0TksfLGrP0zN!zB6i#e?2BJQUhj&PA)wi{GW=Sn_fZC?lLZxI{7gk#IxL15T4K|m56 zNW*|c*83P%2c$Htp9@D;N>oUhBf1|k?y0`X`q*l1SL|eFVr`4S&OGs)w^pp}OxTu) zZPr`k3)uD)+pIn}#|6#j(PtYw?*bAh1G8X5x=<-N-~l`a*lJZHGyJxgH>k~v50d5h6wrphttxO;YdVm&3IgwVTXc(5 zj@j?AIUbBVg!+e{HZLY4$IOTi>2a8MlFay&MH5ht8lOzEMK>Nu72;E_eAZI7$R1j@QzXQ@rLGW!E<>2u;LGsO@12GrR z5G39`v0U7t_Hy2yv9_hSxbx(>2qv6Z+izgY-5QrmCfj~US39xs&62~wuxUww3 z`PdJr-e_y9W`D(1-{e`2xjIR5fzZOS`SkBsziD{!0+5~&U$EFYe46X1bSqUY-s85+ zd;+8*iGy6VA+1o=^38*K$ng{66F|eU`P2p>Q4e%=AAZiT~7TXgf0 zNUftsu1L02|%2D4WQ5VyMyON7?D)7{8yj0`h5YUBJrF% zoDJzp^j>>G!UYqK4QVyZCkXH(i7Xo0JO-}v~L+feb%x1-Gm zMYc+wsFrMeTMoYQ2E)J&CzfwF!+`gwuM-zyURzb?4Pfr|;(68xUVts{N=9PKg1}ABhSUgGCf{PPOJgV2ZrCyo{>nbqwkt@+ZHnIx$J!p~ z;Q9X~Q#5P9ut`z_Mq;bMv5H~7_pN_~o4hdlA=eu(8Es%-?ffL%6)oT@x`R}D|NNW|3ebK$8*oQyEvH5g3*hgjVQ%YUH zzD?o_=bO!^Ral{UlYoff#IkQJ4ES_kW)5=jxDK{_I=!*wqP^;fLdk^<={waCzPDbz zk|~QLz`ivSm2bCOq?85w-j=7;ihWu4vs=MFI1-L;r(c3cQ>SOWwHttay~K;G5xk_B z%gJxc<2>-KwRpkZ#fG#6)Zn+yzFA&?W=x}$CZfHmN_-uD9=d6{BfKE50BQy@`u`PB7g2#M<2x0Bof z9^WI;`SSD#lCw&XeCoPN@tFS#(^k9RD;}4KZ#JYiFxff9vMR7{=mF}MtUd4XI|kDWSiDwdGB zv9{+bmSkmE87_|Y618t0d=9?-QjmP}U<1r236hU*CxIH%llZhBNL1~<3H-X^8&VdI zjk#xGMo~BUdSEZu2e5EBD`MH#2~9)gT_hN`czy&D5jgE^Nae{`y%AijmZ*F=eoryT zcgr0G22uO?xVT0s7IHDR?zbt<9ObI}J|J=S;LmVky*NxM7S<=$_En`=d=*&$=C&5k z`ToJi+@Dkr_%`th)dRd0Y(6~?q%IPj&!>xUYcCfhA9L3#=AuQziLLuv4!$i{s)U@4 zeeTUlm1O1WpA=WQAK0q75DY7njQOT)2}Z^MLE?sM^JzAa(kP{BB$lcKWDCdU(>R2* zP7s>Ake11Entz5Y{8I_(c2%P-3 zO5Y8ndj$zsTsStQClqIVdD;uk>=0jkv!c6F2i)y#(VYPH@z-_uGn`lkb;WH%bH6bXVaGed#IzVP+?P9UY((l@J4V~%Zk`dIac zZ`bS(N0v**eahiD)oDK6xU!xL;+5-i zpZgcZw*tw6t+q=P-pP@i!U0a`R1TP_Bv`gC(4or3yI7X%;s)~ovX-VwhGqU;o(P^xfjc{(2* z&To)>du&U^8Bidc*beWbIFp;vwh3;9USfk_!ijzExykgvNDSHz@^s&}Pz&>FLGpER zp<=EN>2n}06`xpJ8{bX>bE$HDH}8Jc13sTFP(8pm6}IS}g&yG6ne{E*-RK|c0i-(| zn-@)0M{r`rc0_m85j|X99Ey%876jiGvJ2+i#C@C5cGX+H34A4H`ErTQ$K&RzBYYEh z6WDjM_{3Www(cLnfN?O#v9@c_ew~fay$xLD@ASAo$3Ax-Y}qLV@_bvz)F?f`{elr_SvT_SM7vGftkzyWW?Vjdq&dkd{fyE#)c7`^3UC`yf08q?^|=%U&6A9@l+Re(iKy*BvB}Dd(V!$1 zfNwSqQMK*ws%=|1GF=dSZ{P0$(p*8}W^1eM*FakOvmD!yK2!=IR}gJTeU$>>24zEP zt(c24h7-%aX|Ux~@NK6jfXBV$=|^YuqIWU{&nfO^DoC6Mw)*S_QW`C> z1Ldt2pM2AQmSes z$&s1QfMnHN=c_L5>GGnn>QY~C?NPnO4B6OstTje#XjHfKBp+{!?zo@j(AdO zA0N_2P>yeFvTi8vfk8-4IKC}#wPH|#Alfo>HtK$L5}%eNV^9MmHjU2Pf^@Hz=yF|o zx(apA69j(7PL4^aJ9SZ3&b$v>Y7g$Bwo0Fa`jklo_%j?E(o$7vYD61S!(^4N1QHd! zPxXD8Oc#~lwxJH^)QRQH0L3?Uc&u$MIMYii%v&MWcAU}*7@7{29E^&L7X&22fiyzZ z_7WG;%Rs6Ygj^TWX;=lOCqcR&NK_qgFC1Gp{Q#t;fnp!D3d0P2XY zrGwRdlI{CV)V}YF=S{Wm`zCuw#YJDW3K7y;sS@0CRO<>1kIjPMLwZr|`@FYer`gwF zA2lMs!;iH+*}=EIifkM}0yTRFC>6%rM$sc>x3%>>`p3j)74wIOXp za%u&M^Tvj>L-CFCIF>WtD86xPi?!W^R8m=>p~H!_%>!4@6C1dn$#zXLEwK*_J5!!s zPd;rmcs|+QJ_`)nAie-J9Gg%1$Z>B$V05wd`Cr&l0c5T1$FL=rnzMC;Ja~psoHXq+VXe=%+qLzTYyBJc!gxf=F^{nR4oX(E~G^m3278S zJur8v_~KIlUtw(Uc63<2QX^ZCs6uVduI6-VkcL3=$K|-s9V?(M+ z*0WtvqUqubqqO;SBl=^7AhB9Dq*B!%xcI}dA#GAx&i6}>mz9txm2RJ)wUYc=qL!jT)d%r!1pWBZb-*;@u|cW-7?ipzHej>R^5aF z6pk(9ryer-Lxz z*z&Yp@r~2ThIF8VGdmodxfk04HGWpUZAzw%J_2*Ovw(VzioB{=;=7p_fF&En7m(QD zQ+-EmcRFf&pQEe=JB;(B!HOX)Tb3N>skP>p0Lgjn+F0 z*9^5d+zTgm@-K$%F=E5*$d;Ldz_(_Cgg?WH^`Z_2U5vKvm1;o?XL{~j*bWjK-g~h= zPr>9#qf;(X?DQ$@W{Oj+g{_)z18K5k#;2WDU2P8hROw*ud@y&oJdYb&>~jYuW9}VbNgD0c8$rzYSvjtyytQanX2 zq&E~x=u>R9M!+Cno}P6^4ML@P7t+?2i_b7ws9Ku1a&f2PjPG{cgGy5+^IexMx<6pD zq|rY+lW}Gw>Ruw5hI`?}a^_!{4rvrk85q>^XI|KHTni-L&pvIm0mFZ&_~g4$URMnA z?Lvzcg8&+iEl+idLA+UPNQZ(!+^MtP5xZbZeS?M#CzgGkz&_3j1e3}3N5$N>Vrz?j zyW(3{7t)pBTbUsEcJGbJnA;ph881Gya7DKo)JUUtjso$QCCO6*(56n1e0|viOBvrZ z@Eut!k85DtTfFdXiJetPWc8vrnL3!2OdVVW9+yjII9Y66`bQuQ6D0f@jt!}W>WEwy z()&P~EC@dB^Juc(IvTz8p!np=(>b`Q(x|A0s!M&})KvoMIf;(_VPnBUblPx1;yoPO z?S)B<4~f91py03IJS&; zQ>^wy_qNh%?6g=0eTBMn^YN*q$*4P(wNEAe9Y|9oxA@bM<35T(>@8ch#;dxcVI4lb zqpHQ|VyiV8lq->^gFfNJ+77{XnMN0VtvKWRmHgd`GgSSy`s_u0sI2+#IBZA{A+c1} zzK+<1J7T+3$;ZA%s5^BL_xaemS1HcGp-i0F4O?m|&ZyYu9tgfs(YKT5+A4CRVlE@F zA?*NCq4;u+%cp&cZ@!JIM)A$pP3L3#r|RR)8(XcFN;!~Wwj7_SlmmZn8S6!T#Tnll zmz}E8-CUKv7FQ~j181%+Gn;`lOnl+ww;|=joLV~972R>*@q>ck+hY$$Yt{*pZ@0b~ zNY4q9ZrIaoOUTrx@h> z{pLTwpef>uZ!@}7ZTP-z?J31Ytmxs`@-z`4JuC>mw{Yiy)lUhMkJU$li_a!OdSB_6 ptX!P07?da8*mC?mkf>XI`~Ea=k-F8VK`v8V^!590R4a{g_BHz6$JzfdR58_QY{n%7)xX^m#V0MiD*Jm zKmiLjhzcs8kFY+9H-d?Z8c}1pw%D#l@0)YyeeOK|b-uiF{`u`c^Zw?{d}hv>vkNsl zmz2~k`M<@_ijvktuNXDB^|(RfE+0Ft^`LPRC$=6ly1MnS%d1C@yL52ts|Q!NZ8d1( z#Qk^;my|Ryn|{d zb0ydIO-EzHg0?5u`+t1)D`>kLNIt-9I@_lZzEQ3)SdfC;E0C)+uTIgu8@O9Z$sm(C zSeIK5xnU-=hc0&*>g*nqA$mgWdZH22yP`e$VHjLut~Ahh)QFEr_9m=YA`#IhN#-dal3q>jDW8H7Vv$+ykj#HHv2dIv`aowl$%RJ zRLVj0y(Saj-P^$DHIoa#=Uj&m_MKL4&bA}p$Nj>UT-)VBzURlzSl?scQ*9do-REKq zMKesU)IJ4#o`D>nC|KWu+!XBae)B3|#yT5BKW1_)Tfw`705{(h4%B1!r7$PG54gLU z3#gH_a^XG$aC62&`U=^uaD~BJb?*0`7(1UWaCOEDwvgOs5sG;h4cs5n66B^jk^>5( zk0UwY3jY}70>{0Z-Fyb+v~sEV%2feR`CQvcFsQQcD|-F?K&YUTfLUivJsQXhcu zrJ;q%3UYICVgwFUpMhk)Gz8Sy;{fDI+hjmt+~Uwfdrm7CpHk$GO^p3K*S5x?cW}Y) z{m`LDd~&~cKR0=uV&5yUGTu#IN9b~IxXJ4*U2fOd!gv{SBh0IS$?FbRXZ*t%Ar&_O z-@rj`777EEn^w^q!c#EdFKpJ0hkOj2Mc7QB! z+^Yl<_u6LJ@`TL(HkR27!QC+Pj9Dqv*o<%Tn+ zBDVxqy5WpF%e6fqwtN=5;VfYC{y7%VYG5?eat7tJ1O%Rk97jjyOpv=78uy!5?1F;a z`M7ZN5hUQZ>fhmB!Qbwpsil<**@1wJr%u4h-8FU$D?u8k*^YQ)65{?(r13$M3%FN2 z89O(9@6yN;$W3EY3@tl4H%)dF1yPhrcW9l1&?*Y+Twcc|GQl4%Lay%us! zOr}hidk1orCUcf9*Uc41!2SI$0Kx}_0Fu8(VLW7;WCsya@sukJk}Sy0L1DnbwAcl5 zA$t}EoSTr*v;&mv&q9Vv`h1^J!VKWotj(26-O80Ikc$;Z50Vnk)$OVpFZCq2~1we@1 z?a;X2HXCqaUWA(E6vvCF;N5zm3sj;D@aj&;a;x!N*9G|ZZ@IRsTo)*{$mZI5Rg zY#@`C5WZc2-ZYazBvrX#kYiy4pjQjIz`~dfmzLQk5k_^hKjGxy0|E;pH;o${whUCR zZF68-X72L<%(a~f+bXjydH{V1+pcC?)aS-H@cDi8S-|#2$gz1a6oT9tZp;jLGiVj=qg za+MaSp7tqtw-6{yGMNC#o^xqrGZVaKn09HjX)(|EeM;i z*$I%VHt$O9Q;<6ux#KfKD_yP~N^!Bt1e|XJ(JKP4@E779%RhrC`p>AJhV5|M87~C6 zygv-0I+!hv*tCS4O>_tZ5cL-1c+n5=t_z6zP26TLhf6a}j(uK;U3&-U{w6EP{Q+{z zOeWxpGYGstZgK&gY-jY@<4lg{TQ0L3-Q2)qI@h)}t_fxKz1UG&xwhZJmUn-gScPnR zA!cpNCZPN50raND$z1`tTTBkkBrO5GS0TsCdO%_P42{6cdR^et#Wsl_)5=Aljl-uX z0=L7q%D%7Yt^PZQ9&bpwK9}PT!m*RKLUMaMk^|b*67=Lr78PtVEy25gzlI00@&e?Z zGOuu}RK1%mAkfsMkv9Y(P@h7sx$TG~3vx?c8Us2=9mM@%^A5^s3Es8D4mq!M(&c7B z?m3eQD1ikai*1sx2f4*P9370ualmg1=K#=_wzHzg@>CF2VYb|ckh6o`a8jn{>{gev zfSc(E#2pT%#ZO_ga}oD;CdX>bMW8?GjiUn*$hCbAw!`gv8I@ey$H84kvkkalUXL(d zYI3XwA)y0Js~*UjN5JZk8noO*}1u#;xx9AArxFV&UEmH16p2{}qAcc4+ZD z#~78|R{Jm5cD2Ot?w4yjP^h5wLIs^0J11;ML5{W%;K#HCMt|ihDBueBI^^y)uXt|~ zyqgc?mzx}v(-P!r(S2VunE>y0cNGLM)5@iBv#X$@mzDOef{NM>0?B>NS6sIX`Mg1> zg68O_9KQLFnuSb$3AyIxKJSTw+?yo?9fyo7UR}WS2 zaGYFQ#Gfh|MO6(z&oLK@+CC545j$wx!ogjxhu~(&RhUe`0Qr@x zm4Nx=aGW~)Wmy2+^&O1?H^RAK=Q)cV2RVVpC7_YVB06?jxinr01D?Hw&8wo<@F5P+qNBr2$k|}~-hgxE zBgk=}4B)+|Lx3lzkhA^a(lpyF=LbRVUN=yJhO`8^&Cqz!WO(5e?1J>qx| z*zDU5Q2dyd;N6}8l+O$SgIz}ey2iX>Zxp;+j@vztC$O4UE}-LJKollq;V~i;5uky(^!9N55{d8%BUxzO<3qT-#<20on?HPJ`TF z%UQqy{2AmpCk8wf^u-SOR1k0%x*ojWYMTv+-TesIa+3>qDj4H<=RYzQGW$ozdzE=9 z$i0E%f(^0g=-3&x!da_mt$Yrm#+v&9)AKowsB%3jhk~eSCKE9JEdx<2Ob*Iv2|3$} zgl#k#oNKCFBOt(+2i_QR+t!;7AHtn$d$Pl)D0%~N5FKS(59lCYfW}{&TtEj|1&v82 z7eM1-U}k!pciSPy$B2Mt{<@so1+sbOiS=?Ja+iF$u!pG7UASxXEJQlf_LvYxWQ^#xq!BGY2+nY zaAg+chQ#gc0@OJN>T;76yqoN(;GbUzau3ChKNB2ehwGa?7*upOnW8tuQ{4!|){{%r zH$Z?hM?mRa=n!bCN9710Fx_N$hzJPW1i9Nx4u@x2g51N7sDRkL1)?^ZR{?#diyL7$ zdkC971c367F7abpxsY|jVa5@rt=S5$tN{u&CWDNoCCHUI$O7a(h6V>){;ops?f_5` zICE@++$M_|(10|Afhb^RxuH`y7qLN093QqRPxG{(dc&ilbfJ1M2Jt$Z-q{xcKx(C*dUC zN{{MC*G(Kz03@y4#&DJ+s!DF&asee@>EI5K`vP(c%{%5>pz$!k&5=Feh~MsNCBVDW zU9Ip@M%e7rm{)iWF8XkJ6Y7lf3Q~}k;L854eAx1G%cqMgpHp;Kc1QWNFqwdh&t_!=ZE)2X(9F-n*|5Ym8^C*GS3UvFd<_6S!o1^AlS{?NFyM|D_gveRsMQWT_`TPF zWd02&Z}qv~yA8H1rV9Ihp{N$Sd4+W@$lVz`V4UyJ3t(ju(0jsS7Erbipw+H3Ii^I2 z-6H}~zY&Of+Y!}FCu)f!s;Ms5(M@6je+56!0UGdoy<3p?z|pD30SZUc5{P;bfUY;0 z0Nx*Ufa1rra@*Fm0-$wpo{hKf<9$Mi%3#QGdhTqpg50cF1fFt}7+);}?<#P_ae5|E zxjE~DB$ru$n%HNq?RCg^f3qpd%4o34;Uu814srSJt>?QHT$*k&0l%)e6>_spju&hp z-`_xP;V0=d9hXCv}nW8MX{%hs;AOHEeT>}`m8s)uQ*>gq8RtwlWRLlpHK(PYs+KjwG%*PKMQWa zyfztHNQ%HwLz?T%!6p8rc|fZA(E^ zH%mQ_nq1rQZmL1oRq@#s37TLsh=wXR(&hVXUG8Pb)tF4FE_V!WAJgOH?tmPJBsiLu zP$hAE}ps}Z;@eB)vP`3L!8Uy$g2pFIhQG%arIZmN|4*=Xk>rSCF)y8BQKA+wrdbs zjz6gWv~q3BK=P?(1NRHru8f^^wgJ84&4oJlDP-gM*lFTspts7rf^u4dcjbtCcaz~} zb945eu;p-4G~b`VmV6zgZ{rb%Kr>zLZ6MIoWZ2JzAa{kvz&N=jkh{#}0uXos8V{Hp z_MMin*`v@w&M}#yL|u#yQf9VHV=j$NK@_jmlb@apFB%?2z)bT4QG z&N^2?jtf4iZ^wJUWo>jU@2^2_vIQJ8q$R}tI5#I9smtB%%7Hv-K{GY{OtNPKzo`D zdQ(~g0?W}hI87pbs@yoo`}w+DEvV#h8t~FK+wmT-*+Br5^Xied*<3(pIzWrU{iOpm z;2)8!c7XCA7Kl0nMDe+Ur6x8(^MY|fspSSFw@Rt0;2MU ztCfI1GVYA%OgHbU^vy1Er5Et4u3cT}p@XF*q~aLFpYIa^weln6YRxOwis0RV*h>Bj zhO=_>uINL39c+hvou9eoGuV}FrG0;{?FFv%`kJi}(IzOp7A6yLk2*QFRz?BtrRH70 z+x0-;^Nh&_98>Edx8CFeX1&)yBU8asFPDm)1R7t2EuRWl^10vp5yIHpLIC&E5@?)= z&V8-P1RMli9E~k??>2+Rz)9>6*dbpYKvG&l>^3+W+0+ENodu#c3Pjz8YYAU1iqg0y zc8dELaPx))Fx3RT>hK9T`0oThtIaELu6kEj$oD28-?t0-{tmW$rK-|Hu-4(je=jIh zP;Xqv_y|Cf1-VBcH`F#6a26g2q66pC6L4bjO#zOkm3xTHhwXl5!)urjs9iwgK_-VE z)5>+F76u)6(Do=dP@&WdeeZ184lo;@qq#m$L7NJkp3ZjI20RTMfnY@Yu*L?*ET1*lIQW5%j^Q!b~jf7 z?moj&wq+()X`i{S%t7g~O|$#t+FlYnj8&kLIRP}cZ3r|@b&Vn5Umm{eN-sceG-_q3 zZ8G3II}Ww-sLAmF5@I(5R6b*J0hRnWG<9Yc=%DA7VhhT zPvC6V9ow1li=7Gi9s?ThGdaXNEkSOh8^)UJa&I_%=v{6aN5Y`oTqxS#1^@w`va}VV z@;Mq32dYMTRBmuY1svX6fdI#QzUT>?{Sz8e;C!|zmZ(vnqRLVLN7KqB>R)Iofn(P| zK-T0Jt_X2o4>{fe0;afCI826`cLB%L>khJjJJ@^&8I;o!Vpr=R<8dX(?GDIz=P!Et zY=$i-G1Pflxq!9@pzJf)Q6bx(9NewURtUicM`I^l?mW;q&}0H8g4MBbH%6i+n0JTk zn_U5B0*AAGP#}xqVpk7j)tX#DC7*(8Asxy2P|sV?^+nEjo}1PmN! zg4r1+7f|mHLgP-8+ezQ-c;xOrlfi8#Eg==1T?qti_8m8KvOseo`x;d60v0gnO$HTw zItUo_{sFlu7H|Bh#{Db@8LLr9;{-Rk2iU6)2~HaCr0Jzf1wH<kh8atGd>;gUl|E<_afMQItVDA%{YBFSo#70Z2&4b)o}*NrDBXD z3SJc=Y71;R5wgGKe(%npkxv$F?0ba-UFP8CHUznikgGJWPSU;G=4cGK_$_uc29(}u z7*;sK@`%q(<1#ns5IrI8$Gbs?f0di-N?k`(z%$b(hd^f?fs1frbI@Vu76P>va>L9k z9wLHVIW+iAP^xeCK=6K#$pqZ4_e4e4nq0u$|4PU`V{!qPm(x)KOa*>SE0@{jFu25Q z0;rhp#t?SFTvv{AV@N;Gan|KQD0sIQa`%#L zmJL~uyUOt%a9k{Kkg>RPA#3X(Yim0cb~eXB7V!7J{|(5dm{$SUvOaDM3Ai*q=9t9@ zk(Lm<>#)Q7OeWxo@>G<-VnG7Zdf U5)YW0`Z{IuiNU&N(E82|tP diff --git a/script/testsolo.bo b/script/testsolo.bo index 294076a1b82734a464ecc21f47de0694be114e9d..d44323f3d95452a9e324e599bece697ec63f0381 100644 GIT binary patch delta 49 wcmaDT`%renVh)wVUl%yYF#v%)6OfQmJIleqpr8X}ii1VvK=PZ9b4alP08*q3LjV8( delta 49 vcmaDT`%renVh)x5Ltky=7=S>Y2}sDOo#kL)P|yJ~#lfO-Ao ..." -include "/home/ubuntu/buzz/src/include/vec2.bzz" -#################################################################################################### -# Updater related -# This should be here for the updater to work, changing position of code will crash the updater -#################################################################################################### -updated="update_ack" -update_no=0 -function updated_neigh(){ -neighbors.broadcast(updated, update_no) -} - -TARGET_ALTITUDE = 3.0 -CURSTATE = "TURNEDOFF" - -# Lennard-Jones parameters -TARGET = 10.0 #0.000001001 -EPSILON = 18.0 #0.001 - -# Lennard-Jones interaction magnitude -function lj_magnitude(dist, target, epsilon) { - return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) -} - -# Neighbor data to LJ interaction vector -function lj_vector(rid, data) { - return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) -} - -# Accumulator of neighbor LJ interactions -function lj_sum(rid, data, accum) { - return math.vec2.add(data, accum) -} - -# Calculates and actuates the flocking interaction -function hexagon() { - statef=hexagon - CURSTATE = "HEXAGON" - # Calculate accumulator - var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) - if(neighbors.count() > 0) - math.vec2.scale(accum, 1.0 / neighbors.count()) - # Move according to vector - #print("Robot ", id, "must push ",accum.length, "; ", accum.angle) - uav_moveto(accum.x,accum.y) - -# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS -# timeW =0 -# statef=land -# } else { -# timeW = timeW+1 -# uav_moveto(0.0,0.0) -# } -} - -######################################## -# -# BARRIER-RELATED FUNCTIONS -# -######################################## - -# -# Constants -# -BARRIER_VSTIG = 1 -# ROBOTS = 3 # number of robots in the swarm - -# -# Sets a barrier -# -function barrier_set(threshold, transf) { - statef = function() { - barrier_wait(threshold, transf); - } - barrier = stigmergy.create(BARRIER_VSTIG) -} - -# -# Make yourself ready -# -function barrier_ready() { - barrier.put(id, 1) -} - -# -# Executes the barrier -# -WAIT_TIMEOUT = 200 -timeW=0 -function barrier_wait(threshold, transf) { - barrier.get(id) - CURSTATE = "BARRIERWAIT" - if(barrier.size() >= threshold) { - barrier = nil - transf() - } else if(timeW>=WAIT_TIMEOUT) { - barrier = nil - statef=land - timeW=0 - } - timeW = timeW+1 -} - -# flight status - -function idle() { -statef=idle -CURSTATE = "IDLE" - -} - -function takeoff() { - CURSTATE = "TAKEOFF" - statef=takeoff - log("TakeOff: ", flight.status) - log("Relative position: ", position.altitude) - - if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { - barrier_set(ROBOTS,hexagon) - barrier_ready() - #statef=hexagon - } - else { - log("Altitude: ", TARGET_ALTITUDE) - neighbors.broadcast("cmd", 22) - uav_takeoff(TARGET_ALTITUDE) - } -} -function land() { - CURSTATE = "LAND" - statef=land - log("Land: ", flight.status) - if(flight.status == 2 or flight.status == 3){ - neighbors.broadcast("cmd", 21) - uav_land() - } - else { - timeW=0 - barrier = nil - statef=idle - } -} - -# Executed once at init time. -function init() { - s = swarm.create(1) -# s.select(1) - s.join() - statef=idle - CURSTATE = "IDLE" -} - -# Executed at each time step. -function step() { - if(flight.rc_cmd==22) { - log("cmd 22") - flight.rc_cmd=0 - statef = takeoff - CURSTATE = "TAKEOFF" - neighbors.broadcast("cmd", 22) - } else if(flight.rc_cmd==21) { - log("cmd 21") - log("To land") - flight.rc_cmd=0 - statef = land - CURSTATE = "LAND" - neighbors.broadcast("cmd", 21) - } else if(flight.rc_cmd==16) { - flight.rc_cmd=0 - statef = idle - uav_goto() - } else if(flight.rc_cmd==400) { - flight.rc_cmd=0 - uav_arm() - neighbors.broadcast("cmd", 400) - } else if (flight.rc_cmd==401){ - flight.rc_cmd=0 - uav_disarm() - neighbors.broadcast("cmd", 401) - } -neighbors.listen("cmd", - function(vid, value, rid) { - print("Got (", vid, ",", value, ") from robot #", rid) - if(value==22 and CURSTATE=="IDLE") { - statef=takeoff - } else if(value==21) { - statef=land - } else if(value==400 and CURSTATE=="IDLE") { - uav_arm() - } else if(value==401 and CURSTATE=="IDLE"){ - uav_disarm() - } - } - -) - statef() - log("Current state: ", CURSTATE) - log("Swarm size: ",ROBOTS) -} - -# Executed once when the robot (or the simulator) is reset. -function reset() { -} - -# Executed once at the end of experiment. -function destroy() { -} diff --git a/script/update_sim/testflockfev6.bzz b/script/update_sim/testflockfev6.bzz deleted file mode 100644 index 1845eb9..0000000 --- a/script/update_sim/testflockfev6.bzz +++ /dev/null @@ -1,208 +0,0 @@ -# We need this for 2D vectors -# Make sure you pass the correct include path to "bzzc -I ..." -include "/home/ubuntu/buzz/src/include/vec2.bzz" -#################################################################################################### -# Updater related -# This should be here for the updater to work, changing position of code will crash the updater -#################################################################################################### -updated="update_ack" -update_no=0 -function updated_neigh(){ -neighbors.broadcast(updated, update_no) -} - -TARGET_ALTITUDE = 3.0 -CURSTATE = "TURNEDOFF" - -# Lennard-Jones parameters -TARGET = 10.0 #0.000001001 -EPSILON = 18.0 #0.001 - -# Lennard-Jones interaction magnitude -function lj_magnitude(dist, target, epsilon) { - return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) -} - -# Neighbor data to LJ interaction vector -function lj_vector(rid, data) { - return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) -} - -# Accumulator of neighbor LJ interactions -function lj_sum(rid, data, accum) { - return math.vec2.add(data, accum) -} - -# Calculates and actuates the flocking interaction -function hexagon() { - statef=hexagon - CURSTATE = "HEXAGON" - # Calculate accumulator - var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) - if(neighbors.count() > 0) - math.vec2.scale(accum, 1.0 / neighbors.count()) - # Move according to vector - #print("Robot ", id, "must push ",accum.length, "; ", accum.angle) - uav_moveto(accum.x,accum.y) - -# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS -# timeW =0 -# statef=land -# } else { -# timeW = timeW+1 -# uav_moveto(0.0,0.0) -# } -} - -######################################## -# -# BARRIER-RELATED FUNCTIONS -# -######################################## - -# -# Constants -# -BARRIER_VSTIG = 1 -# ROBOTS = 3 # number of robots in the swarm - -# -# Sets a barrier -# -function barrier_set(threshold, transf) { - statef = function() { - barrier_wait(threshold, transf); - } - barrier = stigmergy.create(BARRIER_VSTIG) -} - -# -# Make yourself ready -# -function barrier_ready() { - barrier.put(id, 1) -} - -# -# Executes the barrier -# -WAIT_TIMEOUT = 200 -timeW=0 -function barrier_wait(threshold, transf) { - barrier.get(id) - CURSTATE = "BARRIERWAIT" - if(barrier.size() >= threshold) { - barrier = nil - transf() - } else if(timeW>=WAIT_TIMEOUT) { - barrier = nil - statef=land - timeW=0 - } - timeW = timeW+1 -} - -# flight status - -function idle() { -statef=idle -CURSTATE = "IDLE" - -} - -function takeoff() { - CURSTATE = "TAKEOFF" - statef=takeoff - log("TakeOff: ", flight.status) - log("Relative position: ", position.altitude) - - if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { - barrier_set(ROBOTS,hexagon) - barrier_ready() - #statef=hexagon - } - else { - log("Altitude: ", TARGET_ALTITUDE) - neighbors.broadcast("cmd", 22) - uav_takeoff(TARGET_ALTITUDE) - } -} -function land() { - CURSTATE = "LAND" - statef=land - log("Land: ", flight.status) - if(flight.status == 2 or flight.status == 3){ - neighbors.broadcast("cmd", 21) - uav_land() - } - else { - timeW=0 - barrier = nil - statef=idle - } -} - -# Executed once at init time. -function init() { - s = swarm.create(1) -# s.select(1) - s.join() - statef=idle - CURSTATE = "IDLE" -} - -# Executed at each time step. -function step() { - if(flight.rc_cmd==22) { - log("cmd 22") - flight.rc_cmd=0 - statef = takeoff - CURSTATE = "TAKEOFF" - neighbors.broadcast("cmd", 22) - } else if(flight.rc_cmd==21) { - log("cmd 21") - log("To land") - flight.rc_cmd=0 - statef = land - CURSTATE = "LAND" - neighbors.broadcast("cmd", 21) - } else if(flight.rc_cmd==16) { - flight.rc_cmd=0 - statef = idle - uav_goto() - } else if(flight.rc_cmd==400) { - flight.rc_cmd=0 - uav_arm() - neighbors.broadcast("cmd", 400) - } else if (flight.rc_cmd==401){ - flight.rc_cmd=0 - uav_disarm() - neighbors.broadcast("cmd", 401) - } -neighbors.listen("cmd", - function(vid, value, rid) { - print("Got (", vid, ",", value, ") from robot #", rid) - if(value==22 and CURSTATE=="IDLE") { - statef=takeoff - } else if(value==21) { - statef=land - } else if(value==400 and CURSTATE=="IDLE") { - uav_arm() - } else if(value==401 and CURSTATE=="IDLE"){ - uav_disarm() - } - } - -) - statef() - log("Current state: ", CURSTATE) - log("Swarm size: ",ROBOTS) -} - -# Executed once when the robot (or the simulator) is reset. -function reset() { -} - -# Executed once at the end of experiment. -function destroy() { -} diff --git a/script/update_sim/testflockfev7.bzz b/script/update_sim/testflockfev7.bzz deleted file mode 100644 index 1845eb9..0000000 --- a/script/update_sim/testflockfev7.bzz +++ /dev/null @@ -1,208 +0,0 @@ -# We need this for 2D vectors -# Make sure you pass the correct include path to "bzzc -I ..." -include "/home/ubuntu/buzz/src/include/vec2.bzz" -#################################################################################################### -# Updater related -# This should be here for the updater to work, changing position of code will crash the updater -#################################################################################################### -updated="update_ack" -update_no=0 -function updated_neigh(){ -neighbors.broadcast(updated, update_no) -} - -TARGET_ALTITUDE = 3.0 -CURSTATE = "TURNEDOFF" - -# Lennard-Jones parameters -TARGET = 10.0 #0.000001001 -EPSILON = 18.0 #0.001 - -# Lennard-Jones interaction magnitude -function lj_magnitude(dist, target, epsilon) { - return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) -} - -# Neighbor data to LJ interaction vector -function lj_vector(rid, data) { - return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) -} - -# Accumulator of neighbor LJ interactions -function lj_sum(rid, data, accum) { - return math.vec2.add(data, accum) -} - -# Calculates and actuates the flocking interaction -function hexagon() { - statef=hexagon - CURSTATE = "HEXAGON" - # Calculate accumulator - var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) - if(neighbors.count() > 0) - math.vec2.scale(accum, 1.0 / neighbors.count()) - # Move according to vector - #print("Robot ", id, "must push ",accum.length, "; ", accum.angle) - uav_moveto(accum.x,accum.y) - -# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS -# timeW =0 -# statef=land -# } else { -# timeW = timeW+1 -# uav_moveto(0.0,0.0) -# } -} - -######################################## -# -# BARRIER-RELATED FUNCTIONS -# -######################################## - -# -# Constants -# -BARRIER_VSTIG = 1 -# ROBOTS = 3 # number of robots in the swarm - -# -# Sets a barrier -# -function barrier_set(threshold, transf) { - statef = function() { - barrier_wait(threshold, transf); - } - barrier = stigmergy.create(BARRIER_VSTIG) -} - -# -# Make yourself ready -# -function barrier_ready() { - barrier.put(id, 1) -} - -# -# Executes the barrier -# -WAIT_TIMEOUT = 200 -timeW=0 -function barrier_wait(threshold, transf) { - barrier.get(id) - CURSTATE = "BARRIERWAIT" - if(barrier.size() >= threshold) { - barrier = nil - transf() - } else if(timeW>=WAIT_TIMEOUT) { - barrier = nil - statef=land - timeW=0 - } - timeW = timeW+1 -} - -# flight status - -function idle() { -statef=idle -CURSTATE = "IDLE" - -} - -function takeoff() { - CURSTATE = "TAKEOFF" - statef=takeoff - log("TakeOff: ", flight.status) - log("Relative position: ", position.altitude) - - if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { - barrier_set(ROBOTS,hexagon) - barrier_ready() - #statef=hexagon - } - else { - log("Altitude: ", TARGET_ALTITUDE) - neighbors.broadcast("cmd", 22) - uav_takeoff(TARGET_ALTITUDE) - } -} -function land() { - CURSTATE = "LAND" - statef=land - log("Land: ", flight.status) - if(flight.status == 2 or flight.status == 3){ - neighbors.broadcast("cmd", 21) - uav_land() - } - else { - timeW=0 - barrier = nil - statef=idle - } -} - -# Executed once at init time. -function init() { - s = swarm.create(1) -# s.select(1) - s.join() - statef=idle - CURSTATE = "IDLE" -} - -# Executed at each time step. -function step() { - if(flight.rc_cmd==22) { - log("cmd 22") - flight.rc_cmd=0 - statef = takeoff - CURSTATE = "TAKEOFF" - neighbors.broadcast("cmd", 22) - } else if(flight.rc_cmd==21) { - log("cmd 21") - log("To land") - flight.rc_cmd=0 - statef = land - CURSTATE = "LAND" - neighbors.broadcast("cmd", 21) - } else if(flight.rc_cmd==16) { - flight.rc_cmd=0 - statef = idle - uav_goto() - } else if(flight.rc_cmd==400) { - flight.rc_cmd=0 - uav_arm() - neighbors.broadcast("cmd", 400) - } else if (flight.rc_cmd==401){ - flight.rc_cmd=0 - uav_disarm() - neighbors.broadcast("cmd", 401) - } -neighbors.listen("cmd", - function(vid, value, rid) { - print("Got (", vid, ",", value, ") from robot #", rid) - if(value==22 and CURSTATE=="IDLE") { - statef=takeoff - } else if(value==21) { - statef=land - } else if(value==400 and CURSTATE=="IDLE") { - uav_arm() - } else if(value==401 and CURSTATE=="IDLE"){ - uav_disarm() - } - } - -) - statef() - log("Current state: ", CURSTATE) - log("Swarm size: ",ROBOTS) -} - -# Executed once when the robot (or the simulator) is reset. -function reset() { -} - -# Executed once at the end of experiment. -function destroy() { -} diff --git a/src/buzz_update.cpp b/src/buzz_update.cpp index 3a77dba..69d9ec8 100644 --- a/src/buzz_update.cpp +++ b/src/buzz_update.cpp @@ -29,7 +29,7 @@ static int updated=0; /*Initialize updater*/ void init_update_monitor(const char* bo_filename, const char* stand_by_script){ - fprintf(stdout,"intiialized file monitor.\n"); + ROS_INFO("intiialized file monitor.\n"); fd=inotify_init1(IN_NONBLOCK); if ( fd < 0 ) { perror( "inotify_init error" ); @@ -48,7 +48,7 @@ void init_update_monitor(const char* bo_filename, const char* stand_by_script){ BO_BUF = (uint8_t*)malloc(bcode_size); if(fread(BO_BUF, 1, bcode_size, fp) < bcode_size) { perror(bo_filename); - fclose(fp); + //fclose(fp); //return 0; } fclose(fp); @@ -65,7 +65,7 @@ void init_update_monitor(const char* bo_filename, const char* stand_by_script){ STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size); if(fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) { perror(stand_by_script); - fclose(fp); + //fclose(fp); //return 0; } fclose(fp); @@ -147,7 +147,7 @@ void code_message_outqueue_append(){ void code_message_inqueue_append(uint8_t* msg,uint16_t size){ updater->inmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); - fprintf(stdout,"in ms append code size %d\n", (int) size); + //ROS_INFO("[DEBUG] Updater append code of size %d\n", (int) size); updater->inmsg_queue->queue = (uint8_t*)malloc(size); updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); memcpy(updater->inmsg_queue->queue, msg, size); @@ -156,9 +156,9 @@ void code_message_inqueue_append(uint8_t* msg,uint16_t size){ void code_message_inqueue_process(){ int size=0; - fprintf(stdout,"[debug]Updater mode %d \n", *(int*)(updater->mode) ); - fprintf(stdout,"[debug] %u : current update number, %u : received update no \n",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue)) ); - fprintf(stdout,"[debug]Updater code size %u \n",(*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)) ) ); + ROS_INFO("[Debug] Updater processing in msg with mode %d \n", *(int*)(updater->mode) ); + ROS_INFO("[Debug] %u : Current update number, %u : Received update no \n",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue)) ); + ROS_INFO("[Debug] Updater received code of size %u \n",(*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)) ) ); if( *(int*) (updater->mode) == CODE_RUNNING){ //fprintf(stdout,"[debug]Inside inmsg code running"); @@ -191,7 +191,6 @@ void update_routine(const char* bcfname, const char* dbgfname){ dbgf_name=(char*)dbgfname; buzzvm_t VM = buzz_utility::get_vm(); - buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); buzzvm_gstore(VM); @@ -199,37 +198,13 @@ void update_routine(const char* bcfname, if(*(int*)updater->mode==CODE_RUNNING){ buzzvm_function_call(VM, "updated_neigh", 0); if(check_update()){ - std::string bzzfile_name(bzz_file); - stringstream bzzfile_in_compile; - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")); - bzzfile_in_compile<update_no) =update_no +1; code_message_outqueue_append(); VM = buzz_utility::get_vm(); - fprintf(stdout,"Update no %d\n", *(uint16_t*)(updater->update_no)); + ROS_INFO("Current Update no %d\n", *(uint16_t*)(updater->update_no)); buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); buzzvm_gstore(VM); neigh=-1; - fprintf(stdout,"Sending code... \n"); + ROS_INFO("Sending code... \n"); code_message_outqueue_append(); } delete_p(BO_BUF); @@ -268,7 +243,7 @@ void update_routine(const char* bcfname, else{ //gettimeofday(&t1, NULL); if(neigh==0 && (!is_msg_present())){ - fprintf(stdout,"Sending code... \n"); + ROS_INFO("Sending code... \n"); code_message_outqueue_append(); } @@ -277,7 +252,7 @@ void update_routine(const char* bcfname, buzzvm_gload(VM); buzzobj_t tObj = buzzvm_stack_at(VM, 1); buzzvm_pop(VM); - fprintf(stdout,"Barrier ..................... %i \n",tObj->i.value); + ROS_INFO("Barrier ..................... %i \n",tObj->i.value); if(tObj->i.value==no_of_robot) { *(int*)(updater->mode) = CODE_RUNNING; gettimeofday(&t2, NULL); @@ -307,12 +282,12 @@ return (uint8_t*)updater->outmsg_queue->size; int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){ if(buzz_utility::buzz_update_init_test(BO_BUF, dbgfname,bcode_size)){ - fprintf(stdout,"Initializtion of script test passed\n"); + ROS_WARN("Initializtion of script test passed\n"); if(buzz_utility::update_step_test()){ /*data logging*/ //start =1; /*data logging*/ - fprintf(stdout,"Step test passed\n"); + ROS_WARN("Step test passed\n"); *(int*) (updater->mode) = CODE_STANDBY; //fprintf(stdout,"updater value = %i\n",updater->mode); delete_p(updater->bcode); @@ -330,12 +305,12 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){ /*Unable to step something wrong*/ else{ if(*(int*) (updater->mode) == CODE_RUNNING){ - fprintf(stdout,"step test failed, stick to old script\n"); + ROS_ERROR("step test failed, stick to old script\n"); buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t)*(size_t*)(updater->bcode_size)); } else{ /*You will never reach here*/ - fprintf(stdout,"step test failed, Return to stand by\n"); + ROS_ERROR("step test failed, Return to stand by\n"); buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname,(size_t) *(size_t*)(updater->standby_bcode_size)); buzzvm_t VM = buzz_utility::get_vm(); @@ -350,12 +325,12 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){ } else { if(*(int*) (updater->mode) == CODE_RUNNING){ - fprintf(stdout,"Initialization test failed, stick to old script\n"); + ROS_ERROR("Initialization test failed, stick to old script\n"); buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname,(int)*(size_t*) (updater->bcode_size)); } else{ /*You will never reach here*/ - fprintf(stdout,"Initialization test failed, Return to stand by\n"); + ROS_ERROR("Initialization test failed, Return to stand by\n"); buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname,(size_t) *(size_t*)(updater->standby_bcode_size)); buzzvm_t VM = buzz_utility::get_vm(); @@ -420,6 +395,23 @@ void updates_set_robots(int robots){ no_of_robot=robots; } +/*-------------------------------------------------------- +/ Create Buzz bytecode from the bzz script inputed +/-------------------------------------------------------*/ +int compile_bzz(){ + /*Compile the buzz code .bzz to .bo*/ + std::string bzzfile_name(bzz_file); + stringstream bzzfile_in_compile; + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); + name = name.substr(0,name.find_last_of(".")); + bzzfile_in_compile << "bzzc -I " << path << "include/"; //<<" "<state); + return 0; + } + // Call the Init() function + if(buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY){ + ROS_ERROR("Error in calling init, VM state : %i", VM->state); + return 0; + } /* All OK */ - ROS_INFO("[%i] INIT DONE!!!", Robot_id); + ROS_INFO("[%i] INIT DONE!!!", Robot_id); return 1;//buzz_update_set(BO_BUF, bdbg_filename, bcode_size); } @@ -554,10 +561,17 @@ static int create_stig_tables() { //cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl; return 0; } + // Execute the global part of the script - buzzvm_execute_script(VM); + if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){ + ROS_ERROR("Error executing global part, VM state : %i",VM->state); + return 0; + } // Call the Init() function - buzzvm_function_call(VM, "init", 0); + if(buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY){ + ROS_ERROR("Error in calling init, VM state : %i", VM->state); + return 0; + } // All OK return 1; } @@ -604,9 +618,15 @@ static int create_stig_tables() { return 0; } // Execute the global part of the script - buzzvm_execute_script(VM); + if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){ + ROS_ERROR("Error executing global part, VM state : %i",VM->state); + return 0; + } // Call the Init() function - buzzvm_function_call(VM, "init", 0); + if(buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY){ + ROS_ERROR("Error in calling init, VM state : %i", VM->state); + return 0; + } // All OK return 1; } @@ -725,19 +745,25 @@ static int create_stig_tables() { } int update_step_test() { - + /*Process available messages*/ + in_message_process(); buzzuav_closures::buzzuav_update_battery(VM); buzzuav_closures::buzzuav_update_prox(VM); buzzuav_closures::buzzuav_update_currentpos(VM); buzzuav_closures::update_neighbors(VM); update_users(); buzzuav_closures::buzzuav_update_flight_status(VM); + //set_robot_var(buzzdict_size(VM->swarmmembers)+1); int a = buzzvm_function_call(VM, "step", 0); - if(a != BUZZVM_STATE_READY) { + + if(a!= BUZZVM_STATE_READY) { + ROS_ERROR("%s: execution terminated abnormally: %s\n\n", + BO_FNAME, + buzz_error_info()); fprintf(stdout, "step test VM state %i\n",a); - fprintf(stdout, " execution terminated abnormally\n\n"); - } + } + return a == BUZZVM_STATE_READY; } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 273c912..1f27497 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -7,11 +7,6 @@ namespace rosbzz_node{ ---------------*/ roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv) { - - home[0]=0.0;home[1]=0.0;home[2]=0.0; - target[0]=0.0;target[1]=0.0;target[2]=0.0; - cur_pos[0]=0.0;cur_pos[1]=0.0;cur_pos[2]=0.0; - ROS_INFO("Buzz_node"); /*Obtain parameters from ros parameter server*/ Rosparameters_get(n_c_priv); @@ -34,7 +29,11 @@ namespace rosbzz_node{ setpoint_counter = 0; fcu_timeout = TIMEOUT; - while(cur_pos[2] == 0.0f){ + cur_pos.longitude = 0; + cur_pos.latitude = 0; + cur_pos.altitude = 0; + + while(cur_pos.latitude == 0.0f){ ROS_INFO("Waiting for GPS. "); ros::Duration(0.5).sleep(); ros::spinOnce(); @@ -113,10 +112,11 @@ namespace rosbzz_node{ get_number_of_robots(); //if(neighbours_pos_map.size() >0) no_of_robots =neighbours_pos_map.size()+1; //buzz_utility::set_robot_var(no_of_robots); - /*Set no of robots for updates*/ + /*Set no of robots for updates TODO only when not updating*/ + //if(multi_msg) updates_set_robots(no_of_robots); - /*run once*/ - ros::spinOnce(); + /*run once*/ + ros::spinOnce(); /*loop rate of ros*/ ros::Rate loop_rate(10); loop_rate.sleep(); @@ -128,7 +128,7 @@ namespace rosbzz_node{ timer_step+=1; maintain_pos(timer_step); - std::cout<< "HOME: " << home[0] << ", " << home[1]; + std::cout<< "HOME: " << home.latitude << ", " << home.longitude; } /* Destroy updater and Cleanup */ //update_routine(bcfname.c_str(), dbgfname.c_str(),1); @@ -215,6 +215,10 @@ namespace rosbzz_node{ else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");} if(node_handle.getParam("topics/stream", stream_client_name)); else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");} + + + + } /*-------------------------------------------------------- @@ -246,6 +250,8 @@ namespace rosbzz_node{ stream_client = n_c.serviceClient(stream_client_name); users_sub = n_c.subscribe("users_pos", 1000, &roscontroller::users_pos,this); + + local_pos_sub = n_c.subscribe("/mavros/local_position/pose", 1000, &roscontroller::local_pos_callback, this); multi_msg=true; } @@ -363,7 +369,8 @@ namespace rosbzz_node{ uint64_t* payload_out_ptr= buzz_utility::obt_out_msg(); uint64_t position[3]; /*Appened current position to message*/ - memcpy(position, cur_pos, 3*sizeof(uint64_t)); + double tmp[3];tmp[0]=cur_pos.latitude;tmp[1]=cur_pos.longitude;tmp[2]=cur_pos.altitude; + memcpy(position, tmp, 3*sizeof(uint64_t)); mavros_msgs::Mavlink payload_out; payload_out.payload64.push_back(position[0]); payload_out.payload64.push_back(position[1]); @@ -457,13 +464,7 @@ namespace rosbzz_node{ Arm(); ros::Duration(0.5).sleep(); // Registering HOME POINT. - if(home[0] == 0){ - //test #1: set home only once -- ok - home[0] = cur_pos[0]; home[1] = cur_pos[1]; home[2] = cur_pos[2]; - //test #2: set home mavros -- nope - //SetMavHomePosition(cur_pos[0], cur_pos[1], cur_pos[2]); - - } + home = cur_pos; } if(current_mode != "GUIDED") SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm) @@ -543,9 +544,9 @@ namespace rosbzz_node{ void roscontroller::set_cur_pos(double latitude, double longitude, double altitude){ - cur_pos [0] = latitude; - cur_pos [1] = longitude; - cur_pos [2] = altitude; + cur_pos.latitude =latitude; + cur_pos.longitude =longitude; + cur_pos.altitude =altitude; } /*----------------------------------------------------------- @@ -586,48 +587,10 @@ namespace rosbzz_node{ } } - void roscontroller::cvt_rangebearing_coordinates(double nei[], double out[], double cur[]){ - - // calculate earth radii - /*double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE))); - double prime_vertical_radius = equatorial_radius * sqrt(temp); - double radius_north = prime_vertical_radius * (1 - excentrity2) * temp; - double radius_east = prime_vertical_radius * cos(DEG2RAD(DEFAULT_REFERENCE_LATITUDE));*/ - - /*double d_lon = nei[1] - cur[1]; - double d_lat = nei[0] - cur[0]; - double ned[3]; - ned[0] = DEG2RAD(d_lat) * radius_north;//EARTH_RADIUS; - ned[1] = -DEG2RAD(d_lon) * radius_east; //EARTH_RADIUS - double ecef[3]; - double llh[3];llh[0]=DEG2RAD(cur[0]);llh[1]=DEG2RAD(cur[1]);llh[2]=cur[2]; - double d = WGS84_E * sin(llh[0]); - double N = WGS84_A / sqrt(1. - d*d); - ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]); - ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]); - ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]); - double ref_ecef[3]; - llh[0]=DEG2RAD(nei[0]);llh[1]=DEG2RAD(nei[1]);llh[2]=nei[2]; - d = WGS84_E * sin(llh[0]); - N = WGS84_A / sqrt(1. - d*d); - ref_ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]); - ref_ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]); - ref_ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]); - double M[3][3]; - ecef2ned_matrix(ref_ecef, M); - double ned[3]; - matrix_multiply(3, 3, 1, (double *)M, ecef, ned); - - out[0] = sqrt(ned[0]*ned[0]+ned[1]*ned[1]); - out[0] = std::floor(out[0] * 1000000) / 1000000; - out[1] = atan2(ned[1],ned[0]); - out[1] = std::floor(out[1] * 1000000) / 1000000; - out[2] = 0.0;*/ - - double d_lon = nei[1] - cur[1]; - double d_lat = nei[0] - cur[0]; - double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS; - double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0])); + void roscontroller::gps_rb(GPS nei_pos, double out[]) + { + float ned_x=0.0, ned_y=0.0; + gps_ned_cur(ned_x, ned_y, nei_pos); out[0] = sqrt(ned_x*ned_x+ned_y*ned_y); //out[0] = std::floor(out[0] * 1000000) / 1000000; out[1] = atan2(ned_y,ned_x); @@ -635,48 +598,30 @@ namespace rosbzz_node{ out[2] = 0.0; } - void roscontroller::cvt_ned_coordinates(double nei[], double out[], double cur[]){ - // calculate earth radii - /*double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE))); - double prime_vertical_radius = equatorial_radius * sqrt(temp); - double radius_north = prime_vertical_radius * (1 - excentrity2) * temp; - double radius_east = prime_vertical_radius * cos(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)); - - double d_lon = nei[1] - cur[1]; - double d_lat = nei[0] - cur[0]; - out[0] = DEG2RAD(d_lat) * radius_north;//EARTH_RADIUS; - out[0] = std::floor(out[0] * 1000000) / 1000000; - out[1] = -DEG2RAD(d_lon) * radius_east; //EARTH_RADIUS - out[1] = std::floor(out[1] * 1000000) / 1000000; - out[2] = cur[2]; - // Using functions of the library Swift Nav (https://github.com/swift-nav/libswiftnav) - double ecef[3]; - double llh[3];llh[0]=DEG2RAD(cur[0]);llh[1]=DEG2RAD(cur[1]);llh[2]=cur[2]; - double d = WGS84_E * sin(llh[0]); - double N = WGS84_A / sqrt(1. - d*d); - ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]); - ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]); - ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]); - double ref_ecef[3]; - llh[0]=DEG2RAD(nei[0]);llh[1]=DEG2RAD(nei[1]);llh[2]=nei[2]; - d = WGS84_E * sin(llh[0]); - N = WGS84_A / sqrt(1. - d*d); - ref_ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]); - ref_ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]); - ref_ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]); - double M[3][3]; - ecef2ned_matrix(ref_ecef, M); - matrix_multiply(3, 3, 1, (double *)M, ecef, out);*/ - - double d_lon = nei[1] - cur[1]; - double d_lat = nei[0] - cur[0]; - out[0] = DEG2RAD(d_lat) * EARTH_RADIUS; - //out[0] = std::floor(out[0] * 1000000) / 1000000; - out[1] = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0])); - //out[1] = std::floor(out[1] * 1000000) / 1000000; - out[2] = 0.0; + void roscontroller::gps_ned_cur(float &ned_x, float &ned_y, GPS t) + { + gps_convert_ned(ned_x, ned_y, + t.longitude, t.latitude, + cur_pos.longitude, cur_pos.latitude); } + void roscontroller::gps_ned_home(float &ned_x, float &ned_y) + { + gps_convert_ned(ned_x, ned_y, + cur_pos.longitude, cur_pos.latitude, + home.longitude, home.latitude); + } + + void roscontroller::gps_convert_ned(float &ned_x, float &ned_y, + double gps_t_lon, double gps_t_lat, + double gps_r_lon, double gps_r_lat) + { + double d_lon = gps_t_lon - gps_r_lon; + double d_lat = gps_t_lat - gps_r_lat; + ned_x = DEG2RAD(d_lat) * EARTH_RADIUS; + ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(gps_t_lat)); + }; + /*------------------------------------------------------ / Update battery status into BVM from subscriber /------------------------------------------------------*/ @@ -722,6 +667,13 @@ namespace rosbzz_node{ set_cur_pos(msg->latitude, msg->longitude, cur_rel_altitude);//msg->altitude); buzzuav_closures::set_currentpos(msg->latitude, msg->longitude, cur_rel_altitude);//msg->altitude); } + + void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose){ + local_pos_new[0] = pose->pose.position.x; + local_pos_new[1] = pose->pose.position.y; + local_pos_new[2] = pose->pose.position.z; + } + void roscontroller::users_pos(const rosbuzz::neigh_pos data){ //ROS_INFO("Altitude out: %f", cur_rel_altitude); @@ -735,9 +687,7 @@ namespace rosbzz_node{ us[0] = data.pos_neigh[it].latitude; us[1] = data.pos_neigh[it].longitude; us[2] = data.pos_neigh[it].altitude; - double out[3]; - cvt_rangebearing_coordinates(us, out, cur_pos); - //buzzuav_closures::set_userspos(out[0], out[1], out[2]); + buzz_utility::add_user(data.pos_neigh[it].position_covariance_type,data.pos_neigh[it].latitude, data.pos_neigh[it].longitude, data.pos_neigh[it].altitude); } @@ -770,16 +720,15 @@ namespace rosbzz_node{ moveMsg.header.stamp = ros::Time::now(); moveMsg.header.seq = setpoint_counter++; moveMsg.header.frame_id = 1; - double local_pos[3]; - cvt_ned_coordinates(cur_pos,local_pos,home); + float ned_x, ned_y; + gps_ned_home(ned_x, ned_y); // ROS_INFO("[%i] ROSBuzz Home: %.7f, %.7f", robot_id, home[0], home[1]); // ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos[0], local_pos[1]); - /*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/ - target[0]+=y; - target[1]+=x; - moveMsg.pose.position.x = target[0];//local_pos[1]+y; - moveMsg.pose.position.y = target[1]; + /*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/ + //target[0]+=y; target[1]+=x; + moveMsg.pose.position.x = local_pos_new[0]+y;//ned_y+y; + moveMsg.pose.position.y = local_pos_new[1]+x;//ned_x+x; moveMsg.pose.position.z = z; moveMsg.pose.orientation.x = 0; @@ -874,9 +823,13 @@ namespace rosbzz_node{ double neighbours_pos_payload[3]; memcpy(neighbours_pos_payload, message_obt, 3*sizeof(uint64_t)); buzz_utility::Pos_struct raw_neigh_pos(neighbours_pos_payload[0],neighbours_pos_payload[1],neighbours_pos_payload[2]); - // cout<<"Got" << neighbours_pos_payload[0] <<", " << neighbours_pos_payload[1] << ", " << neighbours_pos_payload[2] << endl; + GPS nei_pos; + nei_pos.latitude=neighbours_pos_payload[0]; + nei_pos.longitude=neighbours_pos_payload[1]; + nei_pos.altitude=neighbours_pos_payload[2]; double cvt_neighbours_pos_payload[3]; - cvt_rangebearing_coordinates(neighbours_pos_payload, cvt_neighbours_pos_payload, cur_pos); + // cout<<"Got" << neighbours_pos_payload[0] <<", " << neighbours_pos_payload[1] << ", " << neighbours_pos_payload[2] << endl; + gps_rb(nei_pos, cvt_neighbours_pos_payload); /*Extract robot id of the neighbour*/ uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3)); cout << "Rel Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_payload[0] << ", "<< cvt_neighbours_pos_payload[1] << ", "<< cvt_neighbours_pos_payload[2] << endl;