我正在尝试用python编写RRT路径规划算法。尽管代码执行没有错误,但是结果是一个没有输出的纯pygame窗口。
import sys, random, math, pygame
from math import sqrt,cos,sin,atan2
from pygame.locals import *
pygame.init()
class RRT(object):
X_dimension = 0
Y_dimension = 0
Window_size = 0
EPS = 0
Max_nodes = 0
nodes = list()
K_ESCAPE = True
KEYUP = True
QUIT = True
def __init__(self,x,y):
self.x = [10,20,40,50,60,0]
self.y = [15,30,0,54,75,68]
#parameters
self.X_dimension = 1280 #length of the window
self.Y_dimension = 1280 #breadth of the window
self.Window_size = [self.X_dimension, self.Y_dimension] #Window size
self.EPS = 7000 #EPSILON or Incremental Distance
self.Max_nodes = 100 #maximum number of nodes
self.QUIT = QUIT
self.KEYUP = KEYUP
self.K_ESCAPE = K_ESCAPE
#function for calculating euclidean distance
def Calculate_Distance(self):
x= self.x
y=self.y
return sqrt((x[5]-y[5])*(x[5]-y[5])+(x[4]-y[4])*(x[4]-y[4]))
#Function for calculating all the possible points
def Initiate_Sampling(self):
self.EPS = 7000
y = self.y
x = self.x
if self.Calculate_Distance() < 70:
return y, x
else:
theta = atan2(y[1]-x[1], y[0]-x[0])
return x[0] + self.EPS*cos(theta), x[1] + self.EPS*sin(theta)
#Function for displaying the output
def Start_The_Game(self):
pygame.init()
screen = pygame.display.set_mode(self.Window_size)
caption = pygame.display.set_caption("performing RRT")
white = 255, 240, 200
black = 20, 20, 40
screen.fill(black)
return('GAME BEGINS')
#Main Function
def Node_Generation(self, nodes=True):
self.nodes = nodes
self.QUIT = QUIT
self.KEYUP = KEYUP
self.K_ESCAPE = K_ESCAPE
nodes = [(5.0,5.25),(7.0,7.25),(8,8.25)]
#nodes.append(X_dimension/2.0, Y_dimension/2.0)
nodes.append((0.0,0.0))
for i in range(Max_nodes):
rand = random.random()*640.0, random.random()*480.0
nn = self.nodes[0]
for p in nodes:
if self.Calculate_Distance(p,rand) < self.Calculate_Distance(nn,rand):
nn = p
newnode = step_from_to(nn,rand)
nodes.append(newnode)
pygame.draw.line(screen,white,nn,newnode)
pygame.display.update()
#print (i, " ", nodes)
for event in pygame.event.get():
if event.type == QUIT or (event.type == KEYUP and event.key == K_ESCAPE):
pygame.quit()
sys.exit("GAME OVER")
path = RRT(0,0)#write the starting nodes
path.Calculate_Distance()
path.Initiate_Sampling()
path.Start_The_Game()
path.Node_Generation()
上面的结果显示了一个简单的pygame窗口,但没有输出。