/
walk2.py
159 lines (131 loc) · 4.68 KB
/
walk2.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
155
156
157
158
159
import SimpleCV as scv
import numpy as np
import sys
import socket
from clist import CList
from speak import speak
from phoneImu import PhoneIMU
#import userinput2 as uinp
from userinput2 import UserInput
from directions2 import Directions
from getch import _Getch
import datetime
idxmap=["All", "Left", "Center", "Right", "Back"]
dirmap={"all":0, "left":1, "center":2, "right":3, "back":4}
class Walk():
def __init__(self, n=5):
self.cam = scv.Kinect()
#initialize meanvect
self.meanvect = CList(n)
for i in range(n):
self.meanvect.push(self.getavg())
self.oldidx = -1 #old
def getavg(self):
dm=self.cam.getDepthMatrix()
#Get depth and height
h,w = dm.shape #h=480, w=640
left = dm[:, 0:w/2]
center = dm[:, w/4:3*w/4]
right = dm[:, w/2:]
leftMean = np.sum(left)/left.size
centerMean = np.sum(center)/center.size
rightMean = np.sum(right)/right.size
mean = np.sum(dm)/dm.size
return np.array([mean, leftMean, centerMean, rightMean])
def walk(self,n=5):
newidx = -1
smat = self.meanvect.getn(n=n) #sample matrix
median = np.median(smat, axis=0)
minidx = median.argmin()
#backup if against a wall
if median[dirmap["all"]] > 1900:
newidx=dirmap["back"]
#walk in center if walkable
elif median[dirmap["center"]] <= 1100 or minidx == dirmap["center"]:
newidx = dirmap["center"]
else: #pick left or right based on whichever is cleaner
newidx = minidx
#store new sample
mean=self.getavg()
self.meanvect.push(mean)
#print "{} {}".format(newidx, self.oldidx)
if newidx != self.oldidx:
self.oldidx=newidx
return newidx
else:
return -1
def getIPAddr():
return socket.gethostbyname(socket.getfqdn())
if __name__ == '__main__':
#sys.argv
wEn = True #walk
iEn = False #imu
uEn = False #user input
dEn = False #directions
getch = _Getch()
if wEn:
walk = Walk()
if iEn:
print getIPAddr()
ipaddr = getIPAddr() #of this machine
port = 5555
imu = PhoneIMU(ipaddr, port)
olddirc = None #direction reading
newdirc = None
if uEn:
ch = [None]
#uin = UserInput(ch)
#uin.start()
oldch = None #user input
newch = None
if dEn:
start = "40 St George St, Toronto, ON, M5S2E4"
end = "220 Yonge St, Toronto, ON M5B2H1"
directions = Directions(start, end)
steps = directions.getsteps()
lastcall = datetime.datetime.now()
while True:
if uEn:
newch = getch() #uin.get() #get input
print "newch is {}".format(newch)
if newch != oldch:
print "char is {}".format(newch)
oldch = newch
text = None
try:
#speak based on input
if newch == "s": #start location
text = "Start location is {}".format(start)
elif newch == "e": #end location
text = "End location is {}".format(end)
elif newch == "d": #direction (compass heading)
text = "You are facing {}".format(olddirc)
elif newch == "f": #next step
text = steps[0]
elif newch == "q":
pass
#TODO: handle exit logic
if text:
#print text
speak(text)
print "text is '{}'".format(text)
except Exception as e:
print e
if iEn:
newdirc = imu.getdirc() #get direction
if newdirc != olddirc:
print "Dir is {}".format(newdirc)
olddirc = newdirc
if wEn:
ret = walk.walk() #get walkable side
if ret != -1:
d = idxmap[ret]
nav = None
if d != "Center":
if ( lastcall - datetime.datetime.now() ).seconds >2:
nav = "Go {}".format(d)
else:
nav = "Go center"
if nav:
speak(nav)
lastcall = datetime.datetime.now()