Update python quaternion attitude visualizer (plot multiple quaternions, more flexible message binding, more complex vehicle geometry, lighting, show bar graphs)

2026-05-23 21:36:28 +08:00 · 2010-04-20 00:45:58 +00:00
parent fa34c9a5f2
commit d33e7e38bb
1 changed files with 216 additions and 111 deletions
@@ -15,26 +15,57 @@ import getopt

 _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 @@ class MyGLCanvas(wx.glcanvas.GLCanvas):
      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,78 +97,165 @@ class MyGLCanvas(wx.glcanvas.GLCanvas):

      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()
-
-      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()
+      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)

 class MainWindow(wx.Frame):
  """ simple wx.Frame derived class. """
@@ -161,9 +281,22 @@ class MainWindow(wx.Frame):
      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 @@ class MainWindow(wx.Frame):
    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 @@ class MainWindow(wx.Frame):

      
 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()