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='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 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)