Update attitude_viz -- ported to pygame

sw/in_progress/python/attitude_viz.py

@@ -2,8 +2,6 @@
 
 # Tool for visualizing quaternion as rotated cube
 
-import wx
-import wx.glcanvas
 from OpenGL.GLUT import *
 from OpenGL.GLU import *
 from  OpenGL.GL import *
@@ -13,10 +11,10 @@ from ivy.std_api import *
 import logging
 import getopt
 
-_NAME = 'attitude_viz'
+import pygame
+import time
 
-DEFAULT_X = 800
+_NAME = 'attitude_viz'
-DEFAULT_Y = 600
 
 class TelemetryQuat:
   def __init__(self, message_name, index, name):
@@ -38,26 +36,25 @@ class TelemetryValue:
     self.max = max
     self.value = 0
 
-class MyGLCanvas(wx.glcanvas.GLCanvas):
+class Visualization:
   def __init__(self, parent):
-      wx.glcanvas.GLCanvas.__init__(self, parent,-1)
-      self.Bind( wx.EVT_PAINT, self.OnPaint)
-      self.init = False
     self.quats = []
     self.graph_values = []
     self.throttle = 0.0
     self.mode = 0.0
     self.airspeed = 0.0
+    self.display_list = None
+    self.display_dirty = True
     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(' ')
     for telemetry_quat in self.quats:
       if (telemetry_quat.message_name == data[1]):
+        self.display_dirty = True
         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])
@@ -65,57 +62,16 @@ class MyGLCanvas(wx.glcanvas.GLCanvas):
 
     for graph_value in self.graph_values:
       if (graph_value.message_name == data[1]):
+        self.display_dirty = True
         graph_value.value = (float(data[graph_value.index + 0]) + graph_value.offset) / graph_value.scale
   
-    def OnPaint(self,event):
-      if not self.init:
-        self.SetCurrent()
-        self.InitGL()
-
-      try:
-        self.Draw()
-      except:
-        raise 
-      finally:
-        self.SwapBuffers()
-        self.Refresh()
-	
-    def InitGL(self):
-      self.init = True
-      
-      glutInit()
-      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)
-      
-      glPointSize(3.0)
-
-      glMatrixMode(GL_PROJECTION)
-      glLoadIdentity()
-      gluPerspective(7.0, 1.0, 95.0, 105.0)
-
-      glMatrixMode(GL_MODELVIEW)
-
-      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):
+    for angle in range (0, 361, 12):
       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)
@@ -180,6 +136,9 @@ class MyGLCanvas(wx.glcanvas.GLCanvas):
     glutStrokeString(GLUT_STROKE_ROMAN, name)
     glPopMatrix()
 
+    if self.display_list is None:
+      self.display_list = glGenLists(1)
+      glNewList(self.display_list, GL_COMPILE)
       # struts
       glColor3f(0.4, 0.4, 0.4)
       glPushMatrix()
@@ -209,6 +168,9 @@ class MyGLCanvas(wx.glcanvas.GLCanvas):
           self.DrawCircle(discradius)
         glTranslate(2 * wingspan/(strutcount - 1), 0, 0)
       glPopMatrix()
+      glEndList()
+
+    glCallList(self.display_list)
 
   def DrawBar(self, name, value):
     bar_height = 0.12
@@ -233,11 +195,7 @@ class MyGLCanvas(wx.glcanvas.GLCanvas):
   def Draw(self):
     glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
 
-      glMatrixMode(GL_MODELVIEW)
+    glPushMatrix()
-      glLoadIdentity()
-      gluLookAt(0.0, 0.0, 100.0,
-                0.0, 0.0, 0.0,
-                0.0, 1.0, 0.0)
 
     height = 5
 
@@ -252,27 +210,24 @@ class MyGLCanvas(wx.glcanvas.GLCanvas):
     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)
+      try: 
+        telemetry_quat.qi = telemetry_quat.qi * .00003051757812
+        telemetry_quat.qx = telemetry_quat.qx * .00003051757812
+        telemetry_quat.qy = telemetry_quat.qy * .00003051757812
+        telemetry_quat.qz = telemetry_quat.qz * .00003051757812
+        glRotate(360 * math.acos(telemetry_quat.qi ) / math.pi, telemetry_quat.qy, -telemetry_quat.qz, -telemetry_quat.qx)
         glRotate(-90, 1, 0, 0)
         self.DrawVehicle(telemetry_quat.name)
+      except Exception:
+        pass
+      finally:
         glPopMatrix()
         glTranslate(0,  2 * height / (len(self.quats)), 0)
+    glPopMatrix()
 
-class MainWindow(wx.Frame):
+class Visualizer:
-  """ simple wx.Frame derived class. """
+  def __init__(self):
-  def __init__(self, parent, id, title):
+    self.visualization = Visualization(self)
-    wx.Frame.__init__(self, parent, wx.ID_ANY, title, size=(200,100),
-                      style=wx.DEFAULT_FRAME_STYLE|wx.NO_FULL_REPAINT_ON_RESIZE)
-
-    self.GenerateGui()
-    self.InitLayout()
-
-    # bind painting events
-    self.Bind( wx.EVT_CLOSE, self.OnClose)
-    self.Bind( wx.EVT_PAINT, self.OnPaint)
-
-    # display the window
-    self.Show( True)
 
     # listen to Ivy
     logging.getLogger('Ivy').setLevel(logging.WARN)
@@ -297,36 +252,65 @@ class MainWindow(wx.Frame):
     # 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)
+      IvyBindMsg(self.visualization.onmsgproc, bind_string)
-  def OnPaint(self, event):
-    pass
 
-  def GenerateGui( self):
+  def Draw(self):
-    # generate panel and edit box
+    if self.visualization.display_dirty:
-    self.size_x = DEFAULT_X
+      self.visualization.Draw()
-    self.size_y = DEFAULT_X
+      self.visualization.display_dirty = False
   
-    self.panel = MyGLCanvas(self)
+  def OnClose(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.Fit( self)
-      self.SetSizer( box)
-      self.Fit()
-  def OnClose(self, event):
     IvyStop()
-      self.Destroy()
 
+SCREEN_SIZE = (800, 800)
 
-if __name__ == "__main__":
+def resize(width, height):
+  glViewport(0, 0, width, height)
+  glMatrixMode(GL_PROJECTION)
+  glLoadIdentity()
+  gluPerspective(60.0, float(width/height), .1, 100.)
+  glMatrixMode(GL_MODELVIEW)
+  glLoadIdentity()
+
+def init():
+  glutInit()
+  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)
+    
+  glPointSize(3.0)
+
+  glMatrixMode(GL_PROJECTION)
+  glLoadIdentity()
+  gluPerspective(7.0, 1.0, 95.0, 105.0)
+
+  glMatrixMode(GL_MODELVIEW)
+
+  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)
+
+  glMatrixMode(GL_MODELVIEW)
+  glLoadIdentity()
+  gluLookAt(0.0, 0.0, 100.0,
+            0.0, 0.0, 0.0,
+            0.0, 1.0, 0.0)
+
+def run():
   global VEHICLE_QUATS, BAR_VALUES
-  VEHICLE_QUATS = [ ["AP_EST2USER_0", 11, "Loose Ins Gps"], ["BOOZ2_AHRS_REF_QUAT", 2, "Reference"]]
+  VEHICLE_QUATS = [ ["BOOZ2_AHRS_QUAT", 6, "Estimated"], ["BOOZ2_AHRS_QUAT", 2, "Reference"]]
-  BAR_VALUES = [ ["AIRSPEED", 3, "Airspeed (m/s) %i", 0, 1, 40], ["BOOZ2_RADIO_CONTROL", 5, "Throttle (%%) %i", 9600, 96 * 2, 100], ["AP_RC", 6, "RC_MODE %i", 0, 1, 2],["AP_RC", 7, "RC_AUX2 %i", 0, 1, 2]]
+  BAR_VALUES = [ ["BOOZ2_RADIO_CONTROL", 5, "Throttle (%%) %i", 9600, 96 * 2, 100] ]
   window_title = "Attitude_Viz"
   try:
-    opts, args = getopt.getopt(sys.argv[1:], "t:",
+    opts, args = getopt.getopt(sys.argv[1:], "t:", ["title"])
-      ["title"])
     for o,a in opts:
       if o in ("-t", "--title"):
         window_title = a
@@ -335,8 +319,22 @@ if __name__ == "__main__":
     print """usage:
 -t, --title                   set window title
 """
+  pygame.init()
+  screen = pygame.display.set_mode(SCREEN_SIZE, pygame.OPENGL|pygame.DOUBLEBUF)
+  #resize(*SCREEN_SIZE)
+  init()
+  visualizer = Visualizer()
+  while True:
+    for event in pygame.event.get():
+      if event.type == pygame.QUIT:
+        visualizer.OnClose()
+        return
+      if event.type == pygame.KEYUP and event.key == pygame.K_ESCAPE:
+        visualizer.OnClose()
+        return
+    visualizer.Draw()
+    pygame.display.flip()
+    time.sleep(.02)
 
-  app = wx.PySimpleApp()
+if __name__ == "__main__":
-  frame = MainWindow( None, -1, window_title)
+  run()
-  frame.Center()
-  app.MainLoop()