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 testsonar(maxcount=10):
nao.InitSonar()
count = 0
while count < maxcount:
[sl, sr] = nao.ReadSonar()
print sl, sr
count = count + 1
nao.InitSonar(False)
def testsonar(maxcount=10):
try:
print "Testing sonar ..."
nao.InitSonar()
count = 0
while count < maxcount:
[sl, sr] = nao.ReadSonar()
print sl, sr
count = count + 1
nao.InitSonar(False)
print "success."
return True
except:
print "failed."
return False
def RunCommand(cmd):
# decide the action to be taken on input cmd
if cmd == 'su':
print('Standing up ...')
StandUp()
elif cmd == 'cr':
print('Crouching ...')
Crouch()
elif cmd == "loop()":
print('Moving forward ...')
MoveForward()
elif cmd == "mb":
print('Moving backward ...')
MoveBackward()
elif cmd == "ml":
print('Moving left ...')
MoveLeft()
elif cmd == "mr":
print('Moving right ...')
MoveRight()
elif cmd == "tl":
print('Turning left ...')
TurnLeft()
elif cmd == "tr":
print('Turning right ...')
TurnRight()
elif cmd == "sr":
print('Stopping robot ...')
StopRobot()
elif cmd == 'hl':
print('Turning head left ...')
TurnHeadLeft()
elif cmd == 'hr':
print('Turning head right ...')
TurnHeadRight()
elif cmd == 'hu':
print('Turning head up ...')
TurnHeadUp()
elif cmd == 'hd':
print('Turning head down ...')
TurnHeadDown()
elif cmd == 'rg':
gesture_name = raw_input("Enter file name of gesture: ")
nao.RunMovement(gesture_name)
elif cmd == 'gl':
print('Available Gestures: ')
ShowAllGestures()
elif cmd == "rs":
print('Read Sonar: ')
distance = nao.ReadSonar()
print(distance[0] + distance[1]) / 2.0
elif cmd == "doegek": # toegevoegd
print('Doe iets geks ...') # toegevoegd
doeIetsGeks() # toegevoegd
elif cmd == '?':
print("Show help")
ShowCommands()
else:
print("unknown remote control command '" + cmd + "'")
ShowCommands()
def ReadSonar():
# ReadSonar: returns averages of left and right sonar
return nao.ReadSonar()
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
nao.InitProxy(IP)
nao.InitPose()
nao.InitLandMark()
nao.InitSonar()
dt = 0.1 # this value is probably wrong ...
count = 0
maxcount = 2000 # make sure the loop ends
oldtimestamp = 0
oldtime = 0
dtheta = 0
while count < maxcount:
count = count + 1
#### 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: