generator.py
Go to the documentation of this file.
00001 import roslib; roslib.load_manifest('easy_markers')
00002 import tf
00003 import rospy
00004 from visualization_msgs.msg import Marker, MarkerArray
00005 from geometry_msgs.msg import Point, Quaternion
00006 
00007 def get_point(position, scale=1.0):
00008     pt = Point()
00009     if position is None:
00010         pt.x = 0.0
00011         pt.y = 0.0
00012         pt.z = 0.0
00013     elif('x' in dir(position)):
00014         pt.x = position.x
00015         pt.y = position.y
00016         pt.z = position.z
00017     else:
00018         pt.x = position[0]
00019         pt.y = position[1]
00020         pt.z = position[2]
00021 
00022     pt.x /= scale
00023     pt.y /= scale
00024     pt.z /= scale
00025 
00026     return pt
00027     
00028 def get_quat(orientation):
00029     quat = Quaternion()
00030     if orientation is None:
00031         quat.x = 0.0
00032         quat.y = 0.0
00033         quat.z = 0.0
00034         quat.w = 1.0
00035     elif('x' in dir(orientation)):
00036         quat.w = orientation.w
00037         quat.x = orientation.x
00038         quat.y = orientation.y
00039         quat.z = orientation.z
00040     elif len(orientation)==4:
00041         quat.x = orientation[0]
00042         quat.y = orientation[1]
00043         quat.z = orientation[2]
00044         quat.w = orientation[3]
00045     else:
00046         q2 = tf.transformations.quaternion_from_euler(orientation[0],orientation[1],orientation[2])
00047         quat.x = q2[0]
00048         quat.y = q2[1]
00049         quat.z = q2[2]
00050         quat.w = q2[3]
00051     return quat
00052         
00053 
00054 class MarkerGenerator:
00055     def __init__(self):
00056         self.reset()
00057 
00058     def reset(self):
00059         self.counter = 0
00060         self.frame_id = ''
00061         self.ns = 'marker'
00062         self.type = 0
00063         self.action = Marker.ADD
00064         self.scale = [1.0] *3
00065         self.color = [1.0] * 4
00066         self.points = []
00067         self.colors = []
00068         self.text = ''       
00069         self.lifetime = 0.0
00070 
00071     def marker(self, position=None, orientation=None, points=None, colors=None, scale=1.0):
00072         mark = Marker()
00073         mark.header.stamp = rospy.Time.now()
00074         mark.header.frame_id = self.frame_id
00075         mark.ns = self.ns
00076         mark.type = self.type
00077         mark.id = self.counter
00078         mark.action = self.action
00079         mark.scale.x = self.scale[0]
00080         mark.scale.y = self.scale[1]
00081         mark.scale.z = self.scale[2]
00082         mark.color.r = self.color[0]
00083         mark.color.g = self.color[1]
00084         mark.color.b = self.color[2]
00085         mark.color.a = self.color[3]
00086         mark.lifetime = rospy.Duration(self.lifetime)
00087 
00088         if points is not None:
00089             mark.points = []
00090             for point in points:
00091                 mark.points.append(get_point(point, scale))
00092         if colors is not None:
00093             mark.colors = colors
00094 
00095         if position is not None or orientation is not None:
00096             mark.pose.position = get_point(position, scale)
00097             mark.pose.orientation = get_quat(orientation)
00098 
00099            
00100 
00101         self.counter+=1
00102         return mark


easy_markers
Author(s): David V. Lu!!
autogenerated on Wed Aug 26 2015 16:47:36