00001
00002
00003 import time
00004
00005 import rospy
00006 import rosparam
00007
00008 import tf.transformations as tft
00009 import tf2_ros
00010
00011 from frame_editor.constructors_geometry import *
00012 from frame_editor.constructors_std import *
00013 from frame_editor.srv import *
00014 from geometry_msgs.msg import Pose
00015
00016 from visualization_msgs.msg import InteractiveMarkerControl, Marker
00017
00018
00019 class Frame(object):
00020
00021 tf_broadcaster = tf2_ros.TransformBroadcaster()
00022 tf_buffer = tf2_ros.Buffer()
00023 tf_listener = tf2_ros.TransformListener(tf_buffer)
00024
00025 __id_counter = -1
00026
00027 def __init__(self, name, position=(0,0,0), orientation=(0,0,0,1), parent="world", style="none"):
00028 self.name = name
00029 self.position = position
00030 self.orientation = orientation
00031 self.parent = parent
00032 self.style = style
00033 self.color = (0.0, 0.5, 0.5, 0.75)
00034
00035 self.hidden = False
00036 self.marker = None
00037
00038 @classmethod
00039 def create_new_id(cls):
00040 cls.__id_counter = cls.__id_counter + 1
00041 return cls.__id_counter
00042
00043 @property
00044 def pose(self):
00045 return ToPose(self.position, self.orientation)
00046
00047 def print_all(self):
00048 print " {} (parent: {}) {} {}".format(self.name, self.parent, self.position, self.orientation)
00049
00050 def value(self, symbol):
00051 if symbol == 'x':
00052 return self.position[0]
00053 elif symbol == 'y':
00054 return self.position[1]
00055 elif symbol == 'z':
00056 return self.position[2]
00057 else:
00058 rpy = tft.euler_from_quaternion(self.orientation)
00059 if symbol == 'a':
00060 return rpy[0]
00061 elif symbol == 'b':
00062 return rpy[1]
00063 elif symbol == 'c':
00064 return rpy[2]
00065
00066 def set_value(self, symbol, value):
00067 if symbol in ['x', 'y', 'z']:
00068 position = list(self.position)
00069 if symbol == 'x':
00070 position[0] = value
00071 elif symbol == 'y':
00072 position[1] = value
00073 elif symbol == 'z':
00074 position[2] = value
00075 self.position = tuple(position)
00076 else:
00077 rpy = list(tft.euler_from_quaternion(self.orientation))
00078 if symbol == 'a':
00079 rpy[0] = value
00080 elif symbol == 'b':
00081 rpy[1] = value
00082 elif symbol == 'c':
00083 rpy[2] = value
00084 self.orientation = tuple(tft.quaternion_from_euler(*rpy))
00085
00086
00087 class Object_Geometry(Frame):
00088
00089 def __init__(self, name, position, orientation, parent, style):
00090
00091 super(Object_Geometry, self).__init__(name, position, orientation, parent, style)
00092
00093
00094 self.marker = Marker()
00095 self.marker.scale = NewVector3(1, 1, 1)
00096 self.marker.pose = Pose()
00097 self.marker.color = NewColor(self.color[0], self.color[1], self.color[2], self.color[3])
00098 self.marker.id = Frame.create_new_id()
00099
00100 self.update_marker()
00101
00102 def update_marker(self):
00103 self.marker.header.frame_id = self.name
00104 self.marker.header.stamp = rospy.Time.now()
00105
00106 def set_color(self, color):
00107 self.color = color
00108 self.marker.color = NewColor(color[0], color[1], color[2], color[3])
00109 self.update_marker()
00110
00111
00112
00113 class Object_Plane(Object_Geometry):
00114
00115 def __init__(self, name, position, orientation, parent, length=1.0, width=1.0):
00116
00117 self.length = length
00118 self.width = width
00119
00120 super(Object_Plane, self).__init__(name, position, orientation, parent, "plane")
00121
00122 def update_marker(self):
00123 super(Object_Plane, self).update_marker()
00124
00125 l = self.length*0.5
00126 w = self.width*0.5
00127
00128 self.marker.type = Marker.TRIANGLE_LIST
00129 self.marker.points = [
00130 NewPoint(-l, -w, 0.0), NewPoint(l, -w, 0.0), NewPoint(-l, w, 0.0),
00131 NewPoint( l, -w, 0.0), NewPoint(l, w, 0.0), NewPoint(-l, w, 0.0)
00132 ]
00133
00134
00135 class Object_Cube(Object_Geometry):
00136
00137 def __init__(self, name, position, orientation, parent, length=1.0, width=1.0, height=1.0):
00138
00139 self.length = length
00140 self.width = width
00141 self.height = height
00142
00143 super(Object_Cube, self).__init__(name, position, orientation, parent, "cube")
00144
00145 def update_marker(self):
00146 super(Object_Cube, self).update_marker()
00147
00148 self.marker.type = Marker.CUBE
00149 self.marker.scale = NewVector3(self.length, self.width, self.height)
00150
00151
00152 class Object_Sphere(Object_Geometry):
00153
00154 def __init__(self, name, position, orientation, parent, diameter=1.0):
00155
00156 self.diameter = diameter
00157
00158 super(Object_Sphere, self).__init__(name, position, orientation, parent, "sphere")
00159
00160 def update_marker(self):
00161 super(Object_Sphere, self).update_marker()
00162
00163 self.marker.type = Marker.SPHERE
00164 self.marker.scale = NewVector3(self.diameter, self.diameter, self.diameter)
00165
00166
00167 class Object_Axis(Object_Geometry):
00168
00169 def __init__(self, name, position, orientation, parent, length=1.0, width=0.05):
00170
00171 self.length = length
00172 self.width = width
00173
00174 super(Object_Axis, self).__init__(name, position, orientation, parent, "axis")
00175
00176 def update_marker(self):
00177 super(Object_Axis, self).update_marker()
00178
00179 self.marker.type = Marker.ARROW
00180 self.marker.scale = NewVector3(self.length, self.width, self.width)
00181
00182
00183 class Object_Mesh(Object_Geometry):
00184
00185 def __init__(self, name, position, orientation, parent, mesh_path="", scale=1.0):
00186
00187 self.scale = scale
00188 self.path = mesh_path
00189
00190 super(Object_Mesh, self).__init__(name, position, orientation, parent, "mesh")
00191
00192 def update_marker(self):
00193 super(Object_Mesh, self).update_marker()
00194
00195 self.marker.type = Marker.MESH_RESOURCE
00196 self.marker.mesh_resource = "file:"+self.path
00197 self.marker.scale = NewVector3(self.scale, self.scale, self.scale)
00198
00199