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