def getPosEst(self):
meanloc = (np.random.normal(loc=self.pos[0],
scale=self.std / 2.,
size=1)[0],
np.random.normal(loc=self.pos[1],
scale=self.std / 2.,
size=1)[0])
out = Belief(source_stamp=rospy.Time.now(),
source_data="estimate",
data_type="location",
data_sub_type=self.name,
sender=self.name,
dim_names=[self.name + "/xpos", self.name + "/ypos"],
pers=(0.0, 0.0),
means=meanloc,
stds=[self.std, self.std])
for x, y in zip(
np.random.normal(loc=meanloc[0],
scale=self.std,
size=self.npoints),
np.random.normal(loc=meanloc[1],
scale=self.std,
size=self.npoints)):
out.points.append(Pt(point=(x, y)))
return out
def goThatWay(self, moves):
"""
Takes an array of directions to move in and selects a point in
space in that direction. The choice of point is fairly
arbitrary. We try to multiply the direction by a factor large
enough that it won't be within the gnav.zeroDistance of the
current location, unless the intent is really that we should
not move. (That's what the kGo factor is for.)
"""
kGo = 2.0
print "moves", moves
meanMove = [ np.mean(moves[:, 0]), np.mean(moves[:, 1]) ]
sumsq = sum([d**2 for d in meanMove])**0.5
# If the target point is close, don't normalize. Otherwise, set
# a goal in the general direction
if sumsq > 1.0:
meanMove = [ kGo * x/sumsq for x in meanMove ]
self.goal = (self.pos.means[0] + meanMove[0],
self.pos.means[1] + meanMove[1], 0.0)
print "We're at: (%.3f,%.3f)" % (self.pos.means[0], self.pos.means[1])
print "headed to: (%.3f,%.3f)" % (self.goal[0], self.goal[1])
# Eventually this should publish to a goal-setting topic, to be
# read by some goal-setter over at gnav.
self.goalPub.publish(Pt(point=self.goal))
def takeAdvice(self, req):
"""
Takes a piece of advice from its neighbor, rates it, and
returns the rating. The advice is a distribution of points to
which the other robot thinks this robot ought to go. We
compare that advice to our own intentions and return ratings
indicating the degree of agreement between the two: 1.0 is
complete agreement and 0.0 is complete disagreement. We never
return exactly 0.0 as a matter of policy.
"""
if not req.question:
return MRFQueryResponse(no_data=True)
if not self.deltaConfig.any():
return MRFQueryResponse(no_data=True)
# Allocate our intention into bins.
question = self.ptsToArray(req.question)
# Establish bins so that there is one point in each outer bin.
# (The Histogram class in perfesser is set up so the outer
# bins will be empty, which is why we're not using it here.
mins = [min(self.deltaConfig[:, 0]), min(self.deltaConfig[:, 1])]
maxs = [max(self.deltaConfig[:, 0]), max(self.deltaConfig[:, 1])]
bins = [ np.linspace(mn + 1.e-10, mx - 1.e-10, bn) for \
mn,mx,bin in zip(mins, maxs, self.nbins) ]
hist = np.zeros(self.nbins)
# Create a distribution of points that includes neighbor's
# opinions except for the one asking the question.
phi = self.deltaConfig[:, (0, 1)]
messageProduct = [1.0] * phi.shape[0]
for nb in self.neighbors:
if (nb.name != req.asker) and (nb.lastReply.points):
messageProduct = [ m * p.point[0] for m,p in \
zip(messageProduct,
nb.lastReply.points) ]
phi = self.resample(phi, messageProduct)
# Populate bins
for delta in phi:
hist[sum(delta[0] > bins[0])][sum(delta[1] > bins[1])] += 1.0
n = sum(sum(hist))
# Formulate a reply by binning the input data.
m = MRFQueryResponse(no_data=False, source_stamp=req.source_stamp)
for q in req.question:
m.reply.append(Pt(point=( \
hist[sum(q.point[0] > bins[0])][sum(q.point[1] > bins[1])]/n, )))
# print "entering service: ", self.name
# print self.name, "'s opinion: ", self.deltaConfig
# print "question: ",question
# print "hist: ", hist
# print "bins: ", bins
# print m
return m
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 list(self):
"""
Compiles a list of robots and locations in the room.
"""
out = Room(sourceName="listing")
for rb in self.robots.values():
out.names.append(rb.name)
out.locations.append(Pt(point=(rb.x, rb.y)))
return out
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 arrayToBelief(self, inArray):
"""
Takes the numpy-style array in self.pointArray and packs it
into an otherwise empty Belief object. The object is
returned, but should usually have its secondary data filled in
before being published or used anywhere.
"""
assert isinstance(inArray, np.ndarray)
return Belief( points = [ Pt(point=row) for row in inArray ] )
def unwind(self):
"""Returns a Room2D object containing all the connections."""
out = Room2D()
for node in self.maps.values():
rm = Room(sourceName=node.name)
for friend in node.friends:
rm.names.append(friend.name)
rm.locations.append(Pt(point=friend.location))
out.robots.append(rm)
return out
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)
def recordNeighbors(self, req):
"""
At any time, the self.neighbors list should contain a Neighbor
object for each robot whose position we can measure directly.
We are receiving a list of robots and distances (signal
strengths) via the /olsr/neighbors topic and accumulating.
Use this as a callback to the /olsr/neighbors topic. The
result of calling this is an updated self.neighbors array.
"""
### This is a hack until the OLSR ROS node is working. We just
### use a fixed list of all the robots and subtract our name.
rm = Room(sourceName = self.name)
rm.names = req
rm.locations = [ Pt(point = (1.0, 0.0)), Pt(point = (1.0, 0.0)) ]
rm.names.remove(self.name)
self.neighbors = []
for name in rm.names:
self.neighbors.append(Neighbor(name = name))
def checkBump(self, req):
"""
Parses a SensorPacket for the bump and if it's there, fires
off a message about it. There are some provisions to make
sure one bump doesn't trigger too large a number of responses.
"""
if rospy.Time.now().to_sec() < self.waitTime:
return
if req.bumpLeft and req.bumpRight:
direction = 0.0
elif req.bumpLeft:
direction = 1.0
elif req.bumpRight:
direction = -1.0
else:
return
# Should consider using the Perfesser here for the means() and stds()
bumpEst = Belief(source_stamp=self.curLocation.header.stamp,
sender=self.name,
source_data=self.curLocation.source_data,
data_type=self.guesser.data_type,
data_sub_type=self.guesser.data_sub_type,
pers=self.guesser.periods)
bumpEst.points = []
for p in self.curLocation.points:
# Estimate the point on the robot's perimeter where the bump was
# felt.
psiCorr = p.point[2] + direction * 0.7
# Calculate the opposite-facing direction.
phiCorr = (psiCorr + math.pi) % (2 * math.pi)
phiCorr -= (2 * math.pi) if psiCorr > math.pi else 0.0
bumpEst.points.append( \
Pt(point=(p.point[0] + self.robotRadius * math.cos(psiCorr),
p.point[1] + self.robotRadius * math.sin(psiCorr),
phiCorr)))
self.guesser.newPoints(bumpEst.points)
bumpEst.means = self.guesser.means()
bumpEst.stds = self.guesser.stds()
if self.bpub:
bumpEst.header.stamp = rospy.Time.now()
self.bpub.publish(bumpEst)
self.waitTime = rospy.Time.now().to_sec() + 5.0
def nearest(self):
"""
Returns a list of robots in the room, sorted from low xpos to high.
"""
robotLocs = []
for rb in self.robots:
robotLocs.append([rb.x, rb.name])
robotLocs.sort()
r = Room(locations=[Pt(point=(loc, )) for loc, name in robotLocs],
names=[name for loc, name in robotLocs])
return r
def getPosEst(self):
out = Belief(source_stamp=rospy.Time.now(),
source_data="estimate",
data_type="location",
data_sub_type=self.name,
sender=self.name,
dim_names=[self.name + "/xpos"],
pers=(0.0, ),
means=[self.xpos],
stds=[self.std])
for x in np.random.normal(loc=self.xpos,
scale=self.std,
size=self.npoints):
out.points.append(Pt(point=(x, )))
return out
def nearest(self):
"""
Returns a list of robots in the room, accompanied by the names and
locations of its neighbors.
"""
out = Room2D()
for rb in self.robots:
out.names.append(rb.name)
nr = Room()
for rbt in self.robots:
d = self.dist(rb.x, rb.y, rbt.x, rbt.y)
if (d > 0.0) and (d < self.neighborDistance):
nr.names.append(rbt.name)
nr.locations.append(Pt(point=(rbt.x, rbt.y)))
out.neighbors.append(nr)
return out
def arrayToPts(self, array):
return [ Pt(point = a) for a in array ]
def outPoints(self):
"""
Returns the point array as a list of Pt objects.
"""
return [ Pt(point = row) for row in self.pointArray ]
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