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
Binary files a/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-36.pyc and b/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-36.pyc differ
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
Binary files a/CoLo-AT/dataset_manager/__pycache__/realworld_dataset_manager.cpython-36.pyc and b/CoLo-AT/dataset_manager/__pycache__/realworld_dataset_manager.cpython-36.pyc differ
diff --git a/CoLo-AT/dataset_manager/realworld_dataset_manager.py b/CoLo-AT/dataset_manager/realworld_dataset_manager.py
index ea3221235f053b7913a3a1d73859f0191cc4976e..b52738571bcbcfca8ba1f015367f78f2eda62de3 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
Binary files a/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-36.pyc and b/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-36.pyc differ
diff --git a/CoLo-AT/simulation_process/sim_manager.py b/CoLo-AT/simulation_process/sim_manager.py
index c4bf8c4b80802d0830f9728bf1f683f2f71e483b..6bf24cae28d550a2a28a19e0c37dfaf3e75659b9 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 405984feb153998c5efc24efb195059a6215fad5..4466974040999ed47759ec8a92c3881b11a76be5 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)
##############################################################################