h = 640
w = 480
directory = "footage/"
format = ".avi"
timestamp = time.ctime(time.time()).split(" ")[4].split(":")
filename = "output-"+timestamp[0]+"-"+timestamp[1]+"-"+timestamp[2]
print(timestamp)
fourcc = cv2.VideoWriter_fourcc('M','J','P','G')
out = cv2.VideoWriter(directory+filename+format, fourcc, 20.0, (int(cap.get(3)),int(cap.get(4))))
graph = prm.initGraph(N,w,h,wallPad,k)
start = prm.Node(np.random.randint(0,w-wallPad),np.random.randint(0,h-wallPad),N)
goal = prm.Node(w/2,h/2,N+1)
enemy = prm.Obstacle(w/2,h/2,60)
ball = prm.Obstacle(np.random.randint(0,w-wallPad),np.random.randint(0,h-wallPad),10)
font = cv2.FONT_HERSHEY_SIMPLEX
topLeft = (20,50)
fontScale = 1
fontColor = (255,255,255)
lineType = 2
while(True):
# Capture frame-by-frame
ret, frame = cap.read()
# Our operations on the frame come here
import colour
from colour import GREEN, BLUE
cap = cv2.VideoCapture(0)
N = 100
k = 10
wallPad = 100
h = 640
w = 480
graph = prm.initGraph(N, w, h, wallPad, k)
start = prm.Node(np.random.randint(0, w - wallPad),
np.random.randint(0, h - wallPad), N)
goal = prm.Node(np.random.randint(0, w - wallPad),
np.random.randint(0, h - wallPad), N + 1)
enemy = prm.Obstacle(w / 2, h / 2, 60)
ball = prm.Obstacle(np.random.randint(0, w - wallPad),
np.random.randint(0, h - wallPad), 10)
while (True):
# Capture frame-by-frame
ret, frame = cap.read()
# Our operations on the frame come here
#gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
array_values = np.array(frame)
frame_rgb = cv2.cvtColor(frame,
cv2.COLOR_BGR2RGB) # Convert from BGR to RGB
image = np.array(frame_rgb).astype(
np.float) / 255 # Convert from Int8 to float
#cs = colour.findCenters(image,tol=0.1,draw=True,frame=frame) # Find colour centres
#print(array_values.shape)
def run(bot, reset=False, game_over=False):
ball = cs[GREEN]
canSeeBall = True
# the ball is the goal
if bot == 0:
# Define points
back = cs[WHITE]
front = cs[RED]
start = [(back[0] + front[0]) / 2, (back[1] + front[1]) / 2]
enemy_front = cs[BLUE]
enemy_back = cs[YELLOW]
enemy = [(enemy_back[0] + enemy_front[0]) / 2,
(enemy_back[1] + enemy_front[1]) / 2]
goal = cs[GREEN]
#goal = [(enemy[0]+ball[0])/2,(enemy[1]+ball[1])/2]
else: # the midpoint of the line between the ball and the other bot is the goal
front = cs[BLUE]
back = cs[YELLOW]
start = [(back[0] + front[0]) / 2, (back[1] + front[1]) / 2]
enemy_front = cs[RED]
enemy_back = cs[WHITE]
enemy = [(enemy_back[0] + enemy_front[0]) / 2,
(enemy_back[1] + enemy_front[1]) / 2]
goal = [(enemy[0] + ball[0]) / 2, (enemy[1] + ball[1]) / 2]
#goal = cs[GREEN]
if reset:
goal = startPoints[bot]
if game_over:
goal = [H / 2, W / 2]
if np.isnan(enemy[0]):
enemy[0] = -enemyRadius
if np.isnan(enemy[1]):
enemy[1] = -enemyRadius
if np.isnan(ball[0]) or np.isnan(ball[1]):
ball = lastBall[bot]
if np.isnan(goal[0]) or np.isnan(goal[1]):
# Can't see ball
#print("Can't see ball, going to centre")
goal = lastBall[bot]
canSeeBall = False
else:
lastBall[bot] = goal
if not (np.isnan(start[0]) or np.isnan(start[1])):
goalDisp = [0, 0]
startNode = prm.Node(start[0], start[1], N)
goalNode = prm.Node(goal[0] + goalDisp[0], goal[1] + goalDisp[1],
N + 1)
frontNode = prm.Node(front[0], front[1], N)
enemy = prm.Obstacle(enemy[0], enemy[1], enemyRadius)
ballNode = prm.Obstacle(ball[0], ball[1], ballRadius)
if not (reset and motion.dist(startNode, goalNode) < 70):
#Find path
x, y = prm.pathPlan(graph,
startNode,
goalNode,
enemy,
ballNode,
k,
avoidBall=False,
draw=True,
w=640,
h=480,
frame=frame,
bot=bot)
next = prm.Node(x, y, 0)
'''
if motion.dist(startNode,goalNode)<enemyRadius:
#print("RESET NEXT TO GOAL")
next = prm.Node(goalNode.x,goalNode.y,0)
'''
last[bot] = angle[bot]
lastl[bot] = distance[bot]
if (turnVel[bot] != 0):
#Turn to goal
Il[bot] = 0
turnVel[bot], angle[bot] = motion.PID_Angle(
startNode, frontNode, next, k_angle[bot], I[bot],
last[bot], tol, bot)
#turnVel[bot],angle[bot] = motion.Stochastic_Angle(startNode, frontNode, next, tol, bot)
if np.isnan(turnVel[bot]):
print("ANGLE IS NAN")
turnVel[bot] = 0
limitSet = [0, 0, 0, 0, 0, turnVel[bot]]
client.send_to_server(limitSet, servers[bot],
sockets[bot]) #Sending sets to servers
results = client.get_replies(sockets[bot])
elif velocity[bot] != 0:
#Move to goal
I[bot] = 0
velocity[bot], distance[bot] = motion.PID_Linear(
startNode, next, k_linear[bot], Il[bot], lastl[bot], toll,
bot)
#velocity[bot],distance[bot] = motion.Stochastic_Linear(startNode, next, toll, bot)
if np.isnan(velocity[bot]):
print("VELOCITY IS NAN")
velocity[bot] = 0
limitSet = [velocity[bot], 0, 0, 0, 0, 0]
client.send_to_server(limitSet, servers[bot],
sockets[bot]) #Sending sets to servers
results = client.get_replies(sockets[bot])
else:
#Do nothing
turnVel[bot] = 3
velocity[bot] = 3
#print("turnVel = %i, Velocity = %i" %(turnVel[bot],velocity[bot]))
#limitSet = [0, 0, 0, 0, 0, 0]
I[bot] += angle[bot]
Il[bot] += distance[bot]
if bot == 0 and motion.dist(startNode,
ballNode) < 70 and canSeeBall:
return True
return False
ball = lastBall
if np.isnan(goal[0]) or np.isnan(goal[1]):
# Can't see ball
print("Can't see ball, going to centre")
goal = lastBall
else:
lastBall = goal
if not (np.isnan(start[0]) or np.isnan(start[1])):
goalDisp = [0, 0]
startNode = prm.Node(start[0], start[1], N)
goalNode = prm.Node(goal[0] + goalDisp[0], goal[1] + goalDisp[1],
N + 1)
frontNode = prm.Node(front[0], front[1], N)
enemy = prm.Obstacle(enemy[0], enemy[1], enemyRadius)
ballNode = prm.Obstacle(ball[0], ball[1], ballRadius)
#Find path
x, y = prm.pathPlan(graph,
startNode,
goalNode,
enemy,
ballNode,
k,
avoidBall=False,
draw=True,
w=640,
h=480,
frame=frame)
next = prm.Node(x, y, 0)
# Define points
# turtlebot
back = cs[WHITE]
front = cs[RED]
goal = cs[GREEN]
if np.isnan(goal[0]) or np.isnan(goal[1]):
# Can't see ball
goal = [W / 2, H / 2]
# Set nodes
startNode = prm.Node(back[0], back[1], N)
goalNode = prm.Node(goal[0], goal[1], N + 1)
frontNode = prm.Node(front[0], front[1], N)
# obstacle
enemy = prm.Obstacle(-10, -10, 1)
# first goal
ball = prm.Obstacle(-10, -10, 1)
#Find path
x, y = prm.pathPlan(graph,
startNode,
goalNode,
enemy,
ball,
k,
avoidBall=False,
draw=False)
next = prm.Node(x, y, 0)
# prev_angle = angle
