def main(): # Process CLI arguments. try: execname, host, port = sys.argv except ValueError: execname = sys.argv[0] print >> sys.stderr, '%s: incorrect number of arguments' % execname print >> sys.stderr, 'usage: %s hostname port' % sys.argv[0] sys.exit(-1) # Connect. # bzrc = BZRC(host, int(port), debug=True) bzrc = BZRC(host, int(port)) agent = GraphAgent(bzrc) # print "Tank located at ({0}, {1})".format(agent.tank.x, agent.tank.y) t0 = time.time() prior = 0.5 worldsize = int(agent.constants['worldsize']) true_pos, true_neg = float(agent.constants['truepositive']), float(agent.constants['truenegative']) prob_occupied = np.ones([worldsize, worldsize]) * prior seen = np.zeros([worldsize, worldsize], dtype=int) # print 'True Positive: ', true_pos # print 'True Negative: ', true_neg cur_goal, cur_point = None, None iters_total = 0 # break the world up into a grid gridsize = 16 waypoints = np.zeros([ gridsize, gridsize ], dtype=bool) try: last_known, iters_here = None, 0 while True: # pick a new goal randomly if we got stuck location = (agent.tank.x, agent.tank.y) if location == last_known: iters_here += 1 if iters_here > 10: print "Got stuck, selecting new goal" iters_here = 0 cur_goal = cur_point = None last_known = location if cur_goal is None or agent.reached_goal(cur_point): # mark waypoint as visited if cur_goal != None: print "Reached waypoint: {0}".format(cur_goal) waypoints[cur_goal] = True wp_nearest = nearest_waypoint(waypoints, location, gridsize, worldsize) if wp_nearest == None: print "Reached all waypoints" command = Command(agent.tank.index, 0, 0, False) results = agent.bzrc.do_commands([ command ]) break else: cur_goal, cur_point = wp_nearest, Point('waypoint', waypoint_coord(wp_nearest, gridsize, worldsize)) print "Selected new waypoint: {0}".format(wp_nearest) agent.set_goal(cur_point, False) time_diff = time.time() - t0 agent.tick(time_diff) # print "Tank located at ({0}, {1})".format(agent.tank.x, agent.tank.y) # update probabilities pos, occ = bzrc.get_occgrid(agent.tank.index) update_occ(prob_occupied, seen, worldsize, (true_pos, true_neg), (pos, occ)) # maybe save image if iters_total % 200 == 0: snapshot = np.rot90(prob_occupied * 255, 1) image.imsave("snapshots/iteration-{0}.png".format(iters_total), snapshot, cmap='gray') iters_total += 1 except KeyboardInterrupt: print "Exiting due to keyboard interrupt." bzrc.close() # save thresholded map print "Saving map..." snapshot = np.rot90(prob_occupied * 255, 1) > 1 image.imsave("snapshots/final.png".format(iters_total), snapshot, cmap='gray') print "Done in {0} iterations and {1:.1f}s".format(iters_total, time.time() - t0)
class Grid(): def __init__(self, port): self.bzrc = BZRC("localhost", int(port)) self.grid = self.bzrc.get_occgrid(0) #print self.grid self.height = len(self.grid[1]) self.width = len(self.grid[1][3]) self.bottom = int(self.grid[0][0]) self.top = self.bottom + self.height self.left = int(self.grid[0][0]) self.right = self.left + self.width # print "Left: " , self.left # print 'Right: ', self.right # print 'Top: ', self.top # print 'Bottom: ', self.bottom #print self.height, self.width self.number_grid = self.grid[1:][0] #print self.number_grid self.grid = [] self.goal = (int(self.bzrc.get_flags()[2].x), int(self.bzrc.get_flags()[2].y)) # print "Goal: ", self.goal self.start = (int(self.bzrc.get_mytanks()[0].x), int(self.bzrc.get_mytanks()[0].y)) # print "START: " , self.start ''' @param goal: tuple(x, y) @returns grid of cell objects ''' def get_grid(self): xList = [] for x in range(self.height): #g = raw_input(self.number_grid[y]) yList = [] for y in range(self.width): reachable = True if self.number_grid[x][y] == 1: reachable = False else: reachable = True #print "reachable at: " + str(x) + " : " + str(y) cell = Cell(self.left + x, self.bottom + y, reachable, self.distance(self.left + x, self.right + y, self.goal)) if (self.left + x, self.top + y) == self.goal: cell.end = True #s = raw_input("Goal Location: " + str(x + self.left) + " : " + str(y + self.bottom) + " : " + str(self.number_grid[y][x])) yList.append(cell) xList.append(yList) #print len(yList), len(xList) self.grid = xList return y def distance(self, x, y, goal): return math.sqrt((abs(x - goal[0])**2 + (abs(y - goal[1])**2))) def get_cell(self, x, y): x = int(x + self.right) y = int(y + self.top) #print "Get Cell: ", x, y try: return self.grid[x][y] except(IndexError): print "Max is: " + str(len(self.grid)) + ", " + str(len(self.grid[0])) print "Error on: Index: " + str(x) + ", " + str(y) sys.exit(0) def get_cell_tuple(self, xy): x = int(xy[0] + self.right) y = int(xy[1] + self.top) return self.grid[x][y] def get_cell_by_cell(self, c): return self.grid[int(c[0])][int(c[1])]
import sys import math import time from bzrc import BZRC, Command port = raw_input("Port?") print port bzrc = BZRC('127.0.0.1', int(port)) f = open("grid.txt", 'w') grid = bzrc.get_occgrid(0) f.write(str(grid[0][1] + grid[0][2])) for line in grid[1]: l = '' for s in line: l += str(s) f.write(l + "\n") #f.write(bzrc.get_occgrid(0))