def testcamera(maxcount=10):
max_resolution = nao.resolution.very_high
nao.InitLandMark()
for i in range(max_resolution + 1):
#face detection
t = time.time()
nao.InitVideo(i)
count = 0
while count < maxcount:
faceposition, detected = nao.ALFacePosition()
print faceposition, detected
count = count + 1
print "Face detection at ", nao.resolution.resolutionar[
i], " complete at ", time.time() - t
#Landmark detection
t = time.time()
count = 0
while count < maxcount:
detected, timestamp, markerinfo = nao.DetectLandMark()
print detected, timestamp, markerinfo
count = count + 1
print "Landmark detection at ", nao.resolution.resolutionar[
i], " complete at ", time.time() - t
def findTar(landmark):
print"Landmark detected"
detected, timestamp, ID = nao.DetectLandMark()
if detected==True:
if ID[0][0]==landmark:
return True,ID
else:
return False,ID
else:
return False,ID
def testlandmark(maxcount=10):
#Landmark detection
nao.InitLandMark()
for i in range(max_resolution + 1):
t = time.time()
count = 0
while count < maxcount:
detected, timestamp, markerinfo = nao.DetectLandMark()
print detected, timestamp, markerinfo
count = count + 1
print "Landmark detection at ", nao.camera_resolution.resolutionar[
i], " complete at ", time.time() - t
def state(targetNum, step):
Bel = 1
oldtimestamp = 0
oldtime = time.time()
pivotRate = 0
dt = 0
spoken = False
while not step:
[SL, SR] = nao.ReadSonar()
if SL == 0.0 and SR == 0.0:
SL = 999
SR = 999
SL, Bel = bbn.Kalman(SL, Bel)
SR, Bel = bbn.Kalman(SR, Bel)
velocity, obstacle, right, step = mp.sensor(SL, SR)
if step == True:
break
m = 0
while m < 5:
detected, timestamp, ID = nao.DetectLandMark()
time.sleep(0.2)
print "detected", detected
if detected:
break
m = m + 1
# compute the time interval
dt = time.time() - oldtime
print "dt ", dt
if detected:
if dt > 1:
dt = 1
if ID[0][0] == targetNum:
oldtime = dt + oldtime
if not spoken:
nao.Say("land mark detected")
time.sleep(0.5)
print "land mark detected", timestamp, ID
spoken = True
if ID[0][3] == 0:
pass
else:
w = 0.1
d = w / atan(ID[0][3])
print "distance = ", d
# approaching target
if d < 0.6:
step = True
break
pivotRate = dt * ID[0][1]
if obstacle == True:
obavoid = mp.fobs([SL, SR])
time.sleep(0.5)
pivotRate = dt * obavoid
nao.Say("Avoiding obstacle")
print "Avoiding obstacle"
pivotRate = mp.turnDir(right, pivotRate)
pivotRate = mp.pivRate(pivotRate)
nao.Move(velocity, 0, pivotRate, 1)
if dt < 0.5:
time.sleep(0.5 - dt)
else:
if not spoken:
nao.Say("Landmark" + str(ID[0][0]) + "detected.")
print "Incorrect Marker ID"
else:
if dt > 1.5:
dt = 1
oldtime = dt + oldtime
if obstacle == False:
turnAngle, steps, avoidDirection, gotohelp = mp.search(
targetNum)
time.sleep(0.5)
if gotohelp == True:
nao.Crouch()
time.sleep(20)
else:
j = 0
while j <= steps:
nao.Walk(0, 0, avoidDirection * 0.3, False)
time.sleep(0.5)
j = j + 1
mp.headReset()
else:
obavoid = mp.fobs([SL, SR])
time.sleep(0.5)
pivotRate = dt * obavoid
nao.Say("Avoiding obstacle")
print "Avoiding obstacle"
time.sleep(0.5)
pivotRate = mp.turnDir(right, pivotRate)
pivotRate = mp.pivRate(pivotRate)
nao.Move(velocity, 0, pivotRate, 1)
if dt < 0.5:
time.sleep(0.5 - dt)
else:
pass
print pivotRate, timestamp, oldtimestamp
return step
#### Sonar ####
[SL, SR] = nao.ReadSonar()
#### put code to deal with sonar data here
# stop if too close to obstacle
if SL < 0.5 and SR < 0.5:
print SL, SR
#do something here
break
elif SL < 0.2 or SR < 0.2:
print SL, SR
#do something here
break
#### detect landmark ####
detected, timestamp, markerinfo = nao.DetectLandMark()
if detected:
# each marker has a number, you can use this to identify it
#if markerinfo[0][0]==80:
print SL, SR, markerinfo[0][1]
#compute the turnrate
dtheta = dt * markerinfo[0][1] # replace this with decent formula!
# limit the turnrate
if dtheta > 0.2:
dtheta = 0.2
elif dtheta < -0.2:
dtheta = -0.2
# move the robot
nao.Move(1.0, 0, dtheta, 1) # head to the target