[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [4812] Update python quaternion attitude visualizer
|
From: |
Allen Ibara |
|
Subject: |
[paparazzi-commits] [4812] Update python quaternion attitude visualizer ( plot multiple quaternions, more flexible message binding, more complex vehicle geometry, lighting, show bar graphs) |
|
Date: |
Tue, 20 Apr 2010 00:45:58 +0000 |
Revision: 4812
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4812
Author: aibara
Date: 2010-04-20 00:45:58 +0000 (Tue, 20 Apr 2010)
Log Message:
-----------
Update python quaternion attitude visualizer (plot multiple quaternions, more
flexible message binding, more complex vehicle geometry, lighting, show bar
graphs)
Modified Paths:
--------------
paparazzi3/trunk/sw/in_progress/python/attitude_viz.py
Modified: paparazzi3/trunk/sw/in_progress/python/attitude_viz.py
===================================================================
--- paparazzi3/trunk/sw/in_progress/python/attitude_viz.py 2010-04-19
13:22:50 UTC (rev 4811)
+++ paparazzi3/trunk/sw/in_progress/python/attitude_viz.py 2010-04-20
00:45:58 UTC (rev 4812)
@@ -15,26 +15,57 @@
_NAME = 'attitude_viz'
-DEFAULT_X = 320
-DEFAULT_Y = 240
+DEFAULT_X = 800
+DEFAULT_Y = 600
+class TelemetryQuat:
+ def __init__(self, message_name, index, name):
+ self.message_name = message_name
+ self.index = index
+ self.name = name
+ self.qi = 1
+ self.qx = 0
+ self.qy = 0
+ self.qz = 0
+
+class TelemetryValue:
+ def __init__(self, message_name, index, name, offset, scale, max):
+ self.message_name = message_name
+ self.index = index
+ self.name = name
+ self.offset = offset
+ self.scale = scale
+ self.max = max
+ self.value = 0
+
class MyGLCanvas(wx.glcanvas.GLCanvas):
def __init__(self, parent):
wx.glcanvas.GLCanvas.__init__(self, parent,-1)
self.Bind( wx.EVT_PAINT, self.OnPaint)
self.init = False
- self.qi = 1
- self.qx = 0
- self.qy = 0
- self.qz = 0
+ self.quats = []
+ self.graph_values = []
+ self.throttle = 0.0
+ self.mode = 0.0
+ self.airspeed = 0.0
+ for message_name, index, name in VEHICLE_QUATS:
+ self.quats.append(TelemetryQuat(message_name, index, name))
+ for message_name, index, name, offset, scale, max in BAR_VALUES:
+ self.graph_values.append(TelemetryValue(message_name, index, name,
offset, scale, max))
+
def onmsgproc(self, agent, *larg):
data = str(larg[0]).split(' ')
- quat_start = DATA_INDEX
- self.qi = float(data[quat_start + 0])
- self.qx = float(data[quat_start + 1])
- self.qy = float(data[quat_start + 2])
- self.qz = float(data[quat_start + 3])
+ for telemetry_quat in self.quats:
+ if (telemetry_quat.message_name == data[1]):
+ telemetry_quat.qi = float(data[telemetry_quat.index + 0])
+ telemetry_quat.qx = float(data[telemetry_quat.index + 1])
+ telemetry_quat.qy = float(data[telemetry_quat.index + 2])
+ telemetry_quat.qz = float(data[telemetry_quat.index + 3])
+
+ for graph_value in self.graph_values:
+ if (graph_value.message_name == data[1]):
+ graph_value.value = (float(data[graph_value.index + 0]) +
graph_value.offset) / graph_value.scale
def OnPaint(self,event):
if not self.init:
@@ -53,10 +84,12 @@
self.init = True
glutInit()
- # set viewing projection
-
glEnable(GL_LINE_SMOOTH)
glEnable(GL_DEPTH_TEST)
+ glEnable(GL_LIGHTING)
+ glEnable(GL_LIGHT0)
+ glEnable(GL_BLEND)
+ glShadeModel (GL_SMOOTH)
glClearColor(1.0, 1.0, 1.0, 1.0)
glClearDepth(1.0)
@@ -64,79 +97,166 @@
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
- gluPerspective(50.0, 1.0, 1.0, 30.0)
+ gluPerspective(7.0, 1.0, 95.0, 105.0)
glMatrixMode(GL_MODELVIEW)
- glLoadIdentity()
- gluLookAt(0.0, 0.0, 10.0,
- 0.0, 0.0, 0.0,
- 0.0, 1.0, 0.0)
+ glLight(GL_LIGHT0, GL_POSITION, [5, 30, -20])
+ glLight(GL_LIGHT0, GL_AMBIENT, [0.5, 0.5, 0.5])
+ glLight(GL_LIGHT0, GL_SPECULAR, [0.0, 0.0, 0.0])
+ glLight(GL_LIGHT0, GL_DIFFUSE, [0.8, 0.8, 0.8])
+ glEnable(GL_COLOR_MATERIAL)
+ glColorMaterial(GL_FRONT, GL_AMBIENT_AND_DIFFUSE)
+
+ def DrawCircle(self, radius):
+ glBegin(GL_TRIANGLE_FAN)
+ glVertex3f(0, 0, 0)
+ for angle in range (0, 361, 8):
+ glVertex3f( math.sin(math.radians(angle)) * radius,
math.cos(math.radians(angle)) * radius, 0)
+ glEnd()
+
+
+ # draw quad centered at origin, z = 0
+ def DrawQuad(self, width, height):
+ glBegin (GL_QUADS)
+ glVertex3f( width, height, 0)
+ glVertex3f( -width, height, 0)
+ glVertex3f( -width, -height, 0)
+ glVertex3f( width, -height, 0)
+ glEnd()
+
+ def DrawBox(self, width, height, depth):
+ glPushMatrix()
+ glTranslate(0, 0, depth)
+ self.DrawQuad(width, height)
+ glTranslate(0, 0, -2 * depth)
+ self.DrawQuad(width, height)
+ glPopMatrix()
+
+ glPushMatrix()
+ glRotate(90, 1, 0, 0)
+ glTranslate(0, 0, height)
+ self.DrawQuad(width, depth)
+ glTranslate(0, 0, -2 * height)
+ self.DrawQuad(width, depth)
+ glPopMatrix()
+
+ glPushMatrix()
+ glRotate(90, 0, 1, 0)
+ glTranslate(0, 0, width)
+ self.DrawQuad(depth, height)
+ glTranslate(0, 0, -2 * width)
+ self.DrawQuad(depth, height)
+ glPopMatrix()
+
+ def DrawVehicle(self, name):
+ wingspan = 2.7
+ separation = 0.7
+ chord = 0.35
+ thickness = 0.08
+ strutcount = 5
+ discradius = 0.45
+ discseparation = 0.01
+
+ #wings
+ glColor3f(0.1, 0.1, 0.9)
+ glPushMatrix()
+ glTranslate(0, 0, separation)
+ self.DrawBox(wingspan, chord, thickness)
+ glColor3f(0.0, 0.0, 0.0)
+ glTranslate(-wingspan, -0.2, thickness + 0.01)
+ glScale(0.004, 0.004, 0.004)
+ glutStrokeString(GLUT_STROKE_ROMAN, name)
+ glPopMatrix()
+
+ glPushMatrix()
+ glTranslate(0, 0, -separation)
+ glColor3f(0.6, 0.6, 0.2)
+ self.DrawBox(wingspan, chord, thickness)
+ glColor3f(0.0, 0.0, 0.0)
+ glTranslate(wingspan, -0.2, -0.01 - thickness)
+ glScale(0.004, 0.004, 0.004)
+ glRotate(180, 0, 1, 0)
+ glutStrokeString(GLUT_STROKE_ROMAN, name)
+ glPopMatrix()
+
+ # struts
+ glColor3f(0.4, 0.4, 0.4)
+ glPushMatrix()
+ glTranslate(-wingspan, 0, 0)
+ glRotate(90, 0, 1, 0)
+ for x in range (0, strutcount):
+ self.DrawBox(separation, chord - .01, thickness)
+ glTranslate(0, 0, 2 * wingspan/(strutcount - 1))
+ glPopMatrix()
+
+ #rotors
+ glColor3f(0.9, 0.1, 0.1)
+ glPushMatrix()
+ glRotate(90, 1, 0, 0)
+ glTranslate(-wingspan, separation, -(chord + .01))
+ for x in range (0, strutcount):
+ if (x != strutcount/2):
+ self.DrawCircle(discradius)
+ glTranslate(2 * wingspan/(strutcount - 1), 0, 0)
+ glPopMatrix()
+
+ glPushMatrix()
+ glRotate(90, 1, 0, 0)
+ glTranslate(-wingspan, -separation, -(chord + .01))
+ for x in range (0, strutcount):
+ if (x != strutcount/2):
+ self.DrawCircle(discradius)
+ glTranslate(2 * wingspan/(strutcount - 1), 0, 0)
+ glPopMatrix()
+
+ def DrawBar(self, name, value):
+ bar_height = 0.12
+ bar_length = 3
+ glPushMatrix()
+ glColor3f(0, 0, 0)
+ glTranslate(-bar_length, -0.09, 0.02)
+ glScale(0.0015, 0.0015, 0.0015)
+ glutStrokeString(GLUT_STROKE_ROMAN, name)
+ glPopMatrix()
+ glColor3f(0.92, 0.92, 0.92)
+ glPushMatrix()
+ glTranslate(0, 0, 0)
+ self.DrawQuad(bar_length, bar_height)
+ glPopMatrix()
+ glPushMatrix()
+ glTranslate(bar_length * value - bar_length, 0, 0.01)
+ glColor3f(0.6, 0.6, 0.6)
+ self.DrawQuad(bar_length * value, bar_height)
+ glPopMatrix()
+
def Draw(self):
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
- gluLookAt(0.0, 0.0, 10.0,
+ gluLookAt(0.0, 0.0, 100.0,
0.0, 0.0, 0.0,
0.0, 1.0, 0.0)
- glRotate(ROTATION_DIRECTION * 360 * math.acos(self.qi) / math.pi,
self.qy, -self.qz, -self.qx)
- if (ROTATE_180):
- glRotate(180, 0, 1, 0)
-
- span = 3 * 0.8
- height = 3 * 0.5
- length = 3 * 0.3
-
- glBegin (GL_QUADS)
- glColor3f( 0.9, 0.0, 0.0)
- glVertex3f( span, length, height)
- glVertex3f( -span, length, height)
- glVertex3f( -span, -length, height)
- glVertex3f( span, -length, height)
- glEnd()
+ height = 5
- glBegin (GL_QUADS)
- glColor3f( 0.0, 0.8, 0.8)
- glVertex3f( span, length, -height)
- glVertex3f( -span, length, -height)
- glVertex3f( -span, -length, -height)
- glVertex3f( span, -length, -height)
- glEnd()
+ glDisable(GL_LIGHTING)
+ glPushMatrix()
+ for graph_value in self.graph_values:
+ self.DrawBar(graph_value.name % (graph_value.value), graph_value.value
/ graph_value.max)
+ glTranslate(0, 0.35, 0)
+ glPopMatrix()
+ glEnable(GL_LIGHTING)
- glBegin (GL_QUADS)
- glColor3f( 0.0, 0.6, 0.6)
- glVertex3f( span, length, height)
- glVertex3f( span, -length, height)
- glVertex3f( span, -length, -height)
- glVertex3f( span, length, -height)
- glEnd()
+ glTranslate(0, -height + (height / len(self.quats) + 1), 0)
+ for telemetry_quat in self.quats:
+ glPushMatrix()
+ glRotate(360 * math.acos(telemetry_quat.qi) / math.pi,
telemetry_quat.qy, -telemetry_quat.qz, -telemetry_quat.qx)
+ self.DrawVehicle(telemetry_quat.name)
+ glPopMatrix()
+ glTranslate(0, 2 * height / (len(self.quats)), 0)
- glBegin (GL_QUADS)
- glColor3f( 0.6, 0.0, 0.0)
- glVertex3f( -span, length, height)
- glVertex3f( -span, -length, height)
- glVertex3f( -span, -length, -height)
- glVertex3f( -span, length, -height)
- glEnd()
-
- glBegin (GL_QUADS)
- glColor3f( 0.0, 0.6, 0.0)
- glVertex3f( span, length, height)
- glVertex3f( -span, length, height)
- glVertex3f( -span, length, -height)
- glVertex3f( span, length, -height)
- glEnd()
-
- glBegin (GL_QUADS)
- glColor3f( 0.6, 0.0, 0.6)
- glVertex3f( span, -length, height)
- glVertex3f( -span, -length, height)
- glVertex3f( -span, -length, -height)
- glVertex3f( span, -length, -height)
- glEnd()
-
class MainWindow(wx.Frame):
""" simple wx.Frame derived class. """
def __init__(self, parent, id, title):
@@ -161,9 +281,22 @@
lambda x,y: y,
lambda x,z: z
)
+
IvyStart("")
- bind_string = "(^.*" + MESSAGE_NAME + ".*$)"
- IvyBindMsg(self.panel.onmsgproc, bind_string)
+
+ # list of all message names
+ messages = []
+
+ # append all message names
+ for vehicle_quat in VEHICLE_QUATS:
+ messages.append(vehicle_quat[0])
+ for bar_value in BAR_VALUES:
+ messages.append(bar_value[0])
+
+ # bind to set of messages (ie, only bind each message once)
+ for message_name in set(messages):
+ bind_string = "(^.*" + message_name + ".*$)"
+ IvyBindMsg(self.panel.onmsgproc, bind_string)
def OnPaint(self, event):
pass
@@ -172,13 +305,11 @@
self.size_x = DEFAULT_X
self.size_y = DEFAULT_X
- self.edit = wx.TextCtrl(self)
self.panel = MyGLCanvas(self)
self.panel.SetClientSize( (self.size_x, self.size_y))
def InitLayout( self):
box = wx.BoxSizer(wx.VERTICAL)
box.Add( self.panel, 0, wx.EXPAND)
- box.Add( self.edit, 0, wx.EXPAND)
box.Fit( self)
self.SetSizer( box)
self.Fit()
@@ -188,49 +319,23 @@
if __name__ == "__main__":
- global DATA_INDEX
- DATA_INDEX = 6
- ROTATION_DIRECTION = 1
- ROTATE_180 = 0
- MESSAGE_NAME = "BOOZ2_AHRS_REF_QUAT"
- TITLE = "Attitude_Viz"
+ global VEHICLE_QUATS, BAR_VALUES
+ VEHICLE_QUATS = [ ["AHRS_DEBUG_QUAT", 2, "JOBY"], ["AHRS_DEBUG_QUAT", 10,
"POINE"], ["AHRS_DEBUG_QUAT", 6, "XSENS Estimation"], ["BOOZ2_AHRS_REF_QUAT",
2, "Reference"]]
+ BAR_VALUES = [ ["AIRSPEED", 3, "Airspeed (m/s) %i", 0, 1, 40],
["BOOZ2_RADIO_CONTROL", 5, "Throttle (%%) %i", 9600, 96 * 2, 100],
["BOOZ2_RADIO_CONTROL", 6, "Mode %i", -9600, -9600, 2]]
+ window_title = "Attitude_Viz"
try:
- opts, args = getopt.getopt(sys.argv[1:], "braod:t:m:",
- ["double-buffer",
- "reverse",
- "rot",
- "title",
- "message",
- "data-index"])
+ opts, args = getopt.getopt(sys.argv[1:], "t:",
+ ["title"])
for o,a in opts:
- if o in ("-b", "--double-buffer"):
- doubleBuffer = True
- elif o in ("-d", "--data-index"):
- DATA_INDEX = int(a)
- elif o in ("-r", "--reverse"):
- ROTATION_DIRECTION=-1
- elif o in ("-o", "--rot"):
- print "rot"
- ROTATE_180=1
- elif o in ("-t", "--title"):
- TITLE=a
- elif o in ("-m", "--message"):
- MESSAGE_NAME=a
- elif o in ("-a", "--alt-message"):
- MESSAGE_NAME = "AHRS_DEBUG"
+ if o in ("-t", "--title"):
+ window_title = a
except getopt.error, msg:
print msg
print """usage:
--b, --double-buffer enable double buffer
--d, --data-index set data index
--r, --reverse reverse rotation direction
--o, --rot rotate 180 in roll
-t, --title set window title
--m, --message set message name
--a, --alt-message use alternate message name
"""
app = wx.PySimpleApp()
- frame = MainWindow( None, -1, TITLE)
+ frame = MainWindow( None, -1, window_title)
frame.Center()
app.MainLoop()
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [4812] Update python quaternion attitude visualizer ( plot multiple quaternions, more flexible message binding, more complex vehicle geometry, lighting, show bar graphs),
Allen Ibara <=