def surveyGuess(self, guesser):
if self.service_stack:
s = self.service_stack.pop(0)
g = GuessRequest()
g.inPoints = guesser.outPoints()
g.means = guesser.means()
g.stds = guesser.stds()
g.pers = guesser.periods
g.data_type = guesser.data_type
g.source_stamp = guesser.stamp
g.source_data = guesser.source_data
service = self.services[s]
resp = service(g)
if self.debug:
s = String()
self.counter += 1
s.data += " CALLED:%d:%s \n" % (self.counter, str(service),)
s.data += str(service.resolved_name) + " "
s.data += resp.source_data + "\n"
s.data += str(resp.source_stamp.secs) + ":" + str(resp.source_stamp.nsecs)
self.pdebug.publish(s)
if not resp.no_data:
guesser.newPoints(resp.outPoints)
guesser.stamp = resp.source_stamp
guesser.source_data = resp.source_data
else:
resp = GuessResponse()
resp.no_data = True
return resp
def answer_server(self, req):
gr = GuessResponse()
gr.sender = self.name
if not self.ready_to_publish:
gr.no_data = True
return gr
gr.no_data = False
self.guesser.handle_guess(req.points)
gr.outPoints = self.guesser.pointList
return gr
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 surveyGuess(self, guesser):
if self.service_stack:
s = self.service_stack.pop(0)
g = GuessRequest()
g.inPoints = guesser.outPoints()
g.means = guesser.means()
g.stds = guesser.stds()
g.pers = guesser.periods
g.data_type = guesser.data_type
g.source_stamp = guesser.stamp
g.source_data = guesser.source_data
service = self.services[s]
resp = service(g)
if self.debug:
s = String()
self.counter += 1
s.data += " CALLED:%d:%s \n" % (
self.counter,
str(service),
)
s.data += str(service.resolved_name) + " "
s.data += resp.source_data + "\n"
s.data += str(resp.source_stamp.secs) + ":" + str(
resp.source_stamp.nsecs)
self.pdebug.publish(s)
if not resp.no_data:
guesser.newPoints(resp.outPoints)
guesser.stamp = resp.source_stamp
guesser.source_data = resp.source_data
else:
resp = GuessResponse()
resp.no_data = True
return resp
def answer_server(self, req):
"""
Provides a reply to a Guess service call. The Perfesser will call
this after we've notified him that we have something worth saying
about the robot's location. The call is simply passed into the
bump_guess Guesser.
"""
gr = GuessResponse()
gr.sender = self.name
if self.guesser.handle_guess(req.inPoints):
gr.no_data = False
gr.outPoints = self.guesser.pointList
gr.source_stamp = self.guesser.stamp
gr.source_data = self.guesser.source_data
else:
gr.no_data = True
return gr
def answer_server(self, req):
gr = GuessResponse()
gr.sender = self.name
if not self.ready_to_publish:
gr.no_data = True
return gr
gr.no_data = False
self.guesser.handle_guess(req.points)
gr.outPoints = self.guesser.pointList
return gr
def answer_server(self, req):
"""
Provides a reply to a Guess service call. The Perfesser will call
this after we've notified him that we have something worth saying
about the robot's location. The call is simply passed into the
bump_guess Guesser.
"""
gr = GuessResponse()
gr.sender = self.name
if self.guesser.handle_guess(req.inPoints):
gr.no_data = False
gr.outPoints = self.guesser.pointList
gr.source_stamp = self.guesser.stamp
gr.source_data = self.guesser.source_data
else:
gr.no_data = True
return gr
def updateBelief(self, req):
"""
Takes a 'belief' object containing a set of points with a time
stamp that indicate a location at some time in the past, and
combines them with the Odometry messages received since then
to formulate a new belief about where we are now.
"""
assert isinstance(req, GuessRequest)
reply = GuessResponse()
# Get the odometry messages since the source time recorded in
# the request. More will be recorded soon.
odoms = self.msmts.fromTime(req.source_stamp)
# If there aren't enough odometry messages recorded, skip it.
if len(odoms) < 2:
reply.no_data = True
return reply
# Create a position array of zeros. The odometry messages
# record what has happened to a position since the given time.
# We will apply those changes to these zeros, and then add the
# result to the input points.
pts = [ Pt(point = [0.0, 0.0, 0.0]) for i in range(self.nPoints) ]
# Add up the position, plus error, since then.
for start, finish in zip(odoms, odoms[1:]):
pts = self.propagateError(pts, start, finish)
reply.source_data += "\n%s -> %s (%.4f -- %d)" % \
(self.tidyPrint(start), self.tidyPrint(finish), \
finish.header.stamp.to_sec(), finish.header.seq)
reply.outPoints = []
# Add the errors to the input points. These are in different
# reference frames, so the addition is not simple. We convert
# the adjustment to polar coordinates and apply it as a relative
# adjustment to the input points.
#
# We're also adding a little wiggle to simulate annealing.
wiggle = lambda x: 1.0 * x * ((2 * np.random.random()) - 1)
# A bit more about the wiggle: a belief is a collection of
# individual points, but is intended to be a representation of
# a probability distribution. Without the wiggle, over time
# the distribution evolves into a collection of individual
# points, and drifts away from being a decent representation
# of a distribution. The wiggling helps keep the definition
# clean, and it also helps adjust to sudden changes, like when
# someone picks up the robot and moves it.
for pt, inpt in zip(pts, req.inPoints):
dR = math.hypot(pt.point[0], pt.point[1])
dPh = math.atan2(pt.point[1], pt.point[0])
dTh = pt.point[2]
dX = dR * math.cos(inpt.point[2] + dPh)
dY = dR * math.sin(inpt.point[2] + dPh)
outpt = Pt(point= [ inpt.point[0] + dX + wiggle(self.posErrorMag),
inpt.point[1] + dY + wiggle(self.posErrorMag),
self.fixAngle(inpt.point[2] + dTh + \
wiggle(self.thetaErrorMag)) ])
reply.outPoints.append(outpt)
# Reply.
reply.no_data = False
reply.sender = self.name
last = odoms[-1]
reply.source_stamp = last.header.stamp
self.pdebug.publish(reply.source_data)
self.lastUpdated = last.header.stamp
return reply
