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 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
tg.plot_points(g.pointArray, 0, 1, 2, "s") o.updateOdom(o1) o.updateOdom(o2) o.updateOdom(o3) o.updateOdom(o4) s = o1.header.stamp s.nsecs += 5000000 greq = GuessRequest() greq.source_stamp = s greq.header.stamp = rospy.Time.now() greq.pers = (0.0, 0.0, 2*math.pi) greq.means = g.means() greq.stds = g.stds() greq.inPoints = g.outPoints() gresp = o.updateBelief(greq) pts = Belief(points = gresp.outPoints) tg.new_graph() pta = g.pointsToArray(pts) print "maxs:", map(max, pta.transpose()) print "mins:", map(min, pta.transpose()) tg.draw_scatter(g.pointsToArray(pts), 0, 1, 2, "s", recalc=False) tg.mainloop()