def DoNavigateState(current_state):
done = False
marker_distance = 3 #init = 5 #m
marker_angle = 0 # radians
face_distance = 0
face_angle = 0
min_distance = 0.2
#the navigate state contains its own loop.
# Would be better to define substates and put the whole thing in separate file (see main loop)
# but here its just the simple while not done loop, with some if statements
while not done:
#sonar
[sl, sr] = nao.ReadSonar()
#landmark detect
landmark_detected, timestamp, markerinfo = nao.DetectLandmark()
if landmark_detected:
[marker_distance, marker_angle] = compute_markerpos(markerinfo)
print('marker_distance')
nao.say("Landmark has been detected!")
if marker_distance < min_distance: # the target is reached
nao.say('We found the exit!')
current_state = 'exit'
done = True
else:
# if no marker detected, do something else
pass
#facedetect
face_detected, timestamp, [face_x, face_y] = nao.DetectFace()
if face_detected:
[face_distance, face_angle] = compute_facepos(face_x, face_y)
nao.say('I see you!')
#face detected, now what?
pass
else:
# if no face detected, do something else
pass
#compute turnrate and velocity
velocity = bn.compute_velocity(
sl, sr) #could be anything, but here: slow down for obstacles
turn_rate = bn.compute_turnrate(
marker_distance,
marker_angle,
sl,
sr,
) #turn towards target and the landmark is the target
# finally, update the robot's speed and turnrate
nao.Move(velocity, 0, turn_rate)
nao.sleep(0.05) # maximum update speed of the robot is 20 Hz or so.
nao.Move(0.0, 0.0,
0.0) # make sure the robot is not moving when exiting this state!
return current_state
def testwalking():
print "Test basic walking ... ",
try:
nao.InitPose()
nao.Move(1, 0, 0.2)
time.sleep(3)
nao.Move(0, 0, 0)
nao.Walk(-0.3, 0, -0.2)
nao.Crouch()
succeeded = True
print "succeeded."
except:
succeeded = False
print "failed."
return succeeded
def MoveRobot(forward_distance, sideward_distance, angle):
# Keeps taking steps until a new Move command is given
#
# Nao can move forward, sideward and turn simultaneously in one step
# forward_distance = Nao forward distance of one step in meter
# sideward_distance = Nao sideward distance of one step in meter
# angle = Nao turn angle of one step in degrees
#
# StandUp() needs to be run once before this command works
frequency = 10 # nr of steps per second
nao.Move(forward_distance, sideward_distance, angle, frequency)
def search(landmark):
turnAngle=0
direction=[0.3,0.4,-0.4,-0.3,-0.3,-0.4,0.4,0.3]
n=0
psi=0
find,ID=findTar(landmark)
nao.Move(0, 0.0, 0, 1)
while k<100 and not find:
for i in direction:
nao.MoveHead(i,0,False,True,timeLists=[[1], [1]])
time.sleep(0.3)
find,ID=findTar(landmark)
print "Finding Landmark"
time.sleep(0.3)
if find:
break
if find:
break
else:
print"Searching for landmark"
nao.Move(0.2, 0.0, -0.5, 1)
nao.Move(0.2, 0.0, -0.5, 1)
nao.Move(0.2, 0.0, -0.5, 1)
nao.Move(0.2, 0.0, -0.5, 1)
nao.sleep(0.3)
nao.Move(0, 0.0, 0, 1)
print"Finding landmark"
find,ID=findTar(landmark)
time.sleep(0.3)
if find:
break
k=k+1
if k==10:
reset=True
time.sleep(0.5)
break
psi=nao.GetYaw()
turnAngle=psi[0]+ID[0][1]
steps=round(turnAngle/0.3,0)
d=0
if turnAngle >0:
d=1
if turnAngle<0:
d=-1
return turnAngle,reset, d, steps
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
import math
import numpy
import naoqi
import motion_planning as mp
import behaviour_based_navigation_empty.py as bbn
robotIP = "192.168.0.115"
nao.InitProxy(robotIP)
nao.InitLandMark()
nao.InitSonar()
global obstacle
obstacle = False
nao.Move(0.5, 0.0, 0.0, 1.0)
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
isAbsolute = False
while (exitwhile == 0):
# Ask for input
if msvcrt.kbhit():
s = msvcrt.getch()
else:
time.sleep(0.001) # wait for 1 ms
s = None
continue
if s == "w":
dx = 1
dy = 0
dtheta = 0
nao.Move(dx, dy, dtheta, frequency)
elif s == "a":
dtheta = 0.1
dx = 0.5
dy = 0
nao.Move(dx, dy, dtheta, frequency)
elif s == "d":
dtheta = -0.1
dx = 0.5
dy = 0
nao.Move(dx, dy, dtheta, frequency)
elif s == "q":
dtheta = 0.4
dx = 0
dy = 0
nao.Move(dx, dy, dtheta, frequency)
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
else:
# stop the robot if you dont see a landmark after some time!
pass
time.sleep(
0.1
) # you should not send too many commands to the robot, so wait 0.1s
print dtheta, dt, timestamp, oldtimestamp
nao.Move(0.0, 0, 0, 0)
nao.Crouch()