Esempio n. 1
0
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)
Esempio n. 2
0
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 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()
Esempio n. 4
0
class Dashboard(wx.Frame):
    _CONFIG_WINDOW_X="/Window/X"
    _CONFIG_WINDOW_Y="/Window/Y"
    
    def __init__(self, parent, id=wx.ID_ANY, title='SLAW Dashboard', pos=wx.DefaultPosition, size=(400, 50), style=wx.CAPTION|wx.CLOSE_BOX):
        wx.Frame.__init__(self, parent, id, title, pos, size, style)
        
        wx.InitAllImageHandlers()
        
        self.SetBackgroundColour(wx.Colour(242,241,240,255))
        
        self.SetTitle('SLAW dashboard (%s)'%rosenv.get_master_uri())

        rospy.init_node('slaw_dashboard', anonymous=True)
        try:
            getattr(rxtools, "initRoscpp")
            rxtools.initRoscpp("slaw_dashboard_cpp", anonymous=True)
        except AttributeError:
            pass
               
        slaw_icons_path = path.join(roslib.packages.get_pkg_dir('slaw_dashboard'), "icons/")
        
        sizer = wx.BoxSizer(wx.HORIZONTAL)
        self.SetSizer(sizer)


        # Diagnostics Panel
        static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Diagnostics"), wx.HORIZONTAL)
        sizer.Add(static_sizer, 0)
        
        # Diagnostics
        self._diagnostics_button = StatusControl(self, wx.ID_ANY, slaw_icons_path, "diagnostics", False)
        self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics"))
        static_sizer.Add(self._diagnostics_button, 0)
        
        # Rosout
        self._rosout_button = StatusControl(self, wx.ID_ANY, slaw_icons_path, "rosout", False)
        self._rosout_button.SetToolTip(wx.ToolTip("Rosout"))
        static_sizer.Add(self._rosout_button, 0)
        
        # SmachViewer
        self._smach_button = StatusControl(self, wx.ID_ANY, slaw_icons_path, "smach", False)
        self._smach_button.SetToolTip(wx.ToolTip("Smach Viewer"))
        static_sizer.Add(self._smach_button, 0)
        self._smach_button.Bind(wx.EVT_LEFT_DOWN, self.on_smach_button_clicked)
        self._smach_button.set_ok()
        
        # Platform
        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, slaw_icons_path, "wheel", False)
        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, slaw_icons_path, "arm", False)
        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)
        
        
        # Driver
        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, slaw_icons_path, "motor", False)
        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)

                                       
        # System info
        self._robot_sysinfo = SysInfoControl(self, wx.ID_ANY, "System", slaw_icons_path)
        sizer.Add(self._robot_sysinfo)
      

        # Apps                        
        static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Apps"), wx.HORIZONTAL)
        sizer.Add(static_sizer, 0)

        # Clear costmap
        self._costmap_ctrl = StatusControl(self, wx.ID_ANY, slaw_icons_path, "costmap", False)
        self._costmap_ctrl.SetToolTip(wx.ToolTip("Clear costmap"))
        static_sizer.Add(self._costmap_ctrl, 0)
        self._costmap_ctrl.Bind(wx.EVT_BUTTON, self.on_costmap_clicked)

        # Cancel goals
        self._goal_ctrl = StatusControl(self, wx.ID_ANY, slaw_icons_path, "cancel-goal", False)
        self._goal_ctrl.SetToolTip(wx.ToolTip("Cancel goal"))
        static_sizer.Add(self._goal_ctrl, 0)
        self._goal_ctrl.Bind(wx.EVT_BUTTON, self.on_goal_clicked)
        
        # Tuck arm
        self._arm_ctrl = StatusControl(self, wx.ID_ANY, slaw_icons_path, "arm-tuck", False)
        self._arm_ctrl.SetToolTip(wx.ToolTip("Tuck arm"))
        static_sizer.Add(self._arm_ctrl, 0)
        self._arm_ctrl.Bind(wx.EVT_BUTTON, self.on_arm_clicked)
        
        # Store location
        self._location_ctrl = StatusControl(self, wx.ID_ANY, slaw_icons_path, "nav", False)
        self._location_ctrl.SetToolTip(wx.ToolTip("Store location"))
        static_sizer.Add(self._location_ctrl, 0)
        self._location_ctrl.Bind(wx.EVT_BUTTON, self.on_location_clicked)
        
        # Wrap up stuff
        self._config = wx.Config("slaw_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)

        # Message subscribers
        self._last_sysinfo_message = 0.0
        self._last_power_state_message = 0.0
        self._last_platform_state_message = 0.0
        
        self._platform_state_message = None
 
        self._sub_sysinfo = rospy.Subscriber('dashboard/sysinfo', SysInfo, self.cb_sysinfo)
        self._sub_platform_state = rospy.Subscriber('dashboard/platform_state', PowerBoardState, self.cb_platform_state)
        
        try:
            rospy.wait_for_service('/move_base/clear_costmaps', 1)
            self._srv_costmap = rospy.ServiceProxy('/move_base/clear_costmaps', Empty)
            self._costmap_ctrl.set_ok()
            self._costmap_ok = True
        except rospy.ROSException, e:
            rospy.logwarn("move base not running yet, can't clear costmap")
            self._costmap_ctrl.set_stale()
            self._costmap_ok = False            
        
        self._goal_client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
        if self._goal_client.wait_for_server(rospy.Duration(1)):
            self._goal_ctrl.set_ok()
            self._goal_ok = True
        else:
            rospy.logwarn("move base not running yet, can't cancel goal")
            self._goal_ctrl.set_stale()
            self._goal_ok = False    
             
        try:
            rospy.wait_for_service('/tuck_arm', 1)
            self._srv_arm = rospy.ServiceProxy('/tuck_arm', Empty)
            self._arm_ctrl.set_ok()
            self._arm_ok = True
        except rospy.ROSException, e:
            rospy.logwarn("/tuck_arm service unavailable")
            self._arm_ctrl.set_stale()
            self._arm_ok = False