generator.py
Go to the documentation of this file.
1 import roslib; roslib.load_manifest('easy_markers')
2 import tf
3 import rospy
4 from visualization_msgs.msg import Marker, MarkerArray
5 from geometry_msgs.msg import Point, Quaternion
6 
7 def get_point(position, scale=1.0):
8  pt = Point()
9  if position is None:
10  pt.x = 0.0
11  pt.y = 0.0
12  pt.z = 0.0
13  elif('x' in dir(position)):
14  pt.x = position.x
15  pt.y = position.y
16  pt.z = position.z
17  else:
18  pt.x = position[0]
19  pt.y = position[1]
20  pt.z = position[2]
21 
22  pt.x /= scale
23  pt.y /= scale
24  pt.z /= scale
25 
26  return pt
27 
28 def get_quat(orientation):
29  quat = Quaternion()
30  if orientation is None:
31  quat.x = 0.0
32  quat.y = 0.0
33  quat.z = 0.0
34  quat.w = 1.0
35  elif('x' in dir(orientation)):
36  quat.w = orientation.w
37  quat.x = orientation.x
38  quat.y = orientation.y
39  quat.z = orientation.z
40  elif len(orientation)==4:
41  quat.x = orientation[0]
42  quat.y = orientation[1]
43  quat.z = orientation[2]
44  quat.w = orientation[3]
45  else:
46  q2 = tf.transformations.quaternion_from_euler(orientation[0],orientation[1],orientation[2])
47  quat.x = q2[0]
48  quat.y = q2[1]
49  quat.z = q2[2]
50  quat.w = q2[3]
51  return quat
52 
53 
55  def __init__(self):
56  self.reset()
57 
58  def reset(self):
59  self.counter = 0
60  self.frame_id = ''
61  self.ns = 'marker'
62  self.type = 0
63  self.action = Marker.ADD
64  self.scale = [1.0] *3
65  self.color = [1.0] * 4
66  self.points = []
67  self.colors = []
68  self.text = ''
69  self.lifetime = 0.0
70 
71  def marker(self, position=None, orientation=None, points=None, colors=None, scale=1.0):
72  mark = Marker()
73  mark.header.stamp = rospy.Time.now()
74  mark.header.frame_id = self.frame_id
75  mark.ns = self.ns
76  mark.type = self.type
77  mark.id = self.counter
78  mark.action = self.action
79  mark.scale.x = self.scale[0]
80  mark.scale.y = self.scale[1]
81  mark.scale.z = self.scale[2]
82  mark.color.r = self.color[0]
83  mark.color.g = self.color[1]
84  mark.color.b = self.color[2]
85  mark.color.a = self.color[3]
86  mark.lifetime = rospy.Duration(self.lifetime)
87 
88  if points is not None:
89  mark.points = []
90  for point in points:
91  mark.points.append(get_point(point, scale))
92  if colors is not None:
93  mark.colors = colors
94 
95  if position is not None or orientation is not None:
96  mark.pose.position = get_point(position, scale)
97  mark.pose.orientation = get_quat(orientation)
98 
99 
100 
101  self.counter+=1
102  return mark
def get_quat(orientation)
Definition: generator.py:28
def marker(self, position=None, orientation=None, points=None, colors=None, scale=1.0)
Definition: generator.py:71
def get_point(position, scale=1.0)
Definition: generator.py:7


easy_markers
Author(s): David V. Lu!!
autogenerated on Fri Jun 7 2019 22:01:08