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 InitNao():
# Check IP: Choregraphe v.1, connect (na kabel connect van de robot)
IP = "127.0.0.1" # IP address of the (Webots) simulator
#IP = "10.1.17.75" # IP address of the real robot ..... Eddy van De Kempel
#IP = "169.254.96.232" # IP address of the real robot ..... Thomas 21 mrt 2017
nao.InitProxy(IP, ) # Initialize motion proxy
nao.InitSonar() # Initialize front-left and front-right sonar
def DoInitState():
robotIP = "192.168.0.117"
nao.InitProxy(robotIP)
nao.InitSonar()
nao.InitLandMark()
nao.InitTrack()
nao.InitPose()
return "navigate"
def DoInitState():
nao_ip = "192.168.0.117"
nao.InitProxy(nao_ip)
nao.InitSonar()
nao.InitLandMark()
nao.InitTrack()
nao.InitPose()
return "navigate"
from naoqi import ALProxy
import nao_nocv_2_0 as nao
import time
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()