asctec_mon: asctec_mon: monitor.py Source File

00001 #!/usr/bin/env python
00002 # AscTec Autopilot Console Monitor
00003 # Copyright (C) 2010, CCNY Robotics Lab
00004 # William Morris <morris@ee.ccny.cuny.edu>
00005 #
00006 # http://robotics.ccny.cuny.edu
00007 #
00008 # This program is free software: you can redistribute it and/or modify
00009 # it under the terms of the GNU General Public License as published by
00010 # the Free Software Foundation, either version 3 of the License, or
00011 # (at your option) any later version.
00012 #
00013 # This program is distributed in the hope that it will be useful,
00014 # but WITHOUT ANY WARRANTY; without even the implied warranty of
00015 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016 # GNU General Public License for more details.
00017 #
00018 # You should have received a copy of the GNU General Public License
00019 # along with this program.  If not, see <http://www.gnu.org/licenses/>.
00020 
00021 ################################
00022 # This code may not be pretty but is seems to work.
00023 # Commands
00024 # q = Quit
00025 # r = Record Bag File
00026 # f = Test flashing the screen
00027 # b = Test the terminal bell
00028 
00029 import roslib; roslib.load_manifest('asctec_mon')
00030 import rospy
00031 import curses
00032 import subprocess
00033 import os
00034 import signal
00035 
00036 from asctec_msgs.msg import LLStatus
00037 from asctec_msgs.msg import IMUCalcData
00038 from asctec_msgs.msg import GPSData
00039 
00040 myscreen = curses.initscr()
00041 curses.start_color()
00042 curses.halfdelay(1)
00043 curses.noecho()
00044 curses.curs_set(0)
00045 (maxx,maxy) = myscreen.getmaxyx()
00046 llwin = curses.newwin(11, maxy, maxx-11, 0)
00047 gpswin = curses.newwin(3, maxy, maxx-14, 0)
00048 recwin = curses.newwin(3, maxy, maxx-17, 0)
00049 imuwin = curses.newwin(maxx-17, maxy, 0, 0)
00050 alarm = 0
00051 alarm_count = 0
00052 alarm_interval = 10
00053 gps_lock = 1
00054 imu_lock = 1
00055 ll_lock = 1
00056 rec_status = 0
00057 rec_enable = 0
00058 rec_cmd = ["rosbag", "record", "-a", "-o","asctec"]
00059 rec_dir = str(os.environ['HOME'])+"/ros/bags"
00060 rec_process = None
00061 bag_name = None
00062 
00063 def drawSignedVal(r,c,w,val,val_max,val_min,big):
00064     center = int(w/2)
00065     if (val > val_max):
00066         val = val_max
00067     if (val < val_min):
00068         val = val_min
00069     if big:
00070         # Draw Top
00071         imuwin.addch(r, c, curses.ACS_ULCORNER)
00072         for n in range(c+1, c+w):
00073             if (n == c+center):
00074                 imuwin.addch(r, n, curses.ACS_TTEE)
00075             else:
00076                 imuwin.addch(r, n, curses.ACS_HLINE)
00077         imuwin.addch(r, c+w, curses.ACS_URCORNER)
00078         r = r + 1
00079 
00080     # Draw Middle
00081     imuwin.addch(r, c, curses.ACS_VLINE)
00082     bar = int(float(val / val_max * center))
00083     if (bar == 0):
00084         imuwin.addch(r, c+center, curses.ACS_VLINE)
00085     elif (bar >= 0):
00086         imuwin.addstr(r, c+center, " "*bar, curses.color_pair(4))
00087         imuwin.addch(r, c+center, curses.ACS_VLINE,curses.color_pair(4))
00088     else:
00089         imuwin.addstr(r, c+center+bar+1, " "*(-1*bar), curses.color_pair(4))
00090         imuwin.addch(r, c+center, curses.ACS_VLINE,curses.color_pair(4))
00091     imuwin.addch(r, c+w, curses.ACS_VLINE)
00092     r = r + 1
00093 
00094     if big:
00095         # Draw Bottom
00096         imuwin.addch(r, c, curses.ACS_LLCORNER)
00097         for n in range(c+1, c+w):
00098             if (n == c+center):
00099                 imuwin.addch(r, n, curses.ACS_BTEE)
00100             else:
00101                 imuwin.addch(r, n, curses.ACS_HLINE)
00102         imuwin.addch(r, c+w, curses.ACS_LRCORNER)
00103 
00104 def drawBattery(r,c,w,battery_val):
00105     global alarm
00106     # Battery Settings
00107     # Taken from http://en.wikipedia.org/wiki/Lithium-ion_polymer_battery
00108     battery_max = 12.7   # Maximum Voltage
00109     battery_warn = 10.0  # Warning Voltage
00110     battery_min = 8.4    # Minimum Voltage
00111 
00112     # Draw Top
00113     llwin.addch(r, c, curses.ACS_ULCORNER)
00114     for n in range(c+1, c+w):
00115         llwin.addch(r, n, curses.ACS_HLINE)
00116     llwin.addch(r, c+w, curses.ACS_URCORNER)
00117     r = r + 1
00118 
00119     # Draw Middle
00120     llwin.addch(r, c, curses.ACS_VLINE)
00121     b = int((battery_val - battery_min)/(battery_max-battery_min)*w)
00122     if battery_val > battery_warn: 
00123         alarm = 0
00124         llwin.addstr(r, c+1, " " * b, curses.color_pair(4))
00125     else:
00126         alarm = 1
00127         llwin.addstr(r, c+1, " " * b, curses.color_pair(5))
00128     llwin.addch(r, c+w, curses.ACS_VLINE)
00129     r = r + 1
00130 
00131     # Draw Bottom
00132     llwin.addch(r, c, curses.ACS_LLCORNER)
00133     for n in range(c+1, c+w):
00134         llwin.addch(r, n, curses.ACS_HLINE)
00135     llwin.addch(r, c+w, curses.ACS_LRCORNER)
00136 
00137 def drawStatusMode(r,c,w,data):
00138     # Draw Top
00139     size = int(w / 3)-1
00140     llwin.addch(r, c, curses.ACS_ULCORNER)
00141     for n in range(c+1, c+w):
00142         if ((n%(size+c))-2):
00143           llwin.addch(r, n, curses.ACS_HLINE)
00144         else:
00145           llwin.addch(r, n, curses.ACS_TTEE)
00146     llwin.addch(r, c+w, curses.ACS_URCORNER)
00147     r = r + 1
00148 
00149     size = size + 2
00150     pos = c+(size*0)
00151     llwin.addch(r, pos, curses.ACS_VLINE)
00152     if (data.compass_enabled):
00153         llwin.addstr(r,pos+1,"Compass",curses.color_pair(3)|curses.A_BOLD)
00154     else:
00155         llwin.addstr(r,pos+1,"Compass",curses.color_pair(0))
00156 
00157     pos = c+(size*1)
00158     llwin.addch(r, pos, curses.ACS_VLINE)
00159     llwin.addstr(r,pos+1,"Flight Time: "+str(data.up_time)+" sec",curses.color_pair(0))
00160 
00161     pos = c+(size*2)
00162     llwin.addch(r, pos, curses.ACS_VLINE)
00163     llwin.addstr(r,pos+1,"CPU: "+str(data.cpu_load),curses.color_pair(0))
00164 
00165     pos = c+w
00166     llwin.addch(r, pos, curses.ACS_VLINE)
00167     r = r + 1
00168 
00169     # Draw Bottom
00170     size = int(w / 3)-1
00171     llwin.addch(r, c, curses.ACS_LLCORNER)
00172     for n in range(c+1, c+w):
00173         if ((n%(size+c))-2):
00174           llwin.addch(r, n, curses.ACS_HLINE)
00175         else:
00176           llwin.addch(r, n, curses.ACS_BTEE)
00177     llwin.addch(r, c+w, curses.ACS_LRCORNER)
00178 
00179 def drawFlightMode(r,c,w,flightMode):
00180     # Draw Top
00181     size = int(w / 5)-2
00182     llwin.addch(r, c, curses.ACS_ULCORNER)
00183     for n in range(c+1, c+w):
00184         if (((n%(size+c))-2) or (n/size+c) > 6):
00185           llwin.addch(r, n, curses.ACS_HLINE)
00186         else:
00187           llwin.addch(r, n, curses.ACS_TTEE)
00188     llwin.addch(r, c+w, curses.ACS_URCORNER)
00189     r = r + 1
00190 
00191     size = size + 2
00192     # There are 5 Flight Modes but the bit index of the serial active
00193     # mode is currently unknown
00194     pos = c+(size*0)
00195     llwin.addch(r, pos, curses.ACS_VLINE)
00196     if ((flightMode|0b01111111)!=0b11111111):
00197         llwin.addstr(r,pos+1,"Emergency",curses.color_pair(0))
00198     else:
00199         llwin.addstr(r,pos+1,"Emergency",curses.color_pair(2)|curses.A_BOLD)
00200 
00201     pos = c+(size*1)
00202     llwin.addch(r, pos, curses.ACS_VLINE)
00203     if ((flightMode|0b11111101)!=0b11111111):
00204         llwin.addstr(r,pos+1,"Height Control",curses.color_pair(0))
00205     else:
00206         llwin.addstr(r,pos+1,"Height Control",curses.color_pair(3)|curses.A_BOLD)
00207 
00208     pos = c+(size*2)
00209     llwin.addch(r, pos, curses.ACS_VLINE)
00210     if ((flightMode|0b11111011)!=0b11111111):
00211         llwin.addstr(r,pos+1,"GPS Mode",curses.color_pair(0))
00212     else:
00213         llwin.addstr(r,pos+1,"GPS Mode",curses.color_pair(3)|curses.A_BOLD)
00214 
00215     pos = c+(size*3)
00216     llwin.addch(r, pos, curses.ACS_VLINE)
00217     if ((flightMode|0b11011111)!=0b11111111):
00218         llwin.addstr(r,pos+1,"Serial Enable",curses.color_pair(0))
00219     else:
00220         llwin.addstr(r,pos+1,"Serial Enable",curses.color_pair(3)|curses.A_BOLD)
00221 
00222     pos = c+(size*4)
00223     llwin.addch(r, pos, curses.ACS_VLINE)
00224     # FIXME: This is probably the wrong bitmask
00225     if ((flightMode|0b10111111)!=0b11111111):
00226         llwin.addstr(r,pos+1,"Serial Active",curses.color_pair(0))
00227     else:
00228         llwin.addstr(r,pos+1,"Serial Active",curses.color_pair(3)|curses.A_BOLD)
00229     pos = c+w
00230     llwin.addch(r, pos, curses.ACS_VLINE)
00231     r = r + 1
00232 
00233     # Draw Bottom
00234     size = int(w / 5)-2
00235     llwin.addch(r, c, curses.ACS_LLCORNER)
00236     for n in range(c+1, c+w):
00237         if (((n%(size+c))-2) or (n/size+c) > 6):
00238           llwin.addch(r, n, curses.ACS_HLINE)
00239         else:
00240           llwin.addch(r, n, curses.ACS_BTEE)
00241     llwin.addch(r, c+w, curses.ACS_LRCORNER)
00242 
00243 def record_update():
00244     global rec_status
00245     global rec_cmd
00246     global rec_dir
00247     global rec_process
00248     global rec_enable
00249     global bag_name
00250 
00251     recwin.clear()
00252     if rec_status:
00253         recattr = curses.color_pair(2)
00254     else:
00255         recattr = curses.color_pair(0)
00256     recwin.attrset(recattr)
00257     (rec_maxx,rec_maxy) = recwin.getmaxyx()
00258     rec_maxy = rec_maxy - 2 # remove space for left and right border
00259     recwin.border(0)
00260     
00261     if rec_enable != rec_status:
00262         if rec_enable:
00263             rec_process = subprocess.Popen(rec_cmd, shell=False, stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, cwd=rec_dir)
00264         else:
00265             bag_name = None
00266         rec_status = rec_enable
00267           
00268     if rec_status:
00269         if (bag_name == None or bag_name == ''):
00270             process = subprocess.Popen(['lsof -c record -Fn -- | grep active | cut -c2-'], shell=True, stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, cwd=rec_dir)
00271             bag_name = process.stdout.readline().rstrip()
00272         recwin.addstr(0,2,"Flight Data Recording",curses.color_pair(2)|curses.A_BOLD)
00273         recwin.attrset(curses.color_pair(0))
00274         recwin.addstr(1, 2, "Filename: ")
00275         recwin.addstr(1,12,bag_name)
00276     else:
00277         if (rec_process != None):
00278             rec_process.send_signal(signal.SIGINT)
00279         recwin.addstr(0,2,"Flight Data Recorder",curses.color_pair(1)|curses.A_BOLD)
00280         recwin.attrset(curses.color_pair(0))
00281         recwin.addstr(1, 2, "Command: ")
00282         recwin.addstr(1,11,' '.join(rec_cmd))
00283     if rec_process != None:
00284       if rec_process.poll() == 0:
00285         rec_process = None
00286 def gps_callback(data):
00287     global gps_lock
00288     gps_lock = 1
00289     gpswin.clear()
00290     (gps_maxx,gps_maxy) = imuwin.getmaxyx()
00291     gps_maxy = gps_maxy - 2 # remove space for left and right border
00292     gcol = 25
00293     gpswin.border(0)
00294     gpswin.addstr(0, 2, "GPS", curses.color_pair(1)|curses.A_BOLD)
00295     lat_val = float(data.latitude)/float(10**7)
00296     gpswin.addstr(1, 2, 'Lat: {0:+012.7f}'.format(lat_val))
00297     lon_val = float(data.longitude)/float(10**7)
00298     gpswin.addstr(1, 21, 'Lon: {0:+012.7f}'.format(lon_val))
00299     height_val = float(data.height)/1000.0
00300     gpswin.addstr(1, 40, 'Height: {0: 7.3f}m'.format(height_val))
00301     heading_val = float(data.heading)/1000.0
00302     gpswin.addstr(1, 58, 'Heading: {0: 7.3f}'.format(heading_val))
00303     gps_lock = 0
00304 
00305 def imu_callback(data):
00306     global imu_lock
00307     imu_lock = 1
00308     imuwin.clear()
00309     (imu_maxx,imu_maxy) = imuwin.getmaxyx()
00310     imu_maxy = imu_maxy - 2 # remove space for left and right border
00311     gcol = 25
00312     imuwin.border(0)
00313     imuwin.addstr(0, 2, "AscTec Quadrotor Console Monitor", curses.color_pair(1)|curses.A_BOLD)
00314 
00315     pos = 1
00316     if (imu_maxx > 16):
00317         pos_inc = 3
00318         big = 1
00319     else:
00320         pos_inc = 1
00321         big = 0
00322 
00323     # Height Graph
00324     ################################
00325     height = float(data.height)/1000.0
00326     imuwin.addstr(pos+big, 2, 'Height:    {0: 8.3f}m'.format(height))
00327     drawSignedVal(pos,gcol,imu_maxy-(gcol+1),height,10.0,-10.0,big)
00328     pos = pos + pos_inc
00329     
00330     # Roll Graph
00331     ################################
00332     roll = float(data.angle_roll)/1000.0
00333     imuwin.addstr(pos+big, 2, "Roll:      %+08.3f"%roll)
00334     imuwin.addch(pos+big, 21, curses.ACS_DEGREE)
00335     drawSignedVal(pos,gcol,imu_maxy-(gcol+1),roll,90.0,-90.0,big)
00336     pos = pos + pos_inc
00337 
00338     # Pitch Graph
00339     ################################
00340     pitch = float(data.angle_nick)/1000.0
00341     imuwin.addstr(pos+big, 2, "Pitch:     %+08.3f"%pitch)
00342     imuwin.addch(pos+big, 21, curses.ACS_DEGREE)
00343     drawSignedVal(pos,gcol,imu_maxy-(gcol+1),pitch,180.0,-180.0,big)
00344     pos = pos + pos_inc
00345 
00346     # Yaw Graph
00347     ################################
00348     yaw = float(data.angle_yaw)/1000.0 -180
00349     imuwin.addstr(pos+big, 2, "Fused Yaw: %+08.3f"%yaw)
00350     imuwin.addch(pos+big, 21, curses.ACS_DEGREE)
00351     drawSignedVal(pos,gcol,imu_maxy-(gcol+1),yaw,180.0,-180.0,big)
00352     pos = pos + pos_inc
00353 
00354     # Compass Graph
00355     ################################
00356     mag = float(data.mag_heading)/1000.0 -180
00357     imuwin.addstr(pos+big, 2, "Compass:   %+08.3f"%mag)
00358     imuwin.addch(pos+big, 21, curses.ACS_DEGREE)
00359     drawSignedVal(pos,gcol,imu_maxy-(gcol+1),mag,180.0,-180.0,big)
00360     pos = pos + pos_inc
00361 
00362     imu_lock = 0
00363 
00364 def callback(data):
00365     global ll_lock
00366     ll_lock = 1
00367     llwin.clear()
00368     (maxx,maxy) = llwin.getmaxyx()
00369     maxy = maxy - 2 # remove space for left and right border
00370     gcol = 20
00371     llwin.border(0)
00372     llwin.addstr(0, 2, "Status", curses.color_pair(1)|curses.A_BOLD)
00373 
00374     # Battery Monitor
00375     ################################
00376     battery_val = float(data.battery_voltage_1)/1000.0
00377     llwin.addstr(2, 2, 'Battery: {0:.3f}V'.format(battery_val))
00378     drawBattery(1,gcol,maxy-(gcol+1),float(data.battery_voltage_1)/1000)
00379 
00380     # Flight Mode Monitor
00381     ################################
00382     drawFlightMode(4,2,maxy-3,data.flightMode)
00383 
00384     # Status Monitor
00385     ################################
00386     drawStatusMode(7,2,maxy-3,data)
00387 
00388     ll_lock = 0
00389 
00390 def listener():
00391     global imuwin, maxx, maxy
00392     global alarm, alarm_count, alarm_interval
00393     global rec_enable
00394 
00395     rospy.init_node('asctec_monitor')
00396     rospy.Subscriber("asctec/LL_STATUS", LLStatus, callback)
00397     rospy.Subscriber("asctec/IMU_CALCDATA", IMUCalcData, imu_callback)
00398     rospy.Subscriber("asctec/GPS_DATA", GPSData, gps_callback)
00399     curses.init_pair(1, curses.COLOR_MAGENTA, curses.COLOR_BLACK)
00400     curses.init_pair(2, curses.COLOR_RED, curses.COLOR_BLACK)
00401     curses.init_pair(3, curses.COLOR_GREEN, curses.COLOR_BLACK)
00402     curses.init_pair(4, curses.COLOR_BLACK, curses.COLOR_WHITE)
00403     curses.init_pair(5, curses.COLOR_BLACK, curses.COLOR_RED)
00404     r = rospy.Rate(10) # Hz
00405     (maxx,maxy) = myscreen.getmaxyx()
00406     while not rospy.is_shutdown():
00407         c = myscreen.getch()
00408         if c == ord('f'): curses.flash()
00409         elif c == ord('b'): curses.beep()
00410         elif c == ord('r'): rec_enable = not rec_enable
00411         elif c == ord('q'): break  # Exit the while()
00412         elif c == curses.KEY_HOME: x = y = 0
00413         (current_maxx,current_maxy) = myscreen.getmaxyx()
00414         if (current_maxx != maxx or current_maxy != maxy):
00415             (maxx,maxy) = myscreen.getmaxyx()
00416             gpswin.mvwin(maxx-14, 0)
00417             llwin.mvwin(maxx-11, 0)
00418             imuwin = curses.newwin(maxx-14, maxy, 0, 0)
00419             #imuwin.refresh()
00420             #llwin.refresh()
00421             #gpswin.refresh()
00422         if (alarm):
00423             alarm_count = alarm_count + 1
00424             if (alarm_count == alarm_interval):
00425                 alarm_count = 0
00426                 curses.flash()
00427                 curses.beep()
00428         if (not gps_lock):
00429             gpswin.refresh()
00430         if (not imu_lock):
00431             imuwin.refresh()
00432         if (not ll_lock):
00433             llwin.refresh()
00434         record_update()
00435         recwin.refresh()
00436         r.sleep()
00437     curses.nocbreak(); myscreen.keypad(0); curses.echo(); curses.curs_set(1)
00438     curses.endwin()
00439 
00440 if __name__ == '__main__':
00441     listener()