def addMap(self, newMapData):
assert isinstance(newMapData, MapData)
self.mapData = newMapData
# Pack the map data into a list of Point objects, since that's
# what the perfesser wants for input. This will make the grid
# into a histogram with which to filter other input points.
self.mapPointList = []
for xi in range(self.mapData.mapArray.shape[0]):
for yi in range(self.mapData.mapArray.shape[1]):
if self.mapData.mapArray[xi,yi] < 0.5:
p = Pt()
p.point = (self.mapData.orig[0] + xi * self.mapData.ints[0],
self.mapData.orig[1] + yi * self.mapData.ints[1],
0.0)
self.mapPointList.append(p)
def addMap(self, newMapData):
assert isinstance(newMapData, MapData)
self.mapData = newMapData
# Pack the map data into a list of Point objects, since that's
# what the perfesser wants for input. This will make the grid
# into a histogram with which to filter other input points.
self.mapPointList = []
for xi in range(self.mapData.mapArray.shape[0]):
for yi in range(self.mapData.mapArray.shape[1]):
if self.mapData.mapArray[xi, yi] < 0.5:
p = Pt()
p.point = (self.mapData.orig[0] +
xi * self.mapData.ints[0],
self.mapData.orig[1] +
yi * self.mapData.ints[1], 0.0)
self.mapPointList.append(p)
def testUpdate(self):
# Create a uniform distribution
rospy.init_node('perfesser')
pts = Belief()
for i in range(1000):
x = np.random.random()
y = np.random.random()
th = np.random.random()
p = Pt()
p.point = [x, y, th]
pts.points.append(p)
xmxflag = False ; ymxflag = False ; thmxflag = False
xmnflag = False ; ymnflag = False ; thmnflag = False
ntrials = 5 ; thres = 0.01
for i in range(ntrials):
self.g.update(pts)
mx = map(max, self.g.pointArray.transpose())
mn = map(min, self.g.pointArray.transpose())
xmxflag = xmxflag or ((math.fabs(mx[0]) - 1.0) < thres)
ymxflag = ymxflag or ((math.fabs(mx[0]) - 1.0) < thres)
thmxflag = thmxflag or ((math.fabs(mx[0]) - 1.0) < thres)
xmnflag = xmnflag or (math.fabs(mn[0]) < thres)
ymnflag = ymnflag or (math.fabs(mn[0]) < thres)
thmnflag = thmnflag or (math.fabs(mn[0]) < thres)
self.setUp()
self.assertTrue(self.pub.testObject)
self.assertTrue(xmxflag)
self.assertTrue(ymxflag)
self.assertTrue(thmxflag)
self.assertTrue(xmnflag)
self.assertTrue(ymnflag)
self.assertTrue(thmnflag)
