def InitMultiscreen(res, screens):
# Open new multiscreen display window
pygame.init()
# Set subwindow sizes
col_count = int(sqrt(screens))
row_count = int(screens / col_count)
height = int(res[1] * zoomout) + 20
width = int(res[0] * zoomout)
# Init main screen
screen = struct()
screen.res = (10 + row_count * width, 10 + col_count * height)
screen.surface = pygame.display.set_mode(screen.res, pygame.FULLSCREEN)
screen.zoom_mode = 0
screen.car_size = res[0] / 50
screen.mesh = pygame.Surface(screen.res)
screen.mesh.fill((255, 255, 255))
pygame.display.set_caption('PyRoSim')
pygame.font.init()
# Init subscreens
screen.subscreen = []
i = 0
for y in range(col_count):
for x in range(row_count):
i += 1
x_pos = 5 + x * width
y_pos = 5 + y * height
screen.subscreen.append(
InitSubscreen(i, (x_pos, y_pos), (width, height)))
pygame.draw.rect(screen.mesh, (0, 0, 0),
(x_pos, y_pos, width - 1, height - 1), 1)
screen.mesh.set_colorkey((255, 255, 255))
return screen
def loadFromFile(self, filename):
with open(filename) as f:
data = [line.strip().split() for line in f]
self.point = [
struct(x=float(line[0]), y=float(line[1]), width=float(line[2]))
for i, line in enumerate(data)
]
self.filename = filename
def InitSubscreen(id, pos, res):
# Open a display window
subscreen = struct()
subscreen.surface = pygame.Surface((res[0], res[1]))
subscreen.position = pos
subscreen.res = res
subscreen.id = id
subscreen.zoom_mode = 2
subscreen.car_size = res[0] / 50
ClrScr(subscreen)
return subscreen
def Init(res):
# Open a display window
pygame.init()
screen = struct()
screen.surface = pygame.display.set_mode((res[0], res[1]))
screen.res = res
screen.zoom_mode = 0
screen.car_size = res[0] / 50
pygame.display.set_caption('PyRoSim')
pygame.font.init()
return screen
def calculateGates(self):
left_right_switch = 0
last_div = 0
last_perpendicular = 0
count = len(self.point)
self.gate = []
for current in range(0, count):
current_gate = struct()
# set current indices
next = (current + 1) % count
previous = (count + current - 1) % count
# set gate line parameters (using formulas y=kx+q, k=(y2-y1)/(x2-x1) and k_perpendicular=-1/k)
y_diff = self.point[previous].y - self.point[next].y
x_diff = self.point[previous].x - self.point[next].x
try:
div = y_diff / x_diff
except ZeroDivisionError:
if last_div >= 0: div = float("inf")
else: div = float("-inf")
last_div = div
try:
perpendicular = 0 - 1 / div
except ZeroDivisionError:
if last_perpendicular >= 0: perpendicular = float("inf")
else: perpendicular = float("-inf")
last_perpendicular = perpendicular
current_gate.k = perpendicular
current_gate.q = self.point[
current].y - current_gate.k * self.point[current].x
if current_gate.k == float("inf") or current_gate.k == float(
"-inf"):
current_gate.q = self.point[current].x
# set gate line angle (k=tan(angle), angle=arctan(k))
current_gate.angle = (atan(current_gate.k) * radian_to_degree +
360) % 360
# set gate border points
radian = current_gate.angle * degree_to_radian
x_delta = self.point[current].width * cos(radian)
y_delta = self.point[current].width * sin(radian)
if y_diff < 0: left_right_switch = 0
if y_diff > 0: left_right_switch = 1
if left_right_switch == 0:
current_gate.left_x = self.point[current].x + x_delta
current_gate.left_y = self.point[current].y + y_delta
current_gate.right_x = self.point[current].x - x_delta
current_gate.right_y = self.point[current].y - y_delta
else:
current_gate.left_x = self.point[current].x - x_delta
current_gate.left_y = self.point[current].y - y_delta
current_gate.right_x = self.point[current].x + x_delta
current_gate.right_y = self.point[current].y + y_delta
# add gate to list
self.gate.append(current_gate)
def saveStats(self, statistics):
stat = struct()
stat.time = self.timer.getSimTime()
stat.distance = self.car.distance
stat.speed = self.car.speed
stat.acceleration = self.car.acceleration
stat.direction = self.car.direction
stat.turn = self.car.turn
stat.goalangle = self.navigator.last_angle
stat.x = self.car.x_pos
stat.y = self.car.y_pos
statistics.append(stat)
def aggregateStats(self, statistics):
res = struct()
res.avg_speed = 0
res.avg_acc = 0
res.avg_turn = 0
res.avg_goalangle = 0
for stat in statistics:
res.avg_speed += abs(stat.speed) / float(len(statistics))
res.avg_acc += abs(stat.acceleration) / float(len(statistics))
res.avg_turn += abs(stat.turn) / float(len(statistics))
res.avg_goalangle += abs(stat.goalangle) / float(len(statistics))
res.time = statistics[-1].time
res.distance = res.avg_speed * res.time
res.lost = self.navigator.lost
res.finished = self.navigator.finished
res.remaining = self.navigator.getRemainingDistance()
return res
def InitSound():
global sounds
if sounds == None:
sounds = struct()
try:
next_path = os.path.normpath(os.getcwd() + '/media/continue.wav')
sounds.next = pygame.mixer.Sound(next_path)
except:
print("Warning - Could not load sound from \'" + next_path + "\'.")
try:
wait_path = os.path.normpath(os.getcwd() + '/media/waiting.wav')
sounds.wait = pygame.mixer.Sound(wait_path)
except:
print("Warning - Could not load sound from \'" + wait_path + "\'.")
try:
end_path = os.path.normpath(os.getcwd() + '/media/applause.wav')
sounds.end = pygame.mixer.Sound(end_path)
except:
print("Warning - Could not load sound from \'" + end_path + "\'.")
# load DLL containing image functions
print "Loading shared library with C functions...",
image_lib = cdll.LoadLibrary("OpenCV_test_DLL.dll")
print "Done."
# get function handles
print "Loading functions of library...",
image_load = image_lib.Load
image_show = image_lib.Show
cvReleaseImage = image_lib.aux_cvReleaseImage
# set return type for functions (because ctypes default is int)
image_load.restype = c_void_p
image_show.restype = None
cvReleaseImage.restype = None
print "Done."
# initialize source
print "Initializing camera",
source = struct()
InitCamera(source)
print "Done."
# show video
while (1):
# get image as PIL image
img = GetImage(source)
# transform image to OpenCV IplImage
cv_img = PIL2Ipl(img)
# show image using OpenCV highgui lib
image_show(cv_img)
# release memory
cvReleaseImage(byref(cv_img))
# init sounds
gui.InitSound()
# set save folder
makedir(folder)
now = datetime.datetime.now()
folder += '/experiment-date-'+str(now.strftime("%Y-%m-%d-time-%H-%M"))
# init track
track = simulation.Track("default_track.nft")
# initial population
evolution_clock.Start()
id = 0
generation = 0
population = []
for i in range(size):
individual = struct()
individual.id = id
individual.genotype = Individual(chromosomes)
individual.fenotype = FCMcontroller(listToMatrix(individual.genotype.chromosome))
population.append(individual)
id += 1
# run simulation for each individual
for ind in population:
# init car
car = simulation.Car(x_pos=track.point[start].x, y_pos=track.point[start].y, direction=track.gate[start].angle+90)
# init navigator
navigator = simulation.Navigator(track, car, start, navdist, stopdist)
# init controller
controller = ind.fenotype
# init timer