def __call__(self, begin=None, sample=True, log=True, random_end=False):
if not begin:
begin = self.begin
ret = []
while len(ret) < 40:
# print('path planning task begin: ', begin, 'random end ', random_end)
# print('path planning task allowed: ', begin in self.allowed_cells_list)
self.solver = a_star.AStar(self.env_config['max_y_idx'],
self.env_config['max_x_idx'])
self.solver.init_grid(self.obstacles_list)
end = random.choice(self.allowed_cells_list) if (
self.randomized_target or random_end) else self.end
# print('path planning task end: ', end)
self.solver.set_task(begin, end)
try:
ret = self.solver.solve()
except:
self.log_path(begin, end, [(1, 1), (1, 1)])
# print('bad begin', begin)
# print('bad end', end)
# print('ret: ', ret)
ret = [(x, y) for y, x in ret]
if log:
self.log_path(begin, end, ret)
if sample:
ret = self.get_jump_points(ret)
for sub_func in self.subscribents:
sub_func(ret)
def __init__(self, env_config, randomized_target):
self.env_config = env_config
self.env_map = cv2.cvtColor(cv2.imread(env_config['env_map']),
cv2.COLOR_BGR2RGB)
begin = np.nonzero(utils.get_colour_map(self.env_map, 255, 0, 0))
self.begin = begin[0][0], begin[1][0]
end = np.nonzero(utils.get_colour_map(self.env_map, 0, 0, 255))
self.end = end[0][0], end[1][0]
self.env_map[begin[0], begin[1], :] = (255, 255, 255)
kernel_size = env_config['kernel_size']
self.env_map_er = cv2.erode(self.env_map,
np.ones((kernel_size, kernel_size),
np.uint8),
iterations=1)
self.obstacles_map = utils.get_colour_map(self.env_map_er, 0, 0, 0)
self.obstacles_list = [
(r, c) for r, c in zip(*np.nonzero(self.obstacles_map))
]
# cv2.imwrite('all_map.png', self.env_map)
self.allowed_cells_map = cv2.erode(self.env_map_er,
np.ones((3, 3), np.uint8),
iterations=1)
# cv2.imwrite('allowed_cells.png', self.allowed_cells_map)
self.allowed_cells_map = utils.get_colour_map(self.allowed_cells_map,
255, 255, 255)
self.allowed_cells_list = [
(r, c) for r, c in zip(*np.nonzero(self.allowed_cells_map))
]
self.solver = a_star.AStar(env_config['max_y_idx'],
env_config['max_x_idx'])
self.subscribents = []
self.randomized_target = randomized_target
def depth_first_search(self):
self.canvas.itemconfig("oval", fill="white", outline="white")
self.search = a_star.AStar(self.graph)
self.search.search_type = "depth-first"
self.search.max_nodes = max_nodes
self.search.initialize("manhattan distance")
self.redraw()
def test_maze(self):
a = pf.AStar()
walls = ((0, 5), (1, 0), (1, 1), (1, 5), (2, 3), (3, 1), (3, 2),
(3, 5), (4, 1), (4, 4), (5, 1))
a.init_grid(8, 10, walls, (0, 0), (5, 6))
path = a.solve()
print path
def visualize_loop(self):
visualizer = a_star.AStar(self.field)
while visualizer.make_step():
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.QUIT
sys.exit()
self.draw_screen()
self.clock.tick(VISUALIZATION_SPEED)
self.draw_screen()
def __init__(self):
self.robot = a_star.AStar()
try:
self.t1 = Thread(target=self.receiver, args=())
self.t1.start()
except:
print "Error: unable to start thread"
self.message = None
def grid_exec():
raw_str = request.args['info']
json_str = json.loads(raw_str)
r = json_str['rows']
c = json_str['cols']
start = tuple(json_str['start'])
end = tuple(json_str['end'])
walls = tuple([tuple(x) for x in json_str['walls']])
a = pf.AStar()
a.init_grid(r, c, walls, start, end)
path = a.solve()
return jsonify(path)
def main():
camera = cameraService.startCamera()
robot = a_star.AStar()
moves = [(0, 0), (0, 0), (0, 0)]
try:
while (True):
frame = cameraService.getFrameFromCamera(camera=camera)
error = cameraService.findDirectionError(frame, show=True)
print robot.read_distance()
if robot.read_distance() < 3000:
if all(x == (0,0) for x in moves):
robot.play_notes("eee")
time.sleep(0.5)
robot.motors(-300, 0)
time.sleep(3)
robot.motors(0, 0)
continue
robot.play_notes("cc")
time.sleep(0.5)
robot.motors(-moves[0][0], -moves[0][1])
time.sleep(1.5)
robot.motors(0, 0)
del moves[0]
moves.append((0, 0))
if all(x == (0,0) for x in moves):
robot.motors(-200, 200)
time.sleep(2)
robot.motors(0, 0)
continue
if error < -0.05:
robot.motors(0, 300)
moves.pop()
moves.insert(0, (0, 300))
time.sleep(0.5)
elif error > 0.05:
robot.motors(300, 0)
moves.pop()
moves.insert(0, (300, 0))
time.sleep(0.5)
else:
robot.motors(200, 200)
# moves.pop()
# moves.insert(0, (200, 200))
time.sleep(1.5)
robot.motors(0, 0)
finally:
robot.motors(0, 0)
camera.close()
def __init__(self):
self.a_star = a_star.AStar()
self.map = self.a_star.run()
self.max_i = len(self.map)
self.max_j = len(self.map[0])
self.block_width = 32
self.block_height = 32
self.screen = pygame.display.set_mode(
(self.max_j * self.block_width, self.max_i * self.block_height))
self.white = pygame.image.load('white.png').convert()
self.gray = pygame.image.load('gray.png').convert()
self.red = pygame.image.load('red.png').convert()
self.green = pygame.image.load('green.png').convert()
self.blue = pygame.image.load('blue.png').convert()
self.frame = pygame.image.load('frame.png').convert_alpha()
self.background = [
self.white, self.green, self.red, None, None, None, self.blue,
None, None, self.gray
]
ax = plt.gca()
ax.set_xlim([0, map.size])
ax.set_ylim([0, map.size])
for i in range(map.size):
for j in range(map.size):
if map.IsObstacle(i, j):
rec = Rectangle((i, j), width=1, height=1, color='gray')
ax.add_patch(rec)
else:
rec = Rectangle((i, j),
width=1,
height=1,
edgecolor='gray',
facecolor='w')
ax.add_patch(rec)
rec = Rectangle((0, 0), width=1, height=1, facecolor='b')
ax.add_patch(rec)
rec = Rectangle((map.size - 1, map.size - 1), width=1, height=1, facecolor='r')
ax.add_patch(rec)
plt.axis('equal')
plt.axis('off')
plt.tight_layout()
#plt.show()
a_star = a_star.AStar(map)
a_star.RunAndSaveImage(ax, plt)
import jsonSocket
import logging
import time
import a_star
#import speech
logger = logging.getLogger("RI-Project")
robot = a_star.AStar()
class PiServer(jsonSocket.ThreadedServer):
def __init__(self):
super(PiServer, self).__init__(address='192.168.0.11')
self.timeout = 2.0
# self.voice=speech.Speech()
def _processMessage(self, obj):
""" virtual method """
if obj != '':
x = obj.get("x")
y = obj.get("y")
obstacle = obj.get("obstacle")
x, y = self._parser(x, y)
if obstacle == "1":
pass
# self.voice.obstacle()
#print x, y, obstacle
try:
print robot.read_distance()[0]
except:
def main():
"""
Generates plots for all requested scenarios for HW#1
Parts 7 & 9 will appear on the same Figure
"""
# Part 3 - Offline A* Search - Large Grid Size
path3_1 = a_star.AStar([.5, -1.5], [0.5, 1.5], 1, 0, online_check=0)
path3_1.start_planning()
path3_2 = a_star.AStar([4.5, 3.5], [4.5, -1.5], 1, 0, online_check=0)
path3_2.start_planning()
path3_3 = a_star.AStar([-0.5, 5.5], [1.5, -3.5], 1, 0, online_check=0)
path3_3.start_planning()
# Part 5 - Online A* Search - Large Grid Size
part5_1 = a_star.AStar([.5, -1.5], [0.5, 1.5], 1, 0)
part5_1.start_planning()
part5_2 = a_star.AStar([4.5, 3.5], [4.5, -1.5], 1, 0)
part5_2.start_planning()
part5_3 = a_star.AStar([-0.5, 5.5], [1.5, -3.5], 1, 0)
part5_3.start_planning()
# Part 7 - Online A* Search - Small Grid Size
path7_1 = a_star.AStar([2.45, -3.55], [0.95, -1.55], .1, .3)
path7_1.start_planning()
path7_2 = a_star.AStar([4.95, -0.05], [2.45, 0.25], .1, .3)
path7_2.start_planning()
path7_3 = a_star.AStar([-0.55, 1.45], [1.95, 3.95], .1, .3)
path7_3.start_planning()
# Part 9 - Robot Driving Simulation of paths from Part 7 -
# The results will appear on the plots genterated in Part 7
a_star.Robot([2.45, -3.55], [0.95, -1.55], final_path=path7_1.fin_path, grid_obj=path7_1.field)
a_star.Robot([4.95, -0.05], [2.45, 0.25], final_path=path7_2.fin_path, grid_obj=path7_2.field)
a_star.Robot([-0.55, 1.45], [1.95, 3.95], final_path=path7_3.fin_path, grid_obj=path7_3.field)
# Part 10 - Robot Driving Simulation with simultaneous Online A* search
# using Part 7 Start & Goal Combinations
a_star.Robot([2.45, -3.55], [0.95, -1.55])
a_star.Robot([4.95, -0.05], [2.45, 0.25])
a_star.Robot([-0.55, 1.45], [1.95, 3.95])
# Part 11 - Robot Driving Simulation with simultaneous Online A* search
# using Part 3 Start & Goal Combinations
a_star.Robot([.5, -1.5], [0.5, 1.5])
a_star.Robot([.5, -1.5], [0.5, 1.5], cell_size=1, inflate=0)
a_star.Robot([4.5, 3.5], [4.5, -1.5])
a_star.Robot([4.5, 3.5], [4.5, -1.5], cell_size=1, inflate=0)
a_star.Robot([-0.5, 5.5], [1.5, -3.5])
a_star.Robot([-0.5, 5.5], [1.5, -3.5], cell_size=1, inflate=0)
# Keep plots from closing at the end
a_star.plot_show()
def main():
#Clear the screen, prepare the mainlopp, prepare the input file, and the input file list
os.system("clear")
flag = True
f = open("input.txt", "r")
file_list = []
#Print welcome message including the premade graph information. Save the graph information as a list
print("Project Module #1: \n" "A Star \n" "\n" "Premade problems are:")
print f.name
for line in f.readlines():
file_list.append(line)
sys.stdout.write(line)
print(
"\n"
"'Run 0-X' for premade problem \n"
"'Run new' for custom problem \n"
"'Speed test 0-X' for testing run speed \n"
"'Exit' ends the script \n"
"\n")
#This is the mainloop - It reads input from the user and executes the commands
while flag:
the_input = raw_input(" > ")
#Run a premade graph
if the_input != "Run new" and the_input.startswith("Run"):
#Get the correct premade graph and add the numbers to a list
number = the_input[4:]
line_list = map(int, re.findall(r'\d+', file_list[int(number)]))
#Add the walls to a list
walls = []
for wall_start in range(7, len(line_list), 4):
wall = []
for wall_number in range(wall_start, wall_start + 4):
wall.append(line_list[wall_number])
walls.append(wall)
#Initialize premade graph, and gui; If the graph is not too large
if line_list[1] <= max_breadth and line_list[2] <= max_height:
premade_graph = Graph(columns=line_list[1],
rows=line_list[2],
a_pos_x=line_list[3],
a_pos_y=line_list[4],
b_pos_x=line_list[5],
b_pos_y=line_list[6],
walls=walls)
#Scale down the size if the graph is too large
if line_list[1] > 50 and line_list[2] > 30:
size = 25 / 5
else:
size = 25
premade_graph_gui = gui.GUI(graph=premade_graph, cellsize=size)
_run_gui(premade_graph_gui)
else:
print "Error: graph too large"
#Run a custom graph
elif the_input == "Run new":
size = input("graph size: ")
start = input("Start position: ")
end = input("End position: ")
walls = input("Wall positions: ")
#Initialize custom graph, and gui; If the graph is not too large
if size[0] <= max_breadth and size[1] <= max_height:
custom_graph = Graph(columns=size[0],
rows=size[1],
a_pos_x=start[0],
a_pos_y=start[1],
b_pos_x=end[0],
b_pos_y=end[1],
walls=walls)
#Scale down the size if the graph is too large
if size[0] > 50 and size[1] > 30:
size = 25 / 5
else:
size = 25
custom_graph_gui = gui.GUI(graph=custom_graph, cellsize=size)
_run_gui(custom_graph_gui)
else:
print "Error: graph too large"
#Run a speed test for algorithm optimalization purposes
elif the_input.startswith("Speed test"):
number = the_input[11:]
line_list = map(int, re.findall(r'\d+', file_list[int(number)]))
#Add the walls to a list
walls = []
for wall_start in range(7, len(line_list), 4):
wall = []
for wall_number in range(wall_start, wall_start + 4):
wall.append(line_list[wall_number])
walls.append(wall)
#Initialize the graph and run AStar
graph = Graph(columns=line_list[1],
rows=line_list[2],
a_pos_x=line_list[3],
a_pos_y=line_list[4],
b_pos_x=line_list[5],
b_pos_y=line_list[6],
walls=walls)
for run in range(10):
search = a_star.AStar(graph)
a = datetime.datetime.now()
search.initialize("manhattan distance")
result = search.complete_solver()
b = datetime.datetime.now()
print result[1], "\n", b - a
#Exit the loop
elif the_input == "Exit":
flag = False
f.close()
point_map.point_list_numpy[end_index][3], c='r', label="B点") # 绘制B点
ax.text(point_map.point_list_numpy[0][1] + 100, point_map.point_list_numpy[0][2] + 100,
point_map.point_list_numpy[0][3] + 100, "A点")
ax.text(point_map.point_list_numpy[end_index][1] + 100, point_map.point_list_numpy[end_index][2] + 100,
point_map.point_list_numpy[end_index][3] + 100, "B点")
# 设置坐标轴
ax.set_zlabel('Z')
ax.set_ylabel('Y')
ax.set_xlabel('X')
plt.legend(loc="best")
# plt.show()
# 创建起终点point对象
start_point = point.Point(point_map.point_list_numpy[0][0],
point_map.point_list_numpy[0][1],
point_map.point_list_numpy[0][2],
point_map.point_list_numpy[0][3],
point_map.point_list_numpy[0][4],
point_map.point_list_numpy[0][5])
end_point = point.Point(point_map.point_list_numpy[end_index][0],
point_map.point_list_numpy[end_index][1],
point_map.point_list_numpy[end_index][2],
point_map.point_list_numpy[end_index][3],
point_map.point_list_numpy[end_index][4],
point_map.point_list_numpy[end_index][5])
a_star = a_star.AStar(point_map, start_point, end_point)
a_star.RunAndSaveImage(ax, plt)