diff --git a/CoLo-AT/Algorithm Comparison.pdf b/CoLo-AT/Algorithm Comparison.pdf
index c2a354708623c45db863e612509ca6068795b2f1..a1d4ee81737be9c0f6d25f3cacc10dabf459062f 100644
Binary files a/CoLo-AT/Algorithm Comparison.pdf and b/CoLo-AT/Algorithm Comparison.pdf differ
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 ae53ffcb4e2b5018885bded3a62c28e30f410d13..4e73685ba4848d50830b3fc9c200733bc22d692b 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/data_analysis/data_analyzer.py b/CoLo-AT/data_analysis/data_analyzer.py
index 30302e3884aa22ef4885c441a9e17d20627b0196..0e2fd6228978f0254cdfaedc08c6b9471097a663 100644
--- a/CoLo-AT/data_analysis/data_analyzer.py
+++ b/CoLo-AT/data_analysis/data_analyzer.py
@@ -76,7 +76,7 @@ class Analyzer():
- def calculate_loc_err_and_trace_state_variance_per_run(self, data_recorder, unit_time_interval = 1, plot_graphs = True):
+ def calculate_loc_err_and_trace_state_variance_per_run(self, data_recorder, unit_time_interval = 1, plot_graphs = True, selected_labels = None):
#recorded_dataline = [time, robot_label, est_x_pos, est_y_pos, trace_state_var, gt_x_pos, gt_y_pos, loc_err, update type]
data = data_recorder.get_recorded_data()
@@ -121,9 +121,11 @@ class Analyzer():
comm_count +=1
- loc_err_per_time_iterval+= data_in_time_order[time_index][7]
- trace_per_time_iterval += data_in_time_order[time_index][4]
- num_dataline_per_time_iterval+=1
+
+ if selected_labels == None or data_in_time_order[time_index][1] in selected_labels:
+ loc_err_per_time_iterval+= data_in_time_order[time_index][7]
+ trace_per_time_iterval += data_in_time_order[time_index][4]
+ num_dataline_per_time_iterval+=1
time_index += 1
@@ -233,7 +235,7 @@ class Analyzer():
fig1.set_title('Location error')
fig1.set_xlabel('Time[s]')
fig1.set_ylabel('RMS[m]')
- fig1.set_ylim(0, 2)
+ 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)
@@ -241,7 +243,7 @@ class Analyzer():
fig2.set_title('Trace of state covariance')
fig2.set_xlabel('Time [s]')
fig2.set_ylabel('Sigma_s [m^2]')
- fig2.set_ylim(0, 0.2)
+ #fig2.set_ylim(0, 0.2)
fig2.legend(loc=1)
fig2.legend(loc='center left', bbox_to_anchor=(1, 0.5))
#fig2.tick_params(labelsize=18)
@@ -255,12 +257,12 @@ class Analyzer():
- def algos_comparison(self, arr_data_recorder, only_trace=None, graph_name = 'Algorithm Comparison'):
+ def algos_comparison(self, arr_data_recorder, only_trace=None, graph_name = 'Algorithm Comparison', selected_labels = None):
print("************Algorithm Comparison***************")
arr_loc_err = []
arr_trace = []
for data_recorder in arr_data_recorder:
- loc_err_per_run, trace_per_run, time_stamps = self.calculate_loc_err_and_trace_state_variance_per_run(data_recorder, plot_graphs = False)
+ loc_err_per_run, trace_per_run, time_stamps = self.calculate_loc_err_and_trace_state_variance_per_run(data_recorder, plot_graphs = False, selected_labels = selected_labels)
if only_trace == None or data_recorder.name not in only_trace:
arr_loc_err.append([time_stamps, loc_err_per_run, data_recorder.name])
arr_trace.append([time_stamps, trace_per_run, data_recorder.name] )
diff --git a/CoLo-AT/dataset_manager/__pycache__/simulated_dataset_manager_w.cpython-36.pyc b/CoLo-AT/dataset_manager/__pycache__/simulated_dataset_manager_w.cpython-36.pyc
index 995425e8175bc1ff852dc294c58f967c09f771fb..43e34a0be729cd1dc1ba44c010498b30d68197cb 100644
Binary files a/CoLo-AT/dataset_manager/__pycache__/simulated_dataset_manager_w.cpython-36.pyc and b/CoLo-AT/dataset_manager/__pycache__/simulated_dataset_manager_w.cpython-36.pyc differ
diff --git a/CoLo-AT/dataset_manager/simulated_dataset_manager_w.py b/CoLo-AT/dataset_manager/simulated_dataset_manager_w.py
index e7a30a532d90fb1e1af7856de4ce4393226eee72..eac9c423c322967505016909dcba1322d54c3757 100755
--- a/CoLo-AT/dataset_manager/simulated_dataset_manager_w.py
+++ b/CoLo-AT/dataset_manager/simulated_dataset_manager_w.py
@@ -48,6 +48,7 @@ def linear_interpolation(end0, end1, x):
y = y0+(x-x0)*(y1-y0)/(x1-x0)
return y
+
class Sim_Dataset_Manager:
def __init__(self, dataset_name):
@@ -73,7 +74,7 @@ class Sim_Dataset_Manager:
self.odometry_data = [[] for i in range(self.num_robots)]
self.groundtruth_data = [[] for i in range(self.num_robots)]
#noise
- self.noise_v = 0.0125
+ self.noise_v = 0.01
self.noise_range = 0.1
self.noise_bearing = 0.035
@@ -92,6 +93,25 @@ class Sim_Dataset_Manager:
self.dataset_data = {'odometry': self.odometry_data, 'measurement': self.measurement_data, 'groundtruth': self.groundtruth_data}
return self.start_time, self.starting_states, self.dataset_data, self.time_arr
+
+ def observation_target(self, rbt_lable, op_idx):
+ target = None
+ if rbt_lable == 1:
+ if op_idx == 1:
+ target = 6
+
+ elif rbt_lable == 2:
+ if op_idx == 2:
+ target = 1
+
+ elif rbt_lable == 3:
+ if op_idx == 1:
+ target = 6
+ elif op_idx == 2:
+ target = 4
+
+ return target
+
def get_start_time(self):
return self.start_time
@@ -110,6 +130,7 @@ class Sim_Dataset_Manager:
def respond(self, req, current_time, need_specific_time = False):
message = req.get_message()
+ valid_respond = False
if need_specific_time:
valid_respond = False
else:
@@ -127,9 +148,9 @@ class Sim_Dataset_Manager:
if op_idx == 0: #propagation
req_type = 'odometry'
- actual_v = np.random.uniform(0.5) # between 0 to 0.5 m/s
+ actual_v = np.random.uniform(-0.25, 0.25) # between 0 to 0.5 m/s
input_v = actual_v + np.random.normal(0, self.noise_v)
- actual_av = 0.1
+ actual_av = 0.02
delta_t = self.op_time_interval[rbt_idx, op_idx]
gt_x = gt_x + actual_v*delta_t*cos(gt_orientation)
gt_y = gt_y + actual_v*delta_t*sin(gt_orientation)
@@ -140,28 +161,31 @@ class Sim_Dataset_Manager:
elif op_idx == 1: # landmark observation
req_type = 'measurement'
- [lm_x, lm_y]= self.landmark_map[6]
- delta_x = lm_x - gt_x
- delta_y = lm_y - gt_y
- meas_range = sqrt(delta_y**2+delta_x**2) + np.random.normal(0, self.noise_range)
- bearing = math.atan2(delta_y, delta_x) - gt_orientation + np.random.normal(0, self.noise_bearing)
- message['data'] = {'time':req_time, 'subject_ID':6, 'measurment_range': meas_range, 'bearing':bearing}
- valid_respond = True
+ observed_label = self.observation_target(self.robot_lables[rbt_idx], op_idx)
+ if observed_label != None:
+ [lm_x, lm_y]= self.landmark_map[6]
+ delta_x = lm_x - gt_x
+ delta_y = lm_y - gt_y
+ meas_range = sqrt(delta_y**2+delta_x**2) + np.random.normal(0, self.noise_range)
+ bearing = math.atan2(delta_y, delta_x) - gt_orientation + np.random.normal(0, self.noise_bearing)
+ message['data'] = {'time':req_time, 'subject_ID':6, 'measurment_range': meas_range, 'bearing':bearing}
+ valid_respond = True
elif op_idx == 2: # relative observation
req_type = 'measurement'
- observed_label = self.robot_lables[(rbt_idx+1)%self.num_robots]
- obser_x = self.current_states[observed_label]['x']
- obser_y = self.current_states[observed_label]['y']
- delta_x = obser_x - gt_x
- delta_y = obser_y - gt_y
- meas_range = sqrt(delta_y**2+delta_x**2) + np.random.normal(0, self.noise_range)
- bearing = math.atan2(delta_y, delta_x) - gt_orientation + np.random.normal(0, self.noise_bearing)
- message['data'] = {'time':req_time, 'subject_ID': observed_label, 'measurment_range': meas_range, 'bearing':bearing}
- valid_respond = True
-
-
+ observed_label = self.observation_target(self.robot_lables[rbt_idx], op_idx)
+ if observed_label != None:
+ #observed_label = self.robot_lables[(rbt_idx+1)%self.num_robots]
+ obser_x = self.current_states[observed_label]['x']
+ obser_y = self.current_states[observed_label]['y']
+ delta_x = obser_x - gt_x
+ delta_y = obser_y - gt_y
+ meas_range = sqrt(delta_y**2+delta_x**2) + np.random.normal(0, self.noise_range)
+ bearing = math.atan2(delta_y, delta_x) - gt_orientation + np.random.normal(0, self.noise_bearing)
+ message['data'] = {'time':req_time, 'subject_ID': observed_label, 'measurment_range': meas_range, 'bearing':bearing}
+ valid_respond = True
+ print(self.robot_lables[rbt_idx], observed_label)
message['groundtruth'] = {'time':req_time, 'x_pos':gt_x, 'y_pos':gt_y, 'orientation':gt_orientation}
self.current_states[self.robot_lables[rbt_idx]] = {'x': gt_x, 'y': gt_y, 'orientation': gt_orientation}
@@ -209,5 +233,5 @@ class Sim_Dataset_Manager:
for label in self.robot_lables:
self.current_states[label] = {'x': self.starting_states[label][1], 'y': self.starting_states[label][2], 'orientation': self.starting_states[label][3]}
- self.time_arr = np.zeros(self.num_robots, self.num_op)
+ self.time_arr = np.zeros((self.num_robots, self.num_op))
return self.start_time, self.starting_states, self.dataset_data, self.time_arr
\ No newline at end of file
diff --git a/CoLo-AT/gs_ci_journal_sim.py b/CoLo-AT/gs_ci_journal_sim.py
index 32d081d0fa98d3659ffe72273992605c5d4de7cb..7adbe720599157ce4300882b91e6c03871949a91 100644
--- a/CoLo-AT/gs_ci_journal_sim.py
+++ b/CoLo-AT/gs_ci_journal_sim.py
@@ -23,7 +23,7 @@ from gs_ci_bound import GS_CI_Bound
##############################################################################
-duration = 60
+duration = 200
robot_labels = [1,2,3,4]
testing_dataset = Sim_Dataset_Manager('testing')
start_time, starting_states, dataset_data, time_arr = testing_dataset.circular_path_4_robots(duration)
@@ -34,15 +34,14 @@ 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)
+sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder, simple_plot = True)
-loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = True)
+loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = True, selected_labels = [1])
analyzer.trajectory_plot(state_recorder)
-#animate_plot(robot_labels, state_recorder, analyzer, testing_dataset.get_landmark_map())
-'''
+
##############################################################################
testing_dataset.dataset_reset()
@@ -51,8 +50,51 @@ 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)
-loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_bound, plot_graphs = True)
+sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_bound, simple_plot = True)
+loc_err_per_run, trace_per_run_b, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_bound, plot_graphs = True, selected_labels = [1])
+
+
+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, 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)
+
+ 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)
+
+ 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)
+
+ 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_bound], only_trace = ['gs_ci_bound'])
-'''
\ No newline at end of file
+analyzer.algos_comparison([state_recorder, state_recorder_bound], only_trace = ['gs_ci_bound'], selected_labels = [1])
\ No newline at end of file
diff --git a/CoLo-AT/localization_algos/__pycache__/ekf_gs_ci2.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/ekf_gs_ci2.cpython-36.pyc
index e0fb6da36a8122d0230cf0396f055b6f7bee79b8..c9eaca6174e11a6bd773ac94e0954da503cad1bf 100644
Binary files a/CoLo-AT/localization_algos/__pycache__/ekf_gs_ci2.cpython-36.pyc and b/CoLo-AT/localization_algos/__pycache__/ekf_gs_ci2.cpython-36.pyc differ
diff --git a/CoLo-AT/localization_algos/__pycache__/gs_ci_bound.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/gs_ci_bound.cpython-36.pyc
index 0011e92cb63c4fc5e20457ee0cf17255617a39e5..ae10aa7792e6ae1c7580c28e4258839aa5d64c59 100644
Binary files a/CoLo-AT/localization_algos/__pycache__/gs_ci_bound.cpython-36.pyc and b/CoLo-AT/localization_algos/__pycache__/gs_ci_bound.cpython-36.pyc differ
diff --git a/CoLo-AT/localization_algos/ekf_gs_ci2.py b/CoLo-AT/localization_algos/ekf_gs_ci2.py
index 9fe960ffb51d3ad49882a0b7e2e9a975e1688c7b..13aa50959e24cafbebeea6aa005903fe421858e0 100644
--- a/CoLo-AT/localization_algos/ekf_gs_ci2.py
+++ b/CoLo-AT/localization_algos/ekf_gs_ci2.py
@@ -12,27 +12,29 @@ class EKF_GS_CI2(ekf_algo_framework):
ekf_algo_framework.__init__(self, algo_name)
def state_variance_init(self, num_robots):
- return 0.01*np.matrix(np.identity(2*num_robots), dtype = float)
+ return 0.03*np.matrix(np.identity(2*num_robots), dtype = float)
def calculate_trace_state_variance(self, robot_data):
[s, orinetations, sigma_s, index] = robot_data
i = 2*index
trace_state_var = np.trace(sigma_s[i:i+2, i:i+2])
- return trace_state_var
+ return np.trace(sigma_s)
def propagation_update(self, robot_data, sensor_data):
[s, orinetations, sigma_s, index] = robot_data
[measurement_data, sensor_covariance] = sensor_data
sigma_odo = sensor_covariance
- var_v = 0.5# variance of the velocity
- exp_v = 0, # expected vecolcity for other robots
+ var_v = 0.25# variance of the velocity
+ exp_v = 0 # expected vecolcity for other robots
i = 2*index
num_robots = int(len(s)/2)
delta_t = measurement_data[0]
v = measurement_data[1]
orinetations[index] = measurement_data[2]
+ gt_orientations = measurement_data[3]
+
self_theta = orinetations[index]
Q = sigma_odo
@@ -49,8 +51,8 @@ class EKF_GS_CI2(ekf_algo_framework):
#print(np.matmul(sigma_odo,rot_mtx(self_theta).getT()))
sigma_s[jj:jj+2, jj:jj+2] += delta_t*delta_t*rot_mtx(self_theta)*Q*rot_mtx(self_theta).getT()
else:
- s[jj,0] = s[jj,0] + cos(orinetations[j])*v*delta_t #x
- s[jj+1,0] = s[jj+1,0] + sin(orinetations[j])*v*delta_t #y
+ s[jj,0] = s[jj,0] + cos(gt_orientations[j])*exp_v*delta_t #x
+ s[jj+1,0] = s[jj+1,0] + sin(gt_orientations[j])*exp_v*delta_t #y
sigma_s[jj:jj+2, jj:jj+2] += delta_t*delta_t*var_v*np.identity(2)
return [s, orinetations, sigma_s]
@@ -131,6 +133,7 @@ class EKF_GS_CI2(ekf_algo_framework):
kalman_gain = sigma_s*H.getT()*sigma_invention.getI()
s = s + kalman_gain*(z - z_hat)
+ print(index, z - z_hat)
sigma_s = sigma_s - kalman_gain*H*sigma_s
return [s, orinetations, sigma_s]
diff --git a/CoLo-AT/localization_algos/gs_ci_bound.py b/CoLo-AT/localization_algos/gs_ci_bound.py
index da3a82097d2c3bd74131201efc44ea3827bb96f0..44f08048b41efe65509bdaaef92c4fdef21f60b2 100644
--- a/CoLo-AT/localization_algos/gs_ci_bound.py
+++ b/CoLo-AT/localization_algos/gs_ci_bound.py
@@ -12,16 +12,16 @@ class GS_CI_Bound(ekf_algo_framework):
self.d_max = 2 # max measurement distance
self.d_var = 0.2
self.bearing_var = 0.2
- self.var_v = 0.1
+ self.var_v = 0.25
def state_variance_init(self, num_robots):
- return 0.01*np.matrix(np.identity(2*num_robots), dtype = float)
+ return 0.04*np.matrix(np.identity(2*num_robots), dtype = float)
def calculate_trace_state_variance(self, robot_data):
[s, orinetations, sigma_s, index] = robot_data
i = 2*index
trace_state_var = np.trace(sigma_s[i:i+2, i:i+2])
- return trace_state_var
+ return np.trace(sigma_s)
#def propagation_update(self, s, th_sigma_s, sigma_odo, index, odo_freq):
def propagation_update(self, robot_data, sensor_data):
diff --git a/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc b/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc
index a3749f65089c47a4d5adccc0f77e5a142a6440b3..e6f893b9bb34363e909e83cc8442b5731bb39e34 100644
Binary files a/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc and b/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc differ
diff --git a/CoLo-AT/robots/robot_system.py b/CoLo-AT/robots/robot_system.py
index e072873133ab19e422d0346f8c2dc12760b9ec87..075be13efdc5fd156693047456735fc42c3ce749 100644
--- a/CoLo-AT/robots/robot_system.py
+++ b/CoLo-AT/robots/robot_system.py
@@ -31,6 +31,7 @@ class RobotSystem:
else:
self.robot_sys = CentralizedRobotSystem(name, robot_labels, loc_algo)
print('created a centralized system for ', self.num_robots, ' robots ')
+ self.gt_orientations = np.zeros(self.num_robots)
def set_starting_state(self, start_state_arr):
if self.distr_sys:
@@ -62,6 +63,9 @@ class RobotSystem:
def set_start_moving_times(self, start_moving_times):
self.prev_prop_times = start_moving_times
+ def gt_update_orientation(self, rbt_idx, update_orientation):
+ self.gt_orientations[rbt_idx] = update_orientation
+
def localization_update(self, rsp):
#message format: {'time': float, 'robot_index': int, 'data': {'time': float, 'velocity': float, 'orientation': float}, 'groundtruth': None}
message = rsp.get_message()
@@ -82,12 +86,14 @@ class RobotSystem:
sensor_covariance = sigma_odo
delta_t = message_data['delta_t']
+ self.gt_update_orientation(robot_index, message_data['orientation'])
+
if delta_t <0:
print('current time: ', message_data['time'] )
print('prev_prop_time: ', self.prev_prop_times[robot_index])
raise Exception("Error incorrect delta_t!")
- sensor_input = [delta_t, v, message_data['orientation']]
+ sensor_input = [delta_t, v, message_data['orientation'], self.gt_orientations]
self.prev_prop_times[robot_index] = message_data['time']
elif rsp_type == 'measurement':
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 9269b36a4870d9616b209d71121adb18806b365c..73c6d492da67ac9b0409341a80ca3aa9d5e6c4c5 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 494515398848b0bb6c677b3161e970d86e3831ab..3024da48a996aa31b354d49d38d0283674617242 100644
--- a/CoLo-AT/simulation_process/sim_manager.py
+++ b/CoLo-AT/simulation_process/sim_manager.py
@@ -18,9 +18,18 @@ class SimulationManager():
receiver_idx = rbt_idx
#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)
+ '''
+
+ if rbt_idx == 0:
+ sender_idx = 1
+
+ if rbt_idx == 1:
+ sender_idx = 2
+
return receiver_idx, sender_idx
def simulated_communication(self, receiver_status, sender_status):
@@ -43,16 +52,15 @@ class SimulationManager():
return False
'''
- if np.random.randint(0,20) <=1:
- return True
- else:
- return False
+ return True
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.robots_cant_observe_lm is not None and self.robot_labels[rbt_idx] in self.robots_cant_observe_lm:
return False
+ '''
return True
@@ -83,7 +91,7 @@ class SimulationManager():
#print("Current time:", self.time)
g_req= request_response.Request_response(None, None)
prev_time = self.time
- valid, self.time , rsp = dm.respond(g_req, self.time)
+ valid_op, self.time , rsp = dm.respond(g_req, self.time)
#print(rsp.get_message())
if self.time < prev_time:
print('Time inconsistant!')
@@ -92,7 +100,7 @@ class SimulationManager():
if valid == False :
break
'''
- if self.allow_operation(rsp) and valid:
+ if valid_op and 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)
@@ -100,7 +108,7 @@ class SimulationManager():
# communication protocall
comm_rsp = None
if comm:
- if rsp.get_type()=='measurement':
+ if valid_op and rsp.get_type()=='measurement':
rbt_idx = rsp.get_robot_index()
receiver_idx, sender_idx = self.comm_controller(rbt_idx, self.robot_system.num_robots, rsp)
@@ -120,6 +128,8 @@ class SimulationManager():
comm_rsp.set_message(message)
if comm_rsp != None:
+ print('comm', robot_labels[sender_idx], robot_labels[receiver_idx])
+
robot_state = robot_system.localization_update(comm_rsp)
state_var.append(robot_state['state variance'])
state_recorder.record_state(rsp, robot_state)
@@ -189,6 +199,7 @@ class SimulationManager():
message['groundtruth'] = rsp.get_groundtruth
comm_rsp.set_message(message)
+
robot_state = robot_system.localization_update(rsp)
#state_recorder.record_state(rsp, robot_state)