draw_3d_circle.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 import rospy
3 
4 PKG='jsk_pcl_ros'
5 
6 import imp
7 try:
8  imp.find_module(PKG)
9 except:
10  import roslib;roslib.load_manifest(PKG)
11 
12 import sys
13 from image_view2.msg import ImageMarker2, PointArrayStamped
14 from geometry_msgs.msg import Point
15 from std_msgs.msg import ColorRGBA
16 import math
17 
18 def usage():
19  print("Usage: ", sys.argv[0], "id frame_id radius [r, g, b]")
20 
22  RESOLUTION = 20
23  def __init__(self, topic, marker_id, frame_id, radius, color):
24  self.topic = topic
25  self.marker_id = marker_id
26  self.frame_id = frame_id
27  self.radius = radius
28  self.fill = True
29  if color:
30  self.color = ColorRGBA()
31  self.color.r = color[0]
32  self.color.g = color[1]
33  self.color.b = color[2]
34  def advertise(self):
35  self.publisher = rospy.Publisher(self.topic, ImageMarker2)
36  def publish(self):
37  marker = ImageMarker2()
38  now = rospy.Time.now()
39  marker.id = self.marker_id
40  marker.header.stamp = now
41  marker.header.frame_id = self.frame_id
42  marker.type = ImageMarker2.POLYGON3D
43  point_array = PointArrayStamped()
44  point_array.header.frame_id = self.frame_id
45  point_array.header.stamp = now
46  for i in range(self.RESOLUTION + 1) + [0]:
47  theta = 2 * math.pi / self.RESOLUTION * i
48  x = self.radius * math.cos(theta)
49  y = self.radius * math.sin(theta)
50  point = Point()
51  point.x = x
52  point.y = y
53  point.z = 0
54  point_array.points.append(point)
55  marker.points3D = point_array
56  if self.color:
57  marker.outline_colors = [self.color]
58  if self.fill:
59  marker.filled = 1
60  else:
61  marker.filled = 0
62  marker.fill_color = self.color
63  self.publisher.publish(marker)
64 
65 def main():
66  # global pub, marker_id, frame_id, radius, use_color, color
67  rospy.init_node("draw_3d_circle")
68  if len(rospy.myargv()) != 4 and len(rospy.myargv()) != 7:
69  usage()
70  exit(1)
71  marker_id = int(rospy.myargv()[1])
72  frame_id = rospy.myargv()[2]
73  radius = float(rospy.myargv()[3])
74  if len(rospy.myargv()) != 7:
75  pub = Drawer3DCircle("image_marker", marker_id, frame_id, radius, None)
76  else:
77  pub = Drawer3DCircle("image_marker", marker_id, frame_id, radius,
78  [float(rospy.myargv()[4]), float(rospy.myargv()[5]), float(rospy.myargv()[6])])
79  pub.advertise()
80  while not rospy.is_shutdown():
81  pub.publish()
82  rospy.sleep(1.0)
83 
84 if __name__ == '__main__':
85  try:
86  main()
87  except rospy.ROSInterruptException:
88  pass
def __init__(self, topic, marker_id, frame_id, radius, color)


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Mon May 3 2021 03:03:46