#!/usr/bin/env python

from geometry_msgs.msg import PolygonStamped
from geometry_msgs.msg import Point32
import math
import rospy

topic = "test_polygon"
publisher = rospy.Publisher(topic, PolygonStamped)

rospy.init_node("send_polygon")

t = 0
while not rospy.is_shutdown():

    p = PolygonStamped()
    p.header.frame_id = "base_link"
    p.header.stamp = rospy.Time.now()

    dr = 0.5 * math.cos(t)
    radii = [1 - dr, 1 + dr]
    radius_index = 0
    num_points = 10
    for i in range(0, num_points):
        point = Point32()
        radius = radii[radius_index]
        radius_index = (radius_index + 1) % 2
        point.x = radius * math.cos(i * 2 * math.pi / num_points)
        point.y = radius * math.sin(i * 2 * math.pi / num_points)
        point.z = 0
        p.polygon.points.append(point)

    publisher.publish(p)

    t += 0.1

    rospy.sleep(0.03)
