def rrt_search(G, tx, ty): # Implement the rrt_algorithm in this section of the code. # You should call genPoint() within this function to #get samples from different distributions. if visualize: canvas = drawSample.SelectRect(xmin=0,ymin=0,xmax=XMAX ,ymax=YMAX, nrects=0, keepcontrol=0)#, rescale=800/1800.) if 0: # line intersection testing obstacles.append( [ 75,60,125,500 ] ) # tall vertical for o in obstacles: canvas.showRect(o, outline='red', fill='blue') lines = [ ( (70,50), (150,150) ), ( (50,50), (150,20) ), ( (20,20), (200,200) ), ( (300,300), (20, 200) ), ( (300,300), (280, 90) ), ] for l in lines: for o in obstacles: lineHitsRect(l[0],l[1],o) canvas.mainloop() if 0: # random obstacle field for nobst in range(0,6000): wall_discretization=SMALLSTEP*2 # walls are on a regular grid. wall_lengthmax=10. # fraction of total (1/lengthmax) x = wall_discretization*int(random.random()*XMAX/wall_discretization) y = wall_discretization*int(random.random()*YMAX/wall_discretization) #length = YMAX/wall_lengthmax length = SMALLSTEP*2 if random.choice([0,1]) >0: obstacles.append( [ x,y,x+SMALLSTEP,y+10+length ] ) # vertical else: obstacles.append( [ x,y,x+10+length,y+SMALLSTEP ] ) # horizontal else: if 0: # hardcoded simple obstacles obstacles.append( [ 300,0,400,95 ] ) # tall vertical # slightly hard obstacles.append( [ 300,805,400,YMAX ] ) # tall vertical #obstacles.append( [ 300,400,1300,430 ] ) # hard obstacles.append( [ 820,220,900,940 ] ) obstacles.append( [ 300,0, 400,95 ] ) # tall vertical obstacles.append( [ 300,100,400,YMAX ] ) # tall vertical obstacles.append( [ 200,300,800,400 ] ) # middle horizontal obstacles.append( [ 380,500,700,550 ] ) # very hard obstacles.append( [ 705,500,XMAX,550 ] ) if visualize: for o in obstacles: canvas.showRect(o, outline='red', fill='blue') maxvertex += 1 while 1: # graph G G = [ [ 0 ] , [] ] # nodes, edges vertices = [ [10,270], [20,280] ] redraw() G[edges].append( (0,1) ) G[nodes].append(1) if visualize: canvas.markit( tx, ty, r=SMALLSTEP ) drawGraph(G) rrt_search(G, tx, ty) #canvas.showRect(rect,fill='red') if visualize: canvas.mainloop()
# Size of our on-screen drawing is arbitrarily small myImageSize = 1024 scalex = bigpic.size[ 0] / myImageSize # scale factor between our picture and the tileServer scaley = bigpic.size[ 1] / myImageSize # scale factor between our picture and the tileServer im = bigpic.resize((myImageSize, myImageSize)) im = im.filter(ImageFilter.BLUR) im = im.filter(ImageFilter.BLUR) im.save( "mytemp.gif" ) # save the image as a GIF and re-load it does to fragile nature of Tk.PhotoImage tkwindow = drawSample.SelectRect(xmin=0, ymin=0, xmax=1024, ymax=1024, nrects=0, keepcontrol=0) #, rescale=800/1800.) root = tkwindow.root root.title("Drone simulation") # Full background image photo = tk.PhotoImage(file="mytemp.gif") tkwindow.imageid = tkwindow.canvas.create_image(0, 0, anchor=tk.NW, image=photo, tags=["background"]) image_storage.append(photo) tkwindow.canvas.pack()
for o in obstacles: if inRect(p_new,o): intersect=1 break if intersect: continue else: maxvertex += 1 vertices.append(p_new) G[nodes].append(maxvertex) G[edges].append(maxvertex-1,maxvertex) if visualize: canvas.polyline( [vertices[maxvertex-1], p_new ] ) if visualize: canvas = drawSample.SelectRect(xmin=0,ymin=0,xmax=XMAX ,ymax=YMAX, nrects=0, keepcontrol=0)#, rescale=800/1800.) if 0: # line intersection testing obstacles.append( [ 75,60,125,500 ] ) # tall vertical for o in obstacles: canvas.showRect(o, outline='red', fill='blue') lines = [ ( (70,50), (150,150) ), ( (50,50), (150,20) ), ( (20,20), (200,200) ), ( (300,300), (20, 200) ), ( (300,300), (280, 90) ), ] for l in lines: for o in obstacles: lineHitsRect(l[0],l[1],o)
graph_node = returnParent(graph_node) path_dist += 1 return (iteration, path_dist) graph_node = len(vertices) - 1 path_dist = 0 while (graph_node != 0): graph_node = returnParent(graph_node) path_dist += 1 return (iteration, path_dist) if visualize: canvas = drawSample.SelectRect(xmin=0, ymin=0, xmax=XMAX, ymax=YMAX, nrects=0, keepcontrol=0) if visualize: for o in obstacles: canvas.showRect(o, outline='red', fill='blue') maxvertex += 1 # if rrt_search worked correctly, it only needs to run once to # find the goal G = [[0], []] # nodes, edges vertices = [[start_x, start_y, start_a]] redraw()