draw_3d_circle.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import rospy
00003 
00004 PKG='jsk_pcl_ros'
00005 
00006 import imp
00007 try:
00008     imp.find_module(PKG)
00009 except:
00010     import roslib;roslib.load_manifest(PKG)
00011 
00012 import sys
00013 from image_view2.msg import ImageMarker2, PointArrayStamped
00014 from geometry_msgs.msg import Point
00015 from std_msgs.msg import ColorRGBA
00016 import math
00017 
00018 def usage():
00019   print "Usage: ", sys.argv[0], "id frame_id radius [r, g, b]"
00020   
00021 class Drawer3DCircle:
00022   RESOLUTION = 20
00023   def __init__(self, topic, marker_id, frame_id, radius, color):
00024     self.topic = topic
00025     self.marker_id = marker_id
00026     self.frame_id = frame_id
00027     self.radius = radius
00028     self.fill = True
00029     if color:
00030       self.color = ColorRGBA()
00031       self.color.r = color[0]
00032       self.color.g = color[1]
00033       self.color.b = color[2]
00034   def advertise(self):
00035     self.publisher = rospy.Publisher(self.topic, ImageMarker2)
00036   def publish(self):
00037     marker = ImageMarker2()
00038     now = rospy.Time.now()
00039     marker.id = self.marker_id
00040     marker.header.stamp = now
00041     marker.header.frame_id = self.frame_id
00042     marker.type = ImageMarker2.POLYGON3D
00043     point_array = PointArrayStamped()
00044     point_array.header.frame_id = self.frame_id
00045     point_array.header.stamp = now
00046     for i in range(self.RESOLUTION + 1) + [0]:
00047       theta = 2 * math.pi / self.RESOLUTION * i
00048       x = self.radius * math.cos(theta)
00049       y = self.radius * math.sin(theta)
00050       point = Point()
00051       point.x = x
00052       point.y = y
00053       point.z = 0
00054       point_array.points.append(point)
00055     marker.points3D = point_array
00056     if self.color:
00057       marker.outline_colors = [self.color]
00058       if self.fill:
00059         marker.filled = 1
00060       else:
00061         marker.filled = 0
00062       marker.fill_color = self.color
00063     self.publisher.publish(marker)
00064 
00065 def main():
00066   # global pub, marker_id, frame_id, radius, use_color, color
00067   rospy.init_node("draw_3d_circle")
00068   if len(rospy.myargv()) != 4 and len(rospy.myargv()) != 7:
00069     usage()
00070     exit(1)
00071   marker_id = int(rospy.myargv()[1])
00072   frame_id = rospy.myargv()[2]
00073   radius = float(rospy.myargv()[3])
00074   if len(rospy.myargv()) != 7:
00075     pub = Drawer3DCircle("image_marker", marker_id, frame_id, radius, None)
00076   else:
00077     pub = Drawer3DCircle("image_marker", marker_id, frame_id, radius, 
00078                          [float(rospy.myargv()[4]), float(rospy.myargv()[5]), float(rospy.myargv()[6])])
00079   pub.advertise()
00080   while not rospy.is_shutdown():
00081     pub.publish()
00082     rospy.sleep(1.0)
00083   
00084 if __name__ == '__main__':
00085   try:
00086     main()
00087   except rospy.ROSInterruptException:
00088     pass


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Tue Jul 2 2019 19:41:42