10 import roslib;roslib.load_manifest(PKG)
13 from image_view2.msg
import ImageMarker2, PointArrayStamped
15 from std_msgs.msg
import ColorRGBA
19 print(
"Usage: ", sys.argv[0],
"id frame_id radius [r, g, b]")
23 def __init__(self, topic, marker_id, frame_id, radius, color):
31 self.color.r = color[0]
32 self.color.g = color[1]
33 self.color.b = color[2]
37 marker = ImageMarker2()
38 now = rospy.Time.now()
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
48 x = self.
radius * math.cos(theta)
49 y = self.
radius * math.sin(theta)
54 point_array.points.append(point)
55 marker.points3D = point_array
57 marker.outline_colors = [self.
color]
62 marker.fill_color = self.
color 63 self.publisher.publish(marker)
67 rospy.init_node(
"draw_3d_circle")
68 if len(rospy.myargv()) != 4
and len(rospy.myargv()) != 7:
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)
78 [float(rospy.myargv()[4]), float(rospy.myargv()[5]), float(rospy.myargv()[6])])
80 while not rospy.is_shutdown():
84 if __name__ ==
'__main__':
87 except rospy.ROSInterruptException:
def __init__(self, topic, marker_id, frame_id, radius, color)