class PR2Frame(wx.Frame):
_CONFIG_WINDOW_X = "/Window/X"
_CONFIG_WINDOW_Y = "/Window/Y"
def __init__(self,
parent,
id=wx.ID_ANY,
title='cob dashboard',
pos=wx.DefaultPosition,
size=(400, 50),
style=wx.CAPTION | wx.CLOSE_BOX | wx.STAY_ON_TOP):
wx.Frame.__init__(self, parent, id, title, pos, size, style)
wx.InitAllImageHandlers()
rospy.init_node('cob3_dashboard', anonymous=True)
try:
getattr(rxtools, "initRoscpp")
rxtools.initRoscpp("cob3_dashboard_cpp", anonymous=True)
except AttributeError:
pass
self.SetTitle('cob dashboard (%s)' % rosenv.get_master_uri())
icons_path = path.join(roslib.packages.get_pkg_dir('pr2_dashboard'),
"icons/")
sizer = wx.BoxSizer(wx.HORIZONTAL)
self.SetSizer(sizer)
static_sizer = wx.StaticBoxSizer(
wx.StaticBox(self, wx.ID_ANY, "Diagnostic"), wx.HORIZONTAL)
sizer.Add(static_sizer, 0)
# Diagnostics
self._diagnostics_button = StatusControl(self, wx.ID_ANY, icons_path,
"diag", True)
self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics"))
static_sizer.Add(self._diagnostics_button, 0)
# Rosout
self._rosout_button = StatusControl(self, wx.ID_ANY, icons_path,
"rosout", True)
self._rosout_button.SetToolTip(wx.ToolTip("Rosout"))
static_sizer.Add(self._rosout_button, 0)
# Motors
self._motors_button = StatusControl(self, wx.ID_ANY, icons_path,
"motor", True)
self._motors_button.SetToolTip(wx.ToolTip("Motors"))
static_sizer.Add(self._motors_button, 0)
self._motors_button.Bind(wx.EVT_LEFT_DOWN, self.on_motors_clicked)
static_sizer = wx.StaticBoxSizer(
wx.StaticBox(self, wx.ID_ANY, "Motors"), wx.HORIZONTAL)
sizer.Add(static_sizer, 0)
# Breakers
breaker_names = ["Peft Arm", "Base/Body", "Right Arm"]
self._breaker_ctrls = []
for i in xrange(0, 3):
ctrl = BreakerControl(self, wx.ID_ANY, i, breaker_names[i],
icons_path)
ctrl.SetToolTip(wx.ToolTip("Breaker %s" % (breaker_names[i])))
self._breaker_ctrls.append(ctrl)
static_sizer.Add(ctrl, 0)
static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "EM"),
wx.HORIZONTAL)
sizer.Add(static_sizer, 0)
# run-stop
self._runstop_ctrl = StatusControl(self, wx.ID_ANY, icons_path,
"runstop", False)
self._runstop_ctrl.SetToolTip(wx.ToolTip("Physical Runstop: Unknown"))
static_sizer.Add(self._runstop_ctrl, 0)
# Wireless run-stop
#self._wireless_runstop_ctrl = StatusControl(self, wx.ID_ANY, icons_path, "runstop-wireless", False)
#self._wireless_runstop_ctrl.SetToolTip(wx.ToolTip("Wireless Runstop: Unknown"))
#static_sizer.Add(self._wireless_runstop_ctrl, 0)
static_sizer = wx.StaticBoxSizer(
wx.StaticBox(self, wx.ID_ANY, "Battery"), wx.HORIZONTAL)
sizer.Add(static_sizer, 0)
# Battery State
self._power_state_ctrl = PowerStateControl(self, wx.ID_ANY, icons_path)
self._power_state_ctrl.SetToolTip(wx.ToolTip("Battery: Stale"))
static_sizer.Add(self._power_state_ctrl, 1, wx.EXPAND)
self._config = wx.Config("pr2_dashboard")
self.Bind(wx.EVT_CLOSE, self.on_close)
self.Layout()
self.Fit()
self._diagnostics_frame = DiagnosticsFrame(self, wx.ID_ANY,
"Diagnostics")
self._diagnostics_frame.Hide()
self._diagnostics_frame.Center()
self._diagnostics_button.Bind(wx.EVT_BUTTON,
self.on_diagnostics_clicked)
self._rosout_frame = RosoutFrame(self, wx.ID_ANY, "Rosout")
self._rosout_frame.Hide()
self._rosout_frame.Center()
self._rosout_button.Bind(wx.EVT_BUTTON, self.on_rosout_clicked)
self.load_config()
self._timer = wx.Timer(self)
self.Bind(wx.EVT_TIMER, self.on_timer)
self._timer.Start(500)
self._dashboard_agg_sub = rospy.Subscriber('dashboard_agg',
DashboardState,
self.dashboard_callback)
self._dashboard_message = None
self._last_dashboard_message_time = 0.0
def __del__(self):
self._dashboard_agg_sub.unregister()
def on_timer(self, evt):
level = self._diagnostics_frame._diagnostics_panel.get_top_level_state(
)
if (level == -1 or level == 3):
if (self._diagnostics_button.set_stale()):
self._diagnostics_button.SetToolTip(
wx.ToolTip("Diagnostics: Stale"))
elif (level >= 2):
if (self._diagnostics_button.set_error()):
self._diagnostics_button.SetToolTip(
wx.ToolTip("Diagnostics: Error"))
elif (level == 1):
if (self._diagnostics_button.set_warn()):
self._diagnostics_button.SetToolTip(
wx.ToolTip("Diagnostics: Warning"))
else:
if (self._diagnostics_button.set_ok()):
self._diagnostics_button.SetToolTip(
wx.ToolTip("Diagnostics: OK"))
self.update_rosout()
if (rospy.get_time() - self._last_dashboard_message_time > 5.0):
self._motors_button.set_stale()
self._power_state_ctrl.set_stale()
[ctrl.reset() for ctrl in self._breaker_ctrls]
self._runstop_ctrl.set_stale()
#self._wireless_runstop_ctrl.set_stale()
ctrls = [
self._motors_button, self._power_state_ctrl, self._runstop_ctrl
]
ctrls.extend(self._breaker_ctrls)
for ctrl in ctrls:
ctrl.SetToolTip(
wx.ToolTip(
"No message received on dashboard_agg in the last 5 seconds"
))
if (rospy.is_shutdown()):
self.Close()
def on_diagnostics_clicked(self, evt):
self._diagnostics_frame.Show()
self._diagnostics_frame.Raise()
def on_rosout_clicked(self, evt):
self._rosout_frame.Show()
self._rosout_frame.Raise()
def on_motors_clicked(self, evt):
menu = wx.Menu()
menu.Bind(wx.EVT_MENU, self.on_reset_motors,
menu.Append(wx.ID_ANY, "Reset"))
menu.Bind(wx.EVT_MENU, self.on_halt_motors,
menu.Append(wx.ID_ANY, "Halt"))
self._motors_button.toggle(True)
self.PopupMenu(menu)
self._motors_button.toggle(False)
def on_reset_motors(self, evt):
# if any of the breakers is not enabled ask if they'd like to enable them
if (self._dashboard_message is not None
and self._dashboard_message.power_board_state_valid):
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
])
if (not all_breakers_enabled):
if (wx.MessageBox(
"Resetting the motors may not work because not all breakers are enabled. Enable all the breakers first?",
"Enable Breakers?", wx.YES_NO | wx.ICON_QUESTION,
self) == wx.YES):
[breaker.set_enable() for breaker in self._breaker_ctrls]
reset = rospy.ServiceProxy("pr2_etherCAT/reset_motors",
std_srvs.srv.Empty)
try:
reset()
except rospy.ServiceException, e:
wx.MessageBox(
"Failed to reset the motors: service call failed with error: %s"
% (e), "Error", wx.OK | wx.ICON_ERROR)
class BilibotFrame(wx.Frame):
_CONFIG_WINDOW_X="/Window/X"
_CONFIG_WINDOW_Y="/Window/Y"
def __init__(self, parent, id=wx.ID_ANY, title='Bilibot Dashboard', pos=wx.DefaultPosition, size=(400, 50), style=wx.CAPTION|wx.CLOSE_BOX|wx.STAY_ON_TOP):
wx.Frame.__init__(self, parent, id, title, pos, size, style)
wx.InitAllImageHandlers()
rospy.init_node('bilibot_dashboard', anonymous=True)
try:
getattr(rxtools, "initRoscpp")
rxtools.initRoscpp("bilibot_dashboard_cpp", anonymous=True)
except AttributeError:
pass
self.SetTitle('Bilibot Dashboard (%s)'%rosenv.get_master_uri())
icons_path = path.join(roslib.packages.get_pkg_dir('bilibot_dashboard'), "icons/")
sizer = wx.BoxSizer(wx.HORIZONTAL)
self.SetSizer(sizer)
static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Diagnostic"), wx.HORIZONTAL)
sizer.Add(static_sizer, 0)
# Diagnostics
self._diagnostics_button = StatusControl(self, wx.ID_ANY, icons_path, "diag", True)
self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics"))
static_sizer.Add(self._diagnostics_button, 0)
# Rosout
self._rosout_button = StatusControl(self, wx.ID_ANY, icons_path, "rosout", True)
self._rosout_button.SetToolTip(wx.ToolTip("Rosout"))
static_sizer.Add(self._rosout_button, 0)
# Motors
self._motors_button = StatusControl(self, wx.ID_ANY, icons_path, "motor", True)
self._motors_button.SetToolTip(wx.ToolTip("Mode"))
static_sizer.Add(self._motors_button, 0)
self._motors_button.Bind(wx.EVT_LEFT_DOWN, self.on_motors_clicked)
static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Breakers"), wx.HORIZONTAL)
sizer.Add(static_sizer, 0)
# Breakers
breaker_names = ["Digital Out 1", "Digital Out 1", "Digital Out 2"]
self._breaker_ctrls = []
for i in xrange(0, 3):
ctrl = BreakerControl(self, wx.ID_ANY, i, breaker_names[i], icons_path)
ctrl.SetToolTip(wx.ToolTip("%s"%(breaker_names[i])))
self._breaker_ctrls.append(ctrl)
static_sizer.Add(ctrl, 0)
# static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Runstops"), wx.HORIZONTAL)
# sizer.Add(static_sizer, 0)
# run-stop
# self._runstop_ctrl = StatusControl(self, wx.ID_ANY, icons_path, "runstop", False)
# self._runstop_ctrl.SetToolTip(wx.ToolTip("Physical Runstop: Unknown"))
# static_sizer.Add(self._runstop_ctrl, 0)
# Wireless run-stop
# self._wireless_runstop_ctrl = StatusControl(self, wx.ID_ANY, icons_path, "runstop-wireless", False)
# self._wireless_runstop_ctrl.SetToolTip(wx.ToolTip("Wireless Runstop: Unknown"))
# static_sizer.Add(self._wireless_runstop_ctrl, 0)
static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Battery"), wx.HORIZONTAL)
sizer.Add(static_sizer, 0)
# Battery State
self._power_state_ctrl = PowerStateControl(self, wx.ID_ANY, icons_path)
self._power_state_ctrl.SetToolTip(wx.ToolTip("Battery: Stale"))
static_sizer.Add(self._power_state_ctrl, 1, wx.EXPAND)
static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Computer"), wx.HORIZONTAL)
sizer.Add(static_sizer, 0)
# Laptop Battery State
self._power_state_ctrl_laptop = PowerStateControl(self, wx.ID_ANY, icons_path)
self._power_state_ctrl_laptop.SetToolTip(wx.ToolTip("Computer Battery: Stale"))
static_sizer.Add(self._power_state_ctrl_laptop, 1, wx.EXPAND)
self._config = wx.Config("bilibot_dashboard")
self.Bind(wx.EVT_CLOSE, self.on_close)
self.Layout()
self.Fit()
self._diagnostics_frame = DiagnosticsFrame(self, wx.ID_ANY, "Diagnostics")
self._diagnostics_frame.Hide()
self._diagnostics_frame.Center()
self._diagnostics_button.Bind(wx.EVT_BUTTON, self.on_diagnostics_clicked)
self._rosout_frame = RosoutFrame(self, wx.ID_ANY, "Rosout")
self._rosout_frame.Hide()
self._rosout_frame.Center()
self._rosout_button.Bind(wx.EVT_BUTTON, self.on_rosout_clicked)
self.load_config()
self._timer = wx.Timer(self)
self.Bind(wx.EVT_TIMER, self.on_timer)
self._timer.Start(500)
self._dashboard_agg_sub = rospy.Subscriber('diagnostics_agg', diagnostic_msgs.msg.DiagnosticArray, self.dashboard_callback)
self._dashboard_message = None
self._last_dashboard_message_time = 0.0
def __del__(self):
self._dashboard_agg_sub.unregister()
def on_timer(self, evt):
level = self._diagnostics_frame._diagnostics_panel.get_top_level_state()
if (level == -1 or level == 3):
if (self._diagnostics_button.set_stale()):
self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Stale"))
elif (level >= 2):
if (self._diagnostics_button.set_error()):
self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Error"))
elif (level == 1):
if (self._diagnostics_button.set_warn()):
self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Warning"))
else:
if (self._diagnostics_button.set_ok()):
self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: OK"))
self.update_rosout()
if (rospy.get_time() - self._last_dashboard_message_time > 5.0):
self._motors_button.set_stale()
self._power_state_ctrl.set_stale()
self._power_state_ctrl_laptop.set_stale()
[ctrl.reset() for ctrl in self._breaker_ctrls]
# self._runstop_ctrl.set_stale()
# self._wireless_runstop_ctrl.set_stale()
ctrls = [self._motors_button, self._power_state_ctrl, self._power_state_ctrl_laptop]
ctrls.extend(self._breaker_ctrls)
for ctrl in ctrls:
ctrl.SetToolTip(wx.ToolTip("No message received on dashboard_agg in the last 5 seconds"))
if (rospy.is_shutdown()):
self.Close()
def on_diagnostics_clicked(self, evt):
self._diagnostics_frame.Show()
self._diagnostics_frame.Raise()
def on_rosout_clicked(self, evt):
self._rosout_frame.Show()
self._rosout_frame.Raise()
def on_motors_clicked(self, evt):
menu = wx.Menu()
menu.Bind(wx.EVT_MENU, self.on_passive_mode, menu.Append(wx.ID_ANY, "Passive Mode"))
menu.Bind(wx.EVT_MENU, self.on_safe_mode, menu.Append(wx.ID_ANY, "Safety Mode"))
menu.Bind(wx.EVT_MENU, self.on_full_mode, menu.Append(wx.ID_ANY, "Full Mode"))
self._motors_button.toggle(True)
self.PopupMenu(menu)
self._motors_button.toggle(False)
def on_passive_mode(self, evt):
passive = rospy.ServiceProxy("/bilibot_node/set_operation_mode",bilibot_node.srv.SetBilibotMode )
try:
passive(bilibot_node.msg.BilibotSensorState.OI_MODE_PASSIVE)
except rospy.ServiceException, e:
wx.MessageBox("Failed to put the bilibot in passive mode: service call failed with error: %s"%(e), "Error", wx.OK|wx.ICON_ERROR)
class DragonbotFrame(wx.Frame):
_CONFIG_WINDOW_X="/Window/X"
_CONFIG_WINDOW_Y="/Window/Y"
def __init__(self, parent, id=wx.ID_ANY, title='Dragonbot Dashboard', pos=wx.DefaultPosition, size=(400, 50), style=wx.CAPTION|wx.CLOSE_BOX|wx.STAY_ON_TOP):
wx.Frame.__init__(self, parent, id, title, pos, size, style)
wx.InitAllImageHandlers()
#self.battStateSub = rospy.Subscriber("battery_state", BatteryState, self.battStateCallback)
self.motorState = 0
rospy.init_node('dragonbot_dashboard', anonymous=True)
try:
getattr(rxtools, "initRoscpp")
rxtools.initRoscpp("dragonbot_dashboard_cpp", anonymous=True)
except AttributeError:
pass
self.SetTitle('Dragonbot Dashboard (%s)'%rosenv.get_master_uri())
icons_path = path.join(roslib.packages.get_pkg_dir('dragonbot_dashboard'), "icons/")
sizer = wx.BoxSizer(wx.HORIZONTAL)
self.SetSizer(sizer)
static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Diagnostic"), wx.HORIZONTAL)
sizer.Add(static_sizer, 0)
# Diagnostics
self._diagnostics_button = StatusControl(self, wx.ID_ANY, icons_path, "diag", True)
self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics"))
static_sizer.Add(self._diagnostics_button, 0)
# Rosout
self._rosout_button = StatusControl(self, wx.ID_ANY, icons_path, "rosout", True)
self._rosout_button.SetToolTip(wx.ToolTip("Rosout"))
static_sizer.Add(self._rosout_button, 0)
# Motors
self._motors_button = StatusControl(self, wx.ID_ANY, icons_path, "motor", True)
self._motors_button.SetToolTip(wx.ToolTip("Motors"))
static_sizer.Add(self._motors_button, 0)
self._motors_button.Bind(wx.EVT_LEFT_DOWN, self.on_motors_clicked)
self.motorStatePub = rospy.Publisher('cmd_motor_state', Int32)
self.motorStateSub = rospy.Subscriber("motor_state", Int32, self.motorStateCallback)
#sizer.Add(static_sizer, 0)
# Battery State (not yet)
#self._power_state_ctrl = PowerStateControl(self, wx.ID_ANY, icons_path)
#self._power_state_ctrl.SetToolTip(wx.ToolTip("Battery: Stale"))
#static_sizer.Add(self._power_state_ctrl, 1, wx.EXPAND)
self._config = wx.Config("dragonbot_dashboard")
self.Bind(wx.EVT_CLOSE, self.on_close)
self.Layout()
self.Fit()
self._diagnostics_frame = DiagnosticsFrame(self, wx.ID_ANY, "Diagnostics")
self._diagnostics_frame.Hide()
self._diagnostics_frame.Center()
self._diagnostics_button.Bind(wx.EVT_BUTTON, self.on_diagnostics_clicked)
self._rosout_frame = RosoutFrame(self, wx.ID_ANY, "Rosout")
self._rosout_frame.Hide()
self._rosout_frame.Center()
self._rosout_button.Bind(wx.EVT_BUTTON, self.on_rosout_clicked)
self.load_config()
self._timer = wx.Timer(self)
self.Bind(wx.EVT_TIMER, self.on_timer)
self._timer.Start(500)
#self._dashboard_agg_sub = rospy.Subscriber('dashboard_agg', DashboardState, self.dashboard_callback)
self._dashboard_message = None
self._last_dashboard_message_time = 0.0
def __del__(self):
self._dashboard_agg_sub.unregister()
def motorStateCallback(self, msg):
self.motorState = msg.data
if(self.motorState):
self._motors_button.set_ok()
self._motors_button.SetToolTip(wx.ToolTip("Motors: Running"))
else:
self._motors_button.set_error()
self._motors_button.SetToolTip(wx.ToolTip("Motors: Halted"))
def on_timer(self, evt):
level = self._diagnostics_frame.get_top_level_state()
if self._diagnostics_frame._diagnostics_panel is not None:
level_old = self._diagnostics_frame._diagnostics_panel.get_top_level_state()
if level != level_old:
print 'level differs: ', level, level_old
if (level == -1 or level == 3):
if (self._diagnostics_button.set_stale()):
self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Stale"))
elif (level >= 2):
if (self._diagnostics_button.set_error()):
self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Error"))
elif (level == 1):
if (self._diagnostics_button.set_warn()):
self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Warning"))
else:
if (self._diagnostics_button.set_ok()):
self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: OK"))
self.update_rosout()
if (rospy.is_shutdown()):
self.Close()
def on_diagnostics_clicked(self, evt):
self._diagnostics_frame.Show()
self._diagnostics_frame.Raise()
def on_rosout_clicked(self, evt):
self._rosout_frame.Show()
self._rosout_frame.Raise()
def on_motors_clicked(self, evt):
menu = wx.Menu()
menu.Bind(wx.EVT_MENU, self.on_reset_motors, menu.Append(wx.ID_ANY, "Enable"))
menu.Bind(wx.EVT_MENU, self.on_halt_motors, menu.Append(wx.ID_ANY, "Disable"))
self._motors_button.toggle(True)
self.PopupMenu(menu)
self._motors_button.toggle(False)
def on_reset_motors(self, evt):
self.motorStatePub.publish(Int32(1))
def on_halt_motors(self, evt):
self.motorStatePub.publish(Int32(0))
def update_rosout(self):
summary_dur = 30.0
if (rospy.get_time() < 30.0):
summary_dur = rospy.get_time() - 1.0
if (summary_dur < 0):
summary_dur = 0.0
summary = self._rosout_frame.get_panel().getMessageSummary(summary_dur)
if (summary.fatal or summary.error):
self._rosout_button.set_error()
elif (summary.warn):
self._rosout_button.set_warn()
else:
self._rosout_button.set_ok()
tooltip = ""
if (summary.fatal):
tooltip += "\nFatal: %s"%(summary.fatal)
if (summary.error):
tooltip += "\nError: %s"%(summary.error)
if (summary.warn):
tooltip += "\nWarn: %s"%(summary.warn)
if (summary.info):
tooltip += "\nInfo: %s"%(summary.info)
if (summary.debug):
tooltip += "\nDebug: %s"%(summary.debug)
if (len(tooltip) == 0):
tooltip = "Rosout: no recent activity"
else:
tooltip = "Rosout: recent activity:" + tooltip
if (tooltip != self._rosout_button.GetToolTip().GetTip()):
self._rosout_button.SetToolTip(wx.ToolTip(tooltip))
def load_config(self):
# Load our window options
(x, y) = self.GetPositionTuple()
(width, height) = self.GetSizeTuple()
if (self._config.HasEntry(self._CONFIG_WINDOW_X)):
x = self._config.ReadInt(self._CONFIG_WINDOW_X)
if (self._config.HasEntry(self._CONFIG_WINDOW_Y)):
y = self._config.ReadInt(self._CONFIG_WINDOW_Y)
self.SetPosition((x, y))
self.SetSize((width, height))
def save_config(self):
config = self._config
(x, y) = self.GetPositionTuple()
(width, height) = self.GetSizeTuple()
config.WriteInt(self._CONFIG_WINDOW_X, x)
config.WriteInt(self._CONFIG_WINDOW_Y, y)
config.Flush()
def on_close(self, event):
self.save_config()
self.Destroy()
class PR2Frame(wx.Frame):
_CONFIG_WINDOW_X="/Window/X"
_CONFIG_WINDOW_Y="/Window/Y"
def __init__(self, parent, id=wx.ID_ANY, title='PR2 Dashboard', pos=wx.DefaultPosition, size=(400, 50), style=wx.CAPTION|wx.CLOSE_BOX|wx.STAY_ON_TOP):
wx.Frame.__init__(self, parent, id, title, pos, size, style)
wx.InitAllImageHandlers()
rospy.init_node('pr2_dashboard', anonymous=True)
try:
getattr(rxtools, "initRoscpp")
rxtools.initRoscpp("pr2_dashboard_cpp", anonymous=True)
except AttributeError:
pass
self.SetTitle('PR2 Dashboard (%s)'%rosenv.get_master_uri())
icons_path = path.join(roslib.packages.get_pkg_dir('pr2_dashboard'), "icons/")
sizer = wx.BoxSizer(wx.HORIZONTAL)
self.SetSizer(sizer)
static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Diagnostic"), wx.HORIZONTAL)
sizer.Add(static_sizer, 0)
# Diagnostics
self._diagnostics_button = StatusControl(self, wx.ID_ANY, icons_path, "diag", True)
self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics"))
static_sizer.Add(self._diagnostics_button, 0)
# Rosout
self._rosout_button = StatusControl(self, wx.ID_ANY, icons_path, "rosout", True)
self._rosout_button.SetToolTip(wx.ToolTip("Rosout"))
static_sizer.Add(self._rosout_button, 0)
# Motors
self._motors_button = StatusControl(self, wx.ID_ANY, icons_path, "motor", True)
self._motors_button.SetToolTip(wx.ToolTip("Motors"))
static_sizer.Add(self._motors_button, 0)
self._motors_button.Bind(wx.EVT_LEFT_DOWN, self.on_motors_clicked)
static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Breakers"), wx.HORIZONTAL)
sizer.Add(static_sizer, 0)
# Breakers
breaker_names = ["Left Arm", "Base/Body", "Right Arm"]
self._breaker_ctrls = []
for i in xrange(0, 3):
ctrl = BreakerControl(self, wx.ID_ANY, i, breaker_names[i], icons_path)
ctrl.SetToolTip(wx.ToolTip("Breaker %s"%(breaker_names[i])))
self._breaker_ctrls.append(ctrl)
static_sizer.Add(ctrl, 0)
static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Runstops"), wx.HORIZONTAL)
sizer.Add(static_sizer, 0)
# run-stop
self._runstop_ctrl = StatusControl(self, wx.ID_ANY, icons_path, "runstop", False)
self._runstop_ctrl.SetToolTip(wx.ToolTip("Physical Runstop: Unknown"))
static_sizer.Add(self._runstop_ctrl, 0)
# Wireless run-stop
self._wireless_runstop_ctrl = StatusControl(self, wx.ID_ANY, icons_path, "runstop-wireless", False)
self._wireless_runstop_ctrl.SetToolTip(wx.ToolTip("Wireless Runstop: Unknown"))
static_sizer.Add(self._wireless_runstop_ctrl, 0)
static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Battery"), wx.HORIZONTAL)
sizer.Add(static_sizer, 0)
# Battery State
self._power_state_ctrl = PowerStateControl(self, wx.ID_ANY, icons_path)
self._power_state_ctrl.SetToolTip(wx.ToolTip("Battery: Stale"))
static_sizer.Add(self._power_state_ctrl, 1, wx.EXPAND)
self._config = wx.Config("pr2_dashboard")
self.Bind(wx.EVT_CLOSE, self.on_close)
self.Layout()
self.Fit()
self._diagnostics_frame = DiagnosticsFrame(self, wx.ID_ANY, "Diagnostics")
self._diagnostics_frame.Hide()
self._diagnostics_frame.Center()
self._diagnostics_button.Bind(wx.EVT_BUTTON, self.on_diagnostics_clicked)
self._rosout_frame = RosoutFrame(self, wx.ID_ANY, "Rosout")
self._rosout_frame.Hide()
self._rosout_frame.Center()
self._rosout_button.Bind(wx.EVT_BUTTON, self.on_rosout_clicked)
self.load_config()
self._timer = wx.Timer(self)
self.Bind(wx.EVT_TIMER, self.on_timer)
self._timer.Start(500)
self._dashboard_agg_sub = rospy.Subscriber('dashboard_agg', DashboardState, self.dashboard_callback)
self._dashboard_message = None
self._last_dashboard_message_time = 0.0
def __del__(self):
self._dashboard_agg_sub.unregister()
def on_timer(self, evt):
level = self._diagnostics_frame.get_top_level_state()
if (level == -1 or level == 3):
if (self._diagnostics_button.set_stale()):
self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Stale"))
elif (level >= 2):
if (self._diagnostics_button.set_error()):
self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Error"))
elif (level == 1):
if (self._diagnostics_button.set_warn()):
self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Warning"))
else:
if (self._diagnostics_button.set_ok()):
self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: OK"))
self.update_rosout()
if (rospy.get_time() - self._last_dashboard_message_time > 5.0):
self._motors_button.set_stale()
self._power_state_ctrl.set_stale()
[ctrl.reset() for ctrl in self._breaker_ctrls]
self._runstop_ctrl.set_stale()
self._wireless_runstop_ctrl.set_stale()
ctrls = [self._motors_button, self._power_state_ctrl, self._runstop_ctrl, self._wireless_runstop_ctrl]
ctrls.extend(self._breaker_ctrls)
for ctrl in ctrls:
ctrl.SetToolTip(wx.ToolTip("No message received on dashboard_agg in the last 5 seconds"))
if (rospy.is_shutdown()):
self.Close()
def on_diagnostics_clicked(self, evt):
self._diagnostics_frame.Show()
self._diagnostics_frame.Raise()
def on_rosout_clicked(self, evt):
self._rosout_frame.Show()
self._rosout_frame.Raise()
def on_motors_clicked(self, evt):
menu = wx.Menu()
menu.Bind(wx.EVT_MENU, self.on_reset_motors, menu.Append(wx.ID_ANY, "Reset"))
menu.Bind(wx.EVT_MENU, self.on_halt_motors, menu.Append(wx.ID_ANY, "Halt"))
self._motors_button.toggle(True)
self.PopupMenu(menu)
self._motors_button.toggle(False)
def on_reset_motors(self, evt):
# if any of the breakers is not enabled ask if they'd like to enable them
if (self._dashboard_message is not None and self._dashboard_message.power_board_state_valid):
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])
if (not all_breakers_enabled):
if (wx.MessageBox("Resetting the motors may not work because not all breakers are enabled. Enable all the breakers first?", "Enable Breakers?", wx.YES_NO|wx.ICON_QUESTION, self) == wx.YES):
[breaker.set_enable() for breaker in self._breaker_ctrls]
reset = rospy.ServiceProxy("pr2_etherCAT/reset_motors", std_srvs.srv.Empty)
try:
reset()
except rospy.ServiceException, e:
wx.MessageBox("Failed to reset the motors: service call failed with error: %s"%(e), "Error", wx.OK|wx.ICON_ERROR)
class PR2Frame(wx.Frame):
_CONFIG_WINDOW_X="/Window/X"
_CONFIG_WINDOW_Y="/Window/Y"
def __init__(self, parent, id=wx.ID_ANY, title='youbot_dashboard', pos=wx.DefaultPosition, size=(400, 50), style=wx.CAPTION|wx.CLOSE_BOX|wx.STAY_ON_TOP):
wx.Frame.__init__(self, parent, id, title, pos, size, style)
wx.InitAllImageHandlers()
rospy.init_node('youbot_dashboard', anonymous=True)
try:
getattr(rxtools, "initRoscpp")
rxtools.initRoscpp("youbot_dashboard_cpp", anonymous=True)
except AttributeError:
pass
self.SetTitle('youbot_dashboard (%s)'%rosenv.get_master_uri())
icons_path = path.join(roslib.packages.get_pkg_dir('pr2_dashboard'), "icons/")
youbot_icons_path = path.join(roslib.packages.get_pkg_dir('youbot_dashboard'), "icons/")
sizer = wx.BoxSizer(wx.HORIZONTAL)
self.SetSizer(sizer)
static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Diagn."), wx.HORIZONTAL)
sizer.Add(static_sizer, 0)
# Diagnostics
self._diagnostics_button = StatusControl(self, wx.ID_ANY, icons_path, "diag", True)
self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics"))
static_sizer.Add(self._diagnostics_button, 0)
# Rosout
self._rosout_button = StatusControl(self, wx.ID_ANY, icons_path, "rosout", True)
self._rosout_button.SetToolTip(wx.ToolTip("Rosout"))
static_sizer.Add(self._rosout_button, 0)
static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Platform"), wx.HORIZONTAL)
sizer.Add(static_sizer, 0)
# base status
self._base_status = StatusControl(self, wx.ID_ANY, youbot_icons_path, "base", True)
self._base_status.SetToolTip(wx.ToolTip("Base Motors: Stale"))
static_sizer.Add(self._base_status, 0)
self._base_status.Bind(wx.EVT_LEFT_DOWN, self.on_base_status_clicked)
# arm status
self._arm_status = StatusControl(self, wx.ID_ANY, youbot_icons_path, "arm", True)
self._arm_status.SetToolTip(wx.ToolTip("Arm Motors: Stale"))
static_sizer.Add(self._arm_status, 0)
self._arm_status.Bind(wx.EVT_LEFT_DOWN, self.on_arm_status_clicked)
static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Drv"), wx.HORIZONTAL)
sizer.Add(static_sizer, 0)
# driver status
self._driver_status = StatusControl(self, wx.ID_ANY, youbot_icons_path, "driver", True)
self._driver_status.SetToolTip(wx.ToolTip("Driver: Stale"))
static_sizer.Add(self._driver_status, 0)
self._driver_status.Bind(wx.EVT_BUTTON, self.on_driver_status_clicked)
static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Battery"), wx.HORIZONTAL)
sizer.Add(static_sizer, 0)
# Battery State
self._power_state_ctrl = PowerStateControl(self, wx.ID_ANY, icons_path)
self._power_state_ctrl.SetToolTip(wx.ToolTip("Battery: Stale"))
static_sizer.Add(self._power_state_ctrl, 1, wx.EXPAND)
self._config = wx.Config("youbot_dashboard")
self.Bind(wx.EVT_CLOSE, self.on_close)
self.Layout()
self.Fit()
self._diagnostics_frame = DiagnosticsFrame(self, wx.ID_ANY, "Diagnostics")
self._diagnostics_frame.Hide()
self._diagnostics_frame.Center()
self._diagnostics_button.Bind(wx.EVT_BUTTON, self.on_diagnostics_clicked)
self._rosout_frame = RosoutFrame(self, wx.ID_ANY, "Rosout")
self._rosout_frame.Hide()
self._rosout_frame.Center()
self._rosout_button.Bind(wx.EVT_BUTTON, self.on_rosout_clicked)
self.load_config()
self._timer = wx.Timer(self)
self.Bind(wx.EVT_TIMER, self.on_timer)
self._timer.Start(500)
self._dashboard_agg_sub = rospy.Subscriber('dashboard_agg', DashboardState, self.dashboard_callback)
self._dashboard_message = None
self._last_dashboard_message_time = 0.0
def __del__(self):
self._dashboard_agg_sub.unregister()
def on_timer(self, evt):
# level = self._diagnostics_frame._diagnostics_panel.get_top_level_state()
level = self._diagnostics_frame.get_top_level_state()
if (level == -1 or level == 3):
if (self._diagnostics_button.set_stale()):
self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Stale"))
elif (level >= 2):
if (self._diagnostics_button.set_error()):
self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Error"))
elif (level == 1):
if (self._diagnostics_button.set_warn()):
self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Warning"))
else:
if (self._diagnostics_button.set_ok()):
self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: OK"))
self.update_rosout()
if (rospy.get_time() - self._last_dashboard_message_time > 5.0):
self._power_state_ctrl.set_stale()
self._arm_status.set_stale()
self._base_status.set_stale()
self._driver_status.set_stale()
ctrls = [self._power_state_ctrl, self._arm_status, self._base_status, self._driver_status]
for ctrl in ctrls:
ctrl.SetToolTip(wx.ToolTip("No message received on dashboard_agg in the last 5 seconds"))
if (rospy.is_shutdown()):
self.Close()
def on_diagnostics_clicked(self, evt):
self._diagnostics_frame.Show()
self._diagnostics_frame.Raise()
def on_rosout_clicked(self, evt):
self._rosout_frame.Show()
self._rosout_frame.Raise()
def on_base_status_clicked(self, evt):
menu = wx.Menu()
menu.Bind(wx.EVT_MENU, self.on_base_switch_on, menu.Append(wx.ID_ANY, "Enable Base Motors"))
menu.Bind(wx.EVT_MENU, self.on_base_switch_off, menu.Append(wx.ID_ANY, "Disable Base Motors"))
self._base_status.toggle(True)
self.PopupMenu(menu)
self._base_status.toggle(False)
def on_base_switch_on(self, evt):
# if any of the breakers is not enabled ask if they'd like to enable them
if (self._dashboard_message is not None and self._dashboard_message.power_board_state_valid):
if (self._dashboard_message.power_board_state.circuit_state[0] == PowerBoardState.STATE_STANDBY):
switch_on_base = rospy.ServiceProxy("/base/switchOnMotors", std_srvs.srv.Empty)
try:
switch_on_base()
except rospy.ServiceException, e:
wx.MessageBox("Failed to switch ON base motors: service call failed with error: %s"%(e), "Error", wx.OK|wx.ICON_ERROR)
elif (self._dashboard_message.power_board_state.circuit_state[0] == PowerBoardState.STATE_ENABLED):
wx.MessageBox("Base motors are already switched ON", "Error", wx.OK|wx.ICON_ERROR)
elif (self._dashboard_message.power_board_state.circuit_state[0] == PowerBoardState.STATE_DISABLED):
wx.MessageBox("Base is not connected", "Error", wx.OK|wx.ICON_ERROR)
class PR2Frame(wx.Frame):
_CONFIG_WINDOW_X = "/Window/X"
_CONFIG_WINDOW_Y = "/Window/Y"
def __init__(self,
parent,
id=wx.ID_ANY,
title='youbot_dashboard',
pos=wx.DefaultPosition,
size=(400, 50),
style=wx.CAPTION | wx.CLOSE_BOX | wx.STAY_ON_TOP):
wx.Frame.__init__(self, parent, id, title, pos, size, style)
wx.InitAllImageHandlers()
rospy.init_node('youbot_dashboard', anonymous=True)
try:
getattr(rxtools, "initRoscpp")
rxtools.initRoscpp("youbot_dashboard_cpp", anonymous=True)
except AttributeError:
pass
self.SetTitle('youbot_dashboard (%s)' % rosenv.get_master_uri())
icons_path = path.join(roslib.packages.get_pkg_dir('pr2_dashboard'),
"icons/")
youbot_icons_path = path.join(
roslib.packages.get_pkg_dir('youbot_dashboard'), "icons/")
sizer = wx.BoxSizer(wx.HORIZONTAL)
self.SetSizer(sizer)
static_sizer = wx.StaticBoxSizer(
wx.StaticBox(self, wx.ID_ANY, "Diagn."), wx.HORIZONTAL)
sizer.Add(static_sizer, 0)
# Diagnostics
self._diagnostics_button = StatusControl(self, wx.ID_ANY, icons_path,
"diag", True)
self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics"))
static_sizer.Add(self._diagnostics_button, 0)
# Rosout
self._rosout_button = StatusControl(self, wx.ID_ANY, icons_path,
"rosout", True)
self._rosout_button.SetToolTip(wx.ToolTip("Rosout"))
static_sizer.Add(self._rosout_button, 0)
static_sizer = wx.StaticBoxSizer(
wx.StaticBox(self, wx.ID_ANY, "Platform"), wx.HORIZONTAL)
sizer.Add(static_sizer, 0)
# base status
self._base_status = StatusControl(self, wx.ID_ANY, youbot_icons_path,
"base", True)
self._base_status.SetToolTip(wx.ToolTip("Base Motors: Stale"))
static_sizer.Add(self._base_status, 0)
self._base_status.Bind(wx.EVT_LEFT_DOWN, self.on_base_status_clicked)
# arm status
self._arm_status = StatusControl(self, wx.ID_ANY, youbot_icons_path,
"arm", True)
self._arm_status.SetToolTip(wx.ToolTip("Arm Motors: Stale"))
static_sizer.Add(self._arm_status, 0)
self._arm_status.Bind(wx.EVT_LEFT_DOWN, self.on_arm_status_clicked)
static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Drv"),
wx.HORIZONTAL)
sizer.Add(static_sizer, 0)
# driver status
self._driver_status = StatusControl(self, wx.ID_ANY, youbot_icons_path,
"driver", True)
self._driver_status.SetToolTip(wx.ToolTip("Driver: Stale"))
static_sizer.Add(self._driver_status, 0)
self._driver_status.Bind(wx.EVT_BUTTON, self.on_driver_status_clicked)
static_sizer = wx.StaticBoxSizer(
wx.StaticBox(self, wx.ID_ANY, "Battery"), wx.HORIZONTAL)
sizer.Add(static_sizer, 0)
# Battery State
self._power_state_ctrl = PowerStateControl(self, wx.ID_ANY, icons_path)
self._power_state_ctrl.SetToolTip(wx.ToolTip("Battery: Stale"))
static_sizer.Add(self._power_state_ctrl, 1, wx.EXPAND)
self._config = wx.Config("youbot_dashboard")
self.Bind(wx.EVT_CLOSE, self.on_close)
self.Layout()
self.Fit()
self._diagnostics_frame = DiagnosticsFrame(self, wx.ID_ANY,
"Diagnostics")
self._diagnostics_frame.Hide()
self._diagnostics_frame.Center()
self._diagnostics_button.Bind(wx.EVT_BUTTON,
self.on_diagnostics_clicked)
self._rosout_frame = RosoutFrame(self, wx.ID_ANY, "Rosout")
self._rosout_frame.Hide()
self._rosout_frame.Center()
self._rosout_button.Bind(wx.EVT_BUTTON, self.on_rosout_clicked)
self.load_config()
self._timer = wx.Timer(self)
self.Bind(wx.EVT_TIMER, self.on_timer)
self._timer.Start(500)
self._dashboard_agg_sub = rospy.Subscriber('dashboard_agg',
DashboardState,
self.dashboard_callback)
self._dashboard_message = None
self._last_dashboard_message_time = 0.0
def __del__(self):
self._dashboard_agg_sub.unregister()
def on_timer(self, evt):
# level = self._diagnostics_frame._diagnostics_panel.get_top_level_state()
level = self._diagnostics_frame.get_top_level_state()
if (level == -1 or level == 3):
if (self._diagnostics_button.set_stale()):
self._diagnostics_button.SetToolTip(
wx.ToolTip("Diagnostics: Stale"))
elif (level >= 2):
if (self._diagnostics_button.set_error()):
self._diagnostics_button.SetToolTip(
wx.ToolTip("Diagnostics: Error"))
elif (level == 1):
if (self._diagnostics_button.set_warn()):
self._diagnostics_button.SetToolTip(
wx.ToolTip("Diagnostics: Warning"))
else:
if (self._diagnostics_button.set_ok()):
self._diagnostics_button.SetToolTip(
wx.ToolTip("Diagnostics: OK"))
self.update_rosout()
if (rospy.get_time() - self._last_dashboard_message_time > 5.0):
self._power_state_ctrl.set_stale()
self._arm_status.set_stale()
self._base_status.set_stale()
self._driver_status.set_stale()
ctrls = [
self._power_state_ctrl, self._arm_status, self._base_status,
self._driver_status
]
for ctrl in ctrls:
ctrl.SetToolTip(
wx.ToolTip(
"No message received on dashboard_agg in the last 5 seconds"
))
if (rospy.is_shutdown()):
self.Close()
def on_diagnostics_clicked(self, evt):
self._diagnostics_frame.Show()
self._diagnostics_frame.Raise()
def on_rosout_clicked(self, evt):
self._rosout_frame.Show()
self._rosout_frame.Raise()
def on_base_status_clicked(self, evt):
menu = wx.Menu()
menu.Bind(wx.EVT_MENU, self.on_base_switch_on,
menu.Append(wx.ID_ANY, "Enable Base Motors"))
menu.Bind(wx.EVT_MENU, self.on_base_switch_off,
menu.Append(wx.ID_ANY, "Disable Base Motors"))
self._base_status.toggle(True)
self.PopupMenu(menu)
self._base_status.toggle(False)
def on_base_switch_on(self, evt):
# if any of the breakers is not enabled ask if they'd like to enable them
if (self._dashboard_message is not None
and self._dashboard_message.power_board_state_valid):
if (self._dashboard_message.power_board_state.circuit_state[0] ==
PowerBoardState.STATE_STANDBY):
switch_on_base = rospy.ServiceProxy("/base/switchOnMotors",
std_srvs.srv.Empty)
try:
switch_on_base()
except rospy.ServiceException, e:
wx.MessageBox(
"Failed to switch ON base motors: service call failed with error: %s"
% (e), "Error", wx.OK | wx.ICON_ERROR)
elif (self._dashboard_message.power_board_state.circuit_state[0] ==
PowerBoardState.STATE_ENABLED):
wx.MessageBox("Base motors are already switched ON", "Error",
wx.OK | wx.ICON_ERROR)
elif (self._dashboard_message.power_board_state.circuit_state[0] ==
PowerBoardState.STATE_DISABLED):
wx.MessageBox("Base is not connected", "Error",
wx.OK | wx.ICON_ERROR)
class NaoFrame(wx.Frame):
_CONFIG_WINDOW_X="/Window/X"
_CONFIG_WINDOW_Y="/Window/Y"
def __init__(self, parent, id=wx.ID_ANY, title='Nao Dashboard', pos=wx.DefaultPosition, size=(400, 50), style=wx.CAPTION|wx.CLOSE_BOX|wx.STAY_ON_TOP):
wx.Frame.__init__(self, parent, id, title, pos, size, style)
wx.InitAllImageHandlers()
rospy.init_node('nao_dashboard', anonymous=True)
try:
getattr(rxtools, "initRoscpp")
rxtools.initRoscpp("nao_dashboard_cpp", anonymous=True)
except AttributeError:
pass
self.SetTitle('Nao Dashboard (%s)'%rosenv.get_master_uri())
icons_path = path.join(roslib.packages.get_pkg_dir('nao_dashboard'), "icons/")
sizer = wx.BoxSizer(wx.HORIZONTAL)
self.SetSizer(sizer)
static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Robot"), wx.HORIZONTAL)
sizer.Add(static_sizer, 0)
self._robot_combobox = wx.ComboBox(self, wx.ID_ANY, "", choices = [])
static_sizer.Add(self._robot_combobox, 0)
gobject.threads_init()
dbus.glib.threads_init()
self.robots = []
self.sys_bus = dbus.SystemBus()
self.avahi_server = dbus.Interface(self.sys_bus.get_object(avahi.DBUS_NAME, '/'), avahi.DBUS_INTERFACE_SERVER)
self.sbrowser = dbus.Interface(self.sys_bus.get_object(avahi.DBUS_NAME, self.avahi_server.ServiceBrowserNew(avahi.IF_UNSPEC,
avahi.PROTO_INET, '_naoqi._tcp', 'local', dbus.UInt32(0))), avahi.DBUS_INTERFACE_SERVICE_BROWSER)
self.sbrowser.connect_to_signal("ItemNew", self.avahiNewItem)
self.sbrowser.connect_to_signal("ItemRemove", self.avahiItemRemove)
static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Diagnostic"), wx.HORIZONTAL)
sizer.Add(static_sizer, 0)
# Diagnostics
self._diagnostics_button = StatusControl(self, wx.ID_ANY, icons_path, "diag", True)
self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics"))
static_sizer.Add(self._diagnostics_button, 0)
# Rosout
self._rosout_button = StatusControl(self, wx.ID_ANY, icons_path, "rosout", True)
self._rosout_button.SetToolTip(wx.ToolTip("Rosout"))
static_sizer.Add(self._rosout_button, 0)
# Joint temperature
self._temp_joint_button = StatusControl(self, wx.ID_ANY, icons_path, "temperature_joints", True)
self._temp_joint_button.SetToolTip(wx.ToolTip("Joint temperatures"))
static_sizer.Add(self._temp_joint_button, 0)
# CPU temperature
self._temp_head_button = StatusControl(self, wx.ID_ANY, icons_path, "temperature_head", True)
self._temp_head_button.SetToolTip(wx.ToolTip("CPU temperature"))
static_sizer.Add(self._temp_head_button, 0)
static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Stiffness"), wx.HORIZONTAL)
sizer.Add(static_sizer, 0)
# Motors
self._motors_button = StatusControl(self, wx.ID_ANY, icons_path, "motor", True)
self._motors_button.SetToolTip(wx.ToolTip("Stiffness"))
self._motors_button._ok = (wx.Bitmap(path.join(icons_path, "stiffness-off-untoggled.png"), wx.BITMAP_TYPE_PNG),
wx.Bitmap(path.join(icons_path, "stiffness-off-toggled.png"), wx.BITMAP_TYPE_PNG))
self._motors_button._warn = (wx.Bitmap(path.join(icons_path, "stiffness-partially-untoggled.png"), wx.BITMAP_TYPE_PNG),
wx.Bitmap(path.join(icons_path, "stiffness-partially-toggled.png"), wx.BITMAP_TYPE_PNG))
self._motors_button._error = (wx.Bitmap(path.join(icons_path, "stiffness-on-untoggled.png"), wx.BITMAP_TYPE_PNG),
wx.Bitmap(path.join(icons_path, "stiffness-on-toggled.png"), wx.BITMAP_TYPE_PNG))
self._motors_button._stale = (wx.Bitmap(path.join(icons_path, "stiffness-stale-untoggled.png"), wx.BITMAP_TYPE_PNG),
wx.Bitmap(path.join(icons_path, "stiffness-stale-toggled.png"), wx.BITMAP_TYPE_PNG))
static_sizer.Add(self._motors_button, 0)
self._motors_button.Bind(wx.EVT_LEFT_DOWN, self.on_motors_clicked)
static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Battery"), wx.HORIZONTAL)
sizer.Add(static_sizer, 0)
# Battery State
self._power_state_ctrl = PowerStateControl(self, wx.ID_ANY, icons_path)
self._power_state_ctrl.SetToolTip(wx.ToolTip("Battery: Stale"))
static_sizer.Add(self._power_state_ctrl, 1, wx.EXPAND)
self._config = wx.Config("nao_dashboard")
self.Bind(wx.EVT_CLOSE, self.on_close)
self.Layout()
self.Fit()
self._diagnostics_frame = DiagnosticsFrame(self, wx.ID_ANY, "Diagnostics")
self._diagnostics_frame.Hide()
self._diagnostics_frame.Center()
self._diagnostics_button.Bind(wx.EVT_BUTTON, self.on_diagnostics_clicked)
self._rosout_frame = RosoutFrame(self, wx.ID_ANY, "Rosout")
self._rosout_frame.Hide()
self._rosout_frame.Center()
self._rosout_button.Bind(wx.EVT_BUTTON, self.on_rosout_clicked)
self.load_config()
self._timer = wx.Timer(self)
self.Bind(wx.EVT_TIMER, self.on_timer)
self._timer.Start(500)
self._agg_sub = rospy.Subscriber('diagnostics_agg', DiagnosticArray, self.diagnostic_callback)
self._last_diagnostics_message_time = 0.0
self.bodyPoseClient = actionlib.SimpleActionClient('body_pose', BodyPoseAction)
self.stiffnessEnableClient = rospy.ServiceProxy("body_stiffness/enable", std_srvs.srv.Empty)
self.stiffnessDisableClient = rospy.ServiceProxy("body_stiffness/disable", std_srvs.srv.Empty)
def __del__(self):
self._dashboard_agg_sub.unregister()
def on_timer(self, evt):
level = self._diagnostics_frame._diagnostics_panel.get_top_level_state()
if (level == -1 or level == 3):
if (self._diagnostics_button.set_stale()):
self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Stale"))
elif (level >= 2):
if (self._diagnostics_button.set_error()):
self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Error"))
elif (level == 1):
if (self._diagnostics_button.set_warn()):
self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Warning"))
else:
if (self._diagnostics_button.set_ok()):
self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: OK"))
self.update_rosout()
if (rospy.get_time() - self._last_diagnostics_message_time > 5.0):
ctrls = [self._motors_button, self._power_state_ctrl, self._temp_head_button, self._temp_joint_button]
for ctrl in ctrls:
ctrl.set_stale()
ctrl.SetToolTip(wx.ToolTip("No message received on diagnostics_agg in the last 5 seconds"))
if (rospy.is_shutdown()):
self.Close()
def on_diagnostics_clicked(self, evt):
self._diagnostics_frame.Show()
self._diagnostics_frame.Raise()
def on_rosout_clicked(self, evt):
self._rosout_frame.Show()
self._rosout_frame.Raise()
def on_motors_clicked(self, evt):
menu = wx.Menu()
menu.Append(ID_INIT_POSE, "Init pose")
menu.Append(ID_SIT_DOWN, "Sit down && remove stiffness")
menu.Append(ID_REMOVE_STIFFNESS, "Remove stiffness immediately")
wx.EVT_MENU(self, ID_INIT_POSE, self.on_init_pose)
wx.EVT_MENU(self, ID_SIT_DOWN, self.on_sit_down)
wx.EVT_MENU(self, ID_REMOVE_STIFFNESS, self.on_remove_stiffness)
#subscriberFound = ...
#menu.Enable(ID_INIT_POSE, subscriberFound);
#menu.Enable(ID_SIT_DOWN, subscriberFound);
#menu.Enable(ID_REMOVE_STIFFNESS, subscriberFound);
self._motors_button.toggle(True)
self.PopupMenu(menu)
self._motors_button.toggle(False)
def on_init_pose(self, evt):
self.stiffnessEnableClient.call()
self.bodyPoseClient.send_goal_and_wait(BodyPoseGoal(pose_name = 'init'))
def on_sit_down(self, evt):
self.bodyPoseClient.send_goal_and_wait(BodyPoseGoal(pose_name = 'crouch'))
state = self.bodyPoseClient.get_state()
if state == actionlib.GoalStatus.SUCCEEDED:
self.stiffnessDisableClient.call()
else:
wx.MessageBox('crouch pose did not succeed: %s - cannot remove stiffness' % self.bodyPoseClient.get_goal_status_text(), 'Error')
rospy.logerror("crouch pose did not succeed: %s", self.bodyPoseClient.get_goal_status_text())
def on_remove_stiffness(self, evt):
msg = wx.MessageDialog(self, 'Caution: Robot may fall. Continue to remove stiffness?', 'Warning', wx.YES_NO | wx.NO_DEFAULT | wx.CENTER | wx.ICON_EXCLAMATION)
if(msg.ShowModal() == wx.ID_YES):
self.stiffnessDisableClient.call()
def on_halt_motors(self, evt):
halt = rospy.ServiceProxy("pr2_etherCAT/halt_motors", std_srvs.srv.Empty)
try:
halt()
except rospy.ServiceException, e:
wx.MessageBox("Failed to halt the motors: service call failed with error: %s"%(e), "Error", wx.OK|wx.ICON_ERROR)
