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 MainFrame(wx.Frame): _CONFIG_WINDOW_X = "/Window/X" _CONFIG_WINDOW_Y = "/Window/Y" def __init__(self, parent, id=wx.ID_ANY, title='Kuka LWR 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('lwr_dashboard', anonymous=True) try: getattr(rxtools, "initRoscpp") rxtools.initRoscpp("lwr_dashboard_cpp", anonymous=True) except AttributeError: print "rxtools error" pass self.SetTitle('Kuka LWR Dashboard (%s)' % rosenv.get_master_uri()) icons_path = path.join(roslib.packages.get_pkg_dir('lwr_dashboard'), "icons/") self._robots = rospy.get_param("robots", [""]) if (len(self._robots) == 0): self._has_many = False else: self._has_many = True sizer = wx.BoxSizer(wx.HORIZONTAL) self.SetSizer(sizer) static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, " ROS state "), wx.HORIZONTAL) sizer.Add(static_sizer, 0) # Diagnostics self._diagnostics_button = StatusControl(self, wx.ID_ANY, icons_path, "btn_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, "btn_rosout", True) self._rosout_button.SetToolTip(wx.ToolTip("Rosout")) static_sizer.Add(self._rosout_button, 0) self.FRI_COMMAND = 1 self.FRI_MONITOR = 2 self._fri_state_button = {} self._fri_control = {} self._fri_mode_topic = {} self._robot_control = {} i = 0 for robot in self._robots: print "Preparing robot: '%s'"%(robot) robot_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, " %s " % robot), wx.HORIZONTAL) # FRI State static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, " FRI "), wx.HORIZONTAL) robot_sizer.Add(static_sizer, 0) self._fri_state_button[robot] = StatusControl(self, wx.ID_ANY, icons_path, "btn_state", True) self._fri_state_button[robot].SetToolTip(wx.ToolTip("FRI state")) self._fri_state_button[robot].Bind(wx.EVT_BUTTON, lambda x, r=robot: self.on_fri_state_clicked(x, r)) static_sizer.Add(self._fri_state_button[robot], 0) self._fri_control[robot] = FRIControl(self, wx.ID_ANY, icons_path) self._fri_control[robot].SetToolTip(wx.ToolTip("FRI: Stale")) static_sizer.Add(self._fri_control[robot], 1, wx.EXPAND) self._fri_mode_topic[robot] = rospy.Publisher("%sarm_controller/fri_set_mode" % robot, std_msgs.msg.Int32) # Robot State static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, " Robot "), wx.HORIZONTAL) robot_sizer.Add(static_sizer, 0) self._robot_control[robot] = RobotControl(self, wx.ID_ANY, icons_path) self._robot_control[robot].SetToolTip(wx.ToolTip("Robot: Stale")) static_sizer.Add(self._robot_control[robot], 1, wx.EXPAND) sizer.Add(robot_sizer, 0) i = i + 1 self._config = wx.Config("elektron_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._topic = "diagnostic" self._subs = [] sub = rospy.Subscriber(self._topic, diagnostic_msgs.msg.DiagnosticArray, self.dashboard_callback) self._subs.append(sub) print "Subs.size %d" % len(self._subs) self._dashboard_message = None self._last_dashboard_message_time = 0.0 def __del__(self): for sub in self._subs: 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): for ctrl in self._fri_control.values(): ctrl.set_stale() for ctrl in self._robot_control.values(): ctrl.set_stale() for ctrl in self._fri_state_button.values(): ctrl.set_stale() 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 dashboard_callback(self, msg): wx.CallAfter(self.new_dashboard_message, msg) def new_dashboard_message(self, msg): self._dashboard_message = msg self._last_dashboard_message_time = rospy.get_time() fri_status = {} robot_status = {} for status in msg.status: parts = status.name.split(" "); robot = parts[0] dtype = parts[1] if dtype == "FRI": for value in status.values: fri_status[value.key] = value.value if dtype == "robot": for value in status.values: robot_status[value.key] = value.value if (fri_status): self._fri_control[robot].set_state(fri_status) self.update_fri(fri_status, robot) else: self._fri_control[robot].set_stale() if (robot_status): self._robot_control[robot].set_state(robot_status) else: self._robot_control[robot].set_stale() ################################################################### # FRI related stuff ################################################################### def update_fri(self, msg, robot): if (msg["State"] == "command"): self._fri_state_button[robot].set_ok() else: self._fri_state_button[robot].set_warn() def on_fri_state_clicked(self, ev, robot): menu = wx.Menu() menu.Bind(wx.EVT_MENU, lambda x, r=robot: self.on_monitor(x, r), menu.Append(wx.ID_ANY, "Monitor")) menu.Bind(wx.EVT_MENU, lambda x, r=robot: self.on_command(x, r), menu.Append(wx.ID_ANY, "Command")) self.PopupMenu(menu) def on_monitor(self, evt, robot): self.set_mode(self.FRI_MONITOR, robot) def on_command(self, evt, robot): self.set_mode(self.FRI_COMMAND, robot) def set_mode(self, mode, robot): self._fri_mode_topic[robot].publish(std_msgs.msg.Int32(mode)) 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() print "Closing..." self.Destroy()
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)
class ElektronFrame(wx.Frame): _CONFIG_WINDOW_X = "/Window/X" _CONFIG_WINDOW_Y = "/Window/Y" def __init__(self, parent, id=wx.ID_ANY, title='Elektron 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('elektron_dashboard', anonymous=True) try: getattr(rxtools, "initRoscpp") rxtools.initRoscpp("elektron_dashboard_cpp", anonymous=True) except AttributeError: pass self.SetTitle('Elektron Dashboard (%s)' % rosenv.get_master_uri()) icons_path = path.join(roslib.packages.get_pkg_dir('elektron_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, "CPU0"), wx.HORIZONTAL) sizer.Add(static_sizer, 0) self._cpu0_state = CpuFrame(self, wx.ID_ANY) static_sizer.Add(self._cpu0_state, 1, wx.EXPAND) static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "CPU1"), wx.HORIZONTAL) sizer.Add(static_sizer, 0) self._cpu1_state = CpuFrame(self, wx.ID_ANY) static_sizer.Add(self._cpu1_state, 1, wx.EXPAND) static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Memory"), wx.HORIZONTAL) sizer.Add(static_sizer, 0) self._mem_state = MemFrame(self, wx.ID_ANY) static_sizer.Add(self._mem_state, 1, wx.EXPAND) # WiFi State self.wlan_interface = rospy.get_param('~wlan_interface', 'eth1') self.wlan_power = rospy.get_param('~wlan_power', 100) static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "WiFi: %s" % self.wlan_interface), wx.HORIZONTAL) sizer.Add(static_sizer, 0) self._wifi_state = WifiControl(self, wx.ID_ANY, icons_path, self.wlan_interface, self.wlan_power) self._wifi_state.SetToolTip(wx.ToolTip("WiFi: Stale")) static_sizer.Add(self._wifi_state, 1, wx.EXPAND) # Laptop Battery State static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Laptop"), wx.HORIZONTAL) sizer.Add(static_sizer, 0) self._power_state_ctrl_laptop = PowerStateControl(self, wx.ID_ANY, icons_path) self._power_state_ctrl_laptop.SetToolTip(wx.ToolTip("Laptop Battery: Stale")) static_sizer.Add(self._power_state_ctrl_laptop, 1, wx.EXPAND) # Laptop Battery State static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Main power"), wx.HORIZONTAL) sizer.Add(static_sizer, 0) self._main_power_ctrl = MainPowerControl(self, wx.ID_ANY, icons_path) static_sizer.Add(self._main_power_ctrl, 1, wx.EXPAND) self._config = wx.Config("elektron_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("/turtlebot_node/set_operation_mode",turtlebot_node.srv.SetTurtlebotMode ) #~ try: #~ passive(turtlebot_node.msg.TurtlebotSensorState.OI_MODE_PASSIVE) #~ except rospy.ServiceException, e: #~ wx.MessageBox("Failed to put the turtlebot in passive mode: service call failed with error: %s"%(e), "Error", wx.OK|wx.ICON_ERROR) #~ #~ def on_safe_mode(self, evt): #~ safe = rospy.ServiceProxy("/turtlebot_node/set_operation_mode",turtlebot_node.srv.SetTurtlebotMode) #~ try: #~ safe(turtlebot_node.msg.TurtlebotSensorState.OI_MODE_SAFE) #~ except rospy.ServiceException, e: #~ wx.MessageBox("Failed to put the turtlebot in safe mode: service call failed with error: %s"%(e), "Error", wx.OK|wx.ICON_ERROR) #~ #~ def on_full_mode(self, evt): #~ full = rospy.ServiceProxy("/turtlebot_node/set_operation_mode", turtlebot_node.srv.SetTurtlebotMode) #~ try: #~ full(turtlebot_node.msg.TurtlebotSensorState.OI_MODE_FULL) #~ except rospy.ServiceException, e: #~ wx.MessageBox("Failed to put the turtlebot in full mode: service call failed with error: %s"%(e), "Error", wx.OK|wx.ICON_ERROR) #~ #~ #~ def dashboard_callback(self, msg): wx.CallAfter(self.new_dashboard_message, msg) def new_dashboard_message(self, msg): self._dashboard_message = msg self._last_dashboard_message_time = rospy.get_time() battery_status = {} laptop_battery_status = {} breaker_status = {} wifi_status = {} cpu0_status = {} cpu1_status = {} mem_status = {} main_power_status = {} op_mode = None for status in msg.status: if status.name == "/Power System/Battery": for value in status.values: battery_status[value.key] = value.value if status.name == "/Power System/Laptop Battery": for value in status.values: laptop_battery_status[value.key] = value.value if status.message == "Stale": laptop_battery_status = {} if status.name == "/Mode/Operating Mode": op_mode = status.message if status.name == "/Digital IO/Digital Outputs": for value in status.values: breaker_status[value.key] = value.value if status.name == "/Network/%s" % self.wlan_interface: for value in status.values: wifi_status[value.key] = value.value if status.name == "/System/cpu0": for value in status.values: cpu0_status[value.key] = value.value if status.name == "/System/cpu1": for value in status.values: cpu1_status[value.key] = value.value if status.name == "/System/Memory": for value in status.values: mem_status[value.key] = value.value if status.name == "/Robot/Main Battery": for value in status.values: main_power_status[value.key] = value.value #~ if (battery_status): #~ self._power_state_ctrl.set_power_state(battery_status) #~ else: #~ self._power_state_ctrl.set_stale() if (laptop_battery_status): self._power_state_ctrl_laptop.set_power_state(laptop_battery_status) else: self._power_state_ctrl_laptop.set_stale() if (cpu0_status): self._cpu0_state.set_state(cpu0_status) if (cpu1_status): self._cpu1_state.set_state(cpu1_status) if (mem_status): self._mem_state.set_state(mem_status) if (main_power_status): self._main_power_ctrl.set_power_state(main_power_status) if (wifi_status): self._wifi_state.set_wifi_state(wifi_status) else: self._wifi_state.set_stale() #~ if (op_mode): #~ if (op_mode=='Full'): #~ if (self._motors_button.set_ok()): #~ self._motors_button.SetToolTip(wx.ToolTip("Mode: Full")) #~ elif(op_mode=='Safe'): #~ if (self._motors_button.set_warn()): #~ self._motors_button.SetToolTip(wx.ToolTip("Mode: Safe")) #~ elif(op_mode=='Passive'): #~ if (self._motors_button.set_error()): #~ self._motors_button.SetToolTip(wx.ToolTip("Mode: Passive")) #~ else: #~ if (self._motors_button.set_stale()): #~ self._motors_button.SetToolTip(wx.ToolTip("Mode: Stale")) #~ if (breaker_status): #~ [ctrl.set_breaker_state(breaker_status) for ctrl in self._breaker_ctrls] 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()