def mainGame(movementInfo):
global playerAccX
global playerAccY
#define publishers
pub_velocity = rospy.Publisher('flappy_vel', Vector3, queue_size=10)
# create laser
laser = Laser(LASERFOV,LASERRES,SCALING)
score = playerIndex = loopIter = 0
playerIndexGen = movementInfo['playerIndexGen']
playerx, playery = int(SCREENWIDTH * 0.13), movementInfo['playery']
#create timer and counter for timer countdown
pygame.time.set_timer(pygame.USEREVENT, 1000)
countdown = 60
#
basex = movementInfo['basex']
baseShift = IMAGES['base'].get_width() - IMAGES['background'].get_width()
# get 2 new pipes to add to upperPipes lowerPipes list
newPipe1 = getRandomPipe()
newPipe2 = getRandomPipe()
# list of upper pipes
upperPipes = [
{'x': SCREENWIDTH + 200, 'y': newPipe1[0]['y']},
{'x': SCREENWIDTH + 200 + PIPESPACING, 'y': newPipe2[0]['y']},
]
# list of lowerpipe
lowerPipes = [
{'x': SCREENWIDTH + 200, 'y': newPipe1[1]['y']},
{'x': SCREENWIDTH + 200 + PIPESPACING, 'y': newPipe2[1]['y']},
]
# player velocity, max velocity, downward accleration, accleration on flap
pipeVelX = 0
playerVelY = 0 # player's velocity along Y
deltaVel = 0.8
betweenPipes = 0
while True:
for event in pygame.event.get():
if event.type == QUIT or (event.type == KEYDOWN and event.key == K_ESCAPE):
pygame.quit()
sys.exit()
if event.type == KEYDOWN and (event.key == K_UP):
#if playery > -2 * IMAGES['player'][0].get_height():
playerVelY -= deltaVel
#playerAccY -= deltaAcc
if event.type == KEYDOWN and (event.key == K_DOWN):
playerVelY += deltaVel
#playerAccY += deltaAcc
if event.type == KEYDOWN and (event.key == K_LEFT):
pipeVelX += deltaVel
#playerAccX += deltaAcc
if event.type == KEYDOWN and (event.key == K_RIGHT):
pipeVelX -= deltaVel
#playerAccX -= deltaAcc
if event.type == pygame.USEREVENT and score > 0:
if countdown > 0:
countdown -= 1
else:
return {
'y': playery,
'groundCrash': crashTest[1],
'basex': basex,
'upperPipes': upperPipes,
'lowerPipes': lowerPipes,
'score': score,
'playerVelY': playerVelY,
'timeRanOut': 1,
}
#update velocity
pipeVelX += playerAccX
playerVelY += playerAccY
#limit velocity
playerVelY = limitVel(playerVelY,1)
pipeVelX = limitVel(pipeVelX,0)
#publish pub_velocity
pub_velocity.publish(Vector3(-SCALING*FPS*pipeVelX,-SCALING*FPS*playerVelY,0))
# check for crash here
crashTest = checkCrash({'x': playerx, 'y': playery, 'index': playerIndex},
upperPipes, lowerPipes)
if crashTest[0]:
return {
'y': playery,
'groundCrash': crashTest[1],
'basex': basex,
'upperPipes': upperPipes,
'lowerPipes': lowerPipes,
'score': score,
'playerVelY': playerVelY,
'timeRanOut': 0,
}
# check for score
playerMidPos = playerx + IMAGES['player'][0].get_width() / 2
pipeCounter = 0
for pipe in upperPipes:
pipeMidPos = pipe['x'] + IMAGES['pipe'][0].get_width() / 2
if pipeMidPos <= playerMidPos < (pipeMidPos + IMAGES['pipe'][0].get_width() / 2):
pipeCounter += 1
if betweenPipes == 0:
score += 1
betweenPipes = 1
SOUNDS['point'].play()
if pipeCounter == 0:
betweenPipes = 0
# playerIndex basex change
if (loopIter + 1) % 3 == 0:
playerIndex = next(playerIndexGen)
loopIter = (loopIter + 1) % 30
basex = -((-basex - pipeVelX) % baseShift)
playerHeight = IMAGES['player'][playerIndex].get_height()
playery += min(playerVelY, BASEY - playery - playerHeight)
# move pipes to left
for uPipe, lPipe in zip(upperPipes, lowerPipes):
uPipe['x'] += pipeVelX
lPipe['x'] += pipeVelX
# add new pipe when first pipe each ? pixels
if (SCREENWIDTH + 200) > upperPipes[-1]['x']:
newPipe = getRandomPipe()
newPipe[0]['x'] = PIPESPACING + upperPipes[-1]['x']
newPipe[1]['x'] = PIPESPACING + upperPipes[-1]['x']
upperPipes.append(newPipe[0])
lowerPipes.append(newPipe[1])
# remove first pipe if its out of the screen
if upperPipes[0]['x'] < -IMAGES['pipe'][0].get_width():
upperPipes.pop(0)
lowerPipes.pop(0)
# draw sprites
SCREEN.blit(IMAGES['background'], (0,0))
for uPipe, lPipe in zip(upperPipes, lowerPipes):
SCREEN.blit(IMAGES['pipe'][0], (uPipe['x'], uPipe['y']))
SCREEN.blit(IMAGES['pipe'][1], (lPipe['x'], lPipe['y']))
SCREEN.blit(IMAGES['base'], (basex, BASEY))
###################################################################
# get bitmap of obstacles
bitmap = getBitmap(upperPipes,lowerPipes,(basex, BASEY))
# do raytracing with Laser
playerMiddle = (playerx + IMAGES['player'][0].get_width() / 2,playery + IMAGES['player'][0].get_height() / 2)
laserPoints = laser.scan(playerMiddle,bitmap)
# display
if DEBUG == 1:
# display obstacles and ray tracing
bitmap = pygame.surfarray.make_surface(bitmap)
SCREEN.blit(bitmap, (0, 0))
for i in range(0,len(laserPoints)):
if laserPoints[i][2] == 1:
pygame.draw.circle(SCREEN,(0,255,0),laserPoints[i][0:2],3,0)
pygame.draw.aaline(SCREEN,(0,255,0),playerMiddle,laserPoints[i][0:2],1)
else :
pygame.draw.aaline(SCREEN,(0,140,0),playerMiddle,laserPoints[i][0:2],1)
###################################################################
# print score so player overlaps the score
showScore(score)
if score > 0:
showCounter(countdown)
playerSurface = pygame.transform.rotate(IMAGES['player'][playerIndex], 0)
SCREEN.blit(playerSurface, (playerx, playery))
pygame.display.update()
FPSCLOCK.tick(FPS)
min_angle = -math.pi / 2.0
resolution_angle = math.pi / (no_of_beams - 1)
noise_variance = 0.0
laser_pos_x = 1.2
laser_pos_y = 0.0
laser_angle = 0.0 * (math.pi / 180.0)
laser = Laser(max_range, min_angle, resolution_angle, no_of_beams,
noise_variance, occ_map, laser_pos_x, laser_pos_y, laser_angle)
# Read positions
positions = np.loadtxt('../data_pose.txt')
# Read corresponding laser scans
#laser_scans = np.loadtxt('../map_scans.txt') #, delimiter=','
# Go through each position and generate a new scan
# Validate each generated scan against recorded ones
counter = 0
all_ranges = []
n = 2000
for i in range(n):
ranges = laser.scan(positions[i, 0], positions[i, 1], positions[i, 2])
all_ranges.append(copy(ranges))
if not counter % 1000: print('iter', counter)
counter += 1
all_ranges = np.array(all_ranges)
np.savetxt('../map_scans_py.txt', all_ranges)
#print("MSE", np.sum(np.abs(all_ranges-laser_scans))/all_ranges.shape[0])
no_of_beams = 181
min_angle = -math.pi/2.0
resolution_angle = math.pi/(no_of_beams-1)
noise_variance = 0.0
laser_pos_x = 1.2
laser_pos_y = 0.0
laser_angle = 0.0 * (math.pi/180.0)
laser = Laser(max_range, min_angle, resolution_angle, no_of_beams, noise_variance, occ_map, laser_pos_x, laser_pos_y, laser_angle)
# Read positions
positions = np.loadtxt('../data_pose.txt')
# Read corresponding laser scans
#laser_scans = np.loadtxt('../map_scans.txt') #, delimiter=','
# Go through each position and generate a new scan
# Validate each generated scan against recorded ones
counter = 0
all_ranges = []
n = 2000
for i in range(n):
ranges = laser.scan(positions[i,0], positions[i,1], positions[i,2])
all_ranges.append(copy(ranges))
if not counter%1000: print('iter', counter)
counter+=1
all_ranges=np.array(all_ranges)
np.savetxt('../map_scans_py.txt', all_ranges)
#print("MSE", np.sum(np.abs(all_ranges-laser_scans))/all_ranges.shape[0])
