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 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, False) self._do_pause = True self._do_finish = False 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() self._last_error = (time.time(), None)
def __init__(self, mcast_port, mcast_group, monitor_port): ''' Initialize method for the Discoverer class :param mcast_port: The port used to publish and receive the multicast messages. :type mcast_port: int :param mcast_group: The IPv4 or IPv6 multicast group used for discovering over nodes. :type mcast_group: str :param monitor_port: The port of the RPC Server, used to get more information about the ROS master. :type monitor_port: int ''' # threading.Thread.__init__(self) self.do_finish = False self.__lock = threading.RLock() # the list with all ROS master neighbors self.masters = dict() # (ip, DiscoveredMaster) # this parameter stores the state of the remote nodes. If the state is changed # the cache for contacts of remote nodes will be cleared. self._changed = False self.ROSMASTER_HZ = rospy.get_param('~rosmaster_hz', Discoverer.ROSMASTER_HZ) self.HEARTBEAT_HZ = rospy.get_param('~heartbeat_hz', Discoverer.HEARTBEAT_HZ) self.MEASUREMENT_INTERVALS = rospy.get_param( '~measurement_intervals', Discoverer.MEASUREMENT_INTERVALS) self.TIMEOUT_FACTOR = rospy.get_param('~timeout_factor', Discoverer.TIMEOUT_FACTOR) self.REMOVE_AFTER = rospy.get_param('~remove_after', Discoverer.REMOVE_AFTER) self.ACTIVE_REQUEST_AFTER = rospy.get_param( '~active_request_after', Discoverer.ACTIVE_REQUEST_AFTER) if self.ACTIVE_REQUEST_AFTER <= 0: rospy.logwarn("active_request_after [sec]: %s <= 0 set to 60" % self.ACTIVE_REQUEST_AFTER) self.ACTIVE_REQUEST_AFTER = 60 self.robots = rospy.get_param('~robot_hosts', []) self.CHANGE_NOTIFICATION_COUNT = rospy.get_param( '~change_notification_count', Discoverer.CHANGE_NOTIFICATION_COUNT) self._current_change_notification_count = 0 self._send_mcast = rospy.get_param('~send_mcast', True) # for cases with more then one master_discovery on the same host and # heartbeat rate is less then 0.1. In this case we have to send a multicast # request reply, because we are bind to the same port. Unicast replies are # not forward to the same port only once. self._addresses = dict() # {address : (int) ocurres} # some parameter checks and info outputs if not self._send_mcast and not self.robots: rospy.logwarn( "This master_discovery is invisible because it send no heart beat messages! Set ~send_mcast to true or add hosts to ~robot_hosts." ) if not self._send_mcast: self.HEARTBEAT_HZ = 1. / self.ACTIVE_REQUEST_AFTER rospy.logwarn( "Multicast is disabled. Use ~active_request_after(%.2f) ot set ~heartbeat_hz to new value: %.4f" % (self.ACTIVE_REQUEST_AFTER, self.HEARTBEAT_HZ)) rospy.loginfo("Check the ROS Master[Hz]: " + str(self.ROSMASTER_HZ)) if self.HEARTBEAT_HZ <= 0.: rospy.logwarn("Heart beat [Hz]: %s is increased to 0.02" % self.HEARTBEAT_HZ) self.HEARTBEAT_HZ = 0.02 if self.HEARTBEAT_HZ > 25.5: rospy.logwarn("Heart beat [Hz]: %s is decreased to 25.5" % self.HEARTBEAT_HZ) self.HEARTBEAT_HZ = 25.5 else: rospy.loginfo("Heart beat [Hz]: %s" % (self.HEARTBEAT_HZ)) rospy.loginfo("Active request after [sec]: %s" % self.ACTIVE_REQUEST_AFTER) rospy.loginfo("Remove after [sec]: %s" % self.REMOVE_AFTER) if self.REMOVE_AFTER <= self.ACTIVE_REQUEST_AFTER: rospy.logwarn( "'Active request after' should be less than 'remove after' to avoid removing masters from list!" ) rospy.loginfo("Robot hosts: " + str(self.robots)) if self.HEARTBEAT_HZ > 0.: count_packets = len(self.robots) + (1 if self._send_mcast else 0) netload = self.HEARTBEAT_HZ * self.NETPACKET_SIZE * count_packets rospy.loginfo("Approx. mininum avg. network load: %.2f bytes/s" % netload) self.current_check_hz = self.ROSMASTER_HZ self.pubstats = rospy.Publisher("~linkstats", LinkStatesStamped, queue_size=1) # test the reachability of the ROS master local_addr = roslib.network.get_local_address() if (local_addr in ['localhost', '127.0.0.1']): rospy.logwarn( "'%s' is not reachable for other systems. Change the ROS_MASTER_URI!" % local_addr) self.mcast_port = mcast_port self.mcast_group = mcast_group # initialize the ROS publishers self.pubchanges = rospy.Publisher("~changes", MasterState, queue_size=10) # initialize the ROS services rospy.Service('~list_masters', DiscoverMasters, self.rosservice_list_masters) rospy.Service('~refresh', std_srvs.srv.Empty, self.rosservice_refresh) # create a thread to monitor the ROS master state mgroup = DiscoverSocket.normalize_mgroup(mcast_group) is_ip6 = self._is_ipv6_group(mgroup) self.master_monitor = MasterMonitor(monitor_port, ipv6=is_ip6) # create timer to check for ros master changes self._timer_ros_changes = threading.Timer(0.1, self.checkROSMaster_loop) # init socket for discovering. Exit on errors! self._init_socket(True) # create a timer monitor the offline ROS master and calculate the link qualities self._timer_stats = threading.Timer(1, self.timed_stats_calculation) # create timer and paramter for heartbeat notifications self._init_notifications = 0 # disable parameter, if HEARTBEAT_HZ is active (> zero) if self.HEARTBEAT_HZ > DiscoveredMaster.MIN_HZ_FOR_QUALILTY: self._init_notifications = self.INIT_NOTIFICATION_COUNT self._current_change_notification_count = self.CHANGE_NOTIFICATION_COUNT self._timer_heartbeat = threading.Timer(1.0, self.send_heardbeat) # set the callback to finish all running threads rospy.on_shutdown(self.finish)