able to generate txt files for trajectory with landamarks

CoLo-AT/Cooperative_Localization_Journal1.py

@@ -51,7 +51,9 @@ def all_algorithms_comp(dataset_path, robot_labels, duration, graph_name, robots
 
 	sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder, simple_plot = False ,  robots_cant_observe_lm = robots_cant_observe_lm)
 	analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = True)
-	analyzer.trajectory_plot(state_recorder)
+	
+	landmark_map = testing_dataset.get_landmark_map()
+	analyzer.trajectory_plot(state_recorder, landmark_map = landmark_map)
 	
 	##############################################################################
 	'''
@@ -69,7 +71,7 @@ def all_algorithms_comp(dataset_path, robot_labels, duration, graph_name, robots
 	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)
@@ -110,4 +112,9 @@ def all_algorithms_comp(dataset_path, robot_labels, duration, graph_name, robots
 	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], graph_name = graph_name)
-	analyzer.generate_result_files([state_recorder, state_recorder_LS_cen, state_recorder_LS_BDA, state_recorder_LS_CI, state_recorder_GS_SCI])
\ No newline at end of file
+	analyzer.generate_result_files([state_recorder, state_recorder_LS_cen, state_recorder_LS_BDA, state_recorder_LS_CI, state_recorder_GS_SCI])
+
+
+
+	print("Animation Start: ")
+	animate_plot(dataset_labels, state_recorder, analyzer, lm = testing_dataset.get_landmark_map())
\ No newline at end of file

CoLo-AT/data_analysis/data_analyzer.py

@@ -283,12 +283,28 @@ class Analyzer():
 		return arr_loc_err, arr_trace
 
 
-	def trajectory_plot(self, data_recorder, robot_labels = None):
+	def trajectory_plot(self, data_recorder, landmark_map = None, robot_labels = None):
 		if robot_labels  == None:
 			robot_labels = self.dataset_labels
 
-		robot_loc_time_unit = self.robot_location_at_unit_time_interval(data_recorder)
 
+		# generating landmark file
+		lm_file = open("landmark_map.txt", "w")
+		lm_file.write('#id' + ' ' + 'x[m]' + ' ' + 'y[m]' + '\n')
+		
+		landmarks_loc = []
+		print(landmark_map)
+		if landmark_map != None:
+			for landmark_id, [x, y] in landmark_map.items():
+				if [x, y] not in landmarks_loc: # duplicate landmark with the same position
+					result_str = '{:d} {:2.4f} {:2.4f} \n'.format(landmark_id, x, y)
+					landmarks_loc.append([x, y])
+					lm_file.write(result_str)
+		lm_file.close
+
+
+		# generating trajectory file
+		robot_loc_time_unit = self.robot_location_at_unit_time_interval(data_recorder)
 
 
 		trajectory_file = open("trajectory.txt", "w")

CoLo-AT/landmark_map.txt

+#id x[m] y[m]
+113 0.7155 -1.9365 
+103 0.8365 1.1135 
+107 -1.2505 -0.8313 
+110 0.9218 -0.7042 
+109 -0.2887 -2.1572 
+108 0.3667 1.2349 
+120 -1.2377 0.0993 
+124 -0.7332 1.2293