-
Notifications
You must be signed in to change notification settings - Fork 5
/
target.py
155 lines (112 loc) · 3.74 KB
/
target.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
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()
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