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 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 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 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
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"
pts.header.stamp = rospy.Time.now()
counter += 1
pub.publish(pts)
rospy.sleep(wait_time)
rospy.spin()
