Minor changes added

Author: AngeloEspinoza

RRT.py

@@ -11,8 +11,8 @@
 parser.add_argument('-o', '--obstacles', type=bool, action=argparse.BooleanOptionalAction, metavar='', required=False, help='Obstacles on the map')
 parser.add_argument('-n', '--nodes', type=int, metavar='', required=False, help='Maximum number of nodes')
 parser.add_argument('-e', '--epsilon', type=float, metavar='', required=False, help='Step size')
-parser.add_argument('-init', '--x_init', nargs='+', type=int, metavar='', required=False, help='Initial node position in x and y respectively')
-parser.add_argument('-goal', '--x_goal', nargs='+', type=int, metavar='', required=False, help='Goal node position in x and y respectively')
+parser.add_argument('-init', '--x_init', nargs='+', type=int, metavar='', required=False, help='Initial node position in X and Y respectively')
+parser.add_argument('-goal', '--x_goal', nargs='+', type=int, metavar='', required=False, help='Goal node position in X and Y respectively')
 parser.add_argument('-srn', '--show_random_nodes', type=bool, action=argparse.BooleanOptionalAction, metavar='', required=False, help='Show random nodes on screen')
 parser.add_argument('-snn', '--show_new_nodes', type=bool, action=argparse.BooleanOptionalAction, metavar='', required=False, help='Show new nodes on screen')
 args = parser.parse_args()
@@ -21,7 +21,7 @@
 WIDTH, HEIGHT = 640, 480
 WINDOW = pygame.display.set_mode(size=(WIDTH, HEIGHT))
 pygame.display.set_caption('RRT')
-FPS = 60
+FPS = 120
 
 # Colors 
 WHITE = (255, 255, 255)
@@ -30,42 +30,118 @@
 GREEN = (0, 255, 0)
 BLUE = (0, 0, 255)
 BROWN = (189, 154, 122)
+GRAY = (105, 105, 105)
 
 # RRT parameters
 MAX_NODES = args.nodes if not args.nodes is None else 5000 # Default maximum number of nodes/vertices
 EPSILON = args.epsilon if not args.epsilon is None else 7.0 # Default step size
+obstacles = []
 
 def draw_window():
 	"""Draw the window all white."""
 	WINDOW.fill(WHITE)
 
-def draw_obstacles():
-	"""Draw obstacles on the window.
+# def draw_obstacles():
+# 	"""Draw obstacles on the window.
+
+# 	Parameters
+# 	----------
+# 	None
+
+# 	Returns
+# 	-------
+# 	list
+# 		All the rectangle obstacles.
+# 	"""
+# 	obstacles = []
+# 	obstacle1 = pygame.Rect((WIDTH//2 + 90, HEIGHT//2 - 45, 90, 90))
+# 	obstacle2 = pygame.Rect((WIDTH//2 - 180, HEIGHT//2 - 45, 90, 90))
+# 	pygame.draw.rect(surface=WINDOW, color=RED, rect=obstacle1)
+# 	pygame.draw.rect(surface=WINDOW, color=RED, rect=obstacle2)
+
+# 	# Just decoration borders in black
+# 	pygame.draw.rect(surface=WINDOW, color=BLACK, rect=obstacle1, width=2)
+# 	pygame.draw.rect(surface=WINDOW, color=BLACK, rect=obstacle2, width=2)
+
+# 	obstacles.append(obstacle1)
+# 	obstacles.append(obstacle2)
+
+# 	return obstacles
 
+def make_obstacles_T(initial_point):
+	"""
+	Given a initial point, it makes a obstacle with shape of T.
+	
 	Parameters
 	----------
-	None
-
+	initial_point : tuple
+		X and Y coordinates, starting from the top-left most part where
+		the obstacle will be placed.
+	
 	Returns
 	-------
 	list
-		All the rectangle obstacles.
+		A collection of sides composing the T obstacle.			
 	"""
-	obstacles = []
-	obstacle1 = pygame.Rect((WIDTH//2 + 90, HEIGHT//2 - 45, 90, 90))
-	obstacle2 = pygame.Rect((WIDTH//2 - 180, HEIGHT//2 - 45, 90, 90))
-	pygame.draw.rect(surface=WINDOW, color=RED, rect=obstacle1)
-	pygame.draw.rect(surface=WINDOW, color=RED, rect=obstacle2)
+	x, y = initial_point[0], initial_point[1]
+	width, height = 50, 150
+
+	side1 = pygame.Rect(x, y, height, width)
+	side2 = pygame.Rect((x+height//2) - width//2, y, width, height)
+
+	obstacle = [side1, side2]
+
+	return obstacle
+
+def make_obstacles_L(initial_point):
+	"""
+	Given a initial point, it makes a obstacle with shape of L.
+	
+	Parameters
+	----------
+	initial_point : tuple
+		X and Y coordinates, starting from the top-left most part where
+		the obstacle will be placed.
+	
+	Returns
+	-------
+	list
+		A collection of sides composing the L obstacle.
+	"""	
+	x, y = initial_point[0], initial_point[1]
+	width, height = 50, 150
+
+	side1 = pygame.Rect(x, y, width, height)
+	side2 = pygame.Rect(x, y+height-width, height, width)
+
+	obstacle = [side1, side2]
 
-	# Just decoration borders in black
-	pygame.draw.rect(surface=WINDOW, color=BLACK, rect=obstacle1, width=2)
-	pygame.draw.rect(surface=WINDOW, color=BLACK, rect=obstacle2, width=2)
+	return obstacle
+
+def make_obstacles():
+	"""Generate the obstacles to be placed on the final map."""
+
+	obstacle1 = make_obstacles_T(initial_point=(350, 200))
+	obstacle2 = make_obstacles_L(initial_point=(150, 20))
 
 	obstacles.append(obstacle1)
 	obstacles.append(obstacle2)
 
 	return obstacles
 
+def draw_obstacles():
+	"""Draw each side of the obstacles."""
+	obstacles = []
+	obs = make_obstacles()
+
+	for obstacle in obs:
+		for side in obstacle:
+			pygame.draw.rect(surface=WINDOW, color=GRAY,
+				rect=side)
+			obstacles.append(side)
+
+	return obstacles
+
 def is_free(point, obstacles, tree):
 	"""Checks whether a node is colliding with an obstacle or not.
 
@@ -149,10 +225,11 @@ def nearest_neighbor(tree, x_rand):
 	global min_distance
 
 	distances = []
+
 	for state in tree:
 		distance = euclidean_distance(state, x_rand)
 		distances.append(distance)
-
+		
 	# Index of the minimum distance to the generated random node
 	min_distance = np.argmin(distances) 
 	x_near = tree[min_distance]
@@ -217,7 +294,7 @@ def generate_parents(values, parent):
 
 	Returns
 	-------
-	parent : list
+	list
 		Ordered collection of the parents.
 	"""
 	parent_value = values[min_distance] # Value nearest node
@@ -230,7 +307,6 @@ def generate_parents(values, parent):
 
 	return parent
 
-
 def main(has_obstacles, show_random_nodes, show_new_nodes):
 	global is_goal_reached
 
@@ -251,25 +327,33 @@ def main(has_obstacles, show_random_nodes, show_new_nodes):
 
 	k = 0
 	node_value = 0
+	iteration = 0
+	
 	while run and k < MAX_NODES:
 		# Make sure the loop runs at 60 FPS
 		clock.tick(FPS)  
 		for event in pygame.event.get():
 			if event.type == pygame.QUIT:
 				run = False
 
-
 		x_rand = generate_random_node() # Random node 
 		x_near = nearest_neighbor(tree, x_rand) # Nearest neighbor to the random node
 		x_new = new_state(x_rand, x_near, x_goal) # New node
 		tree.append(x_new)
-	
-		# Draw points and lines to the visualization
+
+		# Every n iterations bias the RRT
+		if iteration%10 == 0:
+			x_rand = x_goal
+
+		iteration += 1
+
+		# Draw points and lines to visualization
 		pygame.draw.circle(surface=WINDOW, color=BLUE, center=x_init, radius=3)
 		pygame.draw.circle(surface=WINDOW, color=RED, center=x_goal, radius=3)
 
 		if has_obstacles and not is_simulation_finished:
 			collision_free = is_free(point=x_new, obstacles=obstacles, tree=tree) # Check collision
+			
 			if collision_free:
 				# Append current node value and place it in the parent list 
 				values.append(node_value)
@@ -286,11 +370,13 @@ def main(has_obstacles, show_random_nodes, show_new_nodes):
 				if is_goal_reached:
 					pygame.draw.line(surface=WINDOW, color=BLACK, start_pos=x_new, end_pos=x_goal)
 					is_simulation_finished = True
+					print(iteration)
 		elif not is_simulation_finished:
 			# Append current node value and place it in the parent list 
 			values.append(node_value)
 			parent = generate_parents(values, parent)
 
+
 			if show_random_nodes:
 				pygame.draw.circle(surface=WINDOW, color=GREEN, center=x_rand, radius=3)
 			if show_new_nodes:
@@ -302,6 +388,7 @@ def main(has_obstacles, show_random_nodes, show_new_nodes):
 			if is_goal_reached:
 				pygame.draw.line(surface=WINDOW, color=BLACK, start_pos=x_new, end_pos=x_goal)
 				is_simulation_finished = True
+				print(iteration)
 
 		pygame.display.update()
 		k += 1