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