objects.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import time
4 
5 import rospy
6 import rosparam
7 
8 import tf.transformations as tft
9 import tf2_ros
10 
13 from frame_editor.srv import *
14 import utils_tf
15 
16 from geometry_msgs.msg import Pose
17 
18 from visualization_msgs.msg import InteractiveMarkerControl, Marker
19 import rospkg
20 
21 
22 class Frame(object):
23 
24  tf_broadcaster = None
25  tf_buffer = None
26  tf_listener = None
27 
28  __id_counter = -1
29 
30  def __init__(self, name, position=(0,0,0), orientation=(0,0,0,1), parent="world", style="none"):
31  self.name = name
32  self.position = position
33  self.orientation = orientation
34  self.parent = parent
35  self.style = style
36  self.color = (0.0, 0.5, 0.5, 0.75)
37 
38  self.hidden = False
39  self.marker = None
40 
41  @staticmethod
42  def init_tf():
43  if Frame.tf_buffer is None:
44  Frame.tf_broadcaster = tf2_ros.TransformBroadcaster()
45  Frame.tf_buffer = tf2_ros.Buffer()
46  Frame.tf_listener = tf2_ros.TransformListener(Frame.tf_buffer)
47 
48  @classmethod
49  def create_new_id(cls):
50  cls.__id_counter = cls.__id_counter + 1
51  return cls.__id_counter
52 
53  @property
54  def pose(self):
55  return ToPose(self.position, self.orientation)
56 
57  def print_all(self):
58  print(" {} (parent: {}) {} {}".format(self.name, self.parent, self.position, self.orientation))
59 
60  def value(self, symbol):
61  if symbol == 'x':
62  return self.position[0]
63  elif symbol == 'y':
64  return self.position[1]
65  elif symbol == 'z':
66  return self.position[2]
67  else:
68  rpy = tft.euler_from_quaternion(self.orientation)
69  if symbol == 'a':
70  return rpy[0]
71  elif symbol == 'b':
72  return rpy[1]
73  elif symbol == 'c':
74  return rpy[2]
75 
76  def set_value(self, symbol, value):
77  if symbol in ['x', 'y', 'z']:
78  position = list(self.position)
79  if symbol == 'x':
80  position[0] = value
81  elif symbol == 'y':
82  position[1] = value
83  elif symbol == 'z':
84  position[2] = value
85  self.position = tuple(position)
86  else:
87  rpy = list(tft.euler_from_quaternion(self.orientation))
88  if symbol == 'a':
89  rpy[0] = value
90  elif symbol == 'b':
91  rpy[1] = value
92  elif symbol == 'c':
93  rpy[2] = value
94  self.orientation = tuple(tft.quaternion_from_euler(*rpy))
95 
96  @staticmethod
97  def can_transform(target_frame, source_frame, time_):
98  return utils_tf.can_transform(
99  Frame.tf_buffer, target_frame, source_frame, time_)
100 
101  @staticmethod
102  def wait_for_transform(target_frame, source_frame, timeout):
103  return utils_tf.wait_for_transform(
104  Frame.tf_buffer, target_frame, source_frame, timeout)
105 
106 
108 
109  def __init__(self, name, position, orientation, parent, style):
110 
111  super(Object_Geometry, self).__init__(name, position, orientation, parent, style)
112 
113  ## TODO: put this into interface_marker.py
114  self.marker = Marker()
115  self.marker.scale = NewVector3(1, 1, 1)
116  self.marker.pose = Pose()
117  self.marker.color = NewColor(self.color[0], self.color[1], self.color[2], self.color[3])
118  self.marker.id = Frame.create_new_id()
119  #self.marker.lifetime = 0 # forever
120  self.update_marker()
121 
122  def update_marker(self):
123  self.marker.header.frame_id = self.name
124  self.marker.header.stamp = rospy.Time.now()
125 
126  def set_color(self, color):
127  self.color = color
128  self.marker.color = NewColor(color[0], color[1], color[2], color[3])
129  self.update_marker()
130  #self.marker.update()
131 
132 
134 
135  def __init__(self, name, position, orientation, parent, length=1.0, width=1.0):
136 
137  self.length = length
138  self.width = width
139 
140  super(Object_Plane, self).__init__(name, position, orientation, parent, "plane")
141 
142  def update_marker(self):
143  super(Object_Plane, self).update_marker()
144 
145  l = self.length*0.5
146  w = self.width*0.5
147 
148  self.marker.type = Marker.TRIANGLE_LIST
149  self.marker.points = [
150  NewPoint(-l, -w, 0.0), NewPoint(l, -w, 0.0), NewPoint(-l, w, 0.0),
151  NewPoint( l, -w, 0.0), NewPoint(l, w, 0.0), NewPoint(-l, w, 0.0)
152  ]
153 
154 
156 
157  def __init__(self, name, position, orientation, parent, length=1.0, width=1.0, height=1.0):
158 
159  self.length = length
160  self.width = width
161  self.height = height
162 
163  super(Object_Cube, self).__init__(name, position, orientation, parent, "cube")
164 
165  def update_marker(self):
166  super(Object_Cube, self).update_marker()
167 
168  self.marker.type = Marker.CUBE
169  self.marker.scale = NewVector3(self.length, self.width, self.height)
170 
171 
173 
174  def __init__(self, name, position, orientation, parent, diameter=1.0):
175 
176  self.diameter = diameter
177 
178  super(Object_Sphere, self).__init__(name, position, orientation, parent, "sphere")
179 
180  def update_marker(self):
181  super(Object_Sphere, self).update_marker()
182 
183  self.marker.type = Marker.SPHERE
184  self.marker.scale = NewVector3(self.diameter, self.diameter, self.diameter)
185 
186 
188 
189  def __init__(self, name, position, orientation, parent, length=1.0, width=0.05):
190 
191  self.length = length
192  self.width = width
193 
194  super(Object_Axis, self).__init__(name, position, orientation, parent, "axis")
195 
196  def update_marker(self):
197  super(Object_Axis, self).update_marker()
198 
199  self.marker.type = Marker.ARROW
200  self.marker.scale = NewVector3(self.length, self.width, self.width)
201 
202 
204 
205  def __init__(self, name, position, orientation, parent, package=None, mesh_path="", scale=1.0):
206 
207  self.scale = scale
208  self.path = mesh_path
209  self.package = package
210 
211  super(Object_Mesh, self).__init__(name, position, orientation, parent, "mesh")
212 
213  def update_marker(self):
214  super(Object_Mesh, self).update_marker()
215 
216  self.marker.type = Marker.MESH_RESOURCE
217  if self.package == "" or self.package is None:
218  self.marker.mesh_resource = "file:"+self.path
219  else:
220  self.marker.mesh_resource = "file:"+rospkg.RosPack().get_path(self.package)+"/"+self.path
221 
222  self.marker.scale = NewVector3(self.scale, self.scale, self.scale)
223 
224 # eof
def __init__(self, name, position, orientation, parent, length=1.0, width=1.0, height=1.0)
Definition: objects.py:157
def __init__(self, name, position, orientation, parent, length=1.0, width=1.0)
Definition: objects.py:135
def set_value(self, symbol, value)
Definition: objects.py:76
def wait_for_transform(target_frame, source_frame, timeout)
Definition: objects.py:102
def __init__(self, name, position, orientation, parent, diameter=1.0)
Definition: objects.py:174
def can_transform(target_frame, source_frame, time_)
Definition: objects.py:97
def __init__(self, name, position=(0, 0, 0), orientation=(0, 0, 0, 1), parent="world", style="none")
Definition: objects.py:30
def __init__(self, name, position, orientation, parent, style)
Definition: objects.py:109
def __init__(self, name, position, orientation, parent, length=1.0, width=0.05)
Definition: objects.py:189
def __init__(self, name, position, orientation, parent, package=None, mesh_path="", scale=1.0)
Definition: objects.py:205
def NewColor(r, g, b, a=1.0)
Color ##.
def value(self, symbol)
Definition: objects.py:60


frame_editor
Author(s): ipa-lth , ipa-frn
autogenerated on Wed Oct 21 2020 03:36:00