sample_ROS_PolygonStamped.py

#!/usr/bin/env python
'''
 ROS' test code to see how geometry_msgs/PolygonStamped works. See http://answers.ros.org/question/36009/better-msg-implementation-for-random-complicated/ for the related discussion.
 @author: Isaac Saito <130s _a_t_ lateeye.net>

 Btw, this code doesn't work as I intended; I want it to draw a square...instead, it shows skewed 2D polygon. Ironically this defect fulfils my purpose (i.e. to mismatch the start & end point 8) ).
'''

import roslib; roslib.load_manifest('ros_provingground')
import rospy
from geometry_msgs.msg import Point32
from geometry_msgs.msg import PolygonStamped

def pubPolygon():
    pub = rospy.Publisher('dotOnLine', PolygonStamped)
    rospy.init_node('polygonTalker')
    size_pts = 100
    #size_pts = 20
    i = j = 0
    init_x = 10.0
    init_y = 10.0
    init_z = 2.0

    inc_x =  0.001
    #inc_y =  0.005
    inc_y =  0.001
    x_curr = init_x
    y_curr = init_y

    while not rospy.is_shutdown():
        polst = PolygonStamped()
        polst.header.frame_id = '/frame_polygonTalker'
        #print 'polst len=', len(polst.polygon.points)
        pArr = []
        for i in range(size_pts):
            p = Point32()
            if i < (size_pts * 1/4):
                p.x = x_curr + inc_x * i
                p.y = y_curr + inc_y * i
            elif (size_pts * 1/4) <= i and i < (size_pts * 2/4):
                p.x = x_curr + inc_x * i
                p.y = y_curr - inc_y * i
            elif (size_pts * 2/4) <= i and i < (size_pts * 3/4):
                p.x = x_curr - inc_x * i
                p.y = y_curr - inc_y * i
            elif (size_pts * 3/4) <= i and i < size_pts:
                # p.x = x_curr - inc_x * (i - 0.5) # Trying to mismatch the ending point with starting point.
                # p.y = y_curr + inc_y * (i - 0.5) # Trying to mismatch the ending point with starting point.
                p.x = x_curr - inc_x * i
                p.y = y_curr + inc_y * i
            x_curr = p.x
            y_curr = p.y
            p.z = init_z
            pArr.append(p)
            i = i + 1
            print 'j=', j, 'i = ', i, ' x=', p.x, ' y=', p.y
        #str = "hello world %s"%rospy.get_time()
        #rospy.loginfo(str)
        polst.polygon.points = pArr
        pub.publish(polst)
        rospy.sleep(1.0)
        j = j + 1
        x_curr = x_curr + 0.001
        y_curr = y_curr + 0.001

if __name__ == '__main__':
    try:
        pubPolygon()
    except rospy.ROSInterruptException: pass