From eef51dd888aa120837fc746b45fab44ce14891fc Mon Sep 17 00:00:00 2001
From: Shengkang Chen <shengkangchen@Shengkangs-MacBook-Pro.local>
Date: Fri, 15 Feb 2019 00:43:42 -0800
Subject: [PATCH] added simulated comm
---
.../__pycache__/data_analyzer.cpython-36.pyc | Bin 5574 -> 5574 bytes
.../realworld_dataset_manager.cpython-36.pyc | Bin 12810 -> 13047 bytes
.../realworld_dataset_manager.py | 7 +-
.../__pycache__/sim_manager.cpython-36.pyc | Bin 3941 -> 4833 bytes
CoLo-AT/simulation_process/sim_manager.py | 62 ++++++++++++++----
CoLo-AT/test_simulation.py | 8 +--
6 files changed, 58 insertions(+), 19 deletions(-)
diff --git a/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-36.pyc b/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-36.pyc
index 9b93370356b5023e51bb249b484f896ee3e248c1..9099e8d5a14e68fe056753339e4b41849ba094a8 100644
GIT binary patch
delta 16
XcmX@6eN3C(n3tDJ;z7(t_Why&E93<s
delta 16
XcmX@6eN3C(n3tF9NNMy&_Why&E)fM`
diff --git a/CoLo-AT/dataset_manager/__pycache__/realworld_dataset_manager.cpython-36.pyc b/CoLo-AT/dataset_manager/__pycache__/realworld_dataset_manager.cpython-36.pyc
index 018cc86e57a4288f0eae01ff5857953b2fb5c0b1..5ed681eb5c3773249ce5512ba5e08b9c1e174c34 100644
GIT binary patch
delta 500
zcmZWkOK1~87@lu-vq@KK64O^oQnlEcZEGJEtp{(uDoCj`v}F+^Iw4kQgVQM#NvLQJ
zMNje<1<xL&^dfT+#Z!wQf}j*MQLqIMMf7TqIvc$?@V);38<@E#cJuCVW25_RCx7Y6
z)HC-hM>~DzAJ9XutO2ajPb-B!a_yT3kBCE}s{%hE`o&?9F|J8?qNl?AX3;yrya8IV
zC64KE=m~H_uQ`t+n4ojf6=dm4^s@dOO#{B3j&Il))#;X(z+0-emhg@y+wR~!RoiZ3
zll){EAN8kX7^vy3_S3+&?n+$%erTB<2Y%6d$7}wZ>HGV$7973RS#@w+zwjOc%lcyP
zw1o$HwSNtGPE&&?m#$gt?+F<R{3(O^ZbOQd*bN9IS&19qE-zbV!t2ROva(&W&A<4V
z95HRrlKYIv^8t+pgOKbr;)e?=r)FmJQUz(xzm%I1i#1+QBQh3Ts8igyT5}<<a<X`}
zsB#5aT$mF|E~x8oU>@a|Jwp@r@>>}~Yl6m8cK3hPF^^~EQPcDwD|I>Rwb0YSC1=?r
f8Kdo3!aZQ@u)%zW&J20njnmxFqfkR67HRkcDu;)~
delta 302
zcmeyK+Lgj(%*)FqHa#Y$By1zseI`cf$*-6$F=|iV%dF41X!2WT8Ah$iTr7o>+9@h2
zsx6FB!YMi_>M0sPmPm?bFoUM<<T)%wjAonpS(h*}T5jILHj9HXWU@d1l+8{8vW$!o
zlNkl$7^62Q2p(Z!^xUi}wwsah%;bmSHH_yc$4N9XUYh(+qJj4+0|P@b6OdqFtdgG`
zXed6pRWge4#^wi-?2L@JH@}c_VPt%?Sy#q~k@4;3D%k)=#_y9)%N^rqU<B%80qJ8B
zVyse{{7_D2^B#F^W=8hSFO+VwF`92aq&ba|v2$~zb_6ryq|KZ3)-W>eoE&dpBkHHg
v6jEda)Tzl>)B>j5G({)(8P-o0GLm330g4o>O*SynoE&X5ot2A2fP)hN(1lk7
diff --git a/CoLo-AT/dataset_manager/realworld_dataset_manager.py b/CoLo-AT/dataset_manager/realworld_dataset_manager.py
index ea32212..b527385 100755
--- a/CoLo-AT/dataset_manager/realworld_dataset_manager.py
+++ b/CoLo-AT/dataset_manager/realworld_dataset_manager.py
@@ -23,7 +23,6 @@ def find_nearest_time_idx(l, value):
return None
-
def find_next_time_idx(array, start_idx, value):
i = start_idx
try:
@@ -455,3 +454,9 @@ class RW_Dataset_Manager:
return valid_respond, current_time, req
+ def get_robot_groundtruth(self, gt_time, robot_index):
+ gt_time_idx = find_nearest_time_idx(self.time_arr['groundtruth'][robot_index], gt_time)
+ gt = self.groundtruth_data[robot_index][gt_time_idx]
+
+ return gt
+
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 d643fa9309460c81f3c936277613a0a4947439ee..2abcfecd1515e8497a8c7f92566c428ba0bfdf5e 100644
GIT binary patch
literal 4833
zcmbVQTaP1074EKX+wHr@<GJl7n+b$~13R-2Ag~O3A)CoX$tubY7d3%akE_Pho_5<)
z-Cky_^3$xuA`xjL@c=yV&R^gqukeO=>?b6i7V!g6p7>66+dF$9BDB?~PTfwOs&o2$
zr~GQYUirhn#D_mwH;jK6=N=pU4ZQIQ2sgMH8m_5J=4N%vwRGKfZCw`Jf+!4%gOad8
z9Zk!ka%OUN#?DMLn^kqI7;S~du;$ivn;kZWO}A+pZyLP7i}ws(JY#N)n|F+E=^<7(
zZcnRQTia3`Peh!=T{A6)LF9*ha3$$SJcyDWjhjCfm>YQG?|=xcx63&6xMx6Ti`yve
zY%RCI8@$Xb_e{6Qo4m?v;7YuXwOi@ZonSZ#{UjJg@A;A67qZ77ksd1PeI0LH0a2zh
zJZupY&D&7Sj{KqMveb@6cpwWH@kc|JP<;B{_77tr<L&rRMExT_>i58G-x~EMLlGtM
z_Uof}N19x|xwjoJH1Ni9)Dto01w(I`sk1dcO&!k*q9F0S2C1U;Gp84x(tK+^tVgFq
z59zen0?dys%q(ScbInj})tK4JQuYDEhS=?gCi=Dd-GVHF$HDI>UcgV%N;DaIa<o56
z;<P43PXu>`%z9!G@r>6~Gvd(fW^Xhcdc9GU$Wa&yxrD};<Qk@J*4XsLr}ckg#$p>S
zOd@C1miP@Aj%{!T_0132tS~DohO#tsRFPX72B^J(eb36XigI*|lebi7wZs^hs;sK(
zaK&XqRa7nG){HZAR!1-66;<z0t)Xkxyw*&tfjuawhH9QMWgpq{*W9^d@G@TViQ4Ul
zbl&~n|Bjx&Tu%!p-gp$Jg;OdXk{$YZ@%;OG&iwuQ-tN5ZlX=@G-C}AF{p1jibu5$A
z5+~!d029Cyiu5gvhG{Vx$)O*1TiQ}?b77P?@smmH)^oe$)$-6k@i4V7(sFVrMSM64
zxvWE#w7QtuPbB$QTF&S86W5a736WFRiiwMf<3SAlgde3=obV^jzLvYQ;IRF0IEjLu
z{<_v6Iwlt+^V`fZ9o9D6<|WoJ8{lj<?LO@g|3%i`ZjW4yo(;SU*Z3z~IDvCeBtpp)
zqKuh+wkAB?@R(u)>&$?ouz`)TtQ@o!xCKXHgCbh0${v(dl_V$&oNmo)G^>0D$IyK=
zqng$F#;o?hJh`C^xQC<4YsQiBz?6SVYJ)l=p>f7$b?VcrG?eDg*oW4vL95T2dS2zo
zkbj2OHpzeAPz_!<V)Ad`tU(hlXU<xxHfX69FK%Qy*n=h2R4u*7qExCmXrCFpgjvKd
zV>e5X+)+f?U<E6(S$nn&y;t>$ZRpim)Rs|OQ3cF325V|rt@N2{t4<%0T`KHkeKBWM
ztqwLIeGLI|dBd2kb4RTQrds3W9Rn+009OIGI=HCT^3!Un)z99+vHG#0N;tK@!UE4>
zt>=05qs)#ql7W-S&kd)Kv&0A2xIMl6O8#s<a-PlivnaZGC-G$>_=}tKK~o)*7A|>p
zQ&P;Ew)cXe*bE}XsyM(G6D2S1P8;;;h|Rmc4E(4k(keVYo=8CfJgvl&{ekEu-rKiQ
zoBN5MR{L@^iFhI>$swSjG7d+Hmjp>Drd#vnpQYn20y|LRypokSBEFeGrK_9WOKD*&
zfqe+O6n?$X-xnbTVO@()<3tS8MqeZz)Rna2P^2}g^NG~oU25yL<;9pF>eJvv#A!R8
zPh2#ntBW>oIJ!&iG(S#P7Xlt{cfyg+0aoL*MI%DQr=c&8vX&grJw%)_4p*eD4>E**
z)Kbd&c)A>pdVUy8wbtHbjAIpP(I1b2FdB76l4pvi4y}}&j)mJKE<20>Ky)ITFsQ77
zq$`iQ&e=x3Kz;Ss37epu)&s_a6l^jE{-;{xxfyAdPGsI&MMxTl!Xx`?$nS3G5G-FN
zxr`6xmx;-|2hw}cT1dC?cqDUeijDI7G_-Qga?ZkP<*^iZGjmq;fXqT}S)0y-Tc(9R
z?I11O#i{YslJOY9Jo5`OudG74j`2FI%ZpTfj#hLu&eHf&zAcEI<M@;DybaCgd5IUn
zICsTYAte4eh=DL(F^guA!Z}LYLft|@M@YBL4y&PUl~vHk^jkqs%_8)7&`YD%K^@d;
zkW<0PhS_38w4=qEzVzsu&U3S^r<LiIND(|2@KSm~xljBK&~}aloKZg5w`L`sz&(Zp
zzd|0Qk-)i+r~)J?024AKsAwcWZu=}GxNr2$q^y86lqfuj2fs89OkU)rdyXpHqC}z!
zboebrEv({bbYSvjLI+g^Z4llZA%C9LRE-xlkeDE<)^pwg-%ur0N7+<JMv&k%z_(PB
z+ea26=DnmzxS$${;7e+WP(kZNh(o(+0|8o;0e@rxbASMd`fXl4Vwigcr=;U6aHgX%
zV+CagIKp&)h}e!($=1AR%n<w43WpB7fe7Cjtf-ZI|EsEl9jxKh)>H>^AIKZM1<lrT
z&DPa=F2BKX20*n9WS|>rgXC#h7r-^atv^HRV&7yXjYMT&ER#0$Z&97rON<P;lZ%7r
z_>$HupI1PlKs%YA-bLdzILeN3`~av^gq$7d{XFDc;_V!Dmgi>(r0INKq)D9-(ln9C
zQ@II$bnB1hLK=2@ggx|7Vn~VOGT!(SNFT2Fv(=fY-3aLn+{h$1`j{Qtl&U{_CgoOo
zW@~2NN7A9pi^iIf&oL!kVDz_*3k%!2;@SrY9$i!3n72zxsXndjgn&S3JHB=wb$!_=
z8q+sE`%{c@zu}WmZ0+bE9$y197jury*O%`?a%vttwYq!{9p{(5`u{JhH@pWZZbt&S
zXgn6Z;2`Mfkh(iv3L+ggH#77RxUGGGlqzu%Ccc-*FQPWB_a;&T?=v)hR-S+@yhI~E
zC&qVQkYB<y4H_CS<X5QLCGrXp3bFEQL@tA*RwyF5P1P$@WzjfweA)lSp^zfAhCw91
zPMzPNPKV%%z)`+RZC8oBMr4P`H;H_k$ajelQp)dvAeTQyT7NtV5)Dfkwq1CHZSooo
zC2*71LEHlQPprqfbwW~F(j%+On>6SLMCiiiu1De|p({`v>;pL|o$!1q{ZqFD%{_RG
zi%VrLfUcY69MHi!^NyUuTzQKmzd@26V3?LCcd6}dBJUG<lgL{jt~1()ZE;u4d@uKR
z2VH6BJebTqSfVFB1bGVEY+xH}unt{UKs&&!8c>api(VidXohq^F=`_`vuF%3F2lDX
z@DD$d^iqa%_Vnfd&Bt$dt-Zb7uDRQF=>Guv>%sHViid2BzC7?X&pV#@Vb&wd*g}>K
zQN|<RA@WlqgqP<C9fhPD6x{~(-yFbl%PBgY%A2~qyCmuECO-giYx)|xh1+Qu?8^?C
zQZ|a^kBG6yxU<S{X?8FQB797NByC9FgMf6*+>`>I%ig^yDP&2)YrW7prL>OPNX|~S
VziSjN<2DGT8v18lLW$#K{{hy%=J@~s
delta 1733
zcma)7&2Jk;6rb4-d%f%3^~XB4(|ojqk~)y|GX(@~C`Exo1*k-znjqt3Ho+TzWp-0F
z)fj_ZK><}tHAf^4J#j?hKR_H25)$IDCpd86A3$(Hyti>^5f?1YZ{NH(@6DTezc>En
z<gYX4dD}L>-Ftg|(YtT{xO9pT;*&aMq^>Z^lslxZGLz{nb%)k9X0bFgK+;*}4N}X>
zDf-iJZFMsZUid4)P+?q!5q?Pti3p=3O7W`*eWXw#s2~k0lt3AgK^nd)d<~HXt;W<r
z1~fWoOrZrSVWO{4K}xja8G0EbGM)|5#XOvDlYw<WZ>0nYGze*w$qqT7{B~poSw<Kc
zDT6H9l5K2}$RlN49oV@2z)AcvJA{7&32V5)>qOX0-%<E?AgO^PoDm(k!U|mBvQ!n4
z7(#|xkQ0t@lk@5Prf`D%h_E!cp<ICT<RFD&5<Uk><V8O4KwA(w21!ET(gYb4;ckjm
zGVoYNcr7Xl%zU1J{{%=DNco^F3gc5dB0o;#+CQ26JCWU+ZxNA#(;q;371+AUviA;4
zVPg<DVRGIiX$z|9zIuC6ewdz;f2MBezd`Ycyq*p!ld<0Ct!@-MtQj>!KWem_8-6?d
z`sZ|(o{H_Oy-ugW{aqMgKeo1ELB|in=C&W3-90e%HhNJgmojIUOFRWpc^)8ESSQXV
zwnn%gMt&!D!l)Vf5N5N-na}ImCPe1*^7G7krk>^-QKQ9f$(-p~X6SceMKYf=mmCZj
zz6n4SgPO8wp36{!I?0d^%<IFKl!sJg3R9W3tpfQTq2eb5Sd>ulE=j1U|350;CEGMI
z1d!Z2BIF}{{FrP}mSS?>DwH({d`L)zA4ft83(%Gitd)1(j~pNraA?ep9B>Jle?(Q_
z3os?a%zG-3wJ<0SJmkcV0_9zRLT1NQ^%Aa?gclTne@bHCl9#iyLr;{(%mFJfrwpW+
z5S}Pw`Ma;c{^jxh6;T;SamL)N05_|mDoV@-7J30nE=ZN&zfmn=SAnxIprMZ1>3~l1
zzaY&VDsaPy2rJ>>WN=LWo}HijG~uekFG5;VuxgqW##}8PU4(qWUV-9%V1HD#pwb-Q
zoSG6VOK0lZ7Vxh|`IB<isZu7Nb#BZ&4d$3`q4sDsH&A*8fXT0%m#HQHa8A26E=)7O
z9W@&fcjQI4MqT-?dtrHo=g<M!#R~{U1P=ipIzE9=28h+R-{n=DPRj4y^6(gnQ)n@;
zVWR!M&!<s0hj0Sn34|Gh;|Px-JdSV@AXcF#Vr_S?74dmg>HxLLhwa1ho#r)!1%SGa
zHxMRk>o%`IH{za+b}NkdQ@CmoVM&VI+RAAZ&!CsV!+s-bb$osnbr%qxLpX<U9-waY
zHo~OS4(}W9*v-#4uYoPx0yylrOZm?XWo@-aS8FNxLw+H(3>LmBt-^^HvE>bFsODv^
zfmqAOIy?*geSQgM^=u+9b(>wGnIZeJ@seA-@8~SnI=-&&pn5x|Ccgv@!fIUAq#e
Nc(coT9r;D!>fd1)l~Moz
diff --git a/CoLo-AT/simulation_process/sim_manager.py b/CoLo-AT/simulation_process/sim_manager.py
index c4bf8c4..6bf24ca 100644
--- a/CoLo-AT/simulation_process/sim_manager.py
+++ b/CoLo-AT/simulation_process/sim_manager.py
@@ -14,11 +14,38 @@ class SimulationManager():
def __init__(self, name):
self.name = name
- def sim_process_native(self, robot_labels, dm, robot_system, state_recorder, comm = True, simple_plot = False):
+ def comm_controller(self, rbt_idx, num_robots):
+ receiver_idx = rbt_idx
+ sender_idx = (rbt_idx+1)%(num_robots)
+
+ return receiver_idx, sender_idx
+
+ def simulated_communication(self, receiver_status, sender_status):
+ max_range = 5 # in meters
+ threshold = 0.2
+
+ [receiver_idx, receiver_gt] = receiver_status
+ [sender_idx, sender_gt] = sender_status
+
+ r_x, r_y = receiver_gt['x_pos'], receiver_gt['y_pos']
+ s_x, s_y = sender_gt['x_pos'], sender_gt['y_pos']
+ dis = math.sqrt((r_x-s_x)**2+(r_y-s_y)**2)
+
+ mean = math.exp(-1.5*dis)
+ std = math.exp(-0.5*dis)
+
+ if numpy.random.normal(mean, std) > threshold:
+ return True
+ else:
+ return False
+
+
+ def sim_process_native(self, robot_labels, dm, robot_system, state_recorder, simulated_comm = True, comm = True, simple_plot = False):
#dm: dataset manager
print('******** Simulation Process Started! ********')
print('communication: ', comm)
self.robot_labels = robot_labels
+ self.robot_system = robot_system
start_time = dm.get_start_time()
duration = dm.get_duration()
@@ -44,7 +71,7 @@ class SimulationManager():
if self.time < prev_time:
print('Time inconsistant!')
break
- if valid == False :
+ if valid == False :
break
robot_state = robot_system.localization_update(rsp)
state_var.append(robot_state['state variance'])
@@ -53,18 +80,26 @@ class SimulationManager():
# communication protocall
if comm:
if rsp.get_type()=='measurement':
+
rbt_idx = rsp.get_robot_index()
- comm_rsp = request_response.Comm_req_resp(self.time, rbt_idx)
- message = comm_rsp.get_message()
- sender_idx = (rbt_idx+1)%(robot_system.num_robots)
- sender_id = robot_labels[sender_idx]
- message['data'] = {'subject_ID':sender_id}
- message['groundtruth'] = rsp.get_groundtruth
- comm_rsp.set_message(message)
-
- robot_state = robot_system.localization_update(comm_rsp)
- state_var.append(robot_state['state variance'])
- state_recorder.record_state(rsp, robot_state)
+ receiver_idx, sender_idx = self.comm_controller(rbt_idx, self.robot_system.num_robots)
+
+ receiver_gt = dm.get_robot_groundtruth(self.time, receiver_idx)
+ sender_gt = dm.get_robot_groundtruth(self.time, sender_idx)
+
+ if simulated_comm and self.simulated_communication([sender_idx, sender_gt], [receiver_idx, receiver_gt]):
+ # simulated communication
+ comm_rsp = request_response.Comm_req_resp(self.time, receiver_idx)
+ message = comm_rsp.get_message()
+
+ sender_id = robot_labels[sender_idx]
+ message['data'] = {'subject_ID':sender_id}
+ message['groundtruth'] = rsp.get_groundtruth
+ comm_rsp.set_message(message)
+
+ robot_state = robot_system.localization_update(comm_rsp)
+ state_var.append(robot_state['state variance'])
+ state_recorder.record_state(rsp, robot_state)
if simple_plot:
state_recorder.simple_plot(plot_title=self.name)
@@ -81,6 +116,7 @@ class SimulationManager():
print('******** Simulation Process Started! ********')
req_type_list = ['odometry', 'measurement', 'communication']
self.robot_labels = robot_labels
+ self.robot_system = robot_system
start_time = dm.get_start_time()
duration = dm.get_duration()
diff --git a/CoLo-AT/test_simulation.py b/CoLo-AT/test_simulation.py
index 405984f..4466974 100644
--- a/CoLo-AT/test_simulation.py
+++ b/CoLo-AT/test_simulation.py
@@ -20,7 +20,6 @@ from pprint import pprint
# load algorithms
sys.path.append(os.path.join(os.path.dirname(__file__), "localization_algos"))
-
from centralized_ekf import Centralized_EKF # works
from simple_ekf import Simple_EKF
from ekf_ls_bda import EKF_LS_BDA
@@ -41,14 +40,13 @@ start_time, starting_states, dataset_data, time_arr = testing_dataset.load_datas
analyzer = Analyzer('analyzer', robot_labels)
-
loc_algo = EKF_LS_BDA('algo')
robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
sim = SimulationManager('sim LS-BDA')
-state_recorde_gs_ci = StatesRecorder('LS-BDA', robot_labels)
-sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorde_gs_ci, simple_plot = True)
-loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorde_gs_ci, plot_graphs = True)
+state_recorder_bda = StatesRecorder('LS-BDA', robot_labels)
+sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_bda, simple_plot = True)
+loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_bda, plot_graphs = True)
##############################################################################
--
GitLab