Add files via upload

Author: lukovicaleksa

scripts/rtime_gmapping_node.py

@@ -16,12 +16,12 @@
 from utils import *
 
 P_prior = 0.5	# Prior occupancy probability
-P_occ = 0.9	# Probability that cell is occupied with total confidence
+P_occ = 0.9		# Probability that cell is occupied with total confidence
 P_free = 0.3	# Probability that cell is free with total confidence 
 
-RESOLUTION = 0.025 # Grid resolution in [m]
+RESOLUTION = 0.03 # Grid resolution in [m]
 
-MAP_NAME  = 'stage_4' # map name without extension
+MAP_NAME  = 'world' # map name without extension
 
 if __name__ == '__main__':
 
@@ -49,9 +49,14 @@
 
 		# Create grid map 
 		gridMap = GridMap(X_lim = map_x_lim, 
-				  Y_lim = map_y_lim, 
-			 	  resolution = RESOLUTION, 
-				  p = P_prior)
+						  Y_lim = map_y_lim, 
+						  resolution = RESOLUTION, 
+						  p = P_prior)
+
+		# Init time
+		t_start = perf_counter()
+		sim_time = 0
+		step = 0
 
 		# Main loop
 		while not rospy.is_shutdown():
@@ -109,50 +114,62 @@
 				set_pixel_color(bgr_image, x, y, 'GREEN')
 
 			resized_image = cv2.resize(src = bgr_image, 
-						   dsize = (500, 500), 
-						   interpolation = cv2.INTER_AREA)
+									   dsize = (500, 500), 
+									   interpolation = cv2.INTER_AREA)
 
 			rotated_image = cv2.rotate(src = resized_image, 
-						   rotateCode = cv2.ROTATE_90_COUNTERCLOCKWISE)
+									   rotateCode = cv2.ROTATE_90_COUNTERCLOCKWISE)
 
 			cv2.imshow("Grid map", rotated_image)
 			cv2.waitKey(1)
 
+			# Calculate step time in [s]
+			t_step = perf_counter()
+			step_time = t_step - t_start
+			sim_time += step_time
+			t_start = t_step
+			step += 1 
+
+			print('Step %d ==> %d [ms]' % (step, step_time * 1000))
+
 			rate.sleep()
 
 	except rospy.ROSInterruptException:
 
 		print('\r\nSIMULATION TERMINATED!')
+		print('\nSimulation time: %.2f [s]' % sim_time)
+		print('Average step time: %d [ms]' % (sim_time * 1000 / step))
+		print('Frames per second: %.1f' % (step / sim_time))
 
 		# Saving Grid Map
 		resized_image = cv2.resize(src = gridMap.to_BGR_image(), 
-					   dsize = (500, 500), 
-					   interpolation = cv2.INTER_AREA)
+								   dsize = (500, 500), 
+								   interpolation = cv2.INTER_AREA)
 
 		rotated_image = cv2.rotate(src = resized_image, 
-					   rotateCode = cv2.ROTATE_90_COUNTERCLOCKWISE)
+								   rotateCode = cv2.ROTATE_90_COUNTERCLOCKWISE)
 
 		flag_1 = cv2.imwrite(img = rotated_image * 255.0, 
-				     filename = MAPS_PATH + '/' + MAP_NAME + '_GRID_MAP.png')
+				             filename = MAPS_PATH + '/' + MAP_NAME + '_grid_map_TEST.png')
 
 		# Calculating Maximum likelihood estimate of the map
 		gridMap.calc_MLE()
 
 		# Saving MLE of the Grid Map
 		resized_image_MLE = cv2.resize(src = gridMap.to_BGR_image(), 
-					       dsize = (500, 500), 
-					       interpolation = cv2.INTER_AREA)
+									   dsize = (500, 500), 
+									   interpolation = cv2.INTER_AREA)
 
 		rotated_image_MLE = cv2.rotate(src = resized_image_MLE, 
-					       rotateCode = cv2.ROTATE_90_COUNTERCLOCKWISE)
+								       rotateCode = cv2.ROTATE_90_COUNTERCLOCKWISE)
 
 		flag_2 = cv2.imwrite(img = rotated_image_MLE * 255.0, 
-				     filename = MAPS_PATH + '/' + MAP_NAME + '_GRID_MAP_MLE.png')
+				             filename = MAPS_PATH + '/' + MAP_NAME + '_grid_map_TEST_mle.png')
 
 		if flag_1 and flag_2:
 			print('\nGrid map successfully saved!\n')
 
 		if cv2.waitKey(0) == 27:
 			cv2.destroyAllWindows()
 
-		pass
+		pass