jsk_rviz_plugins: polygon_array_sample.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 from jsk_recognition_msgs.msg import PolygonArray
00005 from geometry_msgs.msg import Polygon, PolygonStamped, Point32
00006 from std_msgs.msg import Header
00007 from math import sin, cos, pi
00008 import numpy as np
00009 def SquarePolygon(header):
00010     p = PolygonStamped()
00011     p.header = header
00012     p.polygon.points = [Point32(x=1.0, y=1.0),
00013                         Point32(x=-1.0, y=1.0),
00014                         Point32(x=-1.0, y=-1.0),
00015                         Point32(x=1.0, y=-1.0)]
00016     return p
00017     
00018 def RectanglePolygon(header):
00019     p = PolygonStamped()
00020     p.header = header
00021     p.polygon.points = [Point32(x=-1.0, y=1.0, z=1.0),
00022                         Point32(x=-2.0, y=1.0, z=1.0),
00023                         Point32(x=-2.0, y=-1.0, z=1.0),
00024                         Point32(x=-1.0, y=-1.0, z=1.0)]
00025     return p
00026 def CirclePolygon(header):
00027     p = PolygonStamped()
00028     p.header = header
00029     for i in range(100):
00030         theta = i / 100.0 * 2.0 * pi
00031         x = 1.0 * cos(theta) + 3.0
00032         y = 1.0 * sin(theta)
00033         p.polygon.points.append(Point32(x=x, y=y))
00034     return p
00035     
00036     # star
00037 def StarPolygon(header):
00038     p = PolygonStamped()
00039     p.header = header
00040     p.polygon.points = [Point32(x= .0000, y= 1.0000 + 3.0),
00041                         Point32(x= .2245, y= .3090 + 3.0),
00042                         Point32(x= .9511, y= .3090 + 3.0),
00043                         Point32(x= .3633, y= -.1180 + 3.0),
00044                         Point32(x= .5878, y= -.8090 + 3.0),
00045                         Point32(x= .0000, y= -.3820 + 3.0),
00046                         Point32(x= -.5878, y= -.8090 + 3.0),
00047                         Point32(x= -.3633, y= -.1180 + 3.0),
00048                         Point32(x= -.9511, y= .3090 + 3.0),
00049                         Point32(x= -.2245, y= .3090 + 3.0)]
00050     return p
00051     
00052 if __name__ == "__main__":
00053     rospy.init_node("polygon_array_sample")
00054     pub = rospy.Publisher("~output", PolygonArray)
00055     r = rospy.Rate(10)
00056     while not rospy.is_shutdown():
00057         msg = PolygonArray()
00058         header = Header()
00059         header.frame_id = "world"
00060         header.stamp = rospy.Time.now()
00061         msg.header = header
00062         msg.polygons = [SquarePolygon(header),
00063                         RectanglePolygon(header),
00064                         CirclePolygon(header),
00065                         StarPolygon(header)]
00066         msg.labels = [0, 1, 2, 3]
00067         msg.likelihood = [np.random.ranf(), np.random.ranf(), np.random.ranf(), np.random.ranf()]
00068         pub.publish(msg)