def test(): img = cv2.imread("arena.jpg") list_of_gridpoints, mapd, rmapd = gp.grid(img, 9) list_of_obs = [] k = 5 o = get_obstacles() for obs in o: for j in range(obs[1] - k, obs[3] + k): for i in range(obs[0] - (k), obs[2] + (k)): cv2.circle(img, (i, j), 1, (255, 120, 255), -1) list_of_obs.append((i, j)) temp = [] for pnt in list_of_gridpoints: if pnt in list_of_obs: cv2.circle(img, pnt, 5, (70, 150, 25), -1) temp.append(pnt) else: cv2.circle(img, pnt, 5, (0, 255, 255), -1) list_of_obs = temp path = [] start = get_start() stop = get_stop() path.append(start.nearest_grid) cv2.circle(img, start.nearest_grid, 5, (0, 255, 0), -1) cv2.circle(img, stop.nearest_grid, 5, (0, 0, 255), -1) for q, point in zip(range(0, len(list_of_obs)), list_of_obs): list_of_obs[q] = mapd[point] path_points = djk.getPath(mapd[start.nearest_grid], mapd[stop.nearest_grid], 9, list_of_obs) for j, point in zip(range(0, len(path_points)), path_points): path_points[j] = rmapd[point] for t in range(1, len(path_points)): path.append(path_points[t]) for i, point in zip(range(0, len(path) - 1), path): draw_arrow( img, point, path[i + 1], (255, 255, 0), ) cv2.imshow("sd", img) cv2.waitKey(0) fil = open("path.txt", "w") fil.write(str(path)) fil.close()
def get_stop(): fo = open("stop.txt", "rw+") data = fo.read().split(',') list_of_gridpoints, mapd, rmapd = gp.grid(cv2.imread("cropped.png"), 9) d = 10000 dat = resource() dat.shape = "stop" dat.real_point = (int(data[0]), int(data[1])) for gridp in list_of_gridpoints: dist = distance(dat.real_point, gridp) if dist < d: d = dist dat.nearest_grid = gridp return dat
def get_path(matches, obstacles, img, color): print matches list_of_obs = [] k = 15 o = obstacles for obs in o: for j in range(obs[2][1] - k, obs[2][1] + obs[2][3] + k): for i in range(obs[2][0] - k, obs[2][0] + obs[2][2] + k): list_of_obs.append((i, j)) list_of_gridpoints, map_dict, rmapd = grph.grid(img, 20) temp = [] for pnt in list_of_gridpoints: if pnt in list_of_obs: temp.append(pnt) list_of_obs = temp list_of_mapped_obs = [] for o in list_of_obs: list_of_mapped_obs.append(map_dict[o]) path = [] listres = [] print map_dict, color, matches mapped_get_front = map_dict[mapped_nearest(get_front(), list_of_gridpoints)] mapped = map_dict[mapped_nearest(matches[color[0]][1], list_of_gridpoints)] path = rdp( djikstra.getPath(mapped_get_front, mapped, 20, list_of_mapped_obs)) path.append(mapped) listres.append(mapped) for i in range(1, len(color)): if type(matches[color[i]]) is not type(9): mapped2 = map_dict[mapped_nearest(matches[color[i]][1], list_of_gridpoints)] path = path + rdp( djikstra.getPath(mapped, mapped2, 20, list_of_mapped_obs)) path.append(mapped2) listres.append(mapped2) mapped = mapped2 print path for j, point in zip(range(0, len(path)), path): path[j] = rmapd[(point[0], point[1])] print path for j, point in zip(range(0, len(listres)), listres): listres[j] = rmapd[(point[0], point[1])] return listres, path
def get_start(): fo = open("start.txt", "r") data = fo.read().split(',') list_of_gridpoints, mapd, rmapd = gp.grid(cv2.imread("arena.jpg"), 9) d = 10000 dat = resource() dat.shape = "start" dat.real_point = (int(data[0]), int(data[1])) for gridp in list_of_gridpoints: dist = distance(dat.real_point, gridp) if dist < d: d = dist dat.nearest_grid = gridp fo.close() return dat
def get_resources(): fo = open("resources.txt", "r") data = fo.read() x = ast.literal_eval(data) list_of_obj_res = [] for i in x: list_of_shit = i.split(',') elem = resource() elem.shape = list_of_shit[0] elem.real_point = (int(list_of_shit[1]), int(list_of_shit[2])) list_of_obj_res.append(elem) list_of_gridpoints, mapd, rmapd = gp.grid(cv2.imread("cropped.png"), 9) for res in list_of_obj_res: d = 10000 for gridp in list_of_gridpoints: dist = distance(res.real_point, gridp) if dist < d: d = dist res.nearest_grid = gridp return list_of_obj_res
def getThePath(): img = cv2.imread("cropped.png") list_of_gridpoints, mapd, rmapd = gp.grid(img, 9) list_of_obs = [] k = 5 o = get_obstacles() for obs in o: for j in range(obs[1] - k, obs[3] + k): for i in range(obs[0] - (k), obs[2] + (k)): list_of_obs.append((i, j)) i = get_resources() temp = [] for pnt in list_of_gridpoints: if pnt in list_of_obs: cv2.circle(img, pnt, 5, (70, 150, 25), -1) temp.append(pnt) else: cv2.circle(img, pnt, 5, (0, 255, 255), -1) list_of_obs = temp start = get_start() stop = get_stop() list_of_obs.append(stop.nearest_grid) list_of_obs.append(start.nearest_grid) for q, point in zip(range(0, len(list_of_obs)), list_of_obs): list_of_obs[q] = mapd[point] #stage 1 #here first take bot center and start , i have started with start center for demo d = 10000 for dat in i: dist = distance(dat.nearest_grid, start.nearest_grid) if dist < d: flag = dat d = dist path = [] path.append(flag.real_point) i.remove(flag) #STAGE 2 PATH TO ALL NODES while len(i) > 0: d = 1000000 for res in i: if flag.shape == "triangle" and res.shape == "square": #djikstra get path points\ path_points = djk.getPath(mapd[flag.nearest_grid], mapd[res.nearest_grid], 9, list_of_obs) for j, point in zip(range(0, len(path_points)), path_points): path_points[j] = rmapd[point] dist = find_distance(path_points) if dist < d: nxt = res d = dist temp_path = path_points elif res.shape == "triangle" and flag.shape == "square": #djikstra get path points\ path_points = djk.getPath(mapd[flag.nearest_grid], mapd[res.nearest_grid], 9, list_of_obs) for j, point in zip(range(0, len(path_points)), path_points): path_points[j] = rmapd[point] dist = find_distance(path_points) if dist < d: nxt = res d = dist temp_path = path_points for t in range(1, len(temp_path) - 1): path.append(temp_path[t]) path.append(nxt.real_point) flag = nxt i.remove(flag) # STAGE 3 MODAFOKA path_points = djk.getPath(mapd[flag.nearest_grid], mapd[stop.nearest_grid], 9, list_of_obs) for j, point in zip(range(0, len(path_points)), path_points): path_points[j] = rmapd[point] for t in range(1, len(path_points)): path.append(path_points[t]) print path return path
res, path = gp.get_path(matches, obstacles, i, colorSequence) print res print path #img = cv2.imread("arena.png") for i, point in zip(range(0, len(path) - 1), path): draw_arrow( resized, (point[0], point[1]), (path[i + 1][0], path[i + 1][1]), ((i * 20), (i * 30), 0), ) list_of_gridpoints, map_dict, rmapd = grph.grid(resized, 20) for point in list_of_gridpoints: cv2.circle(resized, point, 1, (255, 0, 0)) print obstacles list_of_obs = [] k = 15 for obs in obstacles: for j in range( int(obs[2][1]) - k, int(obs[2][1]) + int(obs[2][3]) + k): for i in range( int(obs[2][0]) - k, int(obs[2][0]) + int(obs[2][2]) + k): list_of_obs.append((i, j))