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
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051 import roslib
00052 roslib.load_manifest('p2os_dashboard')
00053
00054 import wx
00055 import wx.aui
00056 import wx.py.shell
00057 import rxtools
00058 import rxtools.cppwidgets as rxtools
00059
00060 import robot_monitor
00061 from robot_monitor.robot_monitor_panel import RobotMonitorPanel
00062 from p2os_driver.msg import BatteryState, MotorState
00063
00064 import std_msgs.msg
00065 import std_srvs.srv
00066
00067 import rospy
00068 from roslib import rosenv
00069
00070 from os import path
00071 import threading
00072
00073 from status_control import StatusControl
00074 from rosout_frame import RosoutFrame
00075
00076 from power_state_control import PowerStateControl
00077
00078 from diagnostics_frame import DiagnosticsFrame
00079
00080 class P2OSFrame(wx.Frame):
00081 _CONFIG_WINDOW_X="/Window/X"
00082 _CONFIG_WINDOW_Y="/Window/Y"
00083
00084 def __init__(self, parent, id=wx.ID_ANY, title='P2OS Dashboard', pos=wx.DefaultPosition, size=(400, 50), style=wx.CAPTION|wx.CLOSE_BOX|wx.STAY_ON_TOP):
00085 wx.Frame.__init__(self, parent, id, title, pos, size, style)
00086
00087 wx.InitAllImageHandlers()
00088
00089 self.motorStatePub = rospy.Publisher('cmd_motor_state', MotorState)
00090 self.motorStateSub = rospy.Subscriber("motor_state", MotorState, self.motorStateCallback)
00091
00092 self.battStateSub = rospy.Subscriber("battery_state", BatteryState, self.battStateCallback)
00093
00094 self.motorState = 0
00095
00096 rospy.init_node('p2os_dashboard', anonymous=True)
00097 try:
00098 getattr(rxtools, "initRoscpp")
00099 rxtools.initRoscpp("p2os_dashboard_cpp", anonymous=True)
00100 except AttributeError:
00101 pass
00102
00103 self.SetTitle('P2OS Dashboard (%s)'%rosenv.get_master_uri())
00104
00105 icons_path = path.join(roslib.packages.get_pkg_dir('p2os_dashboard'), "icons/")
00106
00107 sizer = wx.BoxSizer(wx.HORIZONTAL)
00108 self.SetSizer(sizer)
00109
00110 static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Diagnostic"), wx.HORIZONTAL)
00111 sizer.Add(static_sizer, 0)
00112
00113
00114 self._diagnostics_button = StatusControl(self, wx.ID_ANY, icons_path, "diag", True)
00115 self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics"))
00116 static_sizer.Add(self._diagnostics_button, 0)
00117
00118
00119 self._rosout_button = StatusControl(self, wx.ID_ANY, icons_path, "rosout", True)
00120 self._rosout_button.SetToolTip(wx.ToolTip("Rosout"))
00121 static_sizer.Add(self._rosout_button, 0)
00122
00123
00124 self._motors_button = StatusControl(self, wx.ID_ANY, icons_path, "motor", True)
00125 self._motors_button.SetToolTip(wx.ToolTip("Motors"))
00126 static_sizer.Add(self._motors_button, 0)
00127 self._motors_button.Bind(wx.EVT_LEFT_DOWN, self.on_motors_clicked)
00128
00129 static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Battery"), wx.HORIZONTAL)
00130 sizer.Add(static_sizer, 0)
00131
00132
00133 self._power_state_ctrl = PowerStateControl(self, wx.ID_ANY, icons_path)
00134 self._power_state_ctrl.SetToolTip(wx.ToolTip("Battery: Stale"))
00135 static_sizer.Add(self._power_state_ctrl, 1, wx.EXPAND)
00136
00137 self._config = wx.Config("p2os_dashboard")
00138
00139 self.Bind(wx.EVT_CLOSE, self.on_close)
00140
00141 self.Layout()
00142 self.Fit()
00143
00144 self._diagnostics_frame = DiagnosticsFrame(self, wx.ID_ANY, "Diagnostics")
00145 self._diagnostics_frame.Hide()
00146 self._diagnostics_frame.Center()
00147 self._diagnostics_button.Bind(wx.EVT_BUTTON, self.on_diagnostics_clicked)
00148
00149 self._rosout_frame = RosoutFrame(self, wx.ID_ANY, "Rosout")
00150 self._rosout_frame.Hide()
00151 self._rosout_frame.Center()
00152 self._rosout_button.Bind(wx.EVT_BUTTON, self.on_rosout_clicked)
00153
00154 self.load_config()
00155
00156 self._timer = wx.Timer(self)
00157 self.Bind(wx.EVT_TIMER, self.on_timer)
00158 self._timer.Start(1000)
00159
00160 self.battery_msg_last_time = rospy.Time.now()
00161
00162
00163 def __del__(self):
00164 self._dashboard_agg_sub.unregister()
00165
00166 def battStateCallback(self, msg):
00167 self.battery_msg_last_time = rospy.Time.now()
00168 self._power_state_ctrl.set_power_state(msg)
00169
00170 def motorStateCallback(self, msg):
00171 self.motorState = msg.state
00172
00173 if(self.motorState):
00174 self._motors_button.set_ok()
00175 self._motors_button.SetToolTip(wx.ToolTip("Motors: Running"))
00176 else:
00177 self._motors_button.set_error()
00178 self._motors_button.SetToolTip(wx.ToolTip("Motors: Halted"))
00179
00180 def on_timer(self, evt):
00181 level = self._diagnostics_frame._diagnostics_panel.get_top_level_state()
00182 if (level == -1 or level == 3):
00183 if (self._diagnostics_button.set_stale()):
00184 self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Stale"))
00185 elif (level >= 2):
00186 if (self._diagnostics_button.set_error()):
00187 self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Error"))
00188 elif (level == 1):
00189 if (self._diagnostics_button.set_warn()):
00190 self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Warning"))
00191 else:
00192 if (self._diagnostics_button.set_ok()):
00193 self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: OK"))
00194
00195
00196 if (rospy.Time.now() > self.battery_msg_last_time + rospy.Duration(5.0)):
00197
00198
00199 self._power_state_ctrl.set_stale()
00200
00201 self.update_rosout()
00202 self._power_state_ctrl.updateToolTip()
00203
00204 if (rospy.is_shutdown()):
00205 self.Close()
00206
00207 def on_diagnostics_clicked(self, evt):
00208 self._diagnostics_frame.Show()
00209 self._diagnostics_frame.Raise()
00210
00211 def on_rosout_clicked(self, evt):
00212 self._rosout_frame.Show()
00213 self._rosout_frame.Raise()
00214
00215 def on_motors_clicked(self, evt):
00216 menu = wx.Menu()
00217 menu.Bind(wx.EVT_MENU, self.on_reset_motors, menu.Append(wx.ID_ANY, "Enable"))
00218 menu.Bind(wx.EVT_MENU, self.on_halt_motors, menu.Append(wx.ID_ANY, "Disable"))
00219 self._motors_button.toggle(True)
00220 self.PopupMenu(menu)
00221 self._motors_button.toggle(False)
00222
00223 def on_reset_motors(self, evt):
00224
00225 self.motorStatePub.publish(MotorState(1))
00226
00227 def on_halt_motors(self, evt):
00228 self.motorStatePub.publish(MotorState(0))
00229
00230 def update_rosout(self):
00231 summary_dur = 30.0
00232 if (rospy.get_time() < 30.0):
00233 summary_dur = rospy.get_time() - 1.0
00234
00235 if (summary_dur < 0):
00236 summary_dur = 0.0
00237
00238 summary = self._rosout_frame.get_panel().getMessageSummary(summary_dur)
00239
00240 if (summary.fatal or summary.error):
00241 self._rosout_button.set_error()
00242 elif (summary.warn):
00243 self._rosout_button.set_warn()
00244 else:
00245 self._rosout_button.set_ok()
00246
00247
00248 tooltip = ""
00249 if (summary.fatal):
00250 tooltip += "\nFatal: %s"%(summary.fatal)
00251 if (summary.error):
00252 tooltip += "\nError: %s"%(summary.error)
00253 if (summary.warn):
00254 tooltip += "\nWarn: %s"%(summary.warn)
00255 if (summary.info):
00256 tooltip += "\nInfo: %s"%(summary.info)
00257 if (summary.debug):
00258 tooltip += "\nDebug: %s"%(summary.debug)
00259
00260 if (len(tooltip) == 0):
00261 tooltip = "Rosout: no recent activity"
00262 else:
00263 tooltip = "Rosout: recent activity:" + tooltip
00264
00265 if (tooltip != self._rosout_button.GetToolTip().GetTip()):
00266 self._rosout_button.SetToolTip(wx.ToolTip(tooltip))
00267
00268 def load_config(self):
00269
00270 (x, y) = self.GetPositionTuple()
00271 (width, height) = self.GetSizeTuple()
00272 if (self._config.HasEntry(self._CONFIG_WINDOW_X)):
00273 x = self._config.ReadInt(self._CONFIG_WINDOW_X)
00274 if (self._config.HasEntry(self._CONFIG_WINDOW_Y)):
00275 y = self._config.ReadInt(self._CONFIG_WINDOW_Y)
00276
00277 self.SetPosition((x, y))
00278 self.SetSize((width, height))
00279
00280 def save_config(self):
00281 config = self._config
00282
00283 (x, y) = self.GetPositionTuple()
00284 (width, height) = self.GetSizeTuple()
00285 config.WriteInt(self._CONFIG_WINDOW_X, x)
00286 config.WriteInt(self._CONFIG_WINDOW_Y, y)
00287
00288 config.Flush()
00289
00290 def on_close(self, event):
00291 self.save_config()
00292 self.Destroy()
