added simulated comm

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
+

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()

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)
 
 ##############################################################################