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)