Exemplo n.º 1
0
class OwnMasterMonitoring(QtCore.QObject):
  '''
  A class to monitor the state of the master. Will be used, if no master 
  discovering is available. On changes the 'state_signal' of type 
  L{master_discovery_fkie.msg.MasterState} will be emitted.
  '''
  state_signal = QtCore.Signal(master_discovery_fkie.msg.MasterState)
  '''@ivar: a signal to inform the receiver about new master state. 
  Parameter: L{master_discovery_fkie.msg.MasterState}'''
  
  ROSMASTER_HZ = 1
  '''@ivar: the rate to test ROS master for changes.'''
  
  def init(self, monitor_port):
    '''
    Creates the local monitoring. Call start() to run the local monitoring.
    @param monitor_port: the port of the XML-RPC Server created by monitoring class.
    @type monitor_port: C{int}
    '''
    self._master_monitor = MasterMonitor(monitor_port)
    self._do_pause = True
#    self._local_addr = roslib.network.get_local_address()
#    self._masteruri = roslib.rosenv.get_master_uri()
    self._masteruri = self._master_monitor.getMasteruri()
    self._local_addr = self._master_monitor.getMastername()
    self._masterMonitorThread = threading.Thread(target = self.mastermonitor_loop)
    self._masterMonitorThread.setDaemon(True)
    self._masterMonitorThread.start()

  def mastermonitor_loop(self):
    '''
    The method test periodically the state of the ROS master. The new state will
    be published as 'state_signal'.
    '''
    import os
    current_check_hz = OwnMasterMonitoring.ROSMASTER_HZ
    while (not rospy.is_shutdown()):
      try:
        if not self._do_pause:
          cputimes = os.times()
          cputime_init = cputimes[0] + cputimes[1]
          if self._master_monitor.checkState():
            mon_state = self._master_monitor.getState()
            # publish the new state
            state = MasterState(MasterState.STATE_CHANGED, 
                                ROSMaster(str(self._local_addr), 
                                          str(self._masteruri), 
                                          mon_state.timestamp, 
                                          True, 
                                          rospy.get_name(), 
                                          ''.join(['http://', str(self._local_addr),':',str(self._master_monitor.rpcport)])))
            self.state_signal.emit(state)
          # adapt the check rate to the CPU usage time
          cputimes = os.times()
          cputime = cputimes[0] + cputimes[1] - cputime_init
          if current_check_hz*cputime > 0.4:
            current_check_hz = float(current_check_hz)/2.0
          elif current_check_hz*cputime < 0.20 and current_check_hz < OwnMasterMonitoring.ROSMASTER_HZ:
            current_check_hz = float(current_check_hz)*2.0
      except MasterConnectionException, e:
        rospy.logwarn("Error while master check loop: %s", str(e))
      except RuntimeError, e:
        # will thrown on exit of the app while try to emit the signal
        rospy.logwarn("Error while master check loop: %s", str(e))
      time.sleep(1.0/current_check_hz)