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 getcam(self):
return self.cam
def getavg(self):
dm=self.cam.getDepthMatrix()
#Get depth and height
h,w = dm.shape #h=480, w=640
left = dm[:, 0:w/3] #dm[:, 0:w/2]
center = dm[:, w/3: 2*w/3]#dm[:, w/4:3*w/4]
right = dm[:, 2*w/3:]#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])
#return index of side to walk on; idxmap[index] equals direction to walk
def walk(self,n=5):
newidx = -1
smat = self.meanvect.getn(n=n) #sample matrix
median = np.median(smat, axis=0)
#print "median is {}".format(median)
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
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 getcam(self):
return self.cam
def getavg(self):
dm = self.cam.getDepthMatrix()
#Get depth and height
h, w = dm.shape #h=480, w=640
left = dm[:, 0:w / 3] #dm[:, 0:w/2]
center = dm[:, w / 3:2 * w / 3] #dm[:, w/4:3*w/4]
right = dm[:, 2 * w / 3:] #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])
#return index of side to walk on; idxmap[index] equals direction to walk
def walk(self, n=5):
newidx = -1
smat = self.meanvect.getn(n=n) #sample matrix
median = np.median(smat, axis=0)
#print "median is {}".format(median)
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
class ListTests(unittest.TestCase):
def setUp(self):
self._list = CList()
def test_push(self):
self.assertEqual(self._list.size, 0)
self._list.push(1)
self.assertEqual(self._list.size, 1)
def test_pop(self):
self.assertEqual(self._list.size, 0)
self.assertEqual(self._list.pop(), (0, False))
self._list.push(10)
self.assertEqual(self._list.size, 1)
self.assertEqual(self._list.pop(), (10, True))
self.assertEqual(self._list.size, 0)
