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()
