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()
