def updateMaster(self, master_info): ''' Sets the new information of the master and synchronize the ROS master if needed. :param master_info: new update information for the master :type master_info: MasterInfo ''' try: self.__lock.acquire() if (master_info.name in self.__masters): if (self.__masters[master_info.name].getRosTimestamp() != master_info.getRosTimestamp()): # print 'PUBLISH NEW STATE for', master_info.name self.__masters[master_info.name].txt = master_info.txt[:] self.pubchanges.publish( MasterState( MasterState.STATE_CHANGED, ROSMaster(str(master_info.name), master_info.getMasterUri(), master_info.getRosTimestamp(), master_info.getRosTimestamp(), True, master_info.getTXTValue('zname', ''), master_info.getTXTValue('rpcuri', '')))) self.__masters[master_info.name].lastUpdate = time.time() self.setMasterOnline(master_info.name, True) else: # print "new master:", master_info.name self.__masters[master_info.name] = master_info # create a new polling thread to detect changes self.__pollings[master_info.name] = Polling( self, master_info, self.__callback_update_local if (self.localMasterName == master_info.name) else self.__callback_update_remote, Discoverer.ROSMASTER_HZ) self.pubchanges.publish( MasterState( MasterState.STATE_NEW, ROSMaster(str(master_info.name), master_info.getMasterUri(), master_info.getRosTimestamp(), master_info.getRosTimestamp(), True, master_info.getTXTValue('zname', ''), master_info.getTXTValue('rpcuri', '')))) except: import traceback rospy.logwarn("Error while update master: %s", traceback.format_exc()) finally: self.__lock.release()
def updateMaster(self, master_info): ''' Sets the new information of the master and synchronize the ROS master if needed. :param master_info: new update information for the master :type master_info: MasterInfo ''' try: self.__lock.acquire() network_id = master_info.getTXTValue('network_id') if network_id is None: rospy.logwarn("old zeroconf client on %s detected. Please update multimaster_fkie package!" % master_info.host) if (self._network_id == network_id): if (master_info.name in self.__masters): if (self.__masters[master_info.name].getRosTimestamp() != master_info.getRosTimestamp()): self.__masters[master_info.name].txt = master_info.txt[:] self.pubchanges.publish(MasterState(MasterState.STATE_CHANGED, ROSMaster(str(master_info.name), master_info.getMasterUri(), master_info.getRosTimestamp(), master_info.getRosTimestampLocal(), True, master_info.getTXTValue('zname', ''), master_info.getTXTValue('rpcuri', '')))) self.__masters[master_info.name].lastUpdate = time.time() self.setMasterOnline(master_info.name, True) else: self.__masters[master_info.name] = master_info # create a new polling thread to detect changes self.__pollings[master_info.name] = Polling(self, master_info, self.__callback_update_local if (self.localMasterName == master_info.name) else self.__callback_update_remote, Discoverer.ROSMASTER_HZ) self.pubchanges.publish(MasterState(MasterState.STATE_NEW, ROSMaster(str(master_info.name), master_info.getMasterUri(), master_info.getRosTimestamp(), master_info.getRosTimestampLocal(), True, master_info.getTXTValue('zname', ''), master_info.getTXTValue('rpcuri', '')))) if not self._services_initialized: # initialize the ROS services self._services_initialized = True rospy.Service('~list_masters', DiscoverMasters, self.rosservice_list_masters) # rospy.Service('~refresh', std_srvs.srv.Empty, self.rosservice_refresh) except Exception: rospy.logwarn("Error while update master: %s", traceback.format_exc()) finally: self.__lock.release()
def removeMaster(self, name): ''' Removes the master from the list and ends the synchronization to the given remote master. :param name: the name of the ROS master to remove :type name: str ''' try: self.__lock.acquire() rospy.logdebug("remove master: %s", name) if (name in self.__pollings): r = self.__pollings.pop(name) r.stop() del r if (name in self.__masters): r = self.__masters.pop(name) self.pubchanges.publish(MasterState(MasterState.STATE_REMOVED, ROSMaster(str(r.name), r.getMasterUri(), r.getRosTimestamp(), r.getRosTimestampLocal(), False, r.getTXTValue('zname', ''), r.getTXTValue('rpcuri', '')))) # r.stop() del r except Exception: rospy.logwarn("Error while remove master: %s", traceback.format_exc()) finally: self.__lock.release()
def setMasterOnline(self, name, state): ''' Sets the online state of the ROS master with given name. This method should be used to detect changes of the ROS master states and publish it to the '~masters' topic. :param name: the name of the ROS master. :type name: str :param state: the new state of the ROS master (True is online). :type state: bool ''' try: self.__lock.acquire() m = self.__masters[name] # publish the master list, only on new state if (state != m.online): rospy.loginfo("%s is now %s", m.name, "online" if state else "offline") m.online = state self.pubchanges.publish(MasterState(MasterState.STATE_CHANGED, ROSMaster(str(m.name), m.getMasterUri(), m.getRosTimestamp(), m.getRosTimestampLocal(), state, m.getTXTValue('zname', ''), m.getTXTValue('rpcuri', '')))) except Exception: pass finally: self.__lock.release()
def _remove_offline_hosts(self): with self.__lock: current_time = time.time() to_remove = [] for (k, v) in self.masters.iteritems(): ts_since_last_hb = current_time - v.last_heartbeat_ts ts_since_last_request = current_time - ( v.requests[-1] if v.requests else v.last_heartbeat_ts) if self.REMOVE_AFTER > 0 and ts_since_last_hb > self.REMOVE_AFTER: to_remove.append(k) if not v.mastername is None: self.publish_masterstate( MasterState( MasterState.STATE_REMOVED, ROSMaster(str(v.mastername), v.masteruri, v.timestamp, v.timestamp_local, v.online, v.discoverername, v.monitoruri))) # request updates elif ts_since_last_request > self.ACTIVE_REQUEST_AFTER or ( v.count_requests > 0 and v.online): if v.count_requests >= self.OFFLINE_AFTER_REQUEST_COUNT: v.set_offline() self._send_request2addr(k[0][0], v) for r in to_remove: rospy.logdebug("Remove master discovery: http://%s:%s" % (r[0][0], r[1])) self._rem_address(r[0][0]) del self.masters[r]
def set_offline(self): ''' Sets this master to offline and publish the new state to the ROS network. ''' if not self.callback_master_state is None and self.online: rospy.loginfo('Set host to offline: %s' % self.mastername) self.callback_master_state( MasterState( MasterState.STATE_CHANGED, ROSMaster(str(self.mastername), self.masteruri, self.timestamp, self.timestamp_local, False, self.discoverername, self.monitoruri))) self.online = False
def add_heartbeat(self, timestamp, timestamp_local, rate): ''' Adds a new heartbeat measurement. If it is a new timestamp a ROS message about the change of this ROS master will be published into ROS network. :param timestamp: The new timestamp of the ROS master state :type timestamp: float :param timestamp_local: The timestamp of the state of the remoter ROS master, without the changes maked while a synchronization. :type timestamp_local: float (Default: ``0``) :param rate: The remote rate, which is used to send the heartbeat messages. If the rate is zero the heartbeat is ignored. :type rate: float :return: ``True`` on changes :rtype: bool ''' result = False cur_time = time.time() self.last_heartbeat_ts = cur_time self.count_requests = 0 # publish new master state, if the timestamp is changed if (self.timestamp != timestamp or not self.online or self.timestamp_local != timestamp_local): self.timestamp = timestamp self.timestamp_local = timestamp_local if self.masteruri is not None: # set the state to 'online' self.online = True if self.callback_master_state is not None: self.callback_master_state( MasterState( MasterState.STATE_CHANGED, ROSMaster(str(self.mastername), self.masteruri, self.timestamp, self.timestamp_local, self.online, self.discoverername, self.monitoruri))) result = True if rate >= DiscoveredMaster.MIN_HZ_FOR_QUALILTY: # reset the list, if the heartbeat is changed if self.heartbeat_rate != rate: self.heartbeat_rate = rate self.heartbeats = list() self.heartbeats.append(cur_time) return result
def finish(self, *arg): ''' Callback called on exit of the ros node and publish the empty list of ROSMasters. ''' # publish all master as removed with self.__lock: # tell other loops to finish self.do_finish = True # finish the RPC server and timer self.master_monitor.shutdown() for (_, master) in self.masters.iteritems(): if not master.mastername is None: self.publish_masterstate( MasterState( MasterState.STATE_REMOVED, ROSMaster(str(master.mastername), master.masteruri, master.timestamp, master.timestamp_local, master.online, master.discoverername, master.monitoruri))) try: self._timer_ros_changes.cancel() except: pass try: self._timer_heartbeat.cancel() except: pass try: self._timer_stats.cancel() except: pass # send notification that the master is going off msg = struct.pack(Discoverer.HEARTBEAT_FMT, 'R', Discoverer.VERSION, int(self.HEARTBEAT_HZ * 10), -1, -1, self.master_monitor.rpcport, -1, -1) self.socket.send2group(msg) # send as unicast for remote_master in self.robots: self.socket.send2addr(msg, remote_master) time.sleep(0.2) self.socket.close()
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() and not self._do_finish): 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.getCurrentState() # publish the new state state = MasterState( MasterState.STATE_CHANGED, ROSMaster( str(self._local_addr), str(self._masteruri), mon_state.timestamp, mon_state.timestamp_local, True, rospy.get_name(), ''.join([ 'http://localhost:', 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.20: current_check_hz = float(current_check_hz) / 2.0 elif current_check_hz * cputime < 0.10 and current_check_hz < OwnMasterMonitoring.ROSMASTER_HZ: current_check_hz = float(current_check_hz) * 2.0 except MasterConnectionException, e: rospy.logwarn( "MasterConnectionError while master check loop: %s" % e) self.err_signal.emit("Error while master check loop: %s" % e) except RuntimeError, e: # will thrown on exit of the app while try to emit the signal rospy.logwarn("RuntimeError while master check loop: %s" % e) self.err_signal.emit("Error while master check loop: %s" % e)
def removeAll(self): ''' Removes all masters and ends the synchronization to these. ''' try: self.__lock.acquire() while self.__pollings: _name, p = self.__pollings.popitem() p.stop() while self.__masters: _name, master = self.__masters.popitem() self.pubchanges.publish(MasterState(MasterState.STATE_REMOVED, ROSMaster(str(master.name), master.getMasterUri(), master.getRosTimestamp(), master.getRosTimestampLocal(), False, master.getTXTValue('zname', ''), master.getTXTValue('rpcuri', '')))) except Exception: rospy.logwarn("Error while removeAll: %s", traceback.format_exc()) finally: self.__lock.release()
def recv_udp_msg(self, msg, address): ''' This method handles the received udp messages. ''' if not rospy.is_shutdown() and not self.do_finish: with self.__lock: try: (version, msg_tuple) = self.msg2masterState(msg, address) if (version in [2, 3]): add_to_list = False (firstc, version, rate, secs, nsecs, monitor_port, secs_l, nsecs_l) = msg_tuple if firstc != 'R': # ignore the message. it does not start with 'R' return master_key = (address, monitor_port) if version >= 3 and secs == 0 and nsecs == 0: # is it a request to update the state # send the current master state to the sender address # TODO: add a filter, if multicast messages are disabled? self._send_current_state2addr(address[0]) add_to_list = not master_key in self.masters elif secs == -1 or secs_l == -1: # remove master if sec and nsec are -1 if self.masters.has_key(master_key): master = self.masters[master_key] if not master.mastername is None: # the contact info of the master is valied, publish the change state_remove = MasterState( MasterState.STATE_REMOVED, ROSMaster(str(master.mastername), master.masteruri, master.timestamp, master.timestamp_local, False, master.discoverername, master.monitoruri)) self.publish_masterstate(state_remove) rospy.loginfo( "Remove master discovery: http://%s:%s, with ROS_MASTER_URI=%s" % (address[0], monitor_port, master.masteruri)) self._rem_address(address[0]) del self.masters[master_key] elif self.masters.has_key(master_key): # update the timestamp of existing master changed = self.masters[master_key].add_heartbeat( float(secs) + float(nsecs) / 1000000000.0, float(secs_l) + float(nsecs_l) / 1000000000.0, float(rate) / 10.0, ) if not self._changed: self._changed = changed else: # or create a new master add_to_list = True if add_to_list: rospy.loginfo( "Detected master discovery: http://%s:%s" % (address[0], monitor_port)) self._add_address(address[0]) is_local = address[0].startswith( '127.') or address[ 0] in roslib.network.get_local_addresses() self.masters[master_key] = DiscoveredMaster( monitoruri=''.join([ 'http://', address[0], ':', str(monitor_port) ]), is_local=is_local, heartbeat_rate=float(rate) / 10.0, timestamp=float(secs) + float(nsecs) / 1000000000.0, timestamp_local=float(secs_l) + float(nsecs_l) / 1000000000.0, callback_master_state=self.publish_masterstate) except Exception, e: rospy.logwarn("Error while decode message: %s", str(e))
def __retrieve_masterinfo(self): ''' Connects to the remote RPC server of the discoverer node and gets the information about the Master URI, name of the service, and other. The ``getMasterInfo()`` method will be used. On problems the connection will be reestablished until the information will be get successful. ''' if self.monitoruri is not None: while self._retrieveThread.is_alive( ) and not rospy.is_shutdown() and (self.mastername is None): try: remote_monitor = xmlrpclib.ServerProxy(self.monitoruri) timestamp, masteruri, mastername, nodename, monitoruri = remote_monitor.masterContacts( ) self._del_error(self.ERR_SOCKET) except: import traceback msg = "socket error [%s]: %s" % (self.monitoruri, traceback.format_exc()) rospy.logwarn(msg) self._add_error(self.ERR_SOCKET, msg) time.sleep(1) else: if float(timestamp) != 0: self.masteruri = masteruri self.mastername = mastername self.discoverername = nodename # self.monitoruri = monitoruri self.timestamp = float(timestamp) self.online = True # resolve the masteruri. Print an error if not reachable try: uri = urlparse(self.masteruri) self.masteruriaddr = socket.gethostbyname( uri.hostname) self._del_error(self.ERR_RESOLVE_NAME) except socket.gaierror: import traceback print traceback.format_exc() msg = "Master discovered with not known hostname ROS_MASTER_URI:='%s'. Fix your network settings and restart master_dicovery!" % str( self.masteruri) rospy.logwarn(msg) self._add_error(self.ERR_RESOLVE_NAME, msg) time.sleep(10) else: # publish new node if self.callback_master_state is not None: rospy.loginfo( "Added master with ROS_MASTER_URI=%s" % (self.masteruri)) self.callback_master_state( MasterState( MasterState.STATE_NEW, ROSMaster(str(self.mastername), self.masteruri, self.timestamp, self.timestamp, self.online, self.discoverername, self.monitoruri))) else: time.sleep(1)