def update_callback(pos = None): # Call path planning algorithm. start, goal = gui.get_start_goal() if not (start==None or goal==None): global optimal_path global visited_nodes try: optimal_path, visited_nodes = astar(start, goal, world_obstacles) except (Exception, e): print (traceback.print_exc()) # Draw new background. common.draw_background(gui, world_obstacles, visited_nodes, optimal_path)
def update_callback(pos = None): # First apply distance transform to world_obstacles. apply_distance_transform() # Call path planning algorithm. start, goal = gui.get_start_goal() if not (start==None or goal==None): global optimal_path global visited_nodes try: optimal_path, visited_nodes = astar(start, goal, world_obstacles) except Exception as e: print traceback.print_exc() # Draw new background. common.draw_background(gui, world_obstacles, visited_nodes, optimal_path, show_visited_nodes)
def toggle_visited_cells(): global show_visited_cells show_visited_cells = not show_visited_cells common.draw_background(gui, world_obstacles, visited_cells, optimal_path, show_visited_cells)
def remove_obstacle(pos): common.set_obstacle(world_obstacles, pos, False) common.draw_background(gui, world_obstacles, visited_cells, optimal_path, show_visited_cells)
show_visited_cells) def update_callback(pos=None): # Call path planning algorithm. start, goal = gui.get_start_goal() if not (start == None or goal == None): global optimal_path global visited_cells try: optimal_path, visited_cells = \ explore_statespace(start, goal, world_obstacles) except Exception, e: print traceback.print_exc() # Draw new background. common.draw_background(gui, world_obstacles, visited_cells, optimal_path, show_visited_cells) # -------------------------------------------------------------------------- # Helper class for curved segments. # -------------------------------------------------------------------------- class CurveSegment: @staticmethod def end_pose(start_pose, curvature, length): """Returns end pose, given start pose, curvature and length.""" x, y, theta = start_pose if curvature == 0.0: # Linear movement. x += length * cos(theta) y += length * sin(theta) return (x, y, theta)
def remove_obstacle(pos): common.set_obstacle(world_obstacles, pos, False) common.draw_background(gui, world_obstacles, visited_nodes, optimal_path)
def add_obstacle(pos): common.set_obstacle(world_obstacles, pos, True) common.draw_background(gui, world_obstacles, visited_nodes, optimal_path)
def clear_obstacles(): global world_obstacles world_obstacles = np.zeros(world_extents, dtype=np.uint8) update_callback() def update_callback(pos = None): # Call path planning algorithm. start, goal = gui.get_start_goal() if not (start==None or goal==None): global optimal_path global visited_nodes try: optimal_path, visited_nodes = dijkstra(start, goal, world_obstacles) except Exception, e: print traceback.print_exc() # Draw new background. common.draw_background(gui, world_obstacles, visited_nodes, optimal_path) # -------------------------------------------------------------------------- # Dijkstra algorithm. # -------------------------------------------------------------------------- # Allowed movements and costs on the grid. # Each tuple is: (movement_x, movement_y, cost). s2 = np.sqrt(2) movements = [ # Direct neighbors (4N). (1,0, 1.), (0,1, 1.), (-1,0, 1.), (0,-1, 1.), # Diagonal neighbors. # Comment this out to play with 4N only (faster). (1,1, s2), (-1,1, s2), (-1,-1, s2), (1,-1, s2), ]
update_callback() def update_callback(pos=None): # Call path planning algorithm. start, goal = gui.get_start_goal() if not (start == None or goal == None): global optimal_path global visited_nodes try: optimal_path, visited_nodes = dijkstra(start, goal, world_obstacles) except Exception, e: print traceback.print_exc() # Draw new background. common.draw_background(gui, world_obstacles, visited_nodes, optimal_path) # -------------------------------------------------------------------------- # Dijkstra algorithm. # -------------------------------------------------------------------------- # Allowed movements and costs on the grid. # Each tuple is: (movement_x, movement_y, cost). s2 = np.sqrt(2) movements = [ # Direct neighbors (4N). (1, 0, 1.), (0, 1, 1.), (-1, 0, 1.), (0, -1, 1.), # Diagonal neighbors.
global max_movement_id max_movement_id = 9 - max_movement_id # Toggles between 3 and 6. update_callback() def update_callback(pos = None): # Call path planning algorithm. start, goal = gui.get_start_goal() if not (start==None or goal==None): global optimal_path global visited_cells try: optimal_path, visited_cells = \ astar_statespace(start, goal, world_obstacles) except Exception, e: print traceback.print_exc() # Draw new background. common.draw_background(gui, world_obstacles, visited_cells, optimal_path, show_visited_cells) # -------------------------------------------------------------------------- # Helper class for curved segments. # -------------------------------------------------------------------------- class CurveSegment: @staticmethod def end_pose(start_pose, curvature, length): """Returns end pose, given start pose, curvature and length.""" x, y, theta = start_pose if curvature == 0.0: # Linear movement. x += length * cos(theta) y += length * sin(theta) return (x, y, theta) else: