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),
]
def add_obstacle(pos):
common.set_obstacle(world_obstacles, pos, True)
common.draw_background(gui, world_obstacles, visited_nodes, optimal_path)
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.
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)
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:
