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