dashboard.py
Go to the documentation of this file.
00001 import roslib;roslib.load_manifest('rqt_biped_robin_dashboard')
00002 import rospy
00003 
00004 import diagnostic_msgs
00005 import create_node.srv
00006 import create_node.msg
00007 
00008 from rqt_robot_dashboard.dashboard import Dashboard
00009 from rqt_robot_dashboard.widgets import MonitorDashWidget, ConsoleDashWidget, MenuDashWidget, BatteryDashWidget, IconToolButton, NavViewDashWidget
00010 from QtGui import QMessageBox, QAction
00011 from python_qt_binding.QtCore import QSize
00012 
00013 from .battery import TurtlebotBattery
00014 
00015 import rospkg
00016 import os.path
00017 
00018 rp = rospkg.RosPack()
00019 
00020 image_path = image_path = os.path.join(rp.get_path('create_dashboard'), 'images')
00021 
00022 class BreakerButton(IconToolButton):
00023     def __init__(self, name, onclick):
00024         self._on_icon = ['bg-green.svg', 'ic-breaker.svg']
00025         self._off_icon = ['bg-red.svg', 'ic-breaker.svg']
00026     
00027         icons = [self._off_icon, self._on_icon]
00028 
00029         super(BreakerButton, self).__init__(name, icons=icons)
00030 
00031         self.setFixedSize(self._icons[0].actualSize(QSize(50,30)))
00032 
00033         self.clicked.connect(onclick)
00034 
00035 class CreateDashboard(Dashboard):
00036     def setup(self, context):
00037         self.message = None
00038 
00039         self._dashboard_message = None
00040         self._last_dashboard_message_time = 0.0
00041 
00042         self._raw_byte = None
00043         self.digital_outs = [0,0,0]
00044 
00045         # These were moved out of get_widgets because they are sometimes not defined
00046         # before being used by dashboard_callback. Could be done more cleanly than this
00047         # though.
00048         self.lap_bat = BatteryDashWidget("Laptop")
00049         self.create_bat = TurtlebotBattery("Create")
00050         self.breakers = [BreakerButton('breaker0', lambda: self.toggle_breaker(0)),
00051                          BreakerButton('breaker1', lambda: self.toggle_breaker(1)),
00052                          BreakerButton('breaker2', lambda: self.toggle_breaker(2))]
00053         
00054         # This is what gets dashboard_callback going eagerly
00055         self._dashboard_agg_sub = rospy.Subscriber('diagnostics_agg', diagnostic_msgs.msg.DiagnosticArray, self.dashboard_callback)
00056         self._power_control = rospy.ServiceProxy('turtlebot_node/set_digital_outputs', create_node.srv.SetDigitalOutputs)
00057 
00058     def get_widgets(self):
00059         self.mode = MenuDashWidget('Mode')
00060 
00061         self.mode.add_action('Full', self.on_full_mode)
00062         self.mode.add_action('Passive', self.on_passive_mode)
00063         self.mode.add_action('Safe', self.on_safe_mode)
00064 
00065         return [[MonitorDashWidget(self.context), ConsoleDashWidget(self.context), self.mode],
00066                 self.breakers,
00067                 [self.lap_bat, self.create_bat],
00068                 [NavViewDashWidget(self.context)]]
00069 
00070     def dashboard_callback(self, msg):
00071         self._dashboard_message = msg
00072         self._last_dashboard_message_time = rospy.get_time()
00073   
00074         battery_status = {} # Used to store TurtlebotBattery status info
00075         breaker_status = {}
00076         laptop_battery_status = {}
00077         op_mode = None
00078         for status in msg.status:
00079             print("Status callback %s"%status.name)
00080             if status.name == "/Power System/Battery":
00081                 for value in status.values:
00082                     print value
00083                     battery_status[value.key]=value.value
00084                     # Create battery actually doesn't check public or check
00085                     # charging state. Could maybe done by looking at +-
00086                     # of current value
00087                     # Below is kobuki battery code.
00088                     # Best place to fix this is in create driver to match the kobuki
00089                     # diagnostics
00090                     #elif value.key == "Charging State":
00091                     # if value.value == "Trickle Charging" or value.value == "Full Charging":
00092                     # self.create_bat.set_charging(True)
00093                     # else:
00094                     # self.create_bat.set_charging(False)
00095             elif status.name == "/Mode/Operating Mode":
00096                 op_mode=status.message
00097             elif status.name == "/Digital IO/Digital Outputs":
00098                 #print "got digital IO"
00099                 for value in status.values:
00100                     breaker_status[value.key]=value.value
00101             elif status.name == "/Power System/Laptop Battery":
00102                 for value in status.values:
00103                     laptop_battery_status[value.key]=value.value
00104 
00105         # If battery diagnostics were found, calculate percentages and stuff
00106         if (laptop_battery_status):
00107             percentage = float(laptop_battery_status['Charge (Ah)'])/float(laptop_battery_status['Capacity (Ah)'])
00108             self.lap_bat.update_perc(percentage*100)
00109             self.lap_bat.update_time(percentage*100)
00110             charging_state = True if float(laptop_battery_status['Current (A)']) > 0.0 else False
00111             self.lap_bat.set_charging(charging_state)
00112 
00113         if (battery_status):
00114             self.create_bat.set_power_state(battery_status)
00115 
00116         if (breaker_status):
00117             self._raw_byte = int(breaker_status['Raw Byte'])
00118             self._update_breakers()
00119 
00120     def toggle_breaker(self, index):
00121         try:
00122             self.digital_outs[index] = not self.digital_outs[index]
00123             power_cmd = create_node.srv.SetDigitalOutputsRequest(self.digital_outs[0], self.digital_outs[1], self.digital_outs[2])
00124             #print power_cmd
00125             self._power_control(power_cmd)
00126 
00127             return True
00128         except rospy.ServiceException, e:
00129             self.message = QMessageBox()
00130             self.message.setText("Service call failed with error: %s"%(e))
00131             self.message.exec_()
00132             return False
00133           
00134         return False
00135 
00136     def _update_breakers(self):
00137         tmp = self._raw_byte
00138         for i in range(0,3):
00139             self.digital_outs[i]=tmp%2
00140             self.breakers[i].update_state(self.digital_outs[i])
00141             tmp = tmp >> 1
00142         
00143     def on_passive_mode(self):
00144         passive = rospy.ServiceProxy("/turtlebot_node/set_operation_mode",create_node.srv.SetTurtlebotMode )
00145         try:
00146             passive(create_node.msg.TurtlebotSensorState.OI_MODE_PASSIVE)
00147         except rospy.ServiceException, e:
00148             self.message = QMessageBox()
00149             self.message.setText("Failed to put the turtlebot in passive mode: service call failed with error: %s"%(e))
00150             self.message.exec_()
00151 
00152     def on_safe_mode(self):
00153         safe = rospy.ServiceProxy("/turtlebot_node/set_operation_mode",create_node.srv.SetTurtlebotMode)
00154         try:
00155           safe(create_node.msg.TurtlebotSensorState.OI_MODE_SAFE)
00156         except rospy.ServiceException, e:
00157           self.message = QMessageBox()
00158           self.message.setText("Failed to put the turtlebot in safe mode: service call failed with error: %s"%(e))
00159           self.message.exec_()
00160 
00161     def on_full_mode(self):
00162         full = rospy.ServiceProxy("/turtlebot_node/set_operation_mode", create_node.srv.SetTurtlebotMode)
00163         try:
00164             full(create_node.msg.TurtlebotSensorState.OI_MODE_FULL)
00165         except rospy.ServiceException, e:
00166             self.message = QMessageBox()
00167             self.message.setText("Failed to put the turtlebot in full mode: service call failed with error: %s"%(e))
00168             self.message.exec_()


rqt_biped_robin_dashboard
Author(s): horst
autogenerated on Mon Jan 6 2014 11:10:06