def __init__(self): self.X1, self.Y1, self.X2, self.Y2 = polygons_to_segments( self.load_polygons("./paths.txt")) self.isovist = iso.Isovist( [self.listify_segs(self.X1, self.Y1, self.X2, self.Y2)]) self.intruder_start_goals = [((0.114, 0.362), (0.55, 0.848)), ((0.378, 0.128), (0.824, 0.562)), ((0.292, 0.034), (0.284, 0.842)), ((0.71, 0.202), (0.37, 0.864)), ((0.91, 0.832), (0.146, 0.572)), ((0.854, 0.598), (0.29, 0.076)), ((0.828, 0.408), (0.12, 0.41)), ((0.614, 0.882), (0.368, 0.152)), ((0.154, 0.608), (0.736, 0.208)), ((0.668, 0.934), (0.742, 0.14)), ((0.12, 0.504), (0.714, 0.018)), ((0.722, 0.808), (0.29, 0.14)), ((0.854, 0.424), (0.19, 0.68)), ((0.802, 0.152), (0.116, 0.456)), ((0.168, 0.158), (0.886, 0.736)), ((0.802, 0.692), (0.166, 0.456)), ((0.764, 0.246), (0.572, 0.866)), ((0.15, 0.69), (0.856, 0.46)), ((0.904, 0.864), (0.084, 0.502)), ((0.782, 0.274), (0.644, 0.88)), ((0.15, 0.184), (0.802, 0.3)), ((0.156, 0.204), (0.854, 0.516)), ((0.968, 0.032), (.9, .9))]
def create_chaser_model(seg_map=None, locs=None, isovist=None): if locs is None: locs = [[0.100, 1 - 0.900], [0.566, 1 - 0.854], [0.761, 1 - 0.665], [0.523, 1 - 0.604], [0.241, 1 - 0.660], [0.425, 1 - 0.591], [0.303, 1 - 0.429], [0.815, 1 - 0.402], [0.675, 1 - 0.075], [0.432, 1 - 0.098]] if seg_map is None: seg_map = polygons_to_segments(load_polygons("./paths.txt")) if isovist is None: isovist = i.Isovist(load_isovist_map()) model = Chaser(isovist=isovist, locs=locs, seg_map=seg_map) return model
def main(): polygonSegments = load_polygons() # isovist stuff isovist = iso.Isovist(polygonSegments) X1, Y1, X2, Y2 = polygons_to_segments(load_polygons_here()) INTRUDER_SEEN_COUNT = 0 for K in range(100): result = roll_out(isovist, X1, Y1, X2, Y2) INTRUDER_SEEN_COUNT += result print "%d/%d" % (INTRUDER_SEEN_COUNT, K + 1)
def __init__(self): self.locs = [[0.100, 1 - 0.900], [0.566, 1 - 0.854], [0.761, 1 - 0.665], [0.523, 1 - 0.604], [0.241, 1 - 0.660], [0.425, 1 - 0.591], [0.303, 1 - 0.429], [0.815, 1 - 0.402], [0.675, 1 - 0.075], [0.432, 1 - 0.098]] self.locs = a2d(self.locs) rx1, ry1, rx2, ry2 = polygons_to_segments(load_polygons("./paths.txt")) self.my_rrt = lambda start_loc, goal_loc: run_rrt( start_loc, goal_loc, rx1, ry1, rx2, ry2) self.isovist = iso.Isovist([self.listify_segs(rx1, ry1, rx2, ry2)]) self.UAVLocation = (0.468, 1 - 0.764) self.UAVForwardVector = (0.0, 0.010)
def __init__(self): halls = [[0.4, 0.0], [0.6, 0.0], [0.6, 0.8], [1.0, 0.8], [1.0, 1.0], [0.0, 1.0], [0.0, 0.8], [0.4, 0.8], [0.4, 0.0]] self.locs = [[0.1, 0.9], [0.5, 0.1], [0.9, 0.9]] halls = a2d(halls) self.locs = a2d(self.locs) # for compatibility with polygons_to_segments tmp_halls = np.vstack((np.mean(halls, axis=0, keepdims=True), halls)) rx1, ry1, rx2, ry2 = polygons_to_segments([tmp_halls]) self.my_rrt = lambda start_loc, goal_loc: run_rrt( start_loc, goal_loc, rx1, ry1, rx2, ry2) self.isovist = iso.Isovist([self.listify_segs(rx1, ry1, rx2, ry2)]) #self.UAVLocation = ( 0.5, 0.11 ) #self.UAVLocation = ( 0.2, 0.82 ) self.UAVLocation = (0.2, 0.801) self.UAVForwardVector = (0.0, 0.010)
def main(): ForceA = False ForceB = False ForceC = False c = np.random.choice(3) if c == 0: ForceA = True if c == 1: ForceB = True if c == 2: ForceC = True # UAVLoc2 = True if np.random.choice(2) == 0: UAVLoc2 = True else: UAVLoc2 = False if len(sys.argv) > 1 and sys.argv[1] == '1': UAVLoc2 = False if len(sys.argv) > 1 and sys.argv[1] == '2': UAVLoc2 = True if len(sys.argv) > 2 and sys.argv[2] == 'a': ForceA = True if len(sys.argv) > 2 and sys.argv[2] == 'c': ForceC = True if len(sys.argv) > 2 and sys.argv[2] == 'b': ForceB = True ''' xdim and ydim of the pygame screen ''' xdim = 500 ydim = 500 backgroundFileName = "./cnts.png" background = pygame.image.load(backgroundFileName) background = pygame.transform.scale(background, (xdim, ydim)) backgroundRect = background.get_rect() intruder_iso_radius = 70 array = np.zeros([xdim, ydim]) screen, clock = InitScreen(xdim, ydim) polygonSegments = load_polygons() # Clear canvas screen.fill((255, 255, 255)) s = pygame.Surface((xdim, ydim)) # the size of your rect s.set_alpha(0) # alpha level s.fill((255, 255, 255)) # this fills the entire surface screen.blit(s, (0, 0)) start_paint = (int(101), int(455)) end_paint = (int(336), int(47)) start = np.atleast_2d([(101 / 500.0), (455 / 500.0)]) #(101, 455) end = np.atleast_2d([(336 / 500.0), (47 / 500.0)]) #(336, 47) original_polygons = load_polygons_here() X1, Y1, X2, Y2 = polygons_to_segments(original_polygons) if ForceA: block = "140 302 141 363 155 332" X1, Y1, X2, Y2 = polygons_to_segments( add_isovist_obstacle(block, original_polygons)) # if ForceB: # block = "6 281 163 284 163 305 52 316 57 483 155 475 149 369 294 370 291 286 327 285 347 376 248 489 14 494 11 347" # X1, Y1, X2, Y2 = polygons_to_segments(add_isovist_obstacle(block, original_polygons)) if ForceB: blockC = "287 297 289 372 311 329" blockA = "139 306 59 300 104 280" temp = add_isovist_obstacle(blockA, original_polygons) temp = add_isovist_obstacle(blockC, temp) X1, Y1, X2, Y2 = polygons_to_segments(temp) if ForceC: block = "65 307 304 305 172 255" X1, Y1, X2, Y2 = polygons_to_segments( add_isovist_obstacle(block, original_polygons)) Update() isovist = iso.Isovist(polygonSegments) agentx = 262 agenty = 214 if UAVLoc2: agentx = 406 agenty = 211 UAVLocation = (agentx, agenty) mouseClick = None UAVForwardVector = (2, 121) # looking south intersections = [(262, 214), (236.0, 260), (286.0, 260)] isovist_block = "262 214 236 260 286 260" if UAVLoc2: intersections = [(406, 211), (384, 254), (430, 254)] isovist_block = "406 211 384 254 430 254" # Get intended path path = run_rrt(start, end, X1, Y1, X2, Y2) intended_path = GetReadablePath(path, screen) isIntruderFound, seen_loc = IsIntruderSeenByUAV(intended_path, intersections) isUAVFound, u_seen_loc = IsUAVSeenByIntruder(UAVLocation, intruder_iso_radius, intended_path) DrawMiniIsovistForPath(intended_path, screen, end_paint, iso_radius=intruder_iso_radius, intersection=u_seen_loc, UAVSawInt=isIntruderFound) intruderColor = (255, 0, 0) if not isIntruderFound: intruderColor = (0, 255, 0) # Draw Polygon for intersections (isovist) isovist_surface = pygame.Surface((xdim, ydim)) isovist_surface.fill((255, 255, 255)) isovist_surface.set_alpha(100) # JUST for drawing the isovist pygame.draw.polygon(isovist_surface, intruderColor, intersections) screen.blit(isovist_surface, isovist_surface.get_rect()) pygame.draw.circle(screen, (100, 100, 100), UAVLocation, 5) pygame.draw.circle(screen, (0, 255, 0), start_paint, 7) pygame.draw.circle(screen, (255, 0, 0), end_paint, 7) # Draw segments for polygon in polygonSegments: for segment in polygon: pygame.draw.line(screen, (0, 0, 0), segment[0], segment[1], 2) # Get re-routed path # if isIntruderFound: # X1, Y1, X2, Y2 = polygons_to_segments(add_isovist_obstacle(isovist_block, original_polygons)) # #X1, Y1, X2, Y2 = polygons_to_segments(original_polygons) # seen_rrt = np.atleast_2d( [(seen_loc[0]/500.0 ),(seen_loc[1]/500.0)] ) # path = run_rrt( seen_rrt, end, X1, Y1, X2, Y2) # rerouted_path = GetReadablePath(path, screen) # # DrawMiniIsovistForPath(rerouted_path, screen, end_paint, iso_radius= intruder_iso_radius, reroute_color=(0, 255, 0)) if seen_loc != None: pygame.draw.circle(screen, (255, 255, 0), seen_loc, 5) if u_seen_loc != None: pygame.draw.circle(screen, (100, 45, 134), u_seen_loc, 5) #write legend pygame.draw.circle(screen, (100, 100, 100), [10, 10], 5) pygame.draw.circle(screen, (0, 255, 0), [10, 40], 7) pygame.draw.circle(screen, (255, 0, 0), [10, 70], 7) pygame.draw.circle(screen, (255, 255, 0), [10, 100], 5) pygame.draw.circle(screen, (100, 45, 134), [10, 130], 5) myfont = pygame.font.SysFont("comicsansms", 15) # render text label = myfont.render("UAV", 2, (0, 0, 0)) screen.blit(label, (30, 3)) label = myfont.render("start", 2, (0, 0, 0)) screen.blit(label, (30, 33)) label = myfont.render("goal", 2, (0, 0, 0)) screen.blit(label, (30, 63)) label = myfont.render("detection of intruder by UAV", 2, (0, 0, 0)) screen.blit(label, (30, 93)) label = myfont.render("detection of UAV by intruder", 2, (0, 0, 0)) screen.blit(label, (30, 123)) label = myfont.render("Intruder detecter UAV = " + str(isUAVFound), 2, (0, 0, 0)) screen.blit(label, (232, 400)) label = myfont.render("UAV detected Intruder = " + str(isIntruderFound), 2, (0, 0, 0)) screen.blit(label, (232, 440)) pfont = pygame.font.SysFont("comicsansms", 30) label = pfont.render("A", 3, (150, 150, 150)) screen.blit(label, (90, 278)) label = pfont.render("B", 3, (150, 150, 150)) screen.blit(label, (250, 278)) label = pfont.render("C", 3, (150, 150, 150)) screen.blit(label, (395, 278)) Update() return [isUAVFound, isIntruderFound, ForceA, ForceB, ForceC, UAVLoc2]
if intruder_seen: print " Detection!" print UAV_loc print Int_loc break return results, intruder_seen # # ========================================================================== # MAIN # ========================================================================== # polygonSegments = load_polygons() isovist = iso.Isovist( polygonSegments ) X1, Y1, X2, Y2 = polygons_to_segments(load_polygons_here()) model = Coolworld() model.isovist = isovist model.X1 = X1 model.Y1 = Y1 model.X2 = X2 model.Y2 = Y2 results = [] #for K in tqdm(xrange( 100 )): for K in xrange( 100 ): print "=============================================" print "ITERATION %d" % K
if __name__ == '__main__': # XXX This is testing the runner model. We can view samples from the prior # conditioned [on the variable list below] #test_chaser() # setup locs = [[0.100, 1 - 0.900], [0.566, 1 - 0.854], [0.761, 1 - 0.665], [0.523, 1 - 0.604], [0.241, 1 - 0.660], [0.425, 1 - 0.591], [0.303, 1 - 0.429], [0.815, 1 - 0.402], [0.675, 1 - 0.075], [0.432, 1 - 0.098]] seg_map = polygons_to_segments(load_polygons("./paths.txt")) isovist = i.Isovist(load_isovist_map()) polys, epolys = load_segs() # # for writing the smart preplanned runner # way_pts = [[0.100, 1-0.9], [.28,.20] , [.26, .32],[.16,.36], [.11, .057], [.179, .086], [.38, .87], [.432, 1-.098]] # way_pts = np.asarray(way_pts) # print way_pts dets = [] simulation_Q_history = [] # TODO: for x simulations runner_intercepted_cnt = 0 for x in xrange(5): sim_id = str(int(time.time())) detection, Q_history = run_simulation(sim_id, locs, seg_map, isovist, polys, epolys)
def main(): ''' xdim and ydim of the pygame screen ''' xdim = 500 ydim = 500 backgroundFileName = "./cnts.png" background = pygame.image.load(backgroundFileName) background = pygame.transform.scale(background, (xdim, ydim)) backgroundRect = background.get_rect() array = np.zeros([xdim, ydim]) screen, clock = InitScreen(xdim, ydim) polygonSegments = load_polygons() # Clear canvas screen.fill((255, 255, 255)) s = pygame.Surface((xdim, ydim)) # the size of your rect s.set_alpha(0) # alpha level s.fill((255, 255, 255)) # this fills the entire surface screen.blit(s, (0, 0)) screen.blit(background, backgroundRect) #### RRT STUFF start_paint = (int(0.1 * 500), int(0.1 * 500)) end_paint = (int(0.9 * 500), int(0.9 * 500)) start = np.atleast_2d([(0.1), (0.1)]) end = np.atleast_2d([(0.9), (0.9)]) X1, Y1, X2, Y2 = polygons_to_segments(load_polygons_here()) # Draw segments for polygon in polygonSegments: for segment in polygon: pygame.draw.line(screen, (225, 225, 225), segment[0], segment[1], 1) #RRTPath = [(200, 120), ( 450, 200)] #Draw hard coded RRT Path #pygame.draw.line(screen, (0, 0, 255), RRTPath[0], RRTPath[1],2) # for point in RRTPath: # pygame.draw.circle(screen, (255,100,255), point, 5) Update() isovist = iso.Isovist(polygonSegments) #(313, 115) agentx = 313 agenty = 215 UAVLocation = (agentx, agenty) mouseClick = None # intended path path = run_rrt(start, end, X1, Y1, X2, Y2) while True: if mouseClick != None: UAVLocation = mouseClick # Clear canvas # screen.fill((255,255,255)) # s = pygame.Surface((xdim,ydim)) # the size of your rect # s.set_alpha(0) # alpha level # s.fill((255,255,255)) # this fills the entire surface # screen.blit(s, (0,0)) screen.blit(background, backgroundRect) # Draw segments ( map ) for polygon in polygonSegments: for segment in polygon: pygame.draw.line(screen, (225, 225, 225), segment[0], segment[1], 1) mouse = pygame.mouse.get_pos() # for i in range(0, X1.shape[0]): # pygame.draw.line(screen, (225, 0, 0), [ X1[i], Y1[i] ], [ X2[i], Y2[i] ],1 ) #print UAVLocation path = run_rrt(start, end, X1, Y1, X2, Y2) readable_path = [] for i in xrange(1, len(path)): s_point = path[i - 1] s_point = (int(s_point[0] * 500), int(s_point[1] * 500)) e_point = path[i] e_point = (int(e_point[0] * 500), int(e_point[1] * 500)) pygame.draw.line(screen, (225, 225, 0), s_point, e_point, 1) readable_path.append(s_point) # getting directions dirx = mouse[0] - UAVLocation[0] diry = mouse[1] - UAVLocation[1] direction = (dirx, diry) pygame.draw.circle(screen, (0, 255, 0), start_paint, 10) pygame.draw.circle(screen, (255, 0, 0), end_paint, 10) UAVForwardVector = direction isIntruderFound = isovist.IsIntruderSeen(readable_path, UAVLocation, UAVForwardVector, UAVFieldOfVision=45) intruderColor = (255, 0, 0) if isIntruderFound: intruderColor = (0, 255, 0) # Draw Polygon for intersections (isovist) isovist_surface = pygame.Surface((xdim, ydim)) isovist_surface.set_alpha(80) # JUST for drawing the isovist intersections = isovist.GetIsovistIntersections( UAVLocation, UAVForwardVector) if intersections != []: pygame.draw.polygon(isovist_surface, intruderColor, intersections) screen.blit(isovist_surface, isovist_surface.get_rect()) pygame.draw.circle(screen, (100, 100, 100), UAVLocation, 5) #pygame.draw.circle(screen, (100,100,100), mouse, 5) #isox,isoy = UpdateMovement(isox,isoy) mouseClick = Update() pygame.time.delay(10)
def main(): ''' xdim and ydim of the pygame screen ''' xdim = 500 ydim = 500 backgroundFileName = "./cnts.png" background = pygame.image.load(backgroundFileName) background = pygame.transform.scale(background, (xdim, ydim)) backgroundRect = background.get_rect() array = np.zeros([xdim, ydim]) screen, clock = InitScreen(xdim, ydim) polygonSegments = load_polygons() # isovist stuff isovist = iso.Isovist( polygonSegments ) # Clear canvas screen.fill((255,255,255)) s = pygame.Surface((xdim,ydim)) # the size of your rect s.set_alpha(0) # alpha level s.fill((255,255,255)) # this fills the entire surface screen.blit(s, (0,0)) screen.blit(background, backgroundRect) X1, Y1, X2, Y2 = polygons_to_segments(load_polygons_here()) for polygon in polygonSegments: for segment in polygon: pygame.draw.line(screen, (225, 225, 225), segment[0], segment[1],1) Update() INTRUDER_SEEN_COUNT = 0 UAV_start = paint_loc( (0.968,0.032)) UAV_end = paint_loc((0.016, 0.02)) #==== testing full isovist area # test_start = paint_loc((0.53, 0.28)) # pygame.draw.circle(screen, (255,255,0), test_start, 7) # isovist_surface = pygame.Surface((xdim,ydim)) # isovist_surface.set_alpha(50) # intersections = isovist.GetIsovistIntersections(test_start, (0,10), full_iso=True) # if intersections != []: # pygame.draw.polygon(isovist_surface, (255,255,0), intersections) # screen.blit(isovist_surface, isovist_surface.get_rect()) # print isovist_area(intersections) # Update() # print "Paused" # raw_input() #======== K = 0 while K <= 0: # SAMPLE INTRUDER IntruderStart = paint_loc((0.06, 0.236)) IntruderGoal = paint_loc((.9,.9)) pygame.draw.circle(screen, (0,255,0), IntruderStart, 5) pygame.draw.circle(screen, (255,0,0), IntruderGoal, 5) # GET INTRUDER PATH AND DRAW INTENDED PATH # intruder_path = run_rrt( rrt_loc(IntruderStart), rrt_loc(IntruderGoal), X1, Y1, X2, Y2) # DrawRRT(intruder_path, screen, IntruderGoal) intruder_path = smart_intruder_rrt(IntruderStart, IntruderGoal, X1, Y1, X2, Y2, isovist, N=10, X=8) #THINK UAV_path = run_rrt( rrt_loc(UAV_start), rrt_loc(UAV_end), X1, Y1, X2, Y2) DrawRRT(UAV_path, screen, UAV_end, highlight = (0,255,0)) #STEPS THE INTRUDER TAKES TO GOAL e_point = None intruder_walking_color = (255, 0, 0) UAV_searching_color = (255, 0, 255) UAV_current_step = 1 UAV_steps = 3 UAV_curr_loc = UAV_start Intruder_curr_loc = IntruderStart for i in xrange(1, len(intruder_path)+1): #DRAW STEP FOR INTRUDER if i == len(intruder_path): #and e_point != None: pygame.draw.line(screen, intruder_walking_color, paint_loc(intruder_path[-1]), IntruderGoal, 1) else: s = intruder_path[i-1] e = intruder_path[i] pygame.draw.line(screen, intruder_walking_color, paint_loc(s), paint_loc(e), 1) #INTRUDER CURRENT LOCATION Intruder_curr_loc = paint_loc(e) if Intruder_curr_loc == None: Intruder_curr_loc = paint_loc(intruder_path[-1]) #FOR EVERY 1 STEP OF INTRUDER, N STEPS FOR UAV stepsToGo = UAV_current_step+UAV_steps if len(UAV_path) - UAV_current_step < UAV_steps: stepsToGo = len(UAV_path) + 1 for j in xrange(UAV_current_step, stepsToGo): if j == len(UAV_path): #and e_point != None: pygame.draw.line(screen, UAV_searching_color, paint_loc(UAV_path[-1]), UAV_end, 1) else: s = UAV_path[j-1] e = UAV_path[j] pygame.draw.line(screen, UAV_searching_color, paint_loc(s), paint_loc(e), 1) # UAV CURRENT LOCATION UAV_curr_loc = paint_loc(e) if UAV_curr_loc == None: UAV_curr_loc = paint_loc(UAV_path[-1]) if dist(Intruder_curr_loc, UAV_curr_loc) <= 70: fv = direction(paint_loc(e),paint_loc(s)) if not(fv[0] == 0 or fv[1] == 0): intersections = isovist.GetIsovistIntersections(UAV_curr_loc, fv) intruder_seen = isovist.FindIntruderAtPoint(Intruder_curr_loc, intersections) if intruder_seen: INTRUDER_SEEN_COUNT += 1 print "INTRUDER SEEN!" #raw_input() Update() pygame.time.delay(10) UAV_current_step += UAV_steps if stepsToGo >= len(UAV_path): south = 50 #THINK AGAIN! UAV_start = go_south(UAV_start, amt=south) UAV_end = go_south(UAV_end, amt=south) if UAV_start[1] >= 500: UAV_start = paint_loc( (0.968,0.032)) UAV_end = paint_loc((0.016, 0.02)) temp = UAV_start UAV_start = UAV_end UAV_end = temp UAV_path = run_rrt( rrt_loc(UAV_start), rrt_loc(UAV_end), X1, Y1, X2, Y2) DrawRRT(UAV_path, screen, UAV_end, highlight = (0,255,0)) UAV_current_step = 1 Update() pygame.time.delay(10) K += 1 # index = getPathIndex() # print index myfont = pygame.font.SysFont("arial", 20) while True: mouseClick = Update() if mouseClick != None: loc = mouseClick[0]/500.0, mouseClick[1]/500.0 print loc,"," pygame.draw.circle(screen, (255,255,0), mouseClick, 7) label = myfont.render(str(loc), 3, (255,255,0)) label_loc = (mouseClick[0] -30, mouseClick[1] - 20) screen.blit(label,label_loc) pygame.time.delay(10)
for i in range(rx1.shape[0]): result.append([(rx1[i], ry1[i]), (rx2[i], ry2[i])]) return result def load_data(): try: with open("rrt_paths-new.dat") as f: x = pickle.load(f) except: x = [] return x X1, Y1, X2, Y2 = polygons_to_segments(load_polygons()) isovist = iso.Isovist([listify_segs(X1, Y1, X2, Y2)]) #===================GIVEN DATA========================== #candidate UAV locations (10) cand_uav_locs = [(0.532, 0.28), (0.66, 0.472), (0.242, 0.484), (0.294, 0.666), (0.458, 0.752), (0.584, 0.64), (0.55, 0.1), (0.364, 0.194), (0.47, 0.466), (0.742, 0.682)] w = [0.08, 0.18, 0.10, 0.10, 0.04, 0.13, 0.08, 0.14, 0.10, 0.05] #set start and goal locations start = np.atleast_2d([(0.1), (0.1)]) end = np.atleast_2d([(0.9), (0.9)]) rrt_paths = load_data()