polygon_array_sample.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import rospy
4 from jsk_recognition_msgs.msg import PolygonArray
5 from geometry_msgs.msg import Polygon, PolygonStamped, Point32
6 from std_msgs.msg import Header
7 from math import sin, cos, pi
8 import numpy as np
9 def SquarePolygon(header):
10  p = PolygonStamped()
11  p.header = header
12  p.polygon.points = [Point32(x=1.0, y=1.0),
13  Point32(x=-1.0, y=1.0),
14  Point32(x=-1.0, y=-1.0),
15  Point32(x=1.0, y=-1.0)]
16  return p
17 
18 def RectanglePolygon(header):
19  p = PolygonStamped()
20  p.header = header
21  p.polygon.points = [Point32(x=-1.0, y=1.0, z=1.0),
22  Point32(x=-2.0, y=1.0, z=1.0),
23  Point32(x=-2.0, y=-1.0, z=1.0),
24  Point32(x=-1.0, y=-1.0, z=1.0)]
25  return p
26 def CirclePolygon(header):
27  p = PolygonStamped()
28  p.header = header
29  for i in range(100):
30  theta = i / 100.0 * 2.0 * pi
31  x = 1.0 * cos(theta) + 3.0
32  y = 1.0 * sin(theta)
33  p.polygon.points.append(Point32(x=x, y=y))
34  return p
35 
36  # star
37 def StarPolygon(header):
38  p = PolygonStamped()
39  p.header = header
40  p.polygon.points = [Point32(x= .0000, y= 1.0000 + 3.0),
41  Point32(x= .2245, y= .3090 + 3.0),
42  Point32(x= .9511, y= .3090 + 3.0),
43  Point32(x= .3633, y= -.1180 + 3.0),
44  Point32(x= .5878, y= -.8090 + 3.0),
45  Point32(x= .0000, y= -.3820 + 3.0),
46  Point32(x= -.5878, y= -.8090 + 3.0),
47  Point32(x= -.3633, y= -.1180 + 3.0),
48  Point32(x= -.9511, y= .3090 + 3.0),
49  Point32(x= -.2245, y= .3090 + 3.0)]
50  return p
51 
52 if __name__ == "__main__":
53  rospy.init_node("polygon_array_sample")
54  pub = rospy.Publisher("~output", PolygonArray)
55  r = rospy.Rate(10)
56  while not rospy.is_shutdown():
57  msg = PolygonArray()
58  header = Header()
59  header.frame_id = "world"
60  header.stamp = rospy.Time.now()
61  msg.header = header
62  msg.polygons = [SquarePolygon(header),
63  RectanglePolygon(header),
64  CirclePolygon(header),
65  StarPolygon(header)]
66  msg.labels = [0, 1, 2, 3]
67  msg.likelihood = [np.random.ranf(), np.random.ranf(), np.random.ranf(), np.random.ranf()]
68  pub.publish(msg)


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Sat Mar 20 2021 03:03:18