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