00001
00002
00003 import roslib; roslib.load_manifest('rviz')
00004 from geometry_msgs.msg import PolygonStamped
00005 from geometry_msgs.msg import Point32
00006 import math
00007 import rospy
00008
00009 topic = 'test_polygon'
00010 publisher = rospy.Publisher(topic, PolygonStamped)
00011
00012 rospy.init_node('send_polygon')
00013
00014 t = 0
00015 while not rospy.is_shutdown():
00016
00017 p = PolygonStamped()
00018 p.header.frame_id = "/base_link"
00019 p.header.stamp = rospy.Time.now()
00020
00021 dr = 0.5 * math.cos( t )
00022 radii = [ 1-dr, 1+dr ]
00023 radius_index = 0
00024 num_points = 10
00025 for i in range( 0, num_points ):
00026 point = Point32()
00027 radius = radii[ radius_index ]
00028 radius_index = (radius_index + 1) % 2
00029 point.x = radius * math.cos( i * 2 * math.pi / num_points )
00030 point.y = radius * math.sin( i * 2 * math.pi / num_points )
00031 point.z = 0
00032 p.polygon.points.append( point )
00033
00034 publisher.publish( p )
00035
00036 t += .1
00037
00038 rospy.sleep(0.03)