00001
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'
00026 ui_filename = 'dbw_mkz_gui.ui'
00027 gui_object_name = 'MkzGui'
00028 update_period = 100
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
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
00049 self.throttle_cmd = 0
00050 self.brake_cmd = 0
00051 self.steering_cmd = 0
00052
00053
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
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
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
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
00091 self.throttle_driver = False
00092 self.throttle_flt1 = False
00093 self.throttle_flt2 = False
00094 self.throttle_fltcon = False
00095
00096
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
00104 self.steering_driver = False
00105 self.steering_flt1 = False
00106 self.steering_flt2 = False
00107 self.steering_fltcon = False
00108
00109
00110 self.gear_driver = False
00111 self.gear_fltbus = False
00112
00113 def updateGuiCallback(self):
00114 current_time = rospy.Time.now()
00115
00116
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
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
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
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
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
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
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
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
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
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
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
00373 self._widget.boo_led.setText('')
00374 self._widget.boo_led.setPalette(self.gray_palette)
00375
00376
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
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
00507 self.setObjectName(self.gui_object_name)
00508
00509
00510 self._widget = QWidget()
00511
00512
00513 ui_file = os.path.join(rospkg.RosPack().get_path(self.package_name), 'resource', self.ui_filename)
00514
00515 loadUi(ui_file, self._widget)
00516
00517 self._widget.setObjectName(self.gui_object_name)
00518
00519 context.add_widget(self._widget)
00520
00521
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