class MapGuesser(object): def __init__(self, name, nPoints, nBins, announce=False): self.name = name self.nBins = nBins self.nPoints = nPoints self.announce = announce self.mapData = MapData(np.array([[0.0,0.0]]), (0.0,0.0), (1.0,1.0)) self.guesser = Guesser(self.name, (0.0, 0.0, 2*math.pi), "location", (self.nBins[0],self.nBins[1],1)) 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 handle_guess(self, greq): """ Takes an input guess (a list of Point objects) and throws out the points that seem impossible. Then resamples to return a list of equally probable points. Most of this functionality is provided via the perfesser's guesser methods. """ # Check input types assert isinstance(greq, GuessRequest) assert isinstance(greq.inPoints, list) assert isinstance(greq.inPoints[0], Pt) self.guesser.newPoints(self.mapPointList) pts = Belief() pts.points = greq.inPoints self.guesser.update(pts) # Return gresp = GuessResponse(sender = self.name, source_stamp = rospy.Time.now(), source_data = "", outPoints = self.guesser.outPoints(), no_data = False) return gresp
class MapGuesser(object): def __init__(self, name, nPoints, nBins, announce=False): self.name = name self.nBins = nBins self.nPoints = nPoints self.announce = announce self.mapData = MapData(np.array([[0.0, 0.0]]), (0.0, 0.0), (1.0, 1.0)) self.guesser = Guesser(self.name, (0.0, 0.0, 2 * math.pi), "location", (self.nBins[0], self.nBins[1], 1)) 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 handle_guess(self, greq): """ Takes an input guess (a list of Point objects) and throws out the points that seem impossible. Then resamples to return a list of equally probable points. Most of this functionality is provided via the perfesser's guesser methods. """ # Check input types assert isinstance(greq, GuessRequest) assert isinstance(greq.inPoints, list) assert isinstance(greq.inPoints[0], Pt) self.guesser.newPoints(self.mapPointList) pts = Belief() pts.points = greq.inPoints self.guesser.update(pts) # Return gresp = GuessResponse(sender=self.name, source_stamp=rospy.Time.now(), source_data="", outPoints=self.guesser.outPoints(), no_data=False) return gresp
def testUpdate(self): # Create a uniform random field g = Guesser("map_filter_test", (0.0, 0.0, 2*math.pi), "location", (10,10,1)) g.uniform(self.mg.nPoints, [[-5.0,5.0],[-5.0,5.0],[0.0,3.0]]) # Use it as a guess against the filter already in mg gr = GuessRequest(inPoints=g.outPoints(), means=g.means(), stds=g.stds(), data_type=g.data_type, pers=g.periods) gresp = self.mg.handle_guess(gr) # There should be no points left where the mapData array was 1.0 xmax = False ; ymin = False ; xyint = False xvec = [ p.point[0] for p in gresp.outPoints ] yvec = [ p.point[1] for p in gresp.outPoints ] print ">>>", max(xvec), min(xvec) print ">>>", max(yvec), min(yvec) mx = map(max, self.mg.guesser.pointArray.transpose()) print ">>>>", mx self.assertTrue(mx[0] < 2.5 and mx[1] < 2.5) mn = map(min, self.mg.guesser.pointArray.transpose()) self.assertTrue(mn[0] > -3.5 and mn[1] > -3.5) cornerTest = False npts = self.mg.guesser.pointArray.shape[0] for i in range(npts): if self.mg.guesser.pointArray[i][0] > 0.25 and \ self.mg.guesser.pointArray[i][1] < -0.5: cornerTest = True self.assertFalse(cornerTest)
def testUpdate(self): # Create a uniform random field g = Guesser("map_filter_test", (0.0, 0.0, 2 * math.pi), "location", (10, 10, 1)) g.uniform(self.mg.nPoints, [[-5.0, 5.0], [-5.0, 5.0], [0.0, 3.0]]) # Use it as a guess against the filter already in mg gr = GuessRequest(inPoints=g.outPoints(), means=g.means(), stds=g.stds(), data_type=g.data_type, pers=g.periods) gresp = self.mg.handle_guess(gr) # There should be no points left where the mapData array was 1.0 xmax = False ymin = False xyint = False xvec = [p.point[0] for p in gresp.outPoints] yvec = [p.point[1] for p in gresp.outPoints] print ">>>", max(xvec), min(xvec) print ">>>", max(yvec), min(yvec) mx = map(max, self.mg.guesser.pointArray.transpose()) print ">>>>", mx self.assertTrue(mx[0] < 2.5 and mx[1] < 2.5) mn = map(min, self.mg.guesser.pointArray.transpose()) self.assertTrue(mn[0] > -3.5 and mn[1] > -3.5) cornerTest = False npts = self.mg.guesser.pointArray.shape[0] for i in range(npts): if self.mg.guesser.pointArray[i][0] > 0.25 and \ self.mg.guesser.pointArray[i][1] < -0.5: cornerTest = True self.assertFalse(cornerTest)
class BumpGuesser(object): def __init__(self, name, nPoints, nBins, wallError, announce=False): self.name = name self.nBins = nBins self.nPoints = nPoints # wallError is the probability that a bump is from a feature that # is on the map. You can bump into moving objects, too. self.wallError = wallError self.announce = announce self.mapData = OccupancyGrid() self.guesser = Guesser(self.name, self.nPoints, (0.0, 0.0, 2*math.pi), "location", HistogramData((self.nBins[0],self.nBins[1],1))) self.ready_to_publish = False self.stamp = rospy.Time.now() def feltBump(self): """ Use this to signal that we've felt a bump and will shortly be contacted about using it for a guess. """ self.stamp = rospy.Time.now() self.ready_to_publish = True def occupiedNeighbor(self, xi, yi): """ Returns True if one of the immediate neighbor cells is occupied. You shouldn't expect good results if you call this on an occupied cell. """ xmax = self.mapData.og.info.width ymax = self.mapData.og.info.height if self.mapData.sampled: # Fails on an occupied cell assert self.mapData.mapArrayS[xi, yi] < 50 for x in range(max(xi - 1, 0), min(xi + 1, xmax)): for y in range(max(yi - 1, 0), min(yi + 1, ymax)): if self.mapData.mapArrayS[x,y] > 50: return True return False else: # Fails on an occupied cell assert self.mapData.mapArray[xi, yi] < 50 for x in range(max(xi - 1, 0), min(xi + 1, xmax)): for y in range(max(yi - 1, 0), min(yi + 1, ymax)): if self.mapData.mapArray[x,y] > 50: return True return False def addMap(self, newMapData): """ Receives a new map. """ assert isinstance(newMapData, MapData) self.mapData = newMapData def handle_guess(self, newPointList): """ Takes an input guess (a list of Point objects) and filters it against a map of points that seem probable because of a recent bump. Then resamples per usual to return a list of equally probable points. """ if not self.ready_to_publish: return False assert isinstance(newPointList, list) assert isinstance(newPointList[0], Point) # Find the limits of the input data. xmax = -1.0e6 ; ymax = -1.0e6 xmin = 1.0e6 ; ymin = 1.0e6 for pt in newPointList: xmax = max(xmax, pt.point[0]) ymax = max(ymax, pt.point[1]) xmin = min(xmin, pt.point[0]) ymin = min(ymin, pt.point[1]) # Shrink the map to accommodate the relevant area self.mapData.sample((xmin,ymin), (xmax,ymax)) # Cruise through the map looking for empty cells next to occupied # ones. These will be the likely cells when a bump is encountered. # # Because of the possibility of bumping an object that isn't on the # map, any empty map cell is possible. Therefore, we run through # the map, packing the map data into a list of Point objects, since # that's what the perfesser wants for input. While we're running # through, we keep a separate list of empty cells next to full ones. wallPointList = [] emptyPointList = [] for xi in range(self.mapData.ogS.info.width): for yi in range(self.mapData.ogS.info.height): if self.mapData.mapArrayS[xi,yi] < 50: p = Point() p.point = self.mapData.transform((xi, yi)) emptyPointList.append(p) if (self.occupiedNeighbor(xi, yi)): newP = Point() newP.point = (p.point[0] + np.random.normal(0.0, self.mapData.ogS.info.resolution/3.0), p.point[1] + np.random.normal(0.0, self.mapData.ogS.info.resolution/3.0), p.point[2]) wallPointList.append(newP) # Using the wallError, sample the two lists together to get a roughly # correct distribution of points to feed to the perfesser. self.mapPointList = [] for i in range(self.nPoints): if i < self.wallError * self.nPoints: self.mapPointList.append(random.choice(wallPointList)) else: self.mapPointList.append(random.choice(emptyPointList)) self.guesser.newPoints(self.mapPointList) pts = Belief() pts.points = newPointList self.guesser.update(pts) self.pointList = self.guesser.outPoints() self.ready_to_publish = False return True
mapArray[xi,yi] = 100 if yi < 3 or yi > 15: mapArray[xi,yi] = 100 if yi < 10 and xi > 10: mapArray[xi,yi] = 75 mapArray.shape = 576 og.data = list(mapArray) mapData = MapData(og) bg.addMap(mapData) import tgraph tg = tgraph.Tgraph(350, 350) g = Guesser("map_filter_test", bg.nPoints, (0.0, 0.0, 2*math.pi), "location", HistogramData((10,10,1))) g.uniform([[-5.0,5.0],[-5.0,5.0],[0.0,3.0]]) bg.handle_guess(g.outPoints()) plist = [] for pt in bg.mapPointList: plist.append(pt.point) tg.draw_scatter(np.array([[-5.0, -5.0, 0.0], [5.0, 5.0, 2.0]]), 0,1,2,"s") tg.plot_points(np.array(plist), 0,1,2, "s") tg.mainloop()
class BumpGuesser(object): def __init__(self, name, nPoints, nBins, wallError, announce=False): self.name = name self.nBins = nBins self.nPoints = nPoints # wallError is the probability that a bump is from a feature that # is on the map. You can bump into moving objects, too. self.wallError = wallError self.announce = announce self.mapData = OccupancyGrid() self.guesser = Guesser( self.name, self.nPoints, (0.0, 0.0, 2 * math.pi), "location", HistogramData((self.nBins[0], self.nBins[1], 1)), ) self.ready_to_publish = False self.stamp = rospy.Time.now() def feltBump(self): """ Use this to signal that we've felt a bump and will shortly be contacted about using it for a guess. """ self.stamp = rospy.Time.now() self.ready_to_publish = True def occupiedNeighbor(self, xi, yi): """ Returns True if one of the immediate neighbor cells is occupied. You shouldn't expect good results if you call this on an occupied cell. """ xmax = self.mapData.og.info.width ymax = self.mapData.og.info.height if self.mapData.sampled: # Fails on an occupied cell assert self.mapData.mapArrayS[xi, yi] < 50 for x in range(max(xi - 1, 0), min(xi + 1, xmax)): for y in range(max(yi - 1, 0), min(yi + 1, ymax)): if self.mapData.mapArrayS[x, y] > 50: return True return False else: # Fails on an occupied cell assert self.mapData.mapArray[xi, yi] < 50 for x in range(max(xi - 1, 0), min(xi + 1, xmax)): for y in range(max(yi - 1, 0), min(yi + 1, ymax)): if self.mapData.mapArray[x, y] > 50: return True return False def addMap(self, newMapData): """ Receives a new map. """ assert isinstance(newMapData, MapData) self.mapData = newMapData def handle_guess(self, newPointList): """ Takes an input guess (a list of Point objects) and filters it against a map of points that seem probable because of a recent bump. Then resamples per usual to return a list of equally probable points. """ if not self.ready_to_publish: return False assert isinstance(newPointList, list) assert isinstance(newPointList[0], Point) # Find the limits of the input data. xmax = -1.0e6 ymax = -1.0e6 xmin = 1.0e6 ymin = 1.0e6 for pt in newPointList: xmax = max(xmax, pt.point[0]) ymax = max(ymax, pt.point[1]) xmin = min(xmin, pt.point[0]) ymin = min(ymin, pt.point[1]) # Shrink the map to accommodate the relevant area self.mapData.sample((xmin, ymin), (xmax, ymax)) # Cruise through the map looking for empty cells next to occupied # ones. These will be the likely cells when a bump is encountered. # # Because of the possibility of bumping an object that isn't on the # map, any empty map cell is possible. Therefore, we run through # the map, packing the map data into a list of Point objects, since # that's what the perfesser wants for input. While we're running # through, we keep a separate list of empty cells next to full ones. wallPointList = [] emptyPointList = [] for xi in range(self.mapData.ogS.info.width): for yi in range(self.mapData.ogS.info.height): if self.mapData.mapArrayS[xi, yi] < 50: p = Point() p.point = self.mapData.transform((xi, yi)) emptyPointList.append(p) if self.occupiedNeighbor(xi, yi): newP = Point() newP.point = ( p.point[0] + np.random.normal(0.0, self.mapData.ogS.info.resolution / 3.0), p.point[1] + np.random.normal(0.0, self.mapData.ogS.info.resolution / 3.0), p.point[2], ) wallPointList.append(newP) # Using the wallError, sample the two lists together to get a roughly # correct distribution of points to feed to the perfesser. self.mapPointList = [] for i in range(self.nPoints): if i < self.wallError * self.nPoints: self.mapPointList.append(random.choice(wallPointList)) else: self.mapPointList.append(random.choice(emptyPointList)) self.guesser.newPoints(self.mapPointList) pts = Belief() pts.points = newPointList self.guesser.update(pts) self.pointList = self.guesser.outPoints() self.ready_to_publish = False return True
if xi < 3 or xi > 15: mapArray[xi, yi] = 100 if yi < 3 or yi > 15: mapArray[xi, yi] = 100 if yi < 10 and xi > 10: mapArray[xi, yi] = 75 mapArray.shape = 576 og.data = list(mapArray) mapData = MapData(og) bg.addMap(mapData) import tgraph tg = tgraph.Tgraph(350, 350) g = Guesser("map_filter_test", bg.nPoints, (0.0, 0.0, 2 * math.pi), "location", HistogramData((10, 10, 1))) g.uniform([[-5.0, 5.0], [-5.0, 5.0], [0.0, 3.0]]) bg.handle_guess(g.outPoints()) plist = [] for pt in bg.mapPointList: plist.append(pt.point) tg.draw_scatter(np.array([[-5.0, -5.0, 0.0], [5.0, 5.0, 2.0]]), 0, 1, 2, "s") tg.plot_points(np.array(plist), 0, 1, 2, "s") tg.mainloop()
tg.plot_points(g.pointArray, 0, 1, 2, "s") o.updateOdom(o1) o.updateOdom(o2) o.updateOdom(o3) o.updateOdom(o4) s = o1.header.stamp s.nsecs += 5000000 greq = GuessRequest() greq.source_stamp = s greq.header.stamp = rospy.Time.now() greq.pers = (0.0, 0.0, 2*math.pi) greq.means = g.means() greq.stds = g.stds() greq.inPoints = g.outPoints() gresp = o.updateBelief(greq) pts = Belief(points = gresp.outPoints) tg.new_graph() pta = g.pointsToArray(pts) print "maxs:", map(max, pta.transpose()) print "mins:", map(min, pta.transpose()) tg.draw_scatter(g.pointsToArray(pts), 0, 1, 2, "s", recalc=False) tg.mainloop()
inarray = np.array([[-5.5, -5.5, 0.0], [5.5, 5.5, 0.6]]) tg.draw_scatter(inarray, 0, 1, 2, "s") parray = np.array([p.point for p in mg.mapPointList]) tg.plot_points(parray, 0, 1, 2, "s") g = Guesser("map_filter_test", (0.0, 0.0, 2 * math.pi), "location", (10, 10, 1)) g.uniform(mg.nPoints, [[-5.0, 5.0], [-5.0, 5.0], [0.0, 3.0]]) tg.new_graph() tg.draw_scatter(g.pointArray, 0, 1, 2, "s", recalc=False) gr = GuessRequest(inPoints=g.outPoints(), means=g.means(), stds=g.stds(), data_type=g.data_type, pers=g.periods) gresp = mg.handle_guess(gr) tg.plot_points(mg.guesser.hist.plottable(), 0, 1, 2, "c") tg.new_graph() parray = np.array([p.point for p in gresp.outPoints]) tg.draw_scatter(parray, 0, 1, 2, "s", recalc=False) tg.mainloop()
inarray = np.array([[-5.5,-5.5,0.0],[5.5,5.5,0.6]]) tg.draw_scatter(inarray, 0,1,2, "s") parray = np.array([p.point for p in mg.mapPointList]) tg.plot_points(parray, 0,1,2, "s") g = Guesser("map_filter_test", (0.0, 0.0, 2*math.pi), "location", (10,10,1)) g.uniform(mg.nPoints, [[-5.0,5.0],[-5.0,5.0],[0.0,3.0]]) tg.new_graph() tg.draw_scatter(g.pointArray, 0,1,2, "s", recalc=False) gr = GuessRequest(inPoints=g.outPoints(), means=g.means(), stds=g.stds(), data_type=g.data_type, pers=g.periods) gresp = mg.handle_guess(gr) tg.plot_points(mg.guesser.hist.plottable(), 0,1,2,"c") tg.new_graph() parray = np.array([p.point for p in gresp.outPoints]) tg.draw_scatter(parray, 0,1,2, "s", recalc=False) tg.mainloop()