def testAnswerServer(self):
# Create a uniform random field
g = Guesser("map_filter_test", self.ms.nPoints,
(0.0, 0.0, 2 * math.pi), "location", (10, 10, 1))
g.uniform([[-5.0, 5.0], [-5.0, 5.0], [0.0, 3.0]])
# Compose a guess request
greq = GuessRequest()
greq.points = g.outPoints()
greq.pers = (0.0, 0.0, 2 * math.pi)
greq.data_type = "location"
gresp = self.ms.answer_server(greq)
parray = np.array([p.point for p in gresp.outPoints])
# There should be no points left where the mapData array was 1.0
xmax = False
ymin = False
xyint = False
mx = map(max, parray.transpose())
self.assertTrue(mx[0] < 2.5 and mx[1] < 2.5)
mn = map(min, parray.transpose())
self.assertTrue(mn[0] > -3.5 and mn[1] > -3.5)
cornerTest = False
npts = parray.shape[0]
for i in range(npts):
if parray[i][0] > 0.25 and \
parray[i][1] < -0.5:
cornerTest = True
self.assertFalse(cornerTest)
def testAnswerServer(self):
# Create a uniform random field
g = Guesser("map_filter_test", self.ms.nPoints, (0.0, 0.0, 2*math.pi),
"location", (10,10,1))
g.uniform([[-5.0,5.0],[-5.0,5.0],[0.0,3.0]])
# Compose a guess request
greq = GuessRequest()
greq.points = g.outPoints()
greq.pers = (0.0, 0.0, 2*math.pi)
greq.data_type = "location"
gresp = self.ms.answer_server(greq)
parray = np.array([p.point for p in gresp.outPoints])
# There should be no points left where the mapData array was 1.0
xmax = False ; ymin = False ; xyint = False
mx = map(max, parray.transpose())
self.assertTrue(mx[0] < 2.5 and mx[1] < 2.5)
mn = map(min, parray.transpose())
self.assertTrue(mn[0] > -3.5 and mn[1] > -3.5)
cornerTest = False
npts = parray.shape[0]
for i in range(npts):
if parray[i][0] > 0.25 and \
parray[i][1] < -0.5:
cornerTest = True
self.assertFalse(cornerTest)
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 testService(self):
ars = ARServer("ar_filter",
1000, (0.0, 0.0, 2 * math.pi),
0.1,
0.02,
.95,
3.0,
locfile=self.tmpfile)
ts = Tags()
t = Tag()
t.id = 1
t.cf = 0.75
t.x = 1.0
t.y = 1.0
t.distance = 2.0**0.5
t.xRot = 0.0
t.yRot = math.pi / 4.0
t.zRot = 0.0
t.xMetric = 1.000
t.yMetric = 1.000
t.zMetric = 0
ts.tags.append(t)
ts.tag_count = 1
ars.arGuesser.handle_image(ts)
g = Guesser("ar_filter_test", (0.0, 0.0, 2 * math.pi), "location",
(1, 1, 1))
xmax = 2.5
ymin = 0.0
g.uniform(ars.nPoints, [[0.0, xmax], [ymin, 2.0], [0.0, 3.25]])
greq = GuessRequest()
greq.inPoints = g.outPoints()
greq.pers = (0.0, 0.0, 2 * math.pi)
greq.data_type = "location"
gr = ars.answer_server(greq)
parray = np.array([p.point for p in gr.outPoints])
mins = map(min, parray.transpose())
maxs = map(max, parray.transpose())
#print maxs, mins, xmax, ymin
self.assertTrue(mins[1] > ymin)
self.assertTrue(maxs[0] < xmax)
def testService(self):
ars = ARServer("ar_filter", 1000, (0.0, 0.0, 2*math.pi), 0.1, 0.02,
.95, 3.0, locfile=self.tmpfile)
ts = Tags()
t = Tag()
t.id = 1
t.cf = 0.75
t.x = 1.0
t.y = 1.0
t.distance = 2.0**0.5
t.xRot = 0.0
t.yRot = math.pi/4.0
t.zRot = 0.0
t.xMetric = 1.000
t.yMetric = 1.000
t.zMetric = 0
ts.tags.append(t)
ts.tag_count = 1
ars.arGuesser.handle_image(ts)
g = Guesser("ar_filter_test", (0.0, 0.0, 2*math.pi),
"location", (1,1,1))
xmax = 2.5 ; ymin = 0.0
g.uniform(ars.nPoints, [[0.0,xmax],[ymin,2.0],[0.0,3.25]])
greq = GuessRequest()
greq.inPoints = g.outPoints()
greq.pers = (0.0, 0.0, 2*math.pi)
greq.data_type = "location"
gr = ars.answer_server(greq)
parray = np.array([p.point for p in gr.outPoints])
mins = map(min, parray.transpose())
maxs = map(max, parray.transpose())
#print maxs, mins, xmax, ymin
self.assertTrue(mins[1] > ymin)
self.assertTrue(maxs[0] < xmax)
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 testUpdate(self):
# Create a uniform random field
g = Guesser("map_filter_test", (0.0, 0.0, 2 * math.pi), "location",
(10, 10, 1))
g.uniform(self.mg.nPoints, [[-5.0, 5.0], [-5.0, 5.0], [0.0, 3.0]])
# Use it as a guess against the filter already in mg
gr = GuessRequest(inPoints=g.outPoints(),
means=g.means(),
stds=g.stds(),
data_type=g.data_type,
pers=g.periods)
gresp = self.mg.handle_guess(gr)
# There should be no points left where the mapData array was 1.0
xmax = False
ymin = False
xyint = False
xvec = [p.point[0] for p in gresp.outPoints]
yvec = [p.point[1] for p in gresp.outPoints]
print ">>>", max(xvec), min(xvec)
print ">>>", max(yvec), min(yvec)
mx = map(max, self.mg.guesser.pointArray.transpose())
print ">>>>", mx
self.assertTrue(mx[0] < 2.5 and mx[1] < 2.5)
mn = map(min, self.mg.guesser.pointArray.transpose())
self.assertTrue(mn[0] > -3.5 and mn[1] > -3.5)
cornerTest = False
npts = self.mg.guesser.pointArray.shape[0]
for i in range(npts):
if self.mg.guesser.pointArray[i][0] > 0.25 and \
self.mg.guesser.pointArray[i][1] < -0.5:
cornerTest = True
self.assertFalse(cornerTest)
t.yRot = math.pi/4.0
t.zRot = 0.0
t.xMetric = 1.000
t.yMetric = 1.000
t.zMetric = 0
ts.tags.append(t)
ts.tag_count = 1
ars.arGuesser.handle_image(ts)
g = Guesser("ar_filter_test", (0.0, 0.0, 2*math.pi),
"location", (1,1,1))
g.uniform(ars.nPoints, [[0.0,2.25],[0.0,2.0],[0.0,3.25]])
greq = GuessRequest()
greq.inPoints = g.outPoints()
greq.pers = (0.0, 0.0, 2*math.pi)
greq.data_type = "location"
import tgraph
tg = tgraph.Tgraph(400,300)
parray = np.array([p.point for p in greq.inPoints])
tg.draw_scatter(parray, 0,1,2, "s")
gr = ars.answer_server(greq)
tg.new_graph()
parray = np.array([p.point for p in ars.arGuesser.tarPts.points])
tg.draw_scatter(parray, 0,1,2, "s", recalc=False)
o.msmts.addMsmt(ol[-1])
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
t.yRot = math.pi / 4.0
t.zRot = 0.0
t.xMetric = 1.000
t.yMetric = 1.000
t.zMetric = 0
ts.tags.append(t)
ts.tag_count = 1
ars.arGuesser.handle_image(ts)
g = Guesser("ar_filter_test", (0.0, 0.0, 2 * math.pi), "location",
(1, 1, 1))
g.uniform(ars.nPoints, [[0.0, 2.25], [0.0, 2.0], [0.0, 3.25]])
greq = GuessRequest()
greq.inPoints = g.outPoints()
greq.pers = (0.0, 0.0, 2 * math.pi)
greq.data_type = "location"
import tgraph
tg = tgraph.Tgraph(400, 300)
parray = np.array([p.point for p in greq.inPoints])
tg.draw_scatter(parray, 0, 1, 2, "s")
gr = ars.answer_server(greq)
tg.new_graph()
parray = np.array([p.point for p in ars.arGuesser.tarPts.points])
tg.draw_scatter(parray, 0, 1, 2, "s", recalc=False)
inarray = np.array([[-5.5, -5.5, 0.0], [5.5, 5.5, 0.6]])
tg.draw_scatter(inarray, 0, 1, 2, "s")
parray = np.array([p.point for p in mg.mapPointList])
tg.plot_points(parray, 0, 1, 2, "s")
g = Guesser("map_filter_test", (0.0, 0.0, 2 * math.pi), "location",
(10, 10, 1))
g.uniform(mg.nPoints, [[-5.0, 5.0], [-5.0, 5.0], [0.0, 3.0]])
tg.new_graph()
tg.draw_scatter(g.pointArray, 0, 1, 2, "s", recalc=False)
gr = GuessRequest(inPoints=g.outPoints(),
means=g.means(),
stds=g.stds(),
data_type=g.data_type,
pers=g.periods)
gresp = mg.handle_guess(gr)
tg.plot_points(mg.guesser.hist.plottable(), 0, 1, 2, "c")
tg.new_graph()
parray = np.array([p.point for p in gresp.outPoints])
tg.draw_scatter(parray, 0, 1, 2, "s", recalc=False)
tg.mainloop()
