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