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
ol.append(Odometry())
th = 0.0
ol[-1].pose.pose.position.x = 0.058
ol[-1].pose.pose.orientation.z = math.sin(th/2.0)
ol[-1].pose.pose.orientation.w = math.cos(th/2.0)
ol[-1].header.stamp = rospy.Time.from_sec(1310140131.643)
o.msmts.addMsmt(ol[-1])
# Arrange empty position array
pts = [ Pt(point = [0.0, 0.0, 0.0]) for i in range(npts) ]
print o.propagateError(pts, init, ol[-1])
greq = GuessRequest()
greq.source_stamp = rospy.Time(secs=init.header.stamp.secs,nsecs=init.header.stamp.nsecs)
# greq.source_stamp.nsecs -= 10000
greq.inPoints = pts
output = o.updateBelief(greq)
print output
from perfesser_guesser import Guesser
g = Guesser("testGuesser", (0.0,0.0,2*math.pi), "location",
(4,4,7))
g.newPoints(output.outPoints)
m = g.means()
print m
assert math.fabs(m[0] - 0.058) < 0.001
