#!/usr/bin/env python
"""
Plot points from a trackem_ros stream.
SCL; 19 August 2012.
"""
import roslib; roslib.load_manifest("slamsynth_ros")
import rospy
from trackem_ros.msg import *
import numpy as np
from display import Display
if __name__ == "__main__":
rospy.init_node("miniviz")
d = Display(mapsize=[0,3.048,0,3.048]) # in meters
d.marker = 'o' # Backwards compatibility until bentobox.cds is updated
rate = rospy.Rate(10)
while not rospy.is_shutdown():
data = rospy.wait_for_message("trackem/calpoints", MTCalPoints)
points = np.array([[p.x, p.y] for p in data.points])
d.clear()
d.scatter(points.T[0], points.T[1])
d.blit()
rate.sleep()
if verbose > 0:
print 'NO AUT, CONTINUING'
continue
(px,py),path = tutil.getNextGoal(aut, (wx,wy), qclose, n=5, verbose=1)
goal = t.quadToWorld((py,px),(0,3))
if verbose > 0:
print "Goal position is " + str(goal)
if verbose > 0:
print 'DRAWING PATH:', path
for addr in path:
matches = t.getMatches(addr,exact=True)
if matches: matches[0].setMark('path')
d.quad(t)
d.pose2d([x,y,h])
d.blit()
if not passive_mode:
bi.goToPoint(goal)
# calling quad more than necessary for sake of prettiness...
d.quad(t)
xl,yl = [],[]
xl,yl = occ.getScatterVectors()
if xl != []:
d.clear()
d.scatter(xl,yl)
d.pose2d([x,y,h])
d.blit()
