#!/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()