Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033 import roslib
00034 roslib.load_manifest('rqt_pr2_dashboard')
00035 import rospy
00036
00037 from pr2_msgs.msg import PowerBoardState, DashboardState
00038 import std_srvs.srv
00039
00040 from rqt_robot_dashboard.dashboard import Dashboard
00041 from rqt_robot_dashboard.monitor_dash_widget import MonitorDashWidget
00042 from rqt_robot_dashboard.console_dash_widget import ConsoleDashWidget
00043
00044 from python_qt_binding.QtCore import QSize
00045 from python_qt_binding.QtGui import QMessageBox
00046
00047 from .pr2_breaker import PR2BreakerButton
00048 from .pr2_battery import PR2Battery
00049 from .pr2_motors import PR2Motors
00050 from .pr2_runstop import PR2Runstops
00051
00052
00053 class PR2Dashboard(Dashboard):
00054 """
00055 Dashboard for PR2s
00056
00057 :param context: the plugin context
00058 :type context: qt_gui.plugin.Plugin
00059 """
00060 def setup(self, context):
00061 self.name = 'PR2 Dashboard'
00062 self.max_icon_size = QSize(50, 30)
00063 self.message = None
00064
00065 self._dashboard_message = None
00066 self._last_dashboard_message_time = 0.0
00067
00068 self._raw_byte = None
00069 self.digital_outs = [0, 0, 0]
00070
00071 self._console = ConsoleDashWidget(self.context, minimal=False)
00072 self._monitor = MonitorDashWidget(self.context)
00073 self._motors = PR2Motors(self.on_reset_motors, self.on_halt_motors)
00074 self._breakers = [PR2BreakerButton('Left Arm', 0),
00075 PR2BreakerButton('Base', 1),
00076 PR2BreakerButton('Right Arm', 2)]
00077
00078 self._runstop = PR2Runstops('RunStops')
00079 self._batteries = [PR2Battery(self.context)]
00080
00081 self._dashboard_agg_sub = rospy.Subscriber('dashboard_agg', DashboardState, self.dashboard_callback)
00082
00083 def get_widgets(self):
00084 return [[self._monitor, self._console, self._motors], self._breakers, [self._runstop], self._batteries]
00085
00086 def dashboard_callback(self, msg):
00087 """
00088 callback to process dashboard_agg messages
00089
00090 :param msg: dashboard_agg DashboardState message
00091 :type msg: pr2_msgs.msg.DashboardState
00092 """
00093 self._dashboard_message = msg
00094 self._last_dashboard_message_time = rospy.get_time()
00095
00096 if (msg.motors_halted_valid):
00097 if (not msg.motors_halted.data):
00098 self._motors.set_ok()
00099 self._motors.setToolTip(self.tr("Motors: Running"))
00100 else:
00101 self._motors.set_error()
00102 self._motors.setToolTip(self.tr("Motors: Halted"))
00103 else:
00104 self._motors.set_stale()
00105 self._motors.setToolTip(self.tr("Motors: Stale"))
00106
00107 if (msg.power_state_valid):
00108 self._batteries[0].set_power_state(msg.power_state)
00109 else:
00110 self._batteries[0].set_stale()
00111
00112 if (msg.power_board_state_valid):
00113 [breaker.set_power_board_state_msg(msg.power_board_state) for breaker in self._breakers]
00114 if msg.power_board_state.run_stop:
00115 self._runstop.set_ok()
00116 self._runstop.setToolTip(self.tr("Physical Runstop: OK\nWireless Runstop: OK"))
00117 elif msg.power_board_state.wireless_stop:
00118 self._runstop.set_physical_engaged()
00119 self._runstop.setToolTip(self.tr("Physical Runstop: Pressed\nWireless Runstop: OK"))
00120 if not msg.power_board_state.wireless_stop:
00121 self._runstop.set_wireless_engaged()
00122 self._runstop.setToolTip(self.tr("Physical Runstop: Unknown\nWireless Runstop: Pressed"))
00123 else:
00124 [breaker.reset() for breaker in self._breakers]
00125 self._runstop.set_stale()
00126 self._runstop.setToolTip(self.tr("Physical Runstop: Stale\nWireless Runstop: Stale"))
00127
00128 def on_reset_motors(self):
00129
00130 if (self._dashboard_message is not None and self._dashboard_message.power_board_state_valid):
00131 all_breakers_enabled = reduce(lambda x, y: x and y, [state == PowerBoardState.STATE_ON for state in self._dashboard_message.power_board_state.circuit_state])
00132 if (not all_breakers_enabled):
00133 if(QMessageBox.question(self._breakers[0], self.tr('Enable Breakers?'), self.tr("Resetting the motors may not work because not all breakers are enabled. Enable all the breakers first?"), QMessageBox.Yes | QMessageBox.No, QMessageBox.Yes) == QMessageBox.Yes):
00134 [breaker.set_enable() for breaker in self._breakers]
00135 reset = rospy.ServiceProxy("pr2_ethercat/reset_motors", std_srvs.srv.Empty)
00136 try:
00137 reset()
00138 except rospy.ServiceException, e:
00139 QMessageBox.critical(self._breakers[0], "Error", "Failed to reset the motors: service call failed with error: %s" % (e))
00140
00141 def on_halt_motors(self):
00142 halt = rospy.ServiceProxy("pr2_ethercat/halt_motors", std_srvs.srv.Empty)
00143 try:
00144 halt()
00145 except rospy.ServiceException, e:
00146 QMessageBox.critical(self._motors, "Error", "Failed to halt the motors: service call failed with error: %s" % (e))
00147
00148 def shutdown_dashboard(self):
00149 self._dashboard_agg_sub.unregister()
00150
00151 def save_settings(self, plugin_settings, instance_settings):
00152 self._console.save_settings(plugin_settings, instance_settings)
00153 self._monitor.save_settings(plugin_settings, instance_settings)
00154
00155 def restore_settings(self, plugin_settings, instance_settings):
00156 self._console.restore_settings(plugin_settings, instance_settings)
00157 self._monitor.restore_settings(plugin_settings, instance_settings)