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, pos=(0.0, 0.0, 0.0)):
self.name = name
# Use this to substitute for measuring the neighbor's position.
self.lsub = rospy.Subscriber("/" + self.name + "/guesser/position/guess",
Belief, self.getPos,
queue_size = 1, buff_size = 200)
# Use this to hold a measurement of the neighboring robot's
# position.
self.location = Belief(data_type = "location",
data_sub_type = self.name,
pers = (0.0, 0.0, 2*math.pi))
# Use this to hold the last question sent to the neighboring
# robot. This 'question' would have been a suggestion about
# appropriate directions for that neighboring robot to move
# in.
self.lastQuestion = Belief(data_type = "destination",
data_sub_type = self.name,
pers = (0.0, 0.0, 2*math.pi))
# This is the reply to the question stored above, a collection
# of ratings of each of the points in the question above.
# Each point is a floating-point value x: 0 < x < 1.
self.lastReply = Belief(data_type = "rating",
data_sub_type = "from " + self.name,
pers = (0.0, 0.0, 2*math.pi))
# This is the service by which we send the lastQuestion and
# receive the lastReply. Note the absolute service name.
self.advise = rospy.ServiceProxy("/" + self.name + "/cover/advice",
MRFQuery, persistent=True)
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 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 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 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 __init__(self, name, pos=(0.0, 0.0)):
self.name = name
# This is a tentative position, used in the demo to figure out
# which robots we can simulate "seeing."
self.pos = pos
self.lsub = rospy.Subscriber(self.name + "/position",
Belief,
self.getPos,
queue_size=1,
buff_size=200)
# Use this to hold our measurement of the neighboring robot's
# position.
self.location = Belief(data_type="location",
data_sub_type=self.name,
pers=(0.0, 0.0))
# Use this to hold the last question sent to the neighboring
# robot. This 'question' would have been a suggestion about
# appropriate targets for that neighboring robot to move
# towards.
self.lastQuestion = Belief(data_type="destination",
data_sub_type=self.name,
pers=(0.0, 0.0))
# This is the reply to the question stored above, a collection
# of ratings of each of the points in the question above.
# Each point is a floating-point value x: 0 < x < 1.
self.lastReply = Belief(data_type="rating",
data_sub_type="from " + self.name,
pers=(0.0, 0.0))
# This is the service by which we send the lastQuestion and
# receive the lastReply.
self.advise = rospy.ServiceProxy(self.name + "/advice", MRFQuery)
# This is the latest suggestion sent to us by this neighbor.
self.lastSuggestion = Belief(data_type="suggestion",
data_sub_type=self.name,
pers=(0.0, 0.0))
# The reply we made to the suggestion above.
self.rating = Belief(data_type="rating",
data_sub_type="from host",
pers=(0.0, 0.0))
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 belief(self):
"""
Returns the belief embodied by the current state of the Guesser.
"""
return Belief( source_stamp = self.stamp,
source_data = self.source_data,
data_type = self.data_type,
data_sub_type = self.data_sub_type,
sender = self.name,
means = self.means(),
stds = self.stds(),
pers = self.periods,
dim_names = self.dim_names,
points = self.outPoints())
def __init__(self, name="bump_mapper", pub=False, radius=0.25):
self.name = name
self.bpub = pub
self.robotRadius = radius
self.waitTime = 0.0
self.curLocation = Belief()
self.curVelocity = Twist()
self.velTimestamp = rospy.Time.now().to_sec()
self.guesser = Guesser(name=self.name,
periods=(0.0, 0.0, 2 * math.pi),
data_type="location",
num_bins=(1, 1, 1),
data_sub_type="obstacle")
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 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 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
def __init__(self):
self.output = Belief()
# experiments, in the coordinate system we've defined for the
# entire floor of the building.
guesser.uniform(npoints, [[28.0, 35.0], [35.0, 40.0], [-math.pi, math.pi]])
# If you're doing simulation experiments, or if there is some
# other reason you want to give the guesser a head start, you can
# use a normal distribution instead.
# guesser.normal([[0.0,0.01], [0.0, 0.01], [0.0, 0.01]])
server = PerfesserServer(rospy.ServiceProxy, debug=True)
rospy.Subscriber('guesser/position/guessers', Announce, server.updateStack)
rospy.sleep(5.0) # wait for the subscriber to be set up
pts = Belief()
pts.sender = "perfesser"
pts.data_type = "location"
pts.pers = (0.0, 0.0, 2 * math.pi)
counter = 0
while True:
r = server.surveyGuess(guesser)
if not r.no_data:
pts.points = guesser.outPoints()
pts.means = guesser.means()
pts.stds = guesser.stds()
pts.source_stamp = guesser.stamp
pts.source_data = guesser.source_data
pts.data_type = "location"
def __init__(self, name, npoints = 7, away = 1.3, space = 0.6,
avoid = 0.5, seek = 4.0, maxDist = 2.0,
velPub = False, goalPub = False):
print "Starting:" , name
self.name = name
self.pos = Belief(data_type = "position", data_sub_type = self.name,
pers = (0.0, 0.0, 2*math.pi))
self.npoints = npoints
# These are coefficients for the opinion formation.
# Away controls the desire to move apart.
self.away = away
# Without this space term, you get an exploding ring more often with
# nothing in the middle.
self.space = space
# Controls the predilection to avoid previously-encountered obstacles.
self.avoid = avoid
# Controls interest in new territory.
self.seek = seek
# Poorly named -- the potential function in the phi.away() method
# attempts to put this distance between each node.
self.maxDist = maxDist
# This is a list of neighboring robots, assumed to contain
# Neighbor() objects, as defined above.
self.neighbors = []
# This is an array of points in configuration space indicating
# where we need to go next.
self.deltaConfig = np.array([[]])
self.twist = Twist()
#** These are moved to gnav
self.maxSpeed = 0.25
self.maxTurn = 0.35
self.kSpeed = 1.0
self.kTurn = 1.0
self.phi = Phi(self.name,
self.away, self.space, self.avoid, self.seek,
self.maxDist)
self.movePeriod = 3.0
self.nextMoveTime = rospy.Time.now().to_sec() + self.movePeriod
# When this is in the future, we are still dealing with a bump. Don't
# register other obstacles within a couple of seconds.
self.bumpFreeTime = 0.0
# Estimated variance in the position measurements. This is important
# in the resampling of the guesses.
self.std = 0.1
# When rating a neighbor's opinion about our desired position,
# use this many bins. Shouldn't be a radically different
# order of magnitude from npoints.
self.nbins = (5,5)
self.velPub = velPub
self.goalPub = goalPub
try:
# assert velPub
assert goalPub
except:
print "must specify a goal publisher"
self.busy = False
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
