diff --git a/CoLo-AT/Cooperative_Localization_Journal1.py b/CoLo-AT/Cooperative_Localization_Journal1.py
index 445f809c2c53e4ba47c3faf0664abd28d61f274e..abad62ffb796d8837b9ea7144699ed952ba32a1b 100644
--- a/CoLo-AT/Cooperative_Localization_Journal1.py
+++ b/CoLo-AT/Cooperative_Localization_Journal1.py
@@ -27,77 +27,84 @@ from ekf_gs_sci2 import EKF_GS_SCI2
# dataset_path = "/Users/shengkangchen/Documents/CoLo/CoLo-D/CoLo-Datasets/official_dataset3/"
+
dataset_path = "/home/william/CoLo/CoLo-D/CoLo-Datasets/official_dataset3/" # for desktop Ubuntu
robot_labels = [1,2,3]
duration = 180 # duration for the simulation in sec
-testing_dataset = RW_Dataset_Manager('testing')
-start_time, starting_states, dataset_data, time_arr = testing_dataset.load_datasets(dataset_path, robot_labels, duration)
-analyzer = Analyzer('analyzer', robot_labels)
-loc_algo = EKF_GS_CI2('algo')
-robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = True)
-sim = SimulationManager('sim gs_ci')
-state_recorder = StatesRecorder('GS_CI', robot_labels)
-sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder, simple_plot = False)
-loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = True)
-##############################################################################
-testing_dataset.dataset_reset()
-loc_algo = GS_CI_Bound('algo')
-robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = True)
+def all_algorithms_comp(dataset_path, robot_labels, duration, graph_name, robots_cant_observe_lm):
+
+
+ testing_dataset = RW_Dataset_Manager('testing')
+ start_time, starting_states, dataset_data, time_arr = testing_dataset.load_datasets(dataset_path, robot_labels, duration)
+ analyzer = Analyzer('analyzer', robot_labels)
+
+ loc_algo = EKF_GS_CI2('algo')
+ robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = True)
+ sim = SimulationManager('sim gs_ci')
+ state_recorder = StatesRecorder('GS_CI', robot_labels)
-sim = SimulationManager('sim gs_ci_bound')
-state_recorder_bound = StatesRecorder('GS_CI_bound', robot_labels, state_var_only = True)
-sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_bound, simple_plot = False)
-loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_bound, plot_graphs = False)
+ sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder, simple_plot = False , robots_cant_observe_lm = robots_cant_observe_lm)
+ loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = True)
+ ##############################################################################
+ '''
+ testing_dataset.dataset_reset()
+ loc_algo = GS_CI_Bound('algo')
+ robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = True)
-##############################################################################
+ sim = SimulationManager('sim gs_ci_bound')
+ state_recorder_bound = StatesRecorder('GS_CI_bound', robot_labels, state_var_only = True)
+ sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_bound, simple_plot = False, robots_cant_observe_lm = robots_cant_observe_lm)
+ loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_bound, plot_graphs = False)
+ '''
+ ##############################################################################
-testing_dataset.dataset_reset()
-loc_algo = Centralized_EKF('algo')
-robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
+ testing_dataset.dataset_reset()
+ loc_algo = Centralized_EKF('algo')
+ robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
-sim = SimulationManager('sim cen_ekf')
-state_recorder_LS_cen = StatesRecorder('LS_cen', robot_labels)
-sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_LS_cen, simple_plot = False)
-loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_cen, plot_graphs = False)
+ sim = SimulationManager('sim cen_ekf')
+ state_recorder_LS_cen = StatesRecorder('LS_cen', robot_labels)
+ sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_LS_cen, simple_plot = False, robots_cant_observe_lm = robots_cant_observe_lm)
+ loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_cen, plot_graphs = False)
-##############################################################################
+ ##############################################################################
-testing_dataset.dataset_reset()
-loc_algo = EKF_LS_BDA('algo')
-robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
+ testing_dataset.dataset_reset()
+ loc_algo = EKF_LS_BDA('algo')
+ robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
-sim = SimulationManager('sim ls_bda')
-state_recorder_LS_BDA = StatesRecorder('LS_BDA', robot_labels)
-sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_LS_BDA, simple_plot = False)
-loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_BDA, plot_graphs = False)
+ sim = SimulationManager('sim ls_bda')
+ state_recorder_LS_BDA = StatesRecorder('LS_BDA', robot_labels)
+ sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_LS_BDA, simple_plot = False, robots_cant_observe_lm = robots_cant_observe_lm)
+ loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_BDA, plot_graphs = False)
-##############################################################################
+ ##############################################################################
-testing_dataset.dataset_reset()
-loc_algo = EKF_LS_CI('algo')
-robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
+ testing_dataset.dataset_reset()
+ loc_algo = EKF_LS_CI('algo')
+ robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
-sim = SimulationManager('sim ls_ci')
-state_recorder_LS_CI = StatesRecorder('LS_CI', robot_labels)
-sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_LS_CI, simple_plot = False)
-loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_CI, plot_graphs = False)
+ sim = SimulationManager('sim ls_ci')
+ state_recorder_LS_CI = StatesRecorder('LS_CI', robot_labels)
+ sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_LS_CI, simple_plot = False, robots_cant_observe_lm = robots_cant_observe_lm)
+ loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_CI, plot_graphs = False)
-##############################################################################
+ ##############################################################################
-testing_dataset.dataset_reset()
-loc_algo = EKF_GS_SCI2('algo')
-robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = True)
+ testing_dataset.dataset_reset()
+ loc_algo = EKF_GS_SCI2('algo')
+ robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = True)
-sim = SimulationManager('sim gs_sci')
-state_recorder_GS_SCI = StatesRecorder('GS_SCI', robot_labels)
-sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_GS_SCI, simple_plot = False)
-loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_GS_SCI, plot_graphs = False)
+ sim = SimulationManager('sim gs_sci')
+ state_recorder_GS_SCI = StatesRecorder('GS_SCI', robot_labels)
+ sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_GS_SCI, simple_plot = False, robots_cant_observe_lm = robots_cant_observe_lm)
+ loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_GS_SCI, plot_graphs = False)
-analyzer.algos_comparison([state_recorder, state_recorder_LS_cen, state_recorder_LS_BDA, state_recorder_LS_CI, state_recorder_GS_SCI, state_recorder_bound], only_trace = ['GS_CI_bound'])
+ analyzer.algos_comparison([state_recorder, state_recorder_LS_cen, state_recorder_LS_BDA, state_recorder_LS_CI, state_recorder_GS_SCI], graph_name = graph_name)
diff --git a/CoLo-AT/__pycache__/Cooperative_Localization_Journal1.cpython-35.pyc b/CoLo-AT/__pycache__/Cooperative_Localization_Journal1.cpython-35.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..d61288577566f07d1c62e5c45568989959feb045
Binary files /dev/null and b/CoLo-AT/__pycache__/Cooperative_Localization_Journal1.cpython-35.pyc differ
diff --git a/CoLo-AT/colo_d1.pdf b/CoLo-AT/colo_d1.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..e89ec15c4e87ec63d59e8d974489179e14fc4d3b
Binary files /dev/null and b/CoLo-AT/colo_d1.pdf differ
diff --git a/CoLo-AT/colo_d2.pdf b/CoLo-AT/colo_d2.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..1d537142ebc33cd13f5aef68d63c176cfb682f90
Binary files /dev/null and b/CoLo-AT/colo_d2.pdf differ
diff --git a/CoLo-AT/colo_d3.pdf b/CoLo-AT/colo_d3.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..1180d28aba82324f8033794146bd84a7237a041e
Binary files /dev/null and b/CoLo-AT/colo_d3.pdf differ
diff --git a/CoLo-AT/colo_d3.png b/CoLo-AT/colo_d3.png
new file mode 100644
index 0000000000000000000000000000000000000000..1b5596b61af2e3366c6e549fa8a3e1781e214df4
Binary files /dev/null and b/CoLo-AT/colo_d3.png differ
diff --git a/CoLo-AT/colo_d4.pdf b/CoLo-AT/colo_d4.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..59c75d9a449fc5144f77dddea4194bf2052e6b65
Binary files /dev/null and b/CoLo-AT/colo_d4.pdf differ
diff --git a/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-35.pyc b/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-35.pyc
index 66602b7cdaafa5773bc1edfc762f0b78535c5c2a..dfcd316fc29984c347269f77a7fd5c413093d7cd 100644
Binary files a/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-35.pyc and b/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-35.pyc differ
diff --git a/CoLo-AT/data_analysis/__pycache__/realtime_plot.cpython-35.pyc b/CoLo-AT/data_analysis/__pycache__/realtime_plot.cpython-35.pyc
index b1e353177970a2d22a0aa09fab3db2049328c9f9..fdb509e8ea38f6e74a44a8a0e3b1d91533e76324 100644
Binary files a/CoLo-AT/data_analysis/__pycache__/realtime_plot.cpython-35.pyc and b/CoLo-AT/data_analysis/__pycache__/realtime_plot.cpython-35.pyc differ
diff --git a/CoLo-AT/data_analysis/data_analyzer.py b/CoLo-AT/data_analysis/data_analyzer.py
index 1d5064c06642ba74d881c7c710b58ada8b20b9d8..a7dcf7bf8fbd1df0b5e44480ea2f51b8ed77b2d4 100644
--- a/CoLo-AT/data_analysis/data_analyzer.py
+++ b/CoLo-AT/data_analysis/data_analyzer.py
@@ -216,7 +216,7 @@ class Analyzer():
return robot_loc_time_unit
- def algos_comparison_graph(self, arr_loc_err, arr_trace, only_trace):
+ def algos_comparison_graph(self, arr_loc_err, arr_trace, only_trace, graph_name):
fig = plt.figure()
fig1 = fig.add_subplot(211)
fig2 = fig.add_subplot(212)
@@ -225,31 +225,37 @@ class Analyzer():
fig1.plot(loc_err[0], loc_err[1], label = loc_err[2])
for trace in arr_trace:
- if trace[2] in only_trace:
+ if only_trace != None and trace[2] in only_trace:
fig2.plot(trace[0], trace[1], '--' ,label = trace[2])
else:
fig2.plot(trace[0], trace[1], label = trace[2])
- fig1.set_title('Location error', fontsize = 24)
- fig1.set_xlabel('Time[s]', fontsize = 22)
- fig1.set_ylabel('RMS[m]', fontsize = 22)
- fig1.set_ylim(0, 1)
- fig1.legend(loc=1, fontsize = 20)
- fig1.tick_params(labelsize=18)
+ fig1.set_title('Location error')
+ fig1.set_xlabel('Time[s]')
+ fig1.set_ylabel('RMS[m]')
+ fig1.set_ylim(0, 0.5)
+ fig1.legend(loc=1)
+ fig1.legend(loc='center left', bbox_to_anchor=(1, 0.5))
+ #fig1.tick_params(labelsize=18)
- fig2.set_title('Trace of state covariance', fontsize = 24)
- fig2.set_xlabel('Time [s]', fontsize = 22)
- fig2.set_ylabel('Sigma_s [m^2]', fontsize = 22)
- fig2.set_ylim(0, 0.1)
- fig2.legend(loc=1, fontsize = 20)
- fig2.tick_params(labelsize=18)
+ fig2.set_title('Trace of state covariance')
+ fig2.set_xlabel('Time [s]')
+ fig2.set_ylabel('Sigma_s [m^2]')
+ fig2.set_ylim(0, 0.05)
+ fig2.legend(loc=1)
+ fig2.legend(loc='center left', bbox_to_anchor=(1, 0.5))
+ #fig2.tick_params(labelsize=18)
plt.subplots_adjust(hspace=.6)
+ plt.savefig(graph_name+'.pdf')
+ print('image saved')
plt.show()
- def algos_comparison(self, arr_data_recorder, only_trace=None):
+
+
+ def algos_comparison(self, arr_data_recorder, only_trace=None, graph_name = 'Algorithm Comparison'):
print("************Algorithm Comparison***************")
arr_loc_err = []
arr_trace = []
@@ -260,7 +266,7 @@ class Analyzer():
arr_trace.append([time_stamps, trace_per_run, data_recorder.name] )
print('Plotting Comparison Graphs')
- self.algos_comparison_graph(arr_loc_err, arr_trace, only_trace)
+ self.algos_comparison_graph(arr_loc_err, arr_trace, only_trace, graph_name)
return arr_loc_err, arr_trace
diff --git a/CoLo-AT/dataset_manager/__pycache__/realworld_dataset_manager.cpython-35.pyc b/CoLo-AT/dataset_manager/__pycache__/realworld_dataset_manager.cpython-35.pyc
index 401ad6cc646bff1fee55da87fdee3ea78068da04..05d541571c3a1f104bf21d05dd2af9bcad2fc929 100644
Binary files a/CoLo-AT/dataset_manager/__pycache__/realworld_dataset_manager.cpython-35.pyc and b/CoLo-AT/dataset_manager/__pycache__/realworld_dataset_manager.cpython-35.pyc differ
diff --git a/CoLo-AT/j1_rw.py b/CoLo-AT/j1_rw.py
new file mode 100644
index 0000000000000000000000000000000000000000..acce482e9389c422714e99a5f4542cb473b4ba3e
--- /dev/null
+++ b/CoLo-AT/j1_rw.py
@@ -0,0 +1,14 @@
+from Cooperative_Localization_Journal1 import all_algorithms_comp
+
+# for all colo datasets
+
+datasets_path = "/Users/shengkangchen/Documents/CoLo/CoLo-D/CoLo-Datasets/official_dataset"
+robot_labels = [1,2,3]
+duration = 180 # duration for the simulation in sec
+
+for i in [1, 2, 3, 4]:
+ dataset_label = str(i)
+ dataset_path = datasets_path + dataset_label + "/"
+ graph_name = 'colo_d'+dataset_label
+ print(dataset_path)
+ all_algorithms_comp(dataset_path, robot_labels, duration, graph_name, robots_cant_observe_lm = [2])
\ No newline at end of file
diff --git a/CoLo-AT/localization_algos/__pycache__/ekf_gs_ci2.cpython-35.pyc b/CoLo-AT/localization_algos/__pycache__/ekf_gs_ci2.cpython-35.pyc
index 644e3723232ce5b977d0f3d3d8841634c8166d5c..0cb9c6ca7d16d801a09daffa4aba2705f19e4f68 100644
Binary files a/CoLo-AT/localization_algos/__pycache__/ekf_gs_ci2.cpython-35.pyc and b/CoLo-AT/localization_algos/__pycache__/ekf_gs_ci2.cpython-35.pyc differ
diff --git a/CoLo-AT/localization_algos/__pycache__/ekf_gs_sci2.cpython-35.pyc b/CoLo-AT/localization_algos/__pycache__/ekf_gs_sci2.cpython-35.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..eb0e1dd3c13bd7aa7155d791624637cefd5d3e62
Binary files /dev/null and b/CoLo-AT/localization_algos/__pycache__/ekf_gs_sci2.cpython-35.pyc differ
diff --git a/CoLo-AT/localization_algos/__pycache__/ekf_ls_bda.cpython-35.pyc b/CoLo-AT/localization_algos/__pycache__/ekf_ls_bda.cpython-35.pyc
index 005931bbcedf15b2cb73c58feec0bec63bdfca66..d55b18a6c0f4845020d49f3c19a74a7cced17cd4 100644
Binary files a/CoLo-AT/localization_algos/__pycache__/ekf_ls_bda.cpython-35.pyc and b/CoLo-AT/localization_algos/__pycache__/ekf_ls_bda.cpython-35.pyc differ
diff --git a/CoLo-AT/localization_algos/__pycache__/ekf_ls_ci.cpython-35.pyc b/CoLo-AT/localization_algos/__pycache__/ekf_ls_ci.cpython-35.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..b6d54346a32c9a8b6ba2991e85c45b9c565672b3
Binary files /dev/null and b/CoLo-AT/localization_algos/__pycache__/ekf_ls_ci.cpython-35.pyc differ
diff --git a/CoLo-AT/localization_algos/__pycache__/gs_ci_bound.cpython-35.pyc b/CoLo-AT/localization_algos/__pycache__/gs_ci_bound.cpython-35.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..42ed054d3cff24e9171b2cf3fe800327196a6610
Binary files /dev/null and b/CoLo-AT/localization_algos/__pycache__/gs_ci_bound.cpython-35.pyc differ
diff --git a/CoLo-AT/requests/.DS_Store b/CoLo-AT/requests/.DS_Store
new file mode 100644
index 0000000000000000000000000000000000000000..1b0c0949cb8e8abe564701939a56003c44d04be9
Binary files /dev/null and b/CoLo-AT/requests/.DS_Store differ
diff --git a/CoLo-AT/robots/.DS_Store b/CoLo-AT/robots/.DS_Store
new file mode 100644
index 0000000000000000000000000000000000000000..8fb90713bc54a375d609acc4e6aa35020f3f2633
Binary files /dev/null and b/CoLo-AT/robots/.DS_Store differ
diff --git a/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-35.pyc b/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-35.pyc
index 019e67c982d57ed0600d4481d0dfbe282cbd1f2e..1446b273a517e5b091890606948321464c55be8b 100644
Binary files a/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-35.pyc and b/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-35.pyc differ
diff --git a/CoLo-AT/simulation_process/__pycache__/state_recorder.cpython-35.pyc b/CoLo-AT/simulation_process/__pycache__/state_recorder.cpython-35.pyc
index 74722abe96c6d241dd00ed16acf1fa8b595bc26b..ec8b4c20cdb13d306d53d8377670fe2adbf3ddb2 100644
Binary files a/CoLo-AT/simulation_process/__pycache__/state_recorder.cpython-35.pyc and b/CoLo-AT/simulation_process/__pycache__/state_recorder.cpython-35.pyc differ
diff --git a/CoLo-AT/simulation_process/sim_manager.py b/CoLo-AT/simulation_process/sim_manager.py
index e454cb1087ef3b1351f4a6e403c5a06a06103a19..944b20f11f02d79650742678fefbdcd3a4b781c8 100644
--- a/CoLo-AT/simulation_process/sim_manager.py
+++ b/CoLo-AT/simulation_process/sim_manager.py
@@ -14,10 +14,13 @@ class SimulationManager():
def __init__(self, name):
self.name = name
- def comm_controller(self, rbt_idx, num_robots):
+ def comm_controller(self, rbt_idx, num_robots, rsp):
receiver_idx = rbt_idx
- sender_idx = (rbt_idx+1)%(num_robots)
-
+ #sender_idx = (rbt_idx+1)%(num_robots)
+ sender_idx = None
+ if rsp.get_type()=='measurement' and rsp.get_data()['subject_ID'] <= 5:
+ sender_id = rsp.get_data()['subject_ID']
+ sender_idx = self.robot_labels.index(sender_id)
return receiver_idx, sender_idx
def simulated_communication(self, receiver_status, sender_status):
@@ -39,13 +42,22 @@ class SimulationManager():
else:
return False
+ def allow_operation(self, rsp):
+ if rsp.get_type()=='measurement':
+ rbt_idx = rsp.get_robot_index()
+ rsp_dasa = rsp.get_data()
+ if rsp_dasa['subject_ID'] > 5 and self.robot_labels[rbt_idx] in self.robots_cant_observe_lm:
+ return False
+ return True
+
- def sim_process_native(self, robot_labels, dm, robot_system, state_recorder, simulated_comm = True, comm = True, simple_plot = False):
+ def sim_process_native(self, robot_labels, dm, robot_system, state_recorder, simulated_comm = True, comm = True, simple_plot = False, robots_cant_observe_lm = None):
#dm: dataset manager
print('******** Simulation Process Started! ********')
print('communication: ', comm)
self.robot_labels = robot_labels
self.robot_system = robot_system
+ self.robots_cant_observe_lm = robots_cant_observe_lm
start_time = dm.get_start_time()
duration = dm.get_duration()
@@ -73,33 +85,38 @@ class SimulationManager():
break
if valid == False :
break
- robot_state = robot_system.localization_update(rsp)
- state_var.append(robot_state['state variance'])
- state_recorder.record_state(rsp, robot_state)
+
+ if self.allow_operation(rsp):
+ robot_state = robot_system.localization_update(rsp)
+ state_var.append(robot_state['state variance'])
+ state_recorder.record_state(rsp, robot_state)
# communication protocall
+ comm_rsp = None
if comm:
if rsp.get_type()=='measurement':
rbt_idx = rsp.get_robot_index()
- 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)
+ receiver_idx, sender_idx = self.comm_controller(rbt_idx, self.robot_system.num_robots, rsp)
+
+ if sender_idx != None and receiver_idx != None:
+ 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)
+
+ if comm_rsp != None:
+ 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)