No tree in trajectory towards goal added

Author: AngeloEspinoza

RRT.py

@@ -25,6 +25,8 @@
 	metavar='', required=False, help='Draws a red line indicating the path to goal')
 parser.add_argument('-mr', '--move_robot', type=bool, action=argparse.BooleanOptionalAction, 
 	metavar='', required=False, help='Shows the movements of the robot from the start to the end')
+parser.add_argument('-kt', '--keep_tree', type=bool, action=argparse.BooleanOptionalAction, 
+	metavar='', required=False, help='Keeps the tree while the robot is moving towards the goal')
 args = parser.parse_args()
 
 # Initialization 
@@ -43,8 +45,7 @@
 
 # Instantiating the environment and the graph
 environment_ = environment.Environment(map_dimensions=MAP_DIMENSIONS)
-graph_ = graph.Graph(start=x_init, goal=x_goal, 
-		map_dimensions=MAP_DIMENSIONS, epsilon=EPSILON)
+graph_ = graph.Graph(start=x_init, goal=x_goal, map_dimensions=MAP_DIMENSIONS, epsilon=EPSILON)
 
 def main():
 	clock = pygame.time.Clock()
@@ -112,10 +113,10 @@ def main():
 					node_value += 1 # Increment the value for the next randomly generated node
 
 					if graph_.is_goal_reached:
-						graph_.draw_local_planner(p1=x_new, p2=x_goal, map_=environment_.map)
 						is_simulation_finished = True
 						graph_.parent = parent
 						graph_.tree = tree
+						graph_.draw_local_planner(p1=x_new, p2=x_goal, map_=environment_.map)
 						graph_.path_to_goal()
 						graph_.get_path_coordinates()
 
@@ -125,7 +126,8 @@ def main():
 					k += 1 # One more node sampled
 
 		if graph_.is_goal_reached and args.move_robot:
-			graph_.draw_trajectory(nears=nears, news=tree, environment_=environment_)
+			graph_.draw_trajectory(nears=nears, news=tree, environment=environment_,
+				obstacles=obstacles, keep_tree=args.keep_tree)
 
 		pygame.display.update()
 

__pycache__/graph.cpython-310.pyc

@@ -276,30 +276,34 @@ def draw_path_to_goal(self, map_):
 			 	end_pos=self.path_coordinates[i+1], width=4)
 
 	def move_robot(self, position, map_):
-		"""Draws the robot moving."""
+		"""Draws the robot moving at the given position."""
 		pygame.draw.circle(surface=map_, color=(0, 0, 255),	center=position, radius=4)
 
 	def draw_tree(self, nears, news, map_):
 		"""Draws the tree constantly. Used to display it in an infinite loop."""
 		for i in range(len(nears)):
 			self.draw_local_planner(p1=nears[i], p2=news[i+1], map_=map_)
 
-	def draw_trajectory(self, nears, news, environment_):
+	def draw_trajectory(self, nears, news, environment, obstacles, keep_tree):
 		"""Draws the robot moving in the map."""
 		for i in range(len(self.path_coordinates)-1):
 			robot_position = self.path_coordinates[::-1][i]
 
+			if obstacles != []:
+				environment.draw_obstacles()
+
 			# Draw inital and final robot configuration
-			self.draw_initial_node(map_=environment_.map)
-			self.draw_goal_node(map_=environment_.map)
-			environment_.draw_obstacles()
+			self.draw_initial_node(map_=environment.map)
+			self.draw_goal_node(map_=environment.map)
 
 			# Draw path to goal, and the robot movement
-			self.draw_path_to_goal(map_=environment_.map)		
-			self.move_robot(position=robot_position, map_=environment_.map)
-			self.draw_tree(nears=nears, news=news, map_=environment_.map)				
+			self.draw_path_to_goal(map_=environment.map)		
+			self.move_robot(position=robot_position, map_=environment.map)
+
+			if keep_tree:
+				self.draw_tree(nears=nears, news=news, map_=environment.map)				
 
 			# Refresh the screen
 			pygame.display.update()
 			pygame.time.delay(20) # Wait 0.1 seconds 
-			environment_.map.fill(self.WHITE)
+			environment.map.fill(self.WHITE)