From 64581a3ebe0bdce7f6d49f8467853c6d65fc657c Mon Sep 17 00:00:00 2001
From: William Chen <billyskc@ucla.edu>
Date: Tue, 4 Sep 2018 17:50:21 -0700
Subject: [PATCH] modified schedule functionality of CoLo-AT
---
.../existing_dataset.cpython-36.pyc | Bin 12920 -> 12584 bytes
CoLo-AT/dataset_manager/existing_dataset.py | 32 +++++++-----------
.../__pycache__/robot_system.cpython-36.pyc | Bin 3636 -> 3628 bytes
CoLo-AT/robots/robot_system.py | 6 ++--
.../__pycache__/sim_manager.cpython-36.pyc | Bin 3882 -> 3922 bytes
.../__pycache__/state_recorder.cpython-36.pyc | Bin 4620 -> 4575 bytes
CoLo-AT/simulation_process/sim_manager.py | 7 +++-
CoLo-AT/simulation_process/state_recorder.py | 3 +-
CoLo-AT/test_simulation.py | 12 ++-----
9 files changed, 27 insertions(+), 33 deletions(-)
diff --git a/CoLo-AT/dataset_manager/__pycache__/existing_dataset.cpython-36.pyc b/CoLo-AT/dataset_manager/__pycache__/existing_dataset.cpython-36.pyc
index 3c4964b7c5fa18e5c4e54efd6d80b0a0f47841d3..67e140af40210d6d0afaeb1ef2c0b4393be92044 100644
GIT binary patch
delta 3277
zcma)8eP~<P6@T}=Crh$EElZYUN4ET-*ovLlYRlR<PNOzWvNj)E8z*IHh9>JiJI}Tx
z=jz$By{l@SlF@{=@pAV;N@=&j%3vQ*4BI+bVXPDigVBG=c(9F*!6=M<z!+tJv^(cM
z+mXX&CUJlF+;h)8I_KPT&eccs`!ntL2LkQhkz3E)xcEl<66tw({7SWKjDOKwTr`X2
z@#EDqRS5?lncjSdyhGf!ZdB-mErfRQp#eovN-%al8c=x{weJEL$|XXTODZK)y{25!
zs77^|b$USb>sOPbbcklIkxQL)j1JQgprSN;UdbL5n|hwuoB!0ygk&~n+Aesz4}&_E
zK<L<k2vU>cz0irhwkN`SZr|E_`>%U%pY3=$JpyK|2hUqYW>)6XmRU9?(;s3SSQH^E
z-tIU|j*5>vx=CInBctI*ko_paafB%`9XSA7jYxV38Md1mi&n8<u_Lg`65B<swIw5~
z$~B1pDpDe&Vz_hs^n-ZbgJ;?+%jE*A&Qz_M?PA5soMDH{MzOZcjIvR&3bbey?Ld{n
z)mm)LS^DN2egW9F_)h1vZw&Q~BOJONJw|dOA3aKJ@oaP|mO<_)!fAvH2xr7^qEChM
zNVfPSa<TEyW5_&#a1y{4)mRD?y%>9#oD#o|^|?Nc+|9qoZjlEb2QsT|Ifo~XBE!YJ
zNY5agMaUtP0J4N-#Ae*|JhQzRN-TIjzrFZYV%dY%NwK$j5@Ia*8P9=jMl<=tVGJwF
zBS&XXBP~7p98v~yT@0PI#l`MHaz(6n|LsB^_hPu`ZaIO*FnJ6uaY7;$v^fd_81zC3
zV6Mv6*fa{s6uyGgb@5iudyp3!`w|m!3y-ocgeL&9TS~n0QVS+sl_s<2Vb9+D?LH4_
zNI$0V04CZ_ejr2I>SE`RmeG^8CUJ?swXNJ=T4oH>1GCol?3j8J1>*?U5q6iH9k5uj
zv|xxr?~S;;5Y6$|!;Gs1OjUMs^ON4oYHGKE(*BdvwvJ(8N5yLYSmOq$x3#iaac<j>
zOHa)JgHVk$T88B^nlTHI^*g_%!Ef2_vQeuQA=Ukm!P199kuf`fiNCXC(wVQJ0(8Aa
z)}-pHEiwar<hnR9@M>^BYzJW+hf(`z^B)6$BIM`d{h=qveMzOB<jTB<yOIiTgW*jo
z6uI4&o451ioXUN?8_?a<>OG<>bF8tCyLp25@O{)z1FyKaPJ>g*2B}Q$(xZZ&5a>y9
zjrw8kg;|5SkEdvOLEli>zd_#1Q%U8rT2I&eE1%^l;IuEeH#9cD+h|8zv3$sBk}jn_
zfD|7{Dh*Hx<h-A!nnbb#d;rW->w|odf*bWT%-~AB-wMu$=EF47Bt)?~c$x<iii1&w
z_wyjEBu16E(j-<y%6C#f$o4h~)tXw9#FhGx72T3X4GpEK@=!vtVqMCvBYiDLGFBJQ
zSg{1E1*%CF6!7Yg4zHep2&VWD&(O|SbSHjXhxZ|RjtRITryel%Wyff3LBMN3ap!Tb
zr8D7D=Hw}$*2B;xmlumHy<#j@OQyBP2(Y30^C<T}L@+lPHZS&d1li^uLMjXJVH?2K
z?>3yh6S#*hFdJvzK>C{qn21iPcOrEj0kfM8Ajpizj^#8a4^XzttWaZ>y@*`d7**uz
zHN&7xN9G}1w^gdFTwG+DqKFrU{g3aL#lr5O09LXcl%-)-mX@u;3e?KWxFc%=tHgFH
z7<)lri5V+S4MFR%#RtRj##Iz%2sIQA!t+sOCbW;DWmYRJg*$x+y$ERlTdhvO$l2-&
z%quw=Ra-9>V2PMx>nJLNilMaKU{rycHHTQbV%qBBDvUMTTaC`l7n~@(fQOvOiuIBE
z$h!E^$dm3a(nX?bLVPlk)cs_Ni0<s2(*i~F2=cz<)1tTBgp4(Y9y)sl#dczKPi)xh
z;^XW=azl(AINbl|m+#=`-D4$)wETnt@%GV`+D#(vAALe)e-Y1(mX5s5JyfSU_XGN%
z5kR3&DU}Ft734#6&>bEP)&o?p2YG;NT!-Wjir<aJ8$qaJNPlQLZWuwR*bs2kR*rKX
zxNY}vT~i7)s1WyHiE$Mg3SqM#=h>RQobVubLDSN>2l(w$VwHyKfeJLNdVmKS+s4eg
zp!UM&=DrmD@u?5iuN`YCLM)vBR)l-h;Qw7`4ZOH*MZ5o2OrOm9?!C)SG)30ed8A}j
z`H(t;@KXeQOtRs-*KLRJ`Gd5Zoh>YyWz%Z8`tK=|ke2wz336|{YUGRZI;z@T66^=a
zdksNWoA|?_BS&sSaBA2h6yj<5c9STHY<)eXON|g6zCj(hDrB<OO@c%e$KP+{;Upxd
zY#tjxvtJ;58$l-TZJ=x)zL{IEVb*)sHTDwjMk>%I3biGpWX_r;S=G`-JFrq*G--hu
zwWVr>-faXe&%XG{M1;I4-ktbV7@dHBRd34aWHu&3xy;33Sg{8Yo<(RNd>P?6gs&hp
z5x$Cm&%M*bq$g;)<B7bNpW#w^_e*grmuMe^^#d?!`^6i%A@@@rZ~HJC6u-~K$rr@G
Ra+7VcTMmM3T%5gc^gjqA_EP`=
delta 3580
zcmZ`+YiwLc6~1%tUGJ{<<$d_Ien0Y9JKl9fNS(;8^Qax%rcs+XX}D?Gu6M4T-R#4e
zy>@LgHygSRAVPziQBhM;sQ7^@gpiOWC_+N;r~Kv*68ob92?<p~r9b-v5a-O@*y|v?
zy5F2RGjr~lGv|DB{o&FVwDn7YK<l|b-n$XM{)^UCa<QDcSuUEXH*$qSE>lch$raZM
z87o&VP0W?nbF5q{nk8#uxy&Zcm&>argRymU;$k_QDde`~#MJUyDT^yAyYG;XiQCZ4
z5}h=}l6GxsP*IdDJbQl{NN3=w{1m`c3_=w{rG%>6ilI@B>d@-+nD|_~Jv2f`p%<kS
z^e{cLO^g^#(gYm?shy5rQ4%S!rJo_j?r-!WAtSqqmaATJQdC1-<Qeg9XwH#c&%leW
ziN~R{P5oD%>R;dQ!`fK*6>>-v!u{m1xF0^%dm1ckMba;tnaUb7F&^oSB+Xa}oy~~9
zgvZ;Sg$ZmH;S9nl(GwXXGh#k6IEsq8o0<hHleX9t^b(}*qSofBnNVdPrhg}rB}c_)
zkrX*0`lCH5qpjHVE_hSM2$U%tlf^dEvg=#1q>u)+A#O$&eF=Q?F@$mPhv+Ox3U_RV
z7-B9q-9CcqqX=I?xP-7Ez8|~XHiKf5`{IjOYT_&^&mo)#FudqYI#*gQ*Ig^=9Nnnv
z@OB3`4YAn%?4jpiUR}$i*X!PrxnZRXxsurstz6M$b7*JxSMA>;C+0y+XpNnN%rLqM
z7xSWg1L1juB*Loz3BpFi*?3gE)p^ac*sS<Z=XKAuX2tEUB@gaxZ^>s}i=HFRir2ay
z9l<kTxNkVVmr#~5xs1|lsC6+6mm&J%Bjj~>^FOc3AhqL|yJ-bF$K*2f$_c78&>{`g
z19(vBEWk>cZL$|}QWwJO2%B<G@Amw5L1ve4p+wD%!MV$qPY;3zQJf-Ipheb!Z~-9E
zu<d2#rLu)*A$P(!j2XK>>h%y)ZtoN>h~a`KIfe?k1s8h(Wf^3-g>LNPyPg>LXV)0R
zB*&vOJbOF1gA+Ru9Ono6cRJ+>Y0ic{%)Fh(#AbJJbGv`;Ur_rFx+A@vU##mG8+KaE
z4jm)5C=B&wr7Lo?SMd#BLx22^T@CEk-9@uf$*h=$A2MEgmd-F{1Y~Zaqu6EA#TpvG
z4K>Y$NEzZELj$DY`tbcAo^lW#%<;;HyB`hzjgWQm{^-jhc_>-!=5F4_yQ%N7#&znS
zR(41!$(6i^ySf$L0*~6Q&_K6h$GJv>(Dp#9LEFpYG;~YfQQ0qHzL&?ll~+}}&+ae1
z#N(i8z2%0nPk0M$>r^ZsYHFlIu?JA%1Ja^W&9~<LJYFMG9pD4pT~qBrK1kv9b|19h
zg57Tgc^?mSDGtUI-p_;3lNeJvl^U@^R$D%tkI*2@?Wqx})wCMvRO}%u+L$dZ)Knho
zQmj~qB3D$OEa`1pGHkW;VJp^!W<jcvTgr~c9y&UE7$O$uLwuNqAL~xcxDG5Gxy0^4
zh@C{moRk&cOgf-LD#f8ER5ab7Qkg!;E<hP_0?H1{eiJ20RRpCkBOC@u=;G~T?bQzU
zE!2D)K~|D1B3W1$QHL4E1`%Y|Ax$_;-~p-b%3(dSyQtL>#t|e(sUX!`N@Z5e6qtr;
z7lIEUF(NaT-B%P1tK{<|vFdkmP^Kg%XWc0hEUdsPGuNF=#_SBNmMc!lNs3ji$+~JC
zXE$+q4k3@rgTTCHmV@e)S1;?qkv@c8gnj@+EyL3C@w&PW?YZ^iY(HigF2lW^VQHGH
zIKt}9TwN_}z_V$1%S(A6dfa&x%{g~~WU5b(4iF~1iI+)CWD?!#v?}f-9=))Cvhp}2
zm+MWDm$AiA19f7-VQ3&S2nPeh9?I9Ojm615u`_l`&FT+bv8f2mgt|~T;VOsJwIQ=0
zr^qGX_#ePngRPs)zAbH|v1&Za%!~!hKxq!YPcj90XX54b0CJ{+2dj`E*&2Qh=%`{^
z$^XCL@9#arzhBR0@yPKH*%TAvJUo@>AuBk^D>cQ|pe4}i;$LG^`VL_~6DP;B)layG
z>fCR&Snj+h@1?rc3KZb$hGL>VrzWW%$VQ`qX{GcL4cY<NxQ_$H0DT1Q5X=wqR;X@?
zQ0pP?gPz1jtvB0tQN7uA)o!!=jV00m6uch_J^%%OaM=Nu>@d(q*aC`yng>bNJYn08
z&JJ@A5AZe~rXi?#={jw*BUHDeJi>uFffJ(3t}BXF)nGSpctD(Pcs#&n(F2+&@L3Er
z>fRjCI3__GdP?h>R=}MY_n>23m2`U;=CuK}VjG5piE<Ytj0P?$`AAhxnWAAkQqs}K
zgZ=?at$aQ9^9z67+MaEqE6FUF)O*C_kk8X})y!7%+WjB4@?m{^+5#Z?DOO#aPeh+C
zl>KX8Qq)1tVLRxDBqK>yn1}n+<M7!6>fT4dTQt$WUvoxCUXnDoybNDw#az{DDnP6m
z`GpC^zq)3nfgqf#^dXu$NI~o!)FF|uM+omB?0qb<x3G)#ZTOj4SuK~Sq&OBvy#y@j
zdY|~+MBfF0Rfz1Q5Kq&um&8b<@$-<RszZ&QAty-(@u(4^!!M}A47G!FsBschMOX4q
z=V!nQkc=e#H$i3}AUr^jB(x1u-3LXUZhT23y!+8%-+)m=98bjtkd}<VI{bG-(~TYM
z)3@{G<8?O%elPB(PPXCA19bF5*~}+8#K);aH%6hyP9RheEQB?Lb%Zw%HW0QDY=pZA
z(sOjx@f<0RJw}j+`T<Hm7H22BT8BVH^Od6_J2~pT<nel3tw}Z{9!z$UuZo{co^Bb$
N*+Ve9Q~Y~!{J-C%HXr~1
diff --git a/CoLo-AT/dataset_manager/existing_dataset.py b/CoLo-AT/dataset_manager/existing_dataset.py
index ec3fc68..966391e 100755
--- a/CoLo-AT/dataset_manager/existing_dataset.py
+++ b/CoLo-AT/dataset_manager/existing_dataset.py
@@ -351,10 +351,14 @@ class Dataset:
v0 = self.dataset_data[req_type][robot_idx][prev_time_idx]['velocity']
v1 = self.dataset_data[req_type][robot_idx][post_time_idx]['velocity']
velocity = linear_interpolation([t0, v0], [t1, v1], req_time)
+ a_v0 = self.dataset_data[req_type][robot_idx][prev_time_idx]['angular velocity']
+ a_v1 = self.dataset_data[req_type][robot_idx][post_time_idx]['angular velocity']
+ a_v = linear_interpolation([t0, a_v0], [t1, a_v1], req_time)
o0 = self.dataset_data[req_type][robot_idx][prev_time_idx]['orientation']
o1 = self.dataset_data[req_type][robot_idx][post_time_idx]['orientation']
orientation = linear_interpolation([t0, o0], [t1, o1], req_time)
- message['data'] = {'time':req_time, 'velocity':velocity, 'orientation':orientation}
+
+ message['data'] = {'time':req_time, 'velocity':velocity, 'angular velocity': a_v, 'orientation':orientation, 'delta_t': 0}
else: #measurement: meas_info = {'time':time, 'subject_ID':subject_ID, 'measurment_range': measurment_range, 'bearing':bearing}
pass
@@ -375,8 +379,8 @@ class Dataset:
lx = matched_gt_data['x_pos']
ly = matched_gt_data['y_pos']
+ np.random.seed(6)
measurment_range = sqrt((lx-gt_x)*(lx-gt_x)+(ly-gt_y)*(ly-gt_y)) + float(np.random.normal(0, sqrt(var_dis), 1))
- # bearing not matching with closest dataline....
bearing = (atan2((ly-gt_y), (lx-gt_x))-gt_orientation)%pi + float(np.random.normal(0, sqrt(var_phi), 1))
if abs(bearing-pi) < abs(bearing):
bearing = bearing-pi
@@ -394,13 +398,8 @@ class Dataset:
message['data'] = self.dataset_data[req_type][robot_idx][time_idx]
message['groundtruth'] = self.dataset_matched_gt_data[req_type][robot_idx][time_idx]
return message
-
- def create_additional_dataline(self, req, time_idx, meas_input_var):
- message = self.create_synthetic_dataline(req, time_idx, meas_input_var)
- return message
-
- def get_dataline_at_specific_time(self, req, time_diff_limit = 0.1):
+ def get_dataline_at_specific_time(self, req, time_diff_limit = 0.2):
message = req.get_message()
req_type = req.get_type()
meas_input_var = [0.01, 0.01]
@@ -414,16 +413,9 @@ class Dataset:
gt_orientation = self.groundtruth_data[robot_idx][g_idx]['orientation']
message['groundtruth'] = {'time':req_time, 'x_pos':gt_x, 'y_pos':gt_y, 'orientation':gt_orientation}
if req_type == 'odometry':
- message['data'] = {'time':req_time, 'velocity':0 , 'orientation':gt_orientation + float(np.random.normal(0, sqrt(odo_input_var[1]), 1) )}
- else:
- subject_ID = 6
- [lx, ly] = self.landmark_map[subject_ID]
- measurment_range = sqrt((lx-gt_x)*(lx-gt_x)+(ly-gt_y)*(ly-gt_y)) + float(np.random.normal(0, sqrt(meas_input_var[0]), 1))
- bearing = (atan2((ly-gt_y), (lx-gt_x))-gt_orientation)%pi + float(np.random.normal(0, sqrt(meas_input_var[1]), 1))
- if abs(bearing-pi) < abs(bearing):
- bearing = bearing-pi
-
- message['data'] = {'time':req_time, 'subject_ID':subject_ID, 'measurment_range': measurment_range, 'bearing':bearing}
+ message['data'] = {'time':req_time, 'velocity':0 , 'angular velocity': 0, 'orientation':gt_orientation, 'delta_t': 0}
+ else:
+ message['data'] = {'time':req_time, 'subject_ID':None, 'measurment_range': 0, 'bearing':0}
valid_dataline = True
time_idx = 0
return valid_dataline, message, req_type, robot_idx, time_idx
@@ -436,9 +428,9 @@ class Dataset:
message['data'] = self.dataset_data[req_type][robot_idx][time_idx]
message['groundtruth'] = self.dataset_matched_gt_data[req_type][robot_idx][time_idx]
+
if abs(respond_time-req_time) > time_diff_limit and self.adding_actifical_dataline:
- message = self.create_additional_dataline(req, time_idx, meas_input_var)
-
+ message = self.create_synthetic_dataline(req, time_idx, meas_input_var)
if req_time > self.end_time:
valid_dataline = False
else:
diff --git a/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc b/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc
index 764c3acefb5a5748df2049ab1d5b37a629ec6b64..f804a9e8365eebc5c47729bdfb6824eaa49232ac 100644
GIT binary patch
delta 926
zcmZWnzi-n(81*@c9Xoc@rfKuLrGQjbY8y}`DwtYTVgMut3J5WnOU?v0u`jxF3sepr
ziWmXuKn!Jo1qp<NK>Q7Cz=p($xnkiT=)|29+5*e-cklb&^YiEXSbbYH7Hr$xnS6A8
z@1>CdoSBL8JU5U>9fBJ;e65gzq9}VO>$kA))^B^AjtBkv3ci42m|I?D+limEJWE4J
zLPuceI*<^O7cw#?K^pX-D62Aimb_Ax<cvy2BxCIl#G^%Kq}GRP|B??=_cMpASWUYD
z-mrVxgUq=oh3vNa7?4$#)lZKq?F9Z7B~iAdpj<mC_#C^akITYhExk6f6b(hnTDq+X
z<}4cP>FlX~tYV?@o{sW84Jkb<*c7IP*z^-Bu&;UzJYm1|^Harb0F+27e?(Sfw$xrh
zoK$i~IGW;_XV=nK!A<r)y)s>j5_zShL`4)=s9cbzN>q+?RC%82Rio-9Wv0q1#&NOw
zCF6s!IqC%)u;U@NYrNAY<qm3W$E;_$)nDy;7l{pX_T8*+9TjbE`7Wif<vN0gJWXiV
z3Ga4YZpAiEf1WJJQLw=c>a~36V1)Sybpwit)5IH)cra+XWI8q+@Ltee4|!%FXi%4M
z4L8;t4{_6XA$3Xw1}&G&#-a`kkPk`Q>EI@3FEYg~KGej%?~umeR`3iao*RVF!vW=m
zznjjV4C00TSn=nWE*+>T&&v9+MQ*=&pteH-o30~5vqdX6bLhUvl%pb?SnehaMTWs@
zmPLc!VZ@p{Q3gq10TbkarfRAcpOPT2CPk&Grogo5r@^>7ET)PH_SvHIC7~x}2~`Ob
m5~d_vmT(YYaJ2(_5u9SL>}m1Czt{zVyY|HJe`EQfBfkMY1_;am
delta 987
zcmZWo&ubGw6yDh+n`E;|nkH?2S}hgYf-#^6tB9b*lNP~>!icPsok_Y)b|=hkVl5j4
zD?O`o@gRbDQ4kdAzu-kguaZO0J$UgdBF-eSmU{W-&3oVX-kX^>Z!0^M_~~R)FXy*!
zzkV4H0m$`G$%JuidnTiG>c3Igl%gm<>$NrNklH=F-L{caTeY2T8~HYMtC!uL&8X`T
z*RM7xt1eU8Axy-2q<WQFsBLc!iM2-8wWPx1*F$?JVA}H$^G$(1F%e@dE9$VE1X*ze
z)6y#NM)+n$Iu})zksf28Fr-fNZ{hAl>9F%o<Wb^4D*iok7d+!D+IC_oh~Q*PeGJ$o
zzN?*GnQVoH9md&yG*H&H^=OdV0{u*o!AdK{eg~N#9ZUpa42nvSYDW474)--TjB^6d
z<0+v^30<tEi7ZQ;Md$J{8N*87a5qJ*>0zxMJk8%nkAR2#OZ3WYehUC4kcv-%5$H{|
zpBFooEr?9ISXF*5b_?9#Ph-pTg&>quMC}3|TDX{(yK+zpqPX-t(ys)SRb{cvf5ncA
zC!CFc;Ct~`T18$+i9gY6X=se@x!b|wx*`9n*B%@dVQ4tS^H7tR!jE8#c^%Wg-yzWG
ziHF1Hh|g+*IsiQw_w1&FOo}PYc*ON6GcDRf%to$7*xWGT*l)X?t`8GqLER&4pware
zX=A861bOB)hTJA$3o^CQMlN;`Yng3o!333b({?f080aD&mOH!wQx<g`lhwylBZ$lv
zV~6BWLH;>WXo-TOwuZ;ii8rQtFfHwa{K@^G`S%1c=`&;zbJVmqNDRNgXhaVU<si#{
z7+LM2=-%t`HKSTcfiN&Y3`_z|)l_L4!jn_^J7Wr{{G;KW%nLRvNGM7uOPG;xLBhXL
o9S?OQc?O)~FOu^`m>G3qiPJLqqX=dpp@DoiIWzH}&`2Ho1J>gj=l}o!
diff --git a/CoLo-AT/robots/robot_system.py b/CoLo-AT/robots/robot_system.py
index ae0849f..d1c9daf 100644
--- a/CoLo-AT/robots/robot_system.py
+++ b/CoLo-AT/robots/robot_system.py
@@ -96,7 +96,7 @@ class RobotSystem:
obj_id = message_data['subject_ID']
meas_range = message_data['measurment_range']
bearing = message_data['bearing']
- if message_data['subject_ID'] > 5: #landmark observation
+ if message_data['subject_ID'] != None and message_data['subject_ID'] > 5: #landmark observation
update_type = 'landmark observation'
landmark_loc = self.landmark_map.get(obj_id)
if landmark_loc != None:
@@ -105,12 +105,13 @@ class RobotSystem:
else:
valid_update = False
else:
- update_type = 'relative observation'
if obj_id in self.robot_labels:
+ update_type = 'relative observation'
obser_index = self.robot_labels.index(obj_id)
valid_update = True
sensor_input = [obser_index, meas_range, bearing, message_data['time'], obj_id]
else:
+ update_type = 'invalid observation'
valid_update = False
sensor_input = None
@@ -125,6 +126,7 @@ class RobotSystem:
sensor_input = [sender_idx, comm_rbt_state, comm_rbt_state_variance]
sensor_covariance = np.matrix([[0.01, 0], [0, 0.01]])
else:
+
print('undefined message type!')
if valid_update:
diff --git a/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-36.pyc b/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-36.pyc
index 6cfd0e6ece49a18042367124c5337d035c92bd4c..1f52de8ccdebd6de988d3d8bcff8fa717cd86d3d 100644
GIT binary patch
delta 740
zcmZvazi-n(6vyw}#BrS1B#raCjZy-lX$b+!FR2v;l?sX&kbnU(7>9F&-6RfYOSIxb
zki0NZQFI_7BnH+>2q7jISXh}D{Rc2G@CPs;?nQ`+;XZoj_rC9!Z?E>PrY>okY8`xf
z>#*@&y@{(U?YmyzZr^u$J;&;|*Sss<1g-PeKfJ@a7>@Xt_-j1RE%}(Rxx66wS*~i4
z>e~ZiQ_~?^5&tfK)GG)fS)3_x{0sbT;z{-;LvHLbVoDKhqFssB`DJATKjhDqi(|u`
z4$&yj3^x}dB5g{$n0AT6;v`;1OfRD<8WmW9<%w2CL|;K7(qQ=zW5lLhp$qY-$YO4R
z8LSve7!583x?5x!riap~#7ckj>qJx+ic$fM%B&3eAlFqF%zeTv{U>dTiK5U1LuL$y
zU`a6mroRJ3GH{220b(g=C#bkplI4Ts;+#ZJiI!$W#4MTK0Usi{Ny|cW4K)q^DY+^(
z_$&DexA+UyubdUzig34yX-il%0Z;(v`2N&586}9_3oJ9>Z&J5$jZ0}#IRNfCz#%{l
zPzN;lgLHX34`y7TS)uShXco`}EC3b-qIj3u_k0SmktR$t*dE$u&+!9#1cKUtqu?zK
z?5)5YdcN;;dbSxjecQAswYH-~-x-)a?@pBdmq0uQp3~~9$3eaXa)tWCiHM$nZ)X5&
zfRljJ0+Hf%{K-R_Ob?sjeXW7J{F%0_Dp<!EoW`oeziP8s;y<*GybRhK{HI>zj^4oQ
Nd`F+b8+=cH^atY`yN&<=
delta 747
zcmXw$-)qxQ6vyvv(kxAz>_@wH-L(GL?Cf0D%}tT{1KAKK>}^lRLG7B9CFVzWlTJHs
ztxyC(2jYDY1i@!tL}U-X`X9K*Jo_*B;)8fkK*IUtoAWE@+<RL2Tv1k4RoVFY@==uk
zsyxKoUVT6Ctol>ib?v5C-?6=}+l=hM*YEj<HVJ&s@*}++5PdTUIu_xvLrdQdT20qJ
zN)qesuHS-+@Y{2o7qP+Kr9a|%wk)l1y(cYj9!oQ-p@f!iTBKo{&kS}Veb>qeA&Hy7
zEtDA@c{9FCkt4oBR4$<>=q)C{SXq96H&{!)KM<S=S{UN~Bo&=JEzrqf3L}3j(wqVv
zquMalFVdnjIYj*u)lA-j3Qn<z`crg@mQ3{)q6MeKmBv-k;l3fpkn_K5ma1h$`BW4&
zM3{qSk{NzP$Q-4p;E3?U2+=GQMrCK(9AmxAl?#~s;UmrPk~8M`F~pELku>K`c2F(P
z-V4j@cV?8IL|XZP7uhEzoW8-gj&Zw&$s$)|1Rw*}nVuc6lk8)BmKAg6paRbGfQx`x
zz#QNbpvn;oVK0ougRUKsd2psVYGne4OU4100X4t^N1SdG>mW>W4VBAA)azIc*A641
zL(&qU&S6aX>~F557$ctTH{76Quyd-uz6|jdh|47GG$Pxx$W`#I0@eW6l4!6OCO4j}
z2dlEz>MY)6U(}mb1#3bM%UI*4U?rtU8qV>Df2HOG32P}?JgTzW*($rMtx4CR>lQoG
h)+=|ojB|e1>-3hqX5`v?vB=;1aViMO20PVW`~wF#xLg1L
diff --git a/CoLo-AT/simulation_process/__pycache__/state_recorder.cpython-36.pyc b/CoLo-AT/simulation_process/__pycache__/state_recorder.cpython-36.pyc
index 8bd85f9ab45ec39fe2de3ed7f2669cf30395c845..2afa61a8e0fddd99c65d8befd44562410b9cfcd7 100644
GIT binary patch
delta 127
zcmeBCxv$J=%*)GlPPIRJ)ke<UOw#uH8Tq-X`sJB9Ihl#M`p)@2`CvlVF=Vp<^9dHt
zbOtbZV88h;yDcLl^JHnxcE-@ji#aDSMoc#0>Sm0de1K~QW8CCQ?kdKl$zQk|8B-^h
a@w75#Z2rR&&CHlRIYQtAqv~W&!NULpD=6~-
delta 172
zcmcbw+@r#2%*)GV^{p>@`9{v&Og;tr8Tq-X`sJB9Ihl#M`oWpGr8$WunfZCTu6bpd
zMfrKTsd**3Y57IE&iVNTsYO7!vQ%B4{N%)(%qp-#{j}1&WRQ;H&G(s3u&@@1GB7Y~
z-oS3l$jCJL9Y;H3$mBB435?;B*}1wIqb5)0+QAq**@wG|F>&&7?ncIx$(}r|jOm-t
U^F%W<W=+->_`s+#Sw`?M05L;6y8r+H
diff --git a/CoLo-AT/simulation_process/sim_manager.py b/CoLo-AT/simulation_process/sim_manager.py
index 4db0376..d6bd79d 100644
--- a/CoLo-AT/simulation_process/sim_manager.py
+++ b/CoLo-AT/simulation_process/sim_manager.py
@@ -113,6 +113,12 @@ class SimulationManager():
if valid == False:
print('invalid responese')
break
+ message = rsp.get_message()
+ if rsp.get_type() == "odometry":
+ delta_t = 1/freqs[0][robot_idx]
+ message['data']['delta_t'] = delta_t
+ rsp.set_message(message)
+
robot_state = robot_system.localization_update(rsp)
state_recorder.record_state(rsp, robot_state)
@@ -137,7 +143,6 @@ class SimulationManager():
sys.quit('Time inconsistant!')
next_possible_time_array[req_type_idx][robot_idx] += 1/freqs[req_type_idx][robot_idx]
-
if simple_plot:
state_recorder.simple_plot(plot_title=self.name)
diff --git a/CoLo-AT/simulation_process/state_recorder.py b/CoLo-AT/simulation_process/state_recorder.py
index 559fb1d..b9ae1f5 100644
--- a/CoLo-AT/simulation_process/state_recorder.py
+++ b/CoLo-AT/simulation_process/state_recorder.py
@@ -67,7 +67,8 @@ class StatesRecorder():
print(updata_type)
print('neg trace: ', recorded_dataline)
- if(loc_err >= 1):
+
+ if(loc_err >= 0.5):
print(updata_type)
print('>1 m loc err: ',recorded_dataline)
print(req.get_message())
diff --git a/CoLo-AT/test_simulation.py b/CoLo-AT/test_simulation.py
index 8080a85..a990e7d 100644
--- a/CoLo-AT/test_simulation.py
+++ b/CoLo-AT/test_simulation.py
@@ -26,17 +26,11 @@ from ekf_ls_ci import EKF_LS_CI
from ekf_gs_bound import EKF_GS_BOUND
from ekf_gs_ci import EKF_GS_CI
-#need to verify these algo
-from ekf_gs_sci2 import EKF_GS_SCI2
-from ekf_ls_ci2 import EKF_LS_CI2
-from ekf_gs_ci2 import EKF_GS_CI2
-from centralized_ekf2 import Centralized_EKF2
-
dataset_path = '/home/william/UTIAS-dataset/MRCLAM_Dataset3/'
dataset_labels = [1,2,3]
-duration = 100 # duration for the simulation in sec
+duration = 150 # duration for the simulation in sec
testing_dataset = Dataset('testing')
start_time, starting_states, dataset_data, time_arr = testing_dataset.load_MRCLAMDatasets(dataset_path, dataset_labels, duration)
@@ -48,8 +42,8 @@ robot = RobotSystem('robot', dataset_labels, loc_algo, distr_sys = False)
sim = SimulationManager('sim Centralized_EKF')
state_recorder = StatesRecorder('recorder',dataset_labels)
-#sim.sim_process_schedule(dataset_labels, testing_dataset, robot, state_recorder, freqs0, simple_plot = True)
-sim.sim_process_naive(dataset_labels, testing_dataset, robot, state_recorder, simple_plot = True)
+sim.sim_process_schedule(dataset_labels, testing_dataset, robot, state_recorder, freqs0, simple_plot = True)
+#sim.sim_process_naive(dataset_labels, testing_dataset, robot, state_recorder, simple_plot = True)
analyzer = Analyzer('analyzer', dataset_labels)
loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = False)
--
GitLab