def testUpdate(self):
# Create a uniform random field
g = Guesser(
"map_filter_test", self.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]])
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))
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 __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 __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 __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 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)
def __init__(self,
dims = (0.0,0.0,2*math.pi),
publisher = False,
maxSpeed = 0.15,
maxTurn = 0.3,
dampSpeed = 1.0,
dampTurn = 1.0,
kSpeed = 1.0,
kTurn = 1.0,
stdThres=100.0,
zeroDistance=0.3,
targetFn=False,
signalFn=False,
debug=False, dbgdir="~/"):
self.maxSpeed = maxSpeed
self.maxTurn = maxTurn
self.dampSpeed = dampSpeed
self.dampTurn = dampTurn
self.kSpeed = kSpeed
self.kTurn = kTurn
self.dims = dims
self.pub = publisher
self.targetFn = targetFn
self.signalFn = signalFn
self.debug = debug
self.dbgdir = dbgdir
self.count = 0
# These are for tracking the position
self.debugX = 0.0
self.debugY = 0.0
self.debugTh = 0.0
if self.debug:
rospy.Subscriber("position", Position, self.debug_track)
self.fl = force.ForceList()
self.gs = goal.GoalStack()
self.zeroDistance = zeroDistance
self.position = [ 0.0 ] * len(self.dims)
self.realPosition = [ 0.0 ] * len(self.dims)
self.stds = [ 0.0 ] * len(self.dims)
self.curstds = 1000.0
# The threshold is a number beyond which we can't reliably
# seek a goal, so the behavior has to change from seeking a
# goal to determining the orientation.
self.stdThres = stdThres
# We maintain the following so we can do derivative-proportional
# damping of the motion. (It's a PD controller.)
self.twist = Twist()
# Use this to signal that the doMove should stop
# working for a moment. (i.e. we have some motion emergency
# to deal with, like a bump)
self.isbusy = False
# These two time stamps are used to control the bump behavior.
self.bumpTimeOne = 0.0
self.bumpTimeTwo = 0.0
# Use these to remember where the current bump was.
self.bumpLeft = False
self.bumpRight = False
# A flag to indicate whether we know where we are to within the
# stdThres. We use the flag to do stuff that has to happen *when*
# we get lost, as opposed to stuff that has to happen *while* we
# are lost, which is indicated with self.lost().
self.amLost = False
# This histogram is used to turn a force into a speed and
# direction. We try to keep the same histogram from one
# invocation of set_move to the next in order to minimize the
# adjustments to the speed and direction of the robot.
self.guesser = Guesser("forces", (0.0, 2*math.pi), "force", (3,3))
self.guesser.makeHist()
self.guesser.hist.all()
class GuessNav(object):
def __init__(self,
dims = (0.0,0.0,2*math.pi),
publisher = False,
maxSpeed = 0.15,
maxTurn = 0.3,
dampSpeed = 1.0,
dampTurn = 1.0,
kSpeed = 1.0,
kTurn = 1.0,
stdThres=100.0,
zeroDistance=0.3,
targetFn=False,
signalFn=False,
debug=False, dbgdir="~/"):
self.maxSpeed = maxSpeed
self.maxTurn = maxTurn
self.dampSpeed = dampSpeed
self.dampTurn = dampTurn
self.kSpeed = kSpeed
self.kTurn = kTurn
self.dims = dims
self.pub = publisher
self.targetFn = targetFn
self.signalFn = signalFn
self.debug = debug
self.dbgdir = dbgdir
self.count = 0
# These are for tracking the position
self.debugX = 0.0
self.debugY = 0.0
self.debugTh = 0.0
if self.debug:
rospy.Subscriber("position", Position, self.debug_track)
self.fl = force.ForceList()
self.gs = goal.GoalStack()
self.zeroDistance = zeroDistance
self.position = [ 0.0 ] * len(self.dims)
self.realPosition = [ 0.0 ] * len(self.dims)
self.stds = [ 0.0 ] * len(self.dims)
self.curstds = 1000.0
# The threshold is a number beyond which we can't reliably
# seek a goal, so the behavior has to change from seeking a
# goal to determining the orientation.
self.stdThres = stdThres
# We maintain the following so we can do derivative-proportional
# damping of the motion. (It's a PD controller.)
self.twist = Twist()
# Use this to signal that the doMove should stop
# working for a moment. (i.e. we have some motion emergency
# to deal with, like a bump)
self.isbusy = False
# These two time stamps are used to control the bump behavior.
self.bumpTimeOne = 0.0
self.bumpTimeTwo = 0.0
# Use these to remember where the current bump was.
self.bumpLeft = False
self.bumpRight = False
# A flag to indicate whether we know where we are to within the
# stdThres. We use the flag to do stuff that has to happen *when*
# we get lost, as opposed to stuff that has to happen *while* we
# are lost, which is indicated with self.lost().
self.amLost = False
# This histogram is used to turn a force into a speed and
# direction. We try to keep the same histogram from one
# invocation of set_move to the next in order to minimize the
# adjustments to the speed and direction of the robot.
self.guesser = Guesser("forces", (0.0, 2*math.pi), "force", (3,3))
self.guesser.makeHist()
self.guesser.hist.all()
#print ">>>>>", self.guesser.hist.bins
def distance(self, pos_start, pos_end):
"""
Pythagorean distance between two points. Ignores periodic
dimensions.
"""
ds = map(lambda st, en, pd: 0 if pd else en - st,
pos_start, pos_end, self.dims)
return sum([ d**2 for d in ds ])**0.5
def achievedGoal(self):
"""
Checks to see if we're more or less at the goal on the top of
the stack.
"""
if self.gs.top():
dist = self.distance(self.position, self.gs.top().coords)
return dist < self.zeroDistance
else:
return False # No goals in stack.
def pushGoal(self, newGoal):
"""
Pushes a goal onto the top of the goal stack.
"""
# If there is a current top goal, remove the force corresponding to it
# from the force list.
if self.gs.top():
self.fl.removeForce(self.gs.top().goalPtr)
# Push the goal onto the stack.
index = self.gs.push(newGoal)
# Create a force for this goal.
goalForce = force.ForceAttract(newGoal.coords, debug=self.debug)
# Add the new force to the list of forces.
forceIndex = self.fl.addForce(goalForce)
# Record its position on the force list in the goal object.
self.gs.top().setPtr(forceIndex)
return index
def popGoal(self, index = 0):
"""
Remove a goal. Either we've gotten there or given up.
"""
if self.gs.top():
# Pop the goal from the stack
oldGoal = self.gs.pop(index)
# Remove the corresponding force from the force list.
self.fl.removeForce(oldGoal.goalPtr)
else:
return False
# If there's still a goal on the stack...
if self.gs.top():
# Create a force to get us to this new goal.
goalForce = force.ForceAttract(self.gs.top().coords, debug=self.debug)
# Add the new force to the force list.
forceIndex = self.fl.addForce(goalForce)
# Record its position on the force list in the goal object.
self.gs.top().setPtr(forceIndex)
return oldGoal
def updateGoal(self, index, newGoal):
"""
Change a goal -- and the resulting force if it has one.
(Which should imply that it's the current top goal on the
stack.)
"""
newGoal = self.gs.update(index, newGoal)
if newGoal.goalPtr:
return self.fl.updateForce(newGoal.goalPtr, newGoal.coords)
else:
return False
def doLostMove(self, belief):
"""
We don't know where we are. Executes a strategy to find a
landmark tag. Basically this just means going somewhere with
an arc, to maximize the chance of seeing a landmark.
"""
if not self.amLost:
self.signalFn(13)
self.twist.linear.x = 0.8 * self.maxSpeed
self.twist.angular.z = 0.2 * self.maxTurn
self.twist.angular.z *= 1.0 if np.random.random() > 0.5 else -1.0
self.amLost = True
return
def doLostBump(self, req):
"""
We're lost, meaning that our position belief isn't very
useful. (Variances > self.stdThres, see self.lost().)
There's no point in recording the obstacle position in this
case. All we need to do is pick another direction to go in,
rotate to it, and go.
"""
tnow = rospy.Time.now().to_sec()
#print "entering doLostBump:", self.bumpTimeOne, self.bumpTimeTwo, tnow
if self.bumpTimeTwo > 0.0:
if tnow > self.bumpTimeTwo:
self.bumpTimeTwo = 0.0
self.bumpLeft = False
self.bumpRight = False
if self.lost():
# Resume search, but try to edge away from the obstacle.
out = self.twist
out.angular.z *= -1.0 if self.bumpLeft else 1.0
else:
# Stop and await further instruction.
out = Twist()
else:
return
elif self.bumpTimeOne > 0.0:
if tnow > self.bumpTimeOne:
self.bumpTimeTwo = self.bumpTimeOne + 3.0 * np.random.random()
self.bumpTimeOne = 0.0
# Rotate some random amount
out = Twist()
out.linear.x = 0.0
out.angular.z = (-1.0 if self.bumpLeft else 1.0) * self.maxTurn
else:
return
else:
self.bumpTimeOne = rospy.Time.now().to_sec() + 2.0
self.bumpLeft = req.bumpLeft
self.bumpRight = req.bumpRight
out = Twist()
out.linear.x = -self.maxSpeed
direction = -1.0 if req.bumpLeft else 1.0
out.angular.z = direction * self.maxTurn
self.pub.publish(out)
return
def doBump(self, req):
"""
We're not lost, which means we have a goal in mind. So find the
current position, move back and to the side a little bit, and
try again. Also record the position of the obstacle and maybe
we can avoid bumping it again.
"""
tnow = rospy.Time.now().to_sec()
#print "entering doBump:", self.bumpTimeOne, self.bumpTimeTwo, tnow
if self.bumpTimeTwo > 0.0:
if tnow > self.bumpTimeTwo:
# Stop the robot, clear the decks.
self.bumpTimeTwo = 0.0
self.bumpLeft = False
self.bumpRight = False
out = Twist()
# Sing about it.
if self.signalFn:
self.signalFn(15)
else:
return
elif self.bumpTimeOne > 0.0:
if tnow > self.bumpTimeOne:
self.bumpTimeTwo = self.bumpTimeOne + 2.7
self.bumpTimeOne = 0.0
# Go straight back for a little bit.
out = Twist()
out.linear.x = self.maxSpeed
out.angular.z = -self.maxTurn if self.bumpLeft else self.maxTurn
else:
return
else:
# Infer the position of the obstacle from the position of the
# robot.
angleCorrection = self.position[2] + \
( 0.7 if req.bumpLeft else 0.0) + (-0.7 if req.bumpRight else 0.0)
self.addForce("R", (self.position[0] + .3 * math.cos(angleCorrection),
self.position[1] + .3 * math.sin(angleCorrection),
self.position[2]), duration=60.0)
#print "obstacle at: %.3f,%.3f,%.3f" % \
# (self.position[0] + .3 * math.cos(angleCorrection),
# self.position[1] + .3 * math.sin(angleCorrection),
# self.position[2])
# Sing about it
if self.signalFn:
self.signalFn(5)
# Create, and then publish a directive to back up.
self.bumpTimeOne = rospy.Time.now().to_sec() + 2.0
self.bumpLeft = req.bumpLeft
self.bumpRight = req.bumpRight
out = Twist()
out.linear.x = -self.maxSpeed
out.angular.z = - self.maxTurn if req.bumpLeft else self.maxTurn
self.pub.publish(out)
return
def perturbGoal(self, req):
"""
We have hit some object and need to do something about it.
The action we take depends on whether we're lost or not. If
we're lost, just rotate some random amount and go.
"""
if not self.isbusy:
if req.bumpLeft or req.bumpRight or \
self.bumpTimeOne > 0.0 or self.bumpTimeTwo > 0.0:
# Make sure we won't be interrupted.
self.isbusy = True
print "*******************************************OUCH****"
if self.lost():
self.doLostBump(req)
else:
self.doBump(req)
# Ok to be interrupted now.
self.isbusy = False
def addForce(self, forceType, nodePos, duration=3600.0):
if forceType == "A":
newForce = force.ForceAttract(nodePos, duration=duration)
return self.fl.addForce(newForce)
elif forceType == "R":
newForce = force.ForceRepel(nodePos, duration=duration)
return self.fl.addForce(newForce)
else:
return 0
def purgeForces(self):
return self.fl.purgeForces()
def removeForce(self, index):
return self.fl.removeForce(index)
def updateForce(self, index, newPos):
return self.fl.updateForce(index, newPos)
def lost(self):
"""
Returns True if the uncertainty in the current position is
higher than the threshold set at initialization.
"""
return (((self.stds[0] + self.stds[1])/2.0) > self.stdThres)
def setModelPosition(self, fixTime):
"""
Given a twist and a delta T, calculate how much the robot has moved.
Store the result so it can be added quickly with fixPosition().
"""
currentTime = rospy.Time.now().to_sec()
self.dT = currentTime - fixTime
self.modeldR = self.twist.linear.x * self.dT * 1.08
self.modeldTh = self.twist.angular.z * self.dT * 0.95
return
def modelPosition(self, point):
"""
Apply the correction calculated in setModelPosition().
"""
return (point[0] + self.modeldR * math.cos(point[2] + 0.5 * self.modeldTh),
point[1] + self.modeldR * math.sin(point[2] + 0.5 * self.modeldTh),
point[2] + 0.7 * self.modeldTh)
def doForceMove(self, belief):
"""
Runs through the points in the input belief and calculates the
'force' on each one. Uses the result to come up with a magnitude
and direction relative to the robot's position, then translates
this into a Twist message, which is published.
"""
# Update all the points in the belief with the model position
# adjustment. Then calculate the 'force' on each point.
forceList = []
for pt in belief.points:
modelPt = self.modelPosition(pt.point)
fX, fY = self.fl.sumForces(modelPt)
# Convert to polar coordinates.
fM = math.hypot(fX, fY)
fD = math.atan2(fY, fX)
# Convert to robot frame
fDR = fD - modelPt[2]
if math.fabs(fDR) > math.pi:
fDR -= math.copysign(2 * math.pi, fDR)
forceList.append(Pt(point = (fM, fDR)))
# We have a long collection of forces, one for each point in the
# belief. We feed them to the guesser in order to get a decent
# estimate of the mode and to use the guesser's binning functions
# in making that estimate.
self.guesser.newPoints(forceList)
means = self.guesser.means()
# This is the mean force direction, more or less.
mfDirs = means[1]
binmeans,binwidths = self.guesser.hist.binmeans()
#print "self.position: ", self.position
#print "%s, %s, %.3f" % (("(%.2f,%.2f)" % tuple(binmeans)),
# ("(%.2f,%.2f)" % tuple(binwidths)),
# mfDirs)
#print self.guesser.hist.bins
# How many bins should we use on the force angle when calculating a
# mode? The closer we are to going in the right direction, the tighter
# the bins should get. But there aren't enough points for too many
# bins, so limit the number.
nAngBins = min(11, 5 + 2 * (int(1.57 / math.fabs(mfDirs))))
#print "::::",nAngBins, self.guesser.hist.numBins[1]
# Decide whether or not to regenerate the bin boundaries.
if (nAngBins != self.guesser.hist.numBins[1]) or \
(math.fabs(binmeans[1] - mfDirs) > binwidths[1]/2.0):
self.guesser.hist.rebin(self.guesser.pointArray, (3,nAngBins))
#print "regenerating (angbins=%d)" % nAngBins
else:
self.guesser.hist.populate(self.guesser.pointArray)
#print self.guesser.hist.str()
# Choose the mode from the histogram and generate a decent force
# value from its boundaries.
fM,fDR = self.guesser.hist.mode()
# Translate this into a Twist object, applying appropriate damping
# and maxima.
fDR = fDR % (2 * math.pi)
fDR = fDR - (2 * math.pi) if fDR > math.pi else fDR
print "fDR = ", fDR
# Calculate some provisional speeds, later to be subject to damping.
if math.fabs(fDR) > 0.2:
# If we're pointing the wrong way, just rotate (linear = 0).
provX = 0.0
else:
provX = min(self.maxSpeed, self.kSpeed * fM)
provZ = math.fabs(self.kTurn * fDR)
provZ = min(self.maxTurn, provZ)
provZ = math.copysign(provZ, fDR)
print "provX, provZ", provX, provZ
# If the last location estimate had a lower variance than this one,
# ignore the new one.
if provX > 0.0 and \
(self.curstds < 0.5 * (belief.stds[0] + belief.stds[1])):
print "out of here..."
return
self.curstds = 0.5 * (belief.stds[0] + belief.stds[1])
# Apply damping and put it into the twist message.
if provX:
self.twist.linear.x += self.dampSpeed * (provX - self.twist.linear.x)
else:
self.twist.linear.x = provX
self.twist.angular.z += self.dampTurn * (provZ - self.twist.angular.z)
return
def doMove(self, belief):
assert isinstance(belief, Belief)
# The process of calculating all the beliefs takes time, and so the
# belief in hand is actually a tiny bit old. We use a model of the
# robot motion to advance each point of the belief to the more or less
# current time. Calling setModelPosition() chooses an incremental
# advance which will then be applied to all the points.
self.setModelPosition(belief.source_stamp.to_sec())
# This is just an estimated position used in debugging.
self.position = self.modelPosition(belief.means)
self.stds = belief.stds
# If we're there, pop the goal stack and execute the victory
# function.
if self.achievedGoal():
self.popGoal()
if self.targetFn:
self.targetFn(self.position)
# This is just in case we're dealing with a bump or something.
if self.isbusy or self.bumpTimeOne > 0.0 or self.bumpTimeTwo > 0.0:
# print "nothing doing"
return
# Get rid of any forces that should have been forgotten.
self.fl.purgeForces()
#print "doMove", self.lost(), self.gs.top(), self.amLost
if self.lost():
# We don't know where we are. Sit and spin and hope we see a
# tag.
self.doLostMove(belief)
elif self.gs.top():
self.amLost = False
# If there's still a goal on the stack, calculate forces and go.
self.doForceMove(belief)
else:
self.amLost = False
# There is no goal on the stack and we know where we are. Stop.
self.twist.linear.x = 0.0
self.twist.angular.z = 0.0
if self.debug:
self.count += 1
filename = self.dbgdir + ("/%05dpts.txt" % (self.count,))
f = open(filename, "w")
f.write(str(self.count) + "\n")
f.write(str(self.twist) + "\n")
f.write("PT, %8.4f, %8.4f, %8.4f\n" % (self.debugX, self.debugY, self.debugTh))
f.write(str(belief) + "\n")
f.close()
# Publish whatever Twist message we've produced.
if self.bumpTimeOne + self.bumpTimeTwo > 0.0:
return
self.pub.publish(self.twist)
def getRealRobotPos(self, tps):
"""
A debugging function, meant to get a position from some external
process, to be incorporated into a debugging message.
"""
for tp in tps.tag_positions:
if tp.id == 2:
self.debugX = tp.x
self.debugY = (36+39) - tp.y
self.debugTh = math.pi - tp.theta
#print "planar report: (%.3f,%.3f,%.3f)" % (self.debugX, self.debugY, self.debugTh)
def debug_track(self, req):
self.debugX = req.x
self.debugY = req.y
self.debugTh = req.theta
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
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] = 1.0
mapData = MapData(mapArray, (-5.0, -5.0), (0.5, 0.5))
mg.addMap(mapData)
import tgraph
tg = tgraph.Tgraph(350, 200)
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")
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()
def testUpdate(self):
# Create a uniform random field
g = Guesser("map_filter_test", self.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]])
# Arrange empty position array
pts = [ Pt(point = [0.0, 0.0, 0.0]) for i in range(npts) ]
print o.propagateError(pts, init, ol[-1])
greq = GuessRequest()
greq.source_stamp = rospy.Time(secs=init.header.stamp.secs,nsecs=init.header.stamp.nsecs)
# greq.source_stamp.nsecs -= 10000
greq.inPoints = pts
output = o.updateBelief(greq)
print output
from perfesser_guesser import Guesser
g = Guesser("testGuesser", (0.0,0.0,2*math.pi), "location",
(4,4,7))
g.newPoints(output.outPoints)
m = g.means()
print m
assert math.fabs(m[0] - 0.058) < 0.001
assert math.fabs(m[1]) < 0.001
assert math.fabs(m[2]) < 0.001
o1 = Odometry()
th = 0.0
o1.pose.pose.position.x = 1.0
o1.pose.pose.orientation.z = math.sin(th/2.0)
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()
mapData = MapData(mapArray, (-5.0, -5.0), (0.5, 0.5))
mg.addMap(mapData)
import tgraph
tg = tgraph.Tgraph(350,200)
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")
