def decide_left_or_right(parsed_frame):
'''
based on the position of the cursor and walls, decide whether to
1) press left (return 'left')
2) press right (return 'right')
3) do nothing (return None)
'''
action = None
start = get_start(parsed_frame)
path = find_path(start, 40, parsed_frame.rot_arr)
if path and len(path) > 8:
path = smooth_path(path)
start_x = start[0]
next_x, next_y = path[1]
min_x_jump = 0
min_y_jump = 0
print '-------------------'
print 'start: (%d, %d)' % start
print 'next: (%d, %d)' % (next_x, next_y)
if next_y - start[1] >= min_y_jump:
if start_x == 0 and next_x > 50 and next_x <= 62 - min_x_jump: # wrap around
action = 'left'
elif start_x == 61 and next_x >= min_x_jump and next_x < 10: # wrap around
action = 'right'
elif next_x > start_x + min_x_jump:
action = 'right'
elif next_x < start_x - min_x_jump:
action = 'left'
return action
def find_path(self, unit, loc):
#FIXME: remember to invalidate paths
path_id = (unit, loc)
if not self.paths.has_key(path_id):
self.paths[path_id] = a_star.find_path(self, unit, loc)
return self.paths[path_id]
def solve(filename, start=(0,0), end=(0,0)):
"""solve one problem and save the solution as an image."""
input_filename = os.path.join('problems', filename)
output_filename = os.path.join('solutions', filename)
problem = GridProblem(input_filename)
path = a_star.find_path(problem, start, end)
save_solution(input_filename, path, output_filename)
# Converts the image into a numpy array
world_map = np.abs(np.array(world_map) - 1)
# Creates an RGB Version of the world map
world_map_rgb = line_plotter.concat_channels(world_map, world_map,
world_map)
# ---------- Plot Layout ----------
# Specify that we require subplots along two rows and two columns
figure, axis = plot.subplots(1, 2)
# Specifying the primary plot title
figure.suptitle('Results for map: ' + maps_to_load)
#print("before A-star")
# ---------- A* Algorithm ----------
# Uses the A_star algorithm to find the shortest route to the goal
path, path_length, computation_time, coordinates_expanded = a_star.find_path(
world_map, start_coordinate, end_coordinate)
# We initialize the visualization object with the number of rovers and the grid size (width x height)
visualizer = RoverDomainVisualizer(1, (150, 150))
path = path[::-1]
for pos in range(len(path) + 10):
if pos >= len(path) - 1:
pos = len(path) - 1
# We update the visualizer
visualizer.update_visualizer([(path[pos][1], path[pos][0])],
[end_coordinate], [True], False, 0.05)
else:
# We update the visualizer
visualizer.update_visualizer([(path[pos][1], path[pos][0])],
[end_coordinate], [False], False,
if i != j and stacks.depth(i) > 0:
neighbor = stacks.move(i, j)
if neighbor.is_legal():
neighbors.append(neighbor)
return neighbors
def heuristic(self, position, goal):
# number of blocks successfully moved to the last peg
return len(position.stacks[-1])
if __name__ == '__main__':
import sys
if len(sys.args) == 1:
number_of_pegs = 3
else:
number_of_pegs = int(sys.args[1])
hanoi = HanoiProblem(number_of_pegs)
# the "points" in the HanoiProblem are of type "Stacks",
# so the we need to instantiate Stacks for the start and ends points.
start = Stacks.make_start(number_of_pegs)
goal = Stacks.make_goal(number_of_pegs)
# then a miracle occurs...
solution = a_star.find_path(hanoi, start, goal)
for position in solution:
print position
xrange_ = [
Point32(x=(v[0] - 50) / 50.0 * 2.5, y=(v[1] - 50) / 50.0 * 2.5)
for v in valid_edges
]
debug.publish(
PointCloud(header=Header(frame_id=topics.MAP_FRAME), points=xrange_))
print 'ideal %s' % (ideal, )
def is_goal(p):
return p in valid_edges
def weight((x, y)):
if costmap[y][x] < 127:
# can't traverse
return float('inf')
if is_goal((x, y)):
return abs(ideal[0] - x) + abs(ideal[1] - y)
return 1
center = MAP_SIZE_PIXELS / 2
path = find_path(start=(x, y),
reached_goal=is_goal,
neighbors=grid_neighbors(costmap, jump_size=SPACING),
weight=weight,
heuristic=lambda v: manhattan(v, ideal))
return path
doodl.marketmap.draw(frame)
moving = 0
for element in doodl.customers:
# print(element.id)
if element.goal != [0, 0]:
# print(MARKET2)
moving = 1
# if MARKET2[element.goal[1], element.goal[0]] == 1:
# element.goal = [0,0] # if destination tile is occuppied skip this round
# element.location = 'stalled' # if destination tile is occuppied skip this round
# else:
MARKET2[element.y, element.x] = 0
start1 = (element.y, element.x)
goal1 = (element.goal[1], element.goal[0])
path = find_path(
MARKET2, start1, goal1,
possible_moves) # will return 0 if no route found
if (path == 0): # no route found
if element.oldlocation == 'stalled': # if already stalled in the previous round, give up the search
element.goal = [0, 0]
element.oldlocation = 'stalled'
print("OOPS! Couldn't calculate route.")
print(
f"Customer {element.id} cannot find the route from {element.oldlocation} {start1} to {element.location} {goal1}!"
)
print(MARKET2)
else:
newx = path[1][1]
newy = path[1][0]
if MARKET2[
newy,
def main():
teststart = node(-6, -6)
testgoal = node(-6, -4)
find_path(teststart, testgoal)
print(testgoal.parent.x)
#Add Lines for robot path
lines = []
iterator = testgoal
while iterator.parent != None:
print("(%d,%d),(%d,%d)" %
(iterator.x, iterator.y, iterator.parent.x, iterator.parent.y))
lines.append([(iterator.x, iterator.y),
(iterator.parent.x, iterator.parent.y)])
iterator = iterator.parent
# for n in neighbors:
# print("%d, %d" % (n.x,n.y))
# lines.append([(n.x,n.y),(test.x,test.y)])
#start grid
fig, ax = plt.subplots()
#Add Obstacles
patches = []
patches.append(Polygon([[-1, -1], [-5, -1], [-5, 2], [-3, 2]]))
patches.append(Polygon([[-2, 2], [2, 2], [2, -4]]))
patches.append(
Polygon([[-1, -2.5], [-5, -2.5], [-5, -5], [1.2, -5], [1.2, -4]]))
for poly in patches:
ax.add_patch(poly)
#Set boundaries
ax.set_xlim([-7, 4])
ax.set_ylim([-7, 4])
#Set size of plot
fig_size = plt.rcParams["figure.figsize"]
fig_size[0] = 12
fig_size[1] = 9
plt.rcParams["figure.figsize"] = fig_size
#draw lines to the plot
lc = mc.LineCollection(lines, linewidths=2)
ax.add_collection(lc)
#Set up Axis
ax.minorticks_on()
ax.xaxis.set_major_locator(plt.MultipleLocator(1))
ax.yaxis.set_major_locator(plt.MultipleLocator(1))
ax.xaxis.set_minor_locator(plt.MultipleLocator(0.25))
ax.yaxis.set_minor_locator(plt.MultipleLocator(0.25))
ax.grid(which='major', linestyle='-', linewidth='0.5', color='black')
ax.grid(which='minor', linestyle='-', linewidth='0.2', color='black')
plt.show()
#
'''plt.axis([-7,4,-7,4])
for idx, space in enumerate(position.spaces):
if idx <= math.floor(number_of_spaces/2) and space == "F":
score += 2
elif idx == math.floor(number_of_spaces/2) and space != "_":
score += 1
elif idx > math.floor(number_of_spaces/2) and space == "T":
score += 2
return score
'''
__main__ runs when we do `python frogs_toads.py`
'''
if __name__ == '__main__':
'''
Works for any number of frogs and toads, although anything greater than 5 takes a bit to complete.
'''
number_of_frogs_and_toads = 9
frogs_and_toads = FrogsAndToadsProblem(number_of_frogs_and_toads)
# the "points" in the FrogsAndToadsProblem are of type "Spaces",
# so the we need to instantiate Spaces for the start and ends points.
start = Spaces.make_start(number_of_frogs_and_toads)
goal = Spaces.make_goal(number_of_frogs_and_toads)
# Find the path using our a_star method.
solution = a_star.find_path(frogs_and_toads, start, goal)
for position in solution:
print position