dbw_mkz_gui.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 import os
00005 import rospkg
00006 
00007 from qt_gui.plugin import Plugin
00008 from python_qt_binding.QtGui import QWidget
00009 from python_qt_binding import loadUi
00010 from rospy.topics import Publisher, Subscriber
00011 from rospy.timer import Timer
00012 from PyQt4.Qt import QTimer, QGraphicsScene, QBrush, QImage, QPixmap, QPalette, QColor
00013 from std_msgs.msg import Float64, Bool, Empty
00014 from dbw_mkz_msgs.msg import ThrottleCmd, ThrottleReport, BrakeCmd, BrakeReport, SteeringCmd, SteeringReport, GearCmd, GearReport
00015 import std_msgs
00016 from dbw_mkz_msgs.msg._Misc1Report import Misc1Report
00017 from dbw_mkz_msgs.msg._TirePressureReport import TirePressureReport
00018 from dbw_mkz_msgs.msg._FuelLevelReport import FuelLevelReport
00019 from sensor_msgs.msg._NavSatFix import NavSatFix
00020 import math
00021 from sensor_msgs.msg._Imu import Imu
00022 
00023 class MkzGui(Plugin):
00024 
00025     package_name = 'dbw_mkz_gui' # Name of ROS package
00026     ui_filename = 'dbw_mkz_gui.ui' # filename of the QT gui resource
00027     gui_object_name = 'MkzGui' # runtime name of the GUI plugin
00028     update_period = 100 # GUI update period in ms
00029     
00030     def __init__(self, context):
00031         self.initVariables()
00032         self.setupWidget(context)
00033         self.initGraphics()
00034         self.bindCallbacks()
00035         self.subscribeTopics()   
00036         self.advertiseTopics()
00037     
00038     def initVariables(self):
00039         self.rad_to_deg = 57.295779515
00040         
00041         # DBW reported values
00042         self.reported_throttle = 0
00043         self.reported_brake = 0
00044         self.reported_boo = False
00045         self.reported_steering = 0
00046         self.reported_gear = '--'
00047         
00048         # DBW commands
00049         self.throttle_cmd = 0
00050         self.brake_cmd = 0
00051         self.steering_cmd = 0
00052         
00053         # Steering wheel button states
00054         self.on_off_btn = False
00055         self.res_cncl_btn = False
00056         self.set_inc_btn = False
00057         self.set_dec_btn = False
00058         self.gap_inc_btn = False
00059         self.gap_dec_btn = False
00060         self.la_on_off_btn = False
00061         
00062         # Message RX status
00063         self.throttle_stamp = rospy.Time()
00064         self.brake_stamp = rospy.Time()
00065         self.steering_stamp = rospy.Time()
00066         self.gear_stamp = rospy.Time()
00067         self.misc_stamp = rospy.Time()
00068         
00069         # Vehicle data labels
00070         self.gps_lat = 0
00071         self.gps_lon = 0
00072         self.speed = 0
00073         self.fl_tire = 0
00074         self.fr_tire = 0
00075         self.rl_tire = 0
00076         self.rr_tire = 0
00077         self.yaw_rate = 0
00078         self.roll_rate = 0
00079         self.fuel = 0
00080         
00081         # Report gear name map
00082         self.gear_map = []
00083         self.gear_map.append('None')
00084         self.gear_map.append('Park')
00085         self.gear_map.append('Reverse')
00086         self.gear_map.append('Neutral')
00087         self.gear_map.append('Drive')
00088         self.gear_map.append('Low')
00089         
00090         # Throttle faults
00091         self.throttle_driver = False
00092         self.throttle_flt1 = False
00093         self.throttle_flt2 = False
00094         self.throttle_fltcon = False
00095         
00096         # Brake faults
00097         self.brake_driver = False
00098         self.brake_flt1 = False
00099         self.brake_flt2 = False
00100         self.brake_fltboo = False
00101         self.brake_fltcon = False
00102         
00103         # Steering faults
00104         self.steering_driver = False
00105         self.steering_flt1 = False
00106         self.steering_flt2 = False
00107         self.steering_fltcon = False
00108         
00109         # Gear faults
00110         self.gear_driver = False
00111         self.gear_fltbus = False
00112         
00113     def updateGuiCallback(self):
00114         current_time = rospy.Time.now()
00115         
00116         # DBW system commands and reports
00117         self._widget.throttle_report_lbl.setText(str(self.reported_throttle))
00118         self._widget.brake_report_lbl.setText(str(self.reported_brake))
00119         self._widget.steering_report_lbl.setText(str(self.reported_steering))
00120         self._widget.gear_report_lbl.setText(self.reported_gear) 
00121         
00122         self._widget.throttle_cmd_lbl.setText(str(self.throttle_cmd))
00123         self._widget.brake_cmd_lbl.setText(str(self.brake_cmd))
00124         self._widget.steering_cmd_lbl.setText(str(self.steering_cmd))
00125         
00126         # Steering wheel buttons  
00127         if (current_time - self.misc_stamp) < rospy.Duration(0.25):
00128             if self.res_cncl_btn:
00129                 self._widget.res_cncl_led.setPalette(self.green_palette)
00130             else:        
00131                 self._widget.res_cncl_led.setPalette(self.red_palette)  
00132                 
00133             if self.on_off_btn:
00134                 self._widget.on_off_led.setPalette(self.green_palette)
00135             else:        
00136                 self._widget.on_off_led.setPalette(self.red_palette)
00137                 
00138             if self.set_inc_btn:
00139                 self._widget.set_inc_led.setPalette(self.green_palette)
00140             else:        
00141                 self._widget.set_inc_led.setPalette(self.red_palette)
00142                 
00143             if self.set_dec_btn:
00144                 self._widget.set_dec_led.setPalette(self.green_palette)
00145             else:        
00146                 self._widget.set_dec_led.setPalette(self.red_palette)
00147                 
00148             if self.gap_inc_btn:
00149                 self._widget.gap_inc_led.setPalette(self.green_palette)
00150             else:        
00151                 self._widget.gap_inc_led.setPalette(self.red_palette)
00152                 
00153             if self.gap_dec_btn:
00154                 self._widget.gap_dec_led.setPalette(self.green_palette)
00155             else:        
00156                 self._widget.gap_dec_led.setPalette(self.red_palette)
00157     
00158             if self.la_on_off_btn:
00159                 self._widget.la_on_off_led.setPalette(self.green_palette)
00160             else:        
00161                 self._widget.la_on_off_led.setPalette(self.red_palette)
00162         else:
00163             self._widget.on_off_led.setPalette(self.gray_palette)
00164             self._widget.res_cncl_led.setPalette(self.gray_palette)
00165             self._widget.set_inc_led.setPalette(self.gray_palette)
00166             self._widget.set_dec_led.setPalette(self.gray_palette)
00167             self._widget.gap_inc_led.setPalette(self.gray_palette)
00168             self._widget.gap_dec_led.setPalette(self.gray_palette)
00169             self._widget.la_on_off_led.setPalette(self.gray_palette)
00170             
00171         # Report RX status LEDs
00172         if (current_time - self.throttle_stamp) < rospy.Duration(0.25):
00173             self._widget.throttle_led.setPalette(self.green_palette)
00174         else:
00175             self._widget.throttle_led.setPalette(self.red_palette)
00176 
00177         if (current_time - self.brake_stamp) < rospy.Duration(0.25):
00178             self._widget.brake_led.setPalette(self.green_palette)
00179         else:
00180             self._widget.brake_led.setPalette(self.red_palette)
00181             
00182         if (current_time - self.steering_stamp) < rospy.Duration(0.25):
00183             self._widget.steering_led.setPalette(self.green_palette)
00184         else:
00185             self._widget.steering_led.setPalette(self.red_palette)
00186             
00187         if (current_time - self.gear_stamp) < rospy.Duration(0.25):
00188             self._widget.gear_led.setPalette(self.green_palette)
00189         else:
00190             self._widget.gear_led.setPalette(self.red_palette)
00191             
00192         # Throttle faults
00193         if (current_time - self.throttle_stamp) < rospy.Duration(0.25):
00194             if self.throttle_driver:
00195                 self._widget.throttle_driver_led.setPalette(self.yellow_palette)
00196             else:
00197                 self._widget.throttle_driver_led.setPalette(self.green_palette)
00198             
00199             if self.throttle_flt1:
00200                 self._widget.throttle_flt1_led.setPalette(self.red_palette)
00201             else:
00202                 self._widget.throttle_flt1_led.setPalette(self.green_palette)
00203                 
00204             if self.throttle_flt2:
00205                 self._widget.throttle_flt2_led.setPalette(self.red_palette)
00206             else:
00207                 self._widget.throttle_flt2_led.setPalette(self.green_palette)
00208                 
00209             if self.throttle_fltcon:
00210                 self._widget.throttle_fltcon_led.setPalette(self.red_palette)
00211             else:
00212                 self._widget.throttle_fltcon_led.setPalette(self.green_palette)
00213         else:
00214             self._widget.throttle_driver_led.setPalette(self.gray_palette)
00215             self._widget.throttle_flt1_led.setPalette(self.gray_palette)
00216             self._widget.throttle_flt2_led.setPalette(self.gray_palette)
00217             self._widget.throttle_fltcon_led.setPalette(self.gray_palette)
00218             
00219         # Brake faults
00220         if (current_time - self.brake_stamp) < rospy.Duration(0.25):
00221             if self.brake_driver:
00222                 self._widget.brake_driver_led.setPalette(self.yellow_palette)
00223             else:
00224                 self._widget.brake_driver_led.setPalette(self.green_palette)    
00225      
00226             if self.brake_flt1:
00227                 self._widget.brake_flt1_led.setPalette(self.red_palette)
00228             else:
00229                 self._widget.brake_flt1_led.setPalette(self.green_palette) 
00230     
00231             if self.brake_flt2:
00232                 self._widget.brake_flt2_led.setPalette(self.red_palette)
00233             else:
00234                 self._widget.brake_flt2_led.setPalette(self.green_palette)
00235                             
00236             if self.brake_fltcon:
00237                 self._widget.brake_fltcon_led.setPalette(self.red_palette)
00238             else:
00239                 self._widget.brake_fltcon_led.setPalette(self.green_palette) 
00240                 
00241             if self.brake_fltboo:
00242                 self._widget.brake_fltb_led.setPalette(self.red_palette)
00243             else:
00244                 self._widget.brake_fltb_led.setPalette(self.green_palette) 
00245         else:
00246             self._widget.brake_driver_led.setPalette(self.gray_palette)
00247             self._widget.brake_flt1_led.setPalette(self.gray_palette)
00248             self._widget.brake_flt2_led.setPalette(self.gray_palette)
00249             self._widget.brake_fltcon_led.setPalette(self.gray_palette)
00250             self._widget.brake_fltb_led.setPalette(self.gray_palette)
00251             
00252         # Steering faults
00253         if (current_time - self.steering_stamp) < rospy.Duration(0.25):
00254             if self.steering_driver:
00255                 self._widget.steering_driver_led.setPalette(self.yellow_palette)
00256             else:
00257                 self._widget.steering_driver_led.setPalette(self.green_palette)
00258                 
00259             if self.steering_flt1:
00260                 self._widget.steering_flt1_led.setPalette(self.red_palette)
00261             else:
00262                 self._widget.steering_flt1_led.setPalette(self.green_palette)      
00263                 
00264             if self.steering_flt2:
00265                 self._widget.steering_flt2_led.setPalette(self.red_palette)
00266             else:
00267                 self._widget.steering_flt2_led.setPalette(self.green_palette) 
00268                 
00269             if self.steering_fltcon:
00270                 self._widget.steering_fltcon_led.setPalette(self.red_palette)
00271             else:
00272                 self._widget.steering_flt2_led.setPalette(self.green_palette)                 
00273         else:
00274             self._widget.steering_driver_led.setPalette(self.gray_palette)
00275             self._widget.steering_flt1_led.setPalette(self.gray_palette)
00276             self._widget.steering_flt2_led.setPalette(self.gray_palette)
00277             self._widget.steering_fltcon_led.setPalette(self.gray_palette)
00278             
00279         # Gear faults
00280         if (current_time - self.gear_stamp) < rospy.Duration(0.25):
00281             if self.gear_driver:
00282                 self._widget.gear_driver_led.setPalette(self.yellow_palette)
00283             else:
00284                 self._widget.gear_driver_led.setPalette(self.green_palette)
00285                 
00286             if self.gear_fltbus:
00287                 self._widget.gear_fltbus_led.setPalette(self.red_palette)
00288             else:
00289                 self._widget.gear_fltbus_led.setPalette(self.green_palette)                        
00290         else:
00291             self._widget.gear_driver_led.setPalette(self.gray_palette)
00292             self._widget.gear_fltbus_led.setPalette(self.gray_palette)
00293 
00294         # Vehicle data labels
00295         self._widget.lat_lbl.setText(str(self.gps_lat))
00296         self._widget.lon_lbl.setText(str(self.gps_lon))
00297         self._widget.speed_lbl.setText(str(self.speed) + ' m/s')
00298         self._widget.fl_pressure_lbl.setText(str(self.fl_tire))
00299         self._widget.fr_pressure_lbl.setText(str(self.fr_tire))
00300         self._widget.rl_pressure_lbl.setText(str(self.rl_tire))   
00301         self._widget.rr_pressure_lbl.setText(str(self.rr_tire))
00302         self._widget.yaw_rate_lbl.setText(str(self.yaw_rate) + ' rad/s')
00303         self._widget.roll_rate_lbl.setText(str(self.roll_rate) + ' rad/s')
00304         self._widget.fuel_lbl.setText(str(self.fuel) + ' %')
00305         
00306         if self.reported_boo:
00307             self._widget.boo_led.setPalette(self.red_palette)
00308         else:
00309             self._widget.boo_led.setPalette(self.gray_palette)
00310     
00311         # Steering wheel buttons
00312         if self.on_off_btn:
00313             self._widget.on_off_led.setPalette(self.green_palette)
00314         else:
00315             self._widget.on_off_led.setPalette(self.gray_palette)
00316     
00317         if self.res_cncl_btn:
00318             self._widget.res_cncl_led.setPalette(self.green_palette)
00319         else:
00320             self._widget.res_cncl_led.setPalette(self.gray_palette)    
00321 
00322         if self.set_inc_btn:
00323             self._widget.set_inc_led.setPalette(self.green_palette)
00324         else:
00325             self._widget.set_inc_led.setPalette(self.gray_palette)     
00326     
00327         if self.set_dec_btn:
00328             self._widget.set_dec_led.setPalette(self.green_palette)
00329         else:
00330             self._widget.set_dec_led.setPalette(self.gray_palette)  
00331             
00332         if self.gap_inc_btn:
00333             self._widget.gap_inc_led.setPalette(self.green_palette)
00334         else:
00335             self._widget.gap_inc_led.setPalette(self.gray_palette)           
00336     
00337         if self.gap_dec_btn:
00338             self._widget.gap_dec_led.setPalette(self.green_palette)
00339         else:
00340             self._widget.gap_dec_led.setPalette(self.gray_palette)
00341             
00342         if self.la_on_off_btn:
00343             self._widget.la_on_off_led.setPalette(self.green_palette)
00344         else:
00345             self._widget.la_on_off_led.setPalette(self.gray_palette)           
00346                 
00347     def initGraphics(self):
00348         
00349         # LED color palettes
00350         self.green_palette = QPalette()
00351         self.green_palette.setColor(QPalette.Window, QColor(0,255,0))
00352 
00353         self.red_palette = QPalette()
00354         self.red_palette.setColor(QPalette.Window, QColor(255,0,0))
00355         
00356         self.yellow_palette = QPalette()
00357         self.yellow_palette.setColor(QPalette.Window, QColor(200, 200, 50))        
00358         
00359         self.gray_palette = QPalette()
00360         self.gray_palette.setColor(QPalette.Window, QColor(80, 80, 80))
00361         
00362         # CAN RX LEDs
00363         self._widget.throttle_led.setText('')
00364         self._widget.throttle_led.setPalette(self.red_palette)
00365         self._widget.brake_led.setText('')
00366         self._widget.brake_led.setPalette(self.red_palette)
00367         self._widget.steering_led.setText('')
00368         self._widget.steering_led.setPalette(self.red_palette)
00369         self._widget.gear_led.setText('')
00370         self._widget.gear_led.setPalette(self.red_palette)
00371         
00372         # BOO LED
00373         self._widget.boo_led.setText('')
00374         self._widget.boo_led.setPalette(self.gray_palette)
00375         
00376         # Fault and driver override LEDs
00377         self._widget.throttle_driver_led.setText('')
00378         self._widget.throttle_driver_led.setPalette(self.gray_palette)
00379         self._widget.throttle_flt1_led.setText('')
00380         self._widget.throttle_flt1_led.setPalette(self.gray_palette)
00381         self._widget.throttle_flt2_led.setText('')
00382         self._widget.throttle_flt2_led.setPalette(self.gray_palette)
00383         self._widget.throttle_fltcon_led.setText('')
00384         self._widget.throttle_fltcon_led.setPalette(self.gray_palette)         
00385         
00386         self._widget.brake_driver_led.setText('')
00387         self._widget.brake_driver_led.setPalette(self.gray_palette)
00388         self._widget.brake_flt1_led.setText('')
00389         self._widget.brake_flt1_led.setPalette(self.gray_palette)
00390         self._widget.brake_flt2_led.setText('')
00391         self._widget.brake_flt2_led.setPalette(self.gray_palette)
00392         self._widget.brake_fltcon_led.setText('')
00393         self._widget.brake_fltcon_led.setPalette(self.gray_palette)         
00394         self._widget.brake_fltb_led.setText('')
00395         self._widget.brake_fltb_led.setPalette(self.gray_palette)   
00396         
00397         self._widget.steering_driver_led.setText('')
00398         self._widget.steering_flt1_led.setText('')
00399         self._widget.steering_flt2_led.setText('')
00400         self._widget.steering_fltcon_led.setText('')
00401         
00402         self._widget.gear_driver_led.setText('')
00403         self._widget.gear_fltbus_led.setText('')    
00404         
00405         # Steering wheel buttons
00406         self._widget.on_off_led.setText('')
00407         self._widget.on_off_led.setPalette(self.gray_palette)  
00408         self._widget.res_cncl_led.setText('')
00409         self._widget.res_cncl_led.setPalette(self.gray_palette)  
00410         self._widget.set_inc_led.setText('')
00411         self._widget.set_inc_led.setPalette(self.gray_palette)  
00412         self._widget.set_dec_led.setText('')
00413         self._widget.set_dec_led.setPalette(self.gray_palette)  
00414         self._widget.gap_inc_led.setText('')
00415         self._widget.gap_inc_led.setPalette(self.gray_palette)  
00416         self._widget.gap_dec_led.setText('')
00417         self._widget.gap_dec_led.setPalette(self.gray_palette)  
00418         self._widget.la_on_off_led.setText('')
00419         self._widget.la_on_off_led.setPalette(self.gray_palette)
00420     
00421     def bindCallbacks(self):
00422         pass
00423     
00424     def subscribeTopics(self):
00425         rospy.Subscriber('vehicle/throttle_report', ThrottleReport, self.recvThrottleReport)
00426         rospy.Subscriber('vehicle/brake_report', BrakeReport, self.recvBrakeReport)
00427         rospy.Subscriber('vehicle/steering_report', SteeringReport, self.recvSteeringReport)
00428         rospy.Subscriber('vehicle/gear_report', GearReport, self.recvGearReport)
00429         rospy.Subscriber('vehicle/misc1_report', Misc1Report, self.recvMisc1Report)
00430         rospy.Subscriber('vehicle/tire_pressure_report', TirePressureReport, self.recvTirePressure)
00431         rospy.Subscriber('vehicle/fuel_level_report', FuelLevelReport, self.recvFuelLevel)
00432         rospy.Subscriber('vehicle/gps/fix', NavSatFix, self.recvGps)
00433         rospy.Subscriber('vehicle/imu/data_raw', Imu, self.recvImu)
00434     
00435     def recvImu(self, msg):
00436         self.yaw_rate = 1e-2 * math.floor(1e2 * msg.angular_velocity.z)
00437         self.roll_rate = 1e-2 * math.floor(1e2 * msg.angular_velocity.x)
00438     
00439     def recvGps(self, msg):
00440         self.gps_lat = 1e-6 * math.floor(1e6 * msg.latitude)
00441         self.gps_lon = 1e-6 * math.floor(1e6 * msg.longitude)
00442     
00443     def recvThrottleReport(self, msg):
00444         self.throttle_stamp = rospy.Time.now()
00445         self.reported_throttle = int(math.floor(100 * msg.pedal_output))
00446         self.throttle_cmd = int(math.floor(100 * msg.pedal_cmd))
00447         
00448         self.throttle_driver = msg.driver
00449         self.throttle_flt1 = msg.fault_ch1
00450         self.throttle_flt2 = msg.fault_ch2
00451         self.throttle_fltcon = msg.fault_connector
00452         
00453     def recvBrakeReport(self, msg):
00454         self.brake_stamp = rospy.Time.now()
00455         self.reported_brake = int(math.floor(100 * msg.pedal_output))
00456         self.brake_cmd = int(math.floor(100 * msg.pedal_cmd))
00457         self.reported_boo = msg.boo_output
00458         
00459         self.brake_driver = msg.driver
00460         self.brake_flt1 = msg.fault_ch1
00461         self.brake_flt2 = msg.fault_ch2
00462         self.brake_fltboo = msg.fault_boo
00463         self.brake_fltcon = msg.fault_connector
00464             
00465     def recvSteeringReport(self, msg):
00466         self.steering_stamp = rospy.Time.now()
00467         self.speed = 0.1 * math.floor(10 * msg.speed)
00468         self.reported_steering = 0.1 * math.floor(self.rad_to_deg * 10 * msg.steering_wheel_angle)
00469         self.steering_cmd = 0.1 * math.floor(self.rad_to_deg * 10 * msg.steering_wheel_angle_cmd)
00470         
00471         self.steering_driver = msg.driver
00472         self.steering_flt1 = msg.fault_bus1
00473         self.steering_flt2 = msg.fault_bus2
00474         self.steering_fltcon = msg.fault_connector
00475     
00476     def recvGearReport(self, msg):
00477         self.gear_stamp = rospy.Time.now()
00478         self.reported_gear = self.gear_map[msg.state.gear]
00479 
00480         self.gear_driver = msg.driver
00481         self.gear_fltbus = msg.fault_bus
00482         
00483     def recvMisc1Report(self, msg):
00484         self.on_off_btn = msg.btn_cc_on_off
00485         self.res_cncl_btn = msg.btn_cc_res_cncl
00486         self.set_inc_btn = msg.btn_cc_set_inc
00487         self.set_inc_btn = msg.btn_cc_set_dec
00488         self.set_inc_btn = msg.btn_cc_gap_inc
00489         self.set_inc_btn = msg.btn_cc_gap_dec
00490         self.set_inc_btn = msg.btn_la_on_off
00491     
00492     def advertiseTopics(self):
00493         pass
00494     
00495     def recvTirePressure(self, msg):
00496         self.fl_tire = msg.front_left
00497         self.fr_tire = msg.front_right
00498         self.rl_tire = msg.rear_left
00499         self.rr_tire = msg.rear_right
00500         
00501     def recvFuelLevel(self, msg):
00502         self.fuel = 0.1 * math.floor(10 * msg.fuel_level)
00503     
00504     def setupWidget(self, context):
00505         super(MkzGui, self).__init__(context)
00506         # Give QObjects reasonable names
00507         self.setObjectName(self.gui_object_name)
00508 
00509         # Create QWidget
00510         self._widget = QWidget()
00511 
00512         # Get path to UI file which should be in the "resource" folder of this package
00513         ui_file = os.path.join(rospkg.RosPack().get_path(self.package_name), 'resource', self.ui_filename)
00514         # Extend the widget with all attributes and children from UI file
00515         loadUi(ui_file, self._widget)
00516         # Give QObjects reasonable names
00517         self._widget.setObjectName(self.gui_object_name)
00518         # Add widget to the user interface
00519         context.add_widget(self._widget)
00520         
00521         # Set up a timer to update the GUI
00522         update_timer = QTimer(self._widget)
00523         update_timer.setInterval(self.update_period);
00524         update_timer.setSingleShot(False);
00525         update_timer.timeout.connect(lambda: self.updateGuiCallback())
00526         update_timer.start()
00527         
00528     def shutdown_plugin(self):
00529         pass
00530 


dbw_mkz_gui
Author(s):
autogenerated on Thu Mar 23 2017 04:09:03