def main(args):
parser = argparse.ArgumentParser(description='PRM Path Planning Algorithm')
parser.add_argument('--numSamples',
type=int,
default=1000,
metavar='N',
help='Number of sampled points')
args = parser.parse_args()
numSamples = args.numSamples
env = open("environment.txt", "r")
l1 = env.readline().split(";")
current = list(map(int, l1[0].split(",")))
destination = list(map(int, l1[1].split(",")))
print("Current: {} Destination: {}".format(current, destination))
print("****Obstacles****")
allObs = []
for l in env:
if (";" in l):
line = l.strip().split(";")
topLeft = list(map(int, line[0].split(",")))
bottomRight = list(map(int, line[1].split(",")))
obs = Obstacle(topLeft, bottomRight)
obs.printFullCords()
allObs.append(obs)
utils = Utils()
utils.drawMap(allObs, current, destination)
prm = PRMController(numSamples, allObs, current, destination)
# Initial random seed to try
initialRandomSeed = 0
prm.runPRM(initialRandomSeed)
def process():
#Inicializar imagen
img = cv.imread('..\cameraControllerPython\Image\map.jpg')
OrigImag = img.copy()
# Load image
image_bgr = img
# Convert to RGB
image_rgb = cv.cvtColor(image_bgr, cv.COLOR_BGR2RGB)
# Rectange values: start x, start y, width, height
rectangle = (25, 25, img.shape[1] - 50, img.shape[0] - 50)
# Create initial mask
mask = np.zeros(image_rgb.shape[:2], np.uint8)
# Create temporary arrays used by grabCut
bgdModel = np.zeros((1, 65), np.float64)
fgdModel = np.zeros((1, 65), np.float64)
# Run grabCut
cv.grabCut(
image_rgb, # Our image
mask, # The Mask
rectangle, # Our rectangle
bgdModel, # Temporary array for background
fgdModel, # Temporary array for background
5, # Number of iterations
cv.GC_INIT_WITH_RECT) # Initiative using our rectangle
# Create mask where sure and likely backgrounds set to 0, otherwise 1
mask_2 = np.where((mask == 2) | (mask == 0), 0, 1).astype('uint8')
# Multiply image with new mask to subtract background
image_rgb_nobg = image_rgb * mask_2[:, :, np.newaxis]
# Show image
plt.imshow(image_rgb_nobg), plt.axis("off")
plt.show()
img = cv.cvtColor(image_rgb_nobg, cv.COLOR_RGB2BGR)
imgray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
ret, thresh = cv.threshold(imgray, 5, 255, 0)
contours, hierarchy = cv.findContours(thresh, cv.RETR_TREE,
cv.CHAIN_APPROX_SIMPLE)
ctrs = img.copy()
cv.drawContours(ctrs, contours, -1, (0, 255, 0), 3)
###cv.imshow('Images', ctrs)
#print(contours)
#print(hierarchy)
decode_preds = {
0: 'car',
1: 'destination',
2: 'house',
3: 'origin',
4: 'tree',
5: 'truck',
6: 'windmill'
}
model = load_model('drone_vision_vgg16.h5')
xL = OrigImag.shape[0] / 100
yL = OrigImag.shape[1] / 100
current = str(round(153 / xL)) + ", " + str(round(478 / yL))
destination = str(round(1034 / xL)) + ", " + str(round(184 / yL))
obstacles = np.array(["empty"])
contoursNew = np.array([[1, 2, 3]])
divisions = img.copy()
i = 0
#print(len(contours))
while i < len(contours):
#print(hierarchy[0][i][3])
(x, y, w, h) = cv.boundingRect(contours[i])
#print((x,y,w,h))
if (hierarchy[0][i][3] == -1 and contours[i].shape[0] > 3 and w > 10
and h > 10):
#print(cont)
print("Number: " + str(i))
divisions = cv.rectangle(divisions, (x, y), (x + w, y + h),
(255, 0, 0), 2)
#imagTopredict = OrigImag[round(x+w/2-75):round(x+w/2+75), round(y+h/2-75):round(y+h/2+75), :].copy()
imagTopredict = OrigImag[y:y + h, x:x + w, :].copy()
#print(imagTopredict.shape)
imagTopredict = cv.resize(imagTopredict, (150, 150))
#imagTopredict = array_to_img(imagTopredict)
# prepare the image for the VGG model
imagTopredict = imagTopredict / 255
imagTopredict = np.array(imagTopredict)[:, :, 0:3]
# convert the image pixels to a numpy array
###imagTopredict = img_to_array(imagTopredict)
# reshape data for the model
# if(imagTopredict.shape[0]<imagTopredict.shape[1]):
# extension = imagTopredict.shape[0]
# else:
# extension = imagTopredict.shape[1]
#imagTopredict = np.stack([imagTopredict], axis=0)
imagTopredict = imagTopredict.reshape(-1, 150, 150, 3)
#imagTopredict = imagTopredict.reshape((1, 150, 150, 3))
#{'DecodeJpeg:0': imagTopredict}
# predict the probability across all output classes
prediction = model.predict(imagTopredict)
if (contours[i].shape[0] == 4):
prediction[0][3] = 1
strPred = decode_preds[np.argmax(prediction)]
strMsg = strPred + ' (' + str(
round(prediction[0][np.argmax(prediction)] * 100, 2)) + '%)'
#print(strMsg)
divisions = cv.putText(divisions, strMsg, (x - 40, y),
cv.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 2)
print(
f'{decode_preds[np.argmax(prediction)]} ({round(prediction[0][np.argmax(prediction)] * 100, 2)}%)'
)
print(cv.HuMoments(cv.moments(contours[i])).flatten())
if (strPred == 'origin'):
current = str(round(
(x + w / 2) / xL)) + "," + str(round((y + h / 2) / yL))
elif (strPred == 'destination'):
destination = str(round(
(x + w / 2) / xL)) + "," + str(round((y + h / 2) / yL))
else:
temp = str(round(x / xL)) + "," + str(round(
y / yL)) + ";" + str(round(
(x + w) / xL)) + "," + str(round((y + h) / yL))
obstacles = np.concatenate((obstacles, [temp]), axis=0)
contoursNew = np.concatenate(
(contoursNew,
[[x + w / 2, y + h / 2,
math.sqrt(w**2 + h**2) / 2]]),
axis=0)
divisions = cv.circle(divisions,
(round(x + w / 2), round(y + h / 2)),
int(math.sqrt(w**2 + h**2) / 2),
(255, 0, 0), 2)
i = i + 1
else:
contours.pop(i)
hierarchy = np.delete(hierarchy, i, 1)
ctrs = img.copy()
cv.drawContours(ctrs, contours, -1, (0, 255, 0), 3)
#print
###cv.imshow('Images 2', ctrs)
cv.imshow('Divisions', divisions)
cv.imwrite("images\divisions.png", divisions)
'''
Probabilistic Road Map
'''
parser = argparse.ArgumentParser(description='PRM Path Planning Algorithm')
parser.add_argument('--numSamples',
type=int,
default=500,
metavar='N',
help='Number of sampled points')
args = parser.parse_args()
numSamples = args.numSamples
current = list(map(int, current.split(",")))
destination = list(map(int, destination.split(",")))
print("Current: {} Destination: {}".format(current, destination))
print("****Obstacles****")
allObs = []
for l in obstacles:
if (";" in l):
line = l.strip().split(";")
topLeft = list(map(int, line[0].split(",")))
bottomRight = list(map(int, line[1].split(",")))
obs = Obstacle(topLeft, bottomRight)
obs.printFullCords()
allObs.append(obs)
utils = Utils()
utils.drawMap(allObs, current, destination)
numSamples = 250
prm = PRMController(numSamples, allObs, current, destination)
# Initial random seed to try
initialRandomSeed = 0
xSol, ySol = prm.runPRM(initialRandomSeed)
xSol = xL * np.array(xSol)
ySol = yL * np.array(ySol)
#Print Solution maps
solutionImg = divisions.copy()
j = 0
for j in range(len(ySol) - 1):
solutionImg = cv.line(solutionImg, (int(xSol[j]), int(ySol[j])),
(int(xSol[j + 1]), int(ySol[j + 1])),
(255, 0, 0), 2)
###cv.imwrite("images\solution.png", solutionImg)
solutionImgO = OrigImag.copy()
j = 0
for j in range(len(ySol) - 1):
solutionImgO = cv.line(solutionImgO, (int(xSol[j]), int(ySol[j])),
(int(xSol[j + 1]), int(ySol[j + 1])),
(255, 0, 0), 2)
###cv.imwrite("images\solutionMap.png", solutionImgO)
#xSol = -0.064637985309549*(xSol-157)
#ySol = -0.065317919075145*(ySol-539)
contoursNew = np.delete(contoursNew, 0, axis=0)
print("contoursNew")
print(contoursNew)
i = 0
while i < len(ySol):
j = i + 2
iteration = 0
while j < len(ySol):
mI = ySol[i]
b = xSol[i]
k = (ySol[j] - ySol[i]) / (xSol[j] - xSol[i])
freeObst = True
for cN in range(len(contoursNew)):
x = (contoursNew[cN, 0] + contoursNew[cN, 1] * k - mI * k +
b * k**2) / (1 + k**2)
y = mI + (x - b) * k
distObs = math.sqrt((contoursNew[cN, 0] - x)**2 +
(contoursNew[cN, 1] - y)**2) - 2
interX = (x < xSol[j] and x > xSol[i]) or (x > xSol[j]
and x < xSol[i])
interY = (y < ySol[j] and y > ySol[i]) or (y > ySol[j]
and y < ySol[i])
if (contoursNew[cN, 2] > distObs and interX and interY):
freeObst = False
if (freeObst):
xSol = np.delete(xSol, range(i + 1, j), axis=0)
ySol = np.delete(ySol, range(i + 1, j), axis=0)
j = i + 2
else:
j = j + 1
iteration = iteration + 1
i = i + 1
j = 0
for j in range(len(ySol) - 1):
solutionImg = cv.line(solutionImg, (int(xSol[j]), int(ySol[j])),
(int(xSol[j + 1]), int(ySol[j + 1])),
(0, 255, 0), 2)
cv.imshow('Soution', solutionImg)
###cv.imwrite("images\solution.png", solutionImg)
solutionImgO = OrigImag.copy()
j = 0
for j in range(len(ySol) - 1):
solutionImgO = cv.line(solutionImgO, (int(xSol[j]), int(ySol[j])),
(int(xSol[j + 1]), int(ySol[j + 1])),
(0, 255, 0), 2)
cv.imshow('Soution Map', solutionImgO)
Sol = np.concatenate(([xSol], [ySol], np.ones((1, np.size(xSol, axis=0)))),
axis=0)
#Sol = np.array([[-0.56648, 0.022008, -2.96859],[0.362136, 0.932125,-559.271],[0,0,1]])*Sol
Sol = np.matmul(
np.array([[-0.064717, -0.000218, 10.2782],
[0.000218, -0.064717, 34.8483], [0, 0, 1]]), Sol)
cv.waitKey(0)
return Sol[0, :], Sol[1, :]
pygame.display.set_caption("Dungeon Crawler")
# Player
playerX = 400
playerY = 480
player = Player(playerX, playerY)
playerX_change = 0
playerY_change = 0
# crate
crateX = 400
crateY = 300
crates = []
crate1 = Obstacle(crateX, crateY)
spikes = Trap(80, 80, "spikes")
x = 300
y = 480
# Enemy
enemies = []
"""
num_enemies = 5
enemies = []
for i in range(num_enemies):
enemyX = random.randint(0, 800)
enemyY = random.randint(0, 600)
enemy = Enemy(enemyX, enemyY)
enemy.x_change = 0.3
def game_loop(self):
#setting up the game screen
screen = pygame.display.set_mode((500, 500))
pygame.display.set_caption('Taxi Driver')
#setting up all the objects and writing
font = pygame.font.SysFont(None, 30)
gameObjs = {}
gameObjs['mytaxi'] = Taxi()
gameObjs['obstacle'] = Obstacle(0, 10)
gameObjs['obstacle2'] = Obstacle(0, 10)
objectList = pygame.sprite.Group()
objectList.add(gameObjs['mytaxi'])
obstacleList = pygame.sprite.Group()
obstacleList.add(gameObjs['obstacle'])
obstacleList.add(gameObjs['obstacle2'])
game_over = False
mytaxi = gameObjs['mytaxi']
obstacles = gameObjs['obstacle']
obstacles2 = gameObjs['obstacle2']
score = 0
mylane0 = Lane(130)
mylane = Lane(230)
mylane2 = Lane(330)
mylane3 = Lane(430)
# the game loop
while not game_over:
score_counter_text = font.render('Score: ' + str(score), True,
green)
# the event loop
for event in pygame.event.get():
if event.type == pygame.QUIT:
game_over = True
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_LEFT:
mytaxi.move('left')
if event.key == pygame.K_RIGHT:
mytaxi.move('right')
screen.fill(black)
#accumulating the score, score goes up by 1 every run through the loop
score = score + 1
#drawing everything on screen
pygame.draw.rect(screen, yellow, mylane.rect)
pygame.draw.rect(screen, yellow, mylane2.rect)
pygame.draw.rect(screen, yellow, mylane0.rect)
pygame.draw.rect(screen, yellow, mylane3.rect)
objectList.draw(screen)
obstacleList.draw(screen)
obstacles.update()
obstacles2.update()
#detecting a collision between the taxi and an obstacle
if pygame.sprite.spritecollide(mytaxi, obstacleList, True):
#using json to keep track of highscore
jfile = open('high_score.json', 'r')
jstr = jfile.read()
jdictionary = json.loads(jstr)
jfile.close()
if jdictionary["High_Score"] < score:
jfile = open('high_score.json', 'w')
jdictionary["High_Score"] = score
jstr2 = json.dumps(jdictionary)
jfile.write(jstr2)
jfile.close()
#exits the loop and calls the exit_menu function after the collision
game_over = True
Taxidriver.exit_menu(self, score, jdictionary["High_Score"])
screen.blit(score_counter_text, [10, 10])
pygame.display.flip()
def prepare_obstacles(self, level):
patterns = self.patterns
x = 0
y = 0
if len(patterns) == 0:
return
if patterns[0] == 1:
x = 370
y = 396
for i in range(13):
o = Obstacle(x, y)
self.obstacle_group.add(o)
y -= 16
x = 430
y = 396
for i in range(13):
o = Obstacle(x, y)
self.obstacle_group.add(o)
y -= 16
x = 370
y = 300
for i in range(7):
x -= 16
o = Obstacle(x, y)
self.obstacle_group.add(o)
x = 430
y = 300
for i in range(7):
x += 16
o = Obstacle(x, y)
self.obstacle_group.add(o)
if patterns[1] == 1:
x = 274
y = 316
for i in range(3):
o = Obstacle(x, y)
self.obstacle_group.add(o)
y += 16
x = 526
y = 284
for i in range(3):
o = Obstacle(x, y)
self.obstacle_group.add(o)
y -= 16
x = 490
y = 348
for i in range(8):
o = Obstacle(x, y)
self.obstacle_group.add(o)
x += 16
x = 310
y = 252
for i in range(8):
o = Obstacle(x, y)
self.obstacle_group.add(o)
x -= 16
x = 262
for i in range(7):
y -= 16
o = Obstacle(x, y)
self.obstacle_group.add(o)
x = 538
y = 348
for i in range(7):
y += 16
o = Obstacle(x, y)
self.obstacle_group.add(o)
if patterns[2] == 1:
x = 384
y = 464
for i in range(3):
o = Obstacle(x, y)
self.obstacle_group.add(o)
x += 16
x = 602
y = 412
for i in range(4):
o = Obstacle(x, y)
self.obstacle_group.add(o)
x += 16
o = Obstacle(x - 16, y + 16)
self.obstacle_group.add(o)
x = 198
y = 172
for i in range(4):
o = Obstacle(x, y)
self.obstacle_group.add(o)
x -= 16
o = Obstacle(x + 16, y - 16)
self.obstacle_group.add(o)
x = 640
y = 284
for i in range(3):
o = Obstacle(x, y)
self.obstacle_group.add(o)
y += 16
x = 592
y = 236
for i in range(4):
o = Obstacle(x, y)
self.obstacle_group.add(o)
y -= 16
x = 544
y = 172
for i in range(6):
o = Obstacle(x, y)
self.obstacle_group.add(o)
x += 16
x = 160
y = 284
for i in range(3):
o = Obstacle(x, y)
self.obstacle_group.add(o)
y += 16
x = 208
y = 348
for i in range(4):
o = Obstacle(x, y)
self.obstacle_group.add(o)
y += 16
x = 176
y = 412
for i in range(6):
o = Obstacle(x, y)
self.obstacle_group.add(o)
x += 16
def update():
# Main Loop - Game Paused
if player_car.paused:
menu_light.color = color.rgb(100, 50, 50)
driving_light1.color = color.black
driving_light2.color = color.black
driving_light3.color = color.black
# Entity(billboard=True, scale=Vec3(10, 10, 10), color=color.black, model="plane", rotation=(-90, 0, 0))
if not inMenu:
invoke(menu.show_main_menu)
dis_able_menu()
# Main Loop - Game Running
else:
camera.rotation = Vec3(25, 0, 0)
camera.position = Vec3(0, 10, -10)
menu_light.color = color.black
driving_light1.color = color.rgb(196, 196, 196)
driving_light2.color = color.rgb(128, 128, 128)
driving_light3.color = color.rgb(64, 64, 64)
driving_light1.position = player_car.ent.position
driving_light1.rotation_x = -90
driving_light2.rotation_x = -90
driving_light3.rotation_x = -90
driving_light2.position = player_car.ent.position + player_car.ent.forward * 15 + Vec3(0, 5, 0)
driving_light3.position = player_car.ent.position + player_car.ent.forward * 40 + Vec3(0, 5, 0)
if inMenu:
dis_able_menu()
Menu.clear_menu()
if player_car.new_game:
while player_car.audio_list:
print(player_car.audio_list)
player_car.audio_list.pop().stop(destroy=True)
player_car.ent.position = Vec3(0, 0, 0)
player_car.new_game = False
player_car.story = new_story()
player_car.story_time = time.time() + 10
player_car.score = 0
# HUD
speed_text.text = f"{round(abs(player_car.speed) * 80, 1)} km/h"
# pos_text.text = f"Pos: {round(player.position[0], 2), round(player.position[1], 2), round(player.position[2], 2)}"
score_text.text = f"SCORE {round(player_car.score)}"
if player_car.story:
if time.time() < player_car.story_time:
story_text.text = f"Solve the case : {player_car.story[0]}, follow the clues..."
story_text.position = (-.8, .45)
story_text.scale = 1.5
else:
story_text.text = ''
health_bar_1.value = round(player_car.hp)
siren_bar_1.value = round(player_car.light_time)
# Arrow
arrow.position = player.position + Vec3(0, 5, 0)
arrow.rotation = arrow.look_at(CheckPoint.checkpoints[0], axis="forward")
if held_keys['w']:
for car in cars:
car.w()
elif held_keys['s']:
for car in cars:
car.s()
if held_keys['space']:
for car in cars:
car.brake(False)
if held_keys['a'] and held_keys['d']:
player_car.steering = None
elif not (held_keys['a'] or held_keys['d']):
player_car.steering = 0
elif held_keys['d']:
for car in cars:
car.d()
elif held_keys['a']:
for car in cars:
car.a()
if player_car.lights:
player_car.light_time -= 1
if player_car.light_time < 0:
player_car.lights = False
siren_audio.stop()
else:
if player_car.light_time < 100:
player_car.light_time += .1
crash_speed = player_car.move([*ignore_list, *CheckPoint.checkpoints])
if crash_speed:
if player_car.hp < 1:
print("end crash")
if not player_car.audio_list:
if crash_speed < (10 / 80):
player_car.audio_list.append(Audio('assets/sfx/slow_crash_end'))
else:
player_car.audio_list.append(Audio('assets/sfx/fast_crash_end'))
player_car.audio_list[-1].play()
else:
if crash_speed > (10 / 80):
print("big crash", crash_speed)
Audio('assets/sfx/short_crash')
player_car.rotate()
if not (held_keys['w'] or held_keys['s]']):
player_car.speed = 0
for checkpoint in CheckPoint.checkpoints:
if checkpoint.is_cleared([]):
player_car.score += checkpoint.lastpoint
Obstacle.shuffle()
player.position = player_car.ent.position
if player_car.lights:
if int(time.time() * 5) % 2 == 0:
siren_light.color = color.red
else:
siren_light.color = color.blue
else:
siren_light.color = color.black33
if player_car.hp <= 0:
player_car.paused = True
reset_game(player_car, Obstacle, CheckPoint, menu)
siren_audio.stop()
dis_able_menu()
)
siren_light = Lighting(player, player.position + Vec3(1, 7, 0), color.black, rotation=player.down)
CheckPoint.init_light(Entity('cube', color=color.rgba(255,5,5,128), scale=(25,25,25)))
city = Entity(model='assets/models/city800', color=COLOR_RUST, position =(0, .1, 0), collider='mesh', reload=True)
car = Entity(model='assets/models/80scop',
texture='assets/models/cars',
position = (0, 0, 4),
scale=1,
collider='box',
name='player_car'
)
CheckPoint.init_car(car)
Obstacle.init_car(car)
CheckPoint.spawn_new()
arrow = Arrow()
player_car = TheCar(car)
menu = Menu(player, player_car)
cars = [player_car]
camera.parent = player_car.ent
speed_text = Text(text=f"", position=(0, -.4), color=color.white66)
pos_text = Text(text=f"", position=(.3, .5), color=color.black)
score_text = Text(text=f"", position=(-.8, -.35), color=COLOR_RUST_2X)
story_text = Text(text=f"", position=(-.3, .2), color=COLOR_RUST_2X)
health_bar_1 = health_bar.HealthBar(bar_color=COLOR_RUST_2X, roundness=.1, value=100, position=(-.8, -.40), animation_duration=0)
siren_bar_1 = health_bar.HealthBar(bar_color=color.rgb(40, 40, 70), roundness=.1, value=100, position=(-.8, -.4375), animation_duration=0)