$search
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.w = q2.w 00048 quat.x = q2.x 00049 quat.y = q2.y 00050 quat.z = q2.z 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