def __init__(self, name, uri, discoverer_name, monitoruri, timestamp, sync_on_demand=False): ''' Initialization method for the SyncThread. @param name: the name of the ROS master synchronized with. @type name: C{str} @param uri: the URI of the ROS master synchronized with @type uri: C{str} @param discoverer_name: the name of the discovery node running on ROS master synchronized with. @type discoverer_name: C{str} @param monitoruri: The URI of RPC server of the discovery node to get the ROS master state by calling a method only once. @type monitoruri: C{str} @param timestamp: The timestamp of the current state of the ROS master info. @type timestamp: C{float64} @param sync_on_demand: Synchronize topics on demand @type sync_on_demand: bool ''' self.name = name self.uri = uri self.discoverer_name = discoverer_name self.monitoruri = monitoruri self.timestamp = timestamp self.timestamp_local = 0. self.timestamp_remote = 0. self.localMasteruri = masteruri_from_ros() rospy.logdebug("SyncThread[%s]: create this sync thread", self.name) # synchronization variables self.__lock_info = threading.RLock() self.__lock_intern = threading.RLock() self._use_filtered_method = None # SyncMasterInfo with currently synchronized nodes, publisher (topic, node, nodeuri), subscriber(topic, node, nodeuri) and services self.__sync_info = None self.__unregistered = False # a list with published topics as a tuple of (topic name, node name, node URL) self.__publisher = [] # a list with subscribed topics as a tuple of (topic name, node name, node URL) self.__subscriber = [] # a list with services as a tuple of (service name, service URL, node name, node URL) self.__services = [] # the state of the own ROS master is used if `sync_on_demand` is enabled or # to determine the type of topic subscribed remote with `Empty` type self.__own_state = None #setup the filter self._filter = FilterInterface() self._filter.load(self.name, ['/rosout', rospy.get_name().replace('/', '/*')+'*', self.discoverer_name.replace('/', '/*')+'*', '/*node_manager', '/*zeroconf'], [], ['/rosout', '/rosout_agg'], ['/'] if sync_on_demand else [], ['/*get_loggers', '/*set_logger_level'], [], # do not sync the bond message of the nodelets!! ['bond/Status'], [], []) # congestion avoidance: wait for random.random*2 sec. If an update request # is received try to cancel and restart the current timer. The timer can be # canceled for maximal MAX_UPDATE_DELAY times. self._update_timer = None self._delayed_update = 0 self.__on_update = False
def __init__(self, name, uri, discoverer_name, monitoruri, timestamp, sync_on_demand=False): ''' Initialization method for the SyncThread. @param name: the name of the ROS master synchronized with. @type name: C{str} @param uri: the URI of the ROS master synchronized with @type uri: C{str} @param discoverer_name: the name of the discovery node running on ROS master synchronized with. @type discoverer_name: C{str} @param monitoruri: The URI of RPC server of the discovery node to get the ROS master state by calling a method only once. @type monitoruri: C{str} @param timestamp: The timestamp of the current state of the ROS master info. @type timestamp: C{float64} @param sync_on_demand: Synchronize topics on demand @type sync_on_demand: bool ''' # init thread threading.Thread.__init__(self) self.masterInfo = MasterInfo(name, uri, discoverer_name, monitoruri, timestamp) self.localMasteruri = masteruri_from_ros() rospy.logdebug("SyncThread[%s]: create this sync thread", self.masterInfo.name) # synchronization variables self.__cv = threading.Condition() self.__lock_info = threading.RLock() self.__sync_info = None #SyncMasterInfo with currently synchronized nodes, publisher (topic, node, nodeuri), subscriber(topic, node, nodeuri) and services self.__stop = False # a list with published topics as a tuple of (topic name, node name, node URL) self.__publisher = [] # a list with subscribed topics as a tuple of (topic name, node name, node URL) self.__subscriber = [] # a list with services as a tuple of (service name, service URL, node name, node URL) self.__services = [] self.__own_state = None #setup the filter self._filter = FilterInterface() self._filter.load(self.masterInfo.name, ['/rosout', rospy.get_name().replace('/', '/*')+'*', self.masterInfo.discoverer_name.replace('/', '/*')+'*', '/*node_manager', '/*zeroconf'], [], ['/rosout', '/rosout_agg'], ['/'] if sync_on_demand else [], ['/*get_loggers', '/*set_logger_level'], [], # do not sync the bond message of the nodelets!! ['bond/Status']) # congestion avoidance: wait for minimum 1 sec. If an update request is received wait # for next 1 second. Force update after maximum 5 sec since first update request. self._ts_first_update_request = None self._ts_last_update_request = None self._use_filtered_method = None self.start()
class SyncThread(object): ''' A thread to synchronize the local ROS master with a remote master. While the synchronization only the topic of the remote ROS master will be registered by the local ROS master. The remote ROS master will be keep unchanged. ''' MAX_UPDATE_DELAY = 5 # times MSG_ANY_TYPE = '*' def __init__(self, name, uri, discoverer_name, monitoruri, timestamp, sync_on_demand=False): ''' Initialization method for the SyncThread. @param name: the name of the ROS master synchronized with. @type name: C{str} @param uri: the URI of the ROS master synchronized with @type uri: C{str} @param discoverer_name: the name of the discovery node running on ROS master synchronized with. @type discoverer_name: C{str} @param monitoruri: The URI of RPC server of the discovery node to get the ROS master state by calling a method only once. @type monitoruri: C{str} @param timestamp: The timestamp of the current state of the ROS master info. @type timestamp: C{float64} @param sync_on_demand: Synchronize topics on demand @type sync_on_demand: bool ''' self.name = name self.uri = uri self.discoverer_name = discoverer_name self.monitoruri = monitoruri self.timestamp = timestamp self.timestamp_local = 0. self.timestamp_remote = 0. self.localMasteruri = masteruri_from_ros() rospy.logdebug("SyncThread[%s]: create this sync thread", self.name) # synchronization variables self.__lock_info = threading.RLock() self.__lock_intern = threading.RLock() self._use_filtered_method = None # SyncMasterInfo with currently synchronized nodes, publisher (topic, node, nodeuri), subscriber(topic, node, nodeuri) and services self.__sync_info = None self.__unregistered = False # a list with published topics as a tuple of (topic name, node name, node URL) self.__publisher = [] # a list with subscribed topics as a tuple of (topic name, node name, node URL) self.__subscriber = [] # a list with services as a tuple of (service name, service URL, node name, node URL) self.__services = [] # the state of the own ROS master is used if `sync_on_demand` is enabled or # to determine the type of topic subscribed remote with `Empty` type self.__own_state = None #setup the filter self._filter = FilterInterface() self._filter.load(self.name, ['/rosout', rospy.get_name().replace('/', '/*')+'*', self.discoverer_name.replace('/', '/*')+'*', '/*node_manager', '/*zeroconf'], [], ['/rosout', '/rosout_agg'], ['/'] if sync_on_demand else [], ['/*get_loggers', '/*set_logger_level'], [], # do not sync the bond message of the nodelets!! ['bond/Status'], [], []) # congestion avoidance: wait for random.random*2 sec. If an update request # is received try to cancel and restart the current timer. The timer can be # canceled for maximal MAX_UPDATE_DELAY times. self._update_timer = None self._delayed_update = 0 self.__on_update = False def getSyncInfo(self): ''' Returns the synchronized publisher, subscriber and services. @rtype: SyncMasterInfo ''' with self.__lock_info: if self.__sync_info is None: # create a sync info result_set = set() result_publisher = [] result_subscriber = [] result_services = [] for (t_n, n_n, n_uri) in self.__publisher: result_publisher.append(SyncTopicInfo(t_n, n_n, n_uri)) result_set.add(n_n) for (t_n, n_n, n_uri) in self.__subscriber: result_subscriber.append(SyncTopicInfo(t_n, n_n, n_uri)) result_set.add(n_n) for (s_n, s_uri, n_n, n_uri) in self.__services: result_services.append(SyncServiceInfo(s_n, s_uri, n_n, n_uri)) result_set.add(n_n) self.__sync_info = SyncMasterInfo(self.uri, list(result_set), result_publisher, result_subscriber, result_services) return self.__sync_info def update(self, name, uri, discoverer_name, monitoruri, timestamp): ''' Sets a request to synchronize the local ROS master with this ROS master. @note: If currently a synchronization is running this request will be ignored! @param name: the name of the ROS master synchronized with. @type name: C{str} @param uri: the URI of the ROS master synchronized with @type uri: C{str} @param discoverer_name: the name of the discovery node running on ROS master synchronized with. @type discoverer_name: C{str} @param monitoruri: The URI of RPC server of the discovery node to get the ROS master state by calling a method only once. @type monitoruri: C{str} @param timestamp: The timestamp of the current state of the ROS master info. @type timestamp: C{float64} ''' # rospy.logdebug("SyncThread[%s]: update request", self.name) with self.__lock_intern: if (self.timestamp_local != timestamp): rospy.logdebug("SyncThread[%s]: update notify new timestamp(%.9f), old(%.9f)", self.name, timestamp, self.timestamp_local) self.name = name self.uri = uri self.discoverer_name = discoverer_name self.monitoruri = monitoruri self.timestamp_remote = timestamp self._request_update() # rospy.logdebug("SyncThread[%s]: update exit", self.name) def setOwnMasterState(self, own_state, sync_on_demand=False): ''' Sets the state of the local ROS master state. If this state is not None, the topics on demand will be synchronized. @param own_state: the state of the local ROS master state @type own_state: C{master_discovery_fkie/MasterInfo} @param sync_on_demand: if True, sync only topic, which are also local exists (Default: False) @type sync_on_demand: bool ''' # rospy.logdebug("SyncThread[%s]: setOwnMasterState", self.name) with self.__lock_intern: timestamp_local = own_state.timestamp_local if self.__own_state is None or (self.__own_state.timestamp_local != timestamp_local): rospy.logdebug("SyncThread[%s]: local state update notify new timestamp(%.9f), old(%.9f)", self.name, timestamp_local, self.__own_state.timestamp_local if not self.__own_state is None else float('nan')) self.__own_state = own_state if sync_on_demand: self._filter.update_sync_topics_pattern(self.__own_state.topic_names) self._request_update() # rospy.logdebug("SyncThread[%s]: setOwnMasterState exit", self.name) def stop(self): ''' Stops running thread. ''' rospy.logdebug(" SyncThread[%s]: stop request", self.name) with self.__lock_intern: if not self._update_timer is None: self._update_timer.cancel() self._unreg_on_finish() rospy.logdebug(" SyncThread[%s]: stop exit", self.name) def _request_update(self): with self.__lock_intern: r = random.random() * 2. # start update timer with a random waiting time to avoid a congestion picks on changes of ROS master state if self._update_timer is None or not self._update_timer.isAlive(): del self._update_timer self._update_timer = threading.Timer(r, self._request_remote_state, args=(self._apply_remote_state,)) self._update_timer.start() else: if self._delayed_update < self.MAX_UPDATE_DELAY: # if the timer thread can be canceled start new one self._update_timer.cancel() # if callback (XMLRPC request) is already running the timer is not canceled -> test for `self.__on_update` if not self._update_timer.isAlive() or not self.__on_update: self._delayed_update += 1 self._update_timer = threading.Timer(r, self._request_remote_state, args=(self._apply_remote_state,)) self._update_timer.start() def _request_remote_state(self, handler): self._delayed_update = 0 self.__on_update = True try: # connect to master_monitor rpc-xml server of remote master discovery socket.setdefaulttimeout(20) remote_monitor = xmlrpclib.ServerProxy(self.monitoruri) # determine the getting method: older versions have not a filtered method if self._use_filtered_method is None: try: self._use_filtered_method = 'masterInfoFiltered' in remote_monitor.system.listMethods() except: self._use_filtered_method = False remote_state = None # get the state informations if self._use_filtered_method: remote_state = remote_monitor.masterInfoFiltered(self._filter.to_list()) else: remote_state = remote_monitor.masterInfo() if not self.__unregistered: handler(remote_state) except: import traceback rospy.logwarn("SyncThread[%s] ERROR: %s", self.name, traceback.format_exc()) finally: self.__on_update = False socket.setdefaulttimeout(None) def _apply_remote_state(self, remote_state): try: stamp = float(remote_state[0]) stamp_local = float(remote_state[1]) remote_masteruri = remote_state[2] # remote_mastername = remote_state[3] publishers = remote_state[4] subscribers = remote_state[5] rservices = remote_state[6] topicTypes = remote_state[7] nodeProviders = remote_state[8] serviceProviders = remote_state[9] # create a multicall object own_master = xmlrpclib.ServerProxy(self.localMasteruri) own_master_multi = xmlrpclib.MultiCall(own_master) # fill the multicall object handler = [] # sync the publishers publisher = [] publisher_to_register = [] for (topic, nodes) in publishers: for node in nodes: topictype = self._getTopicType(topic, topicTypes) nodeuri = self._getNodeUri(node, nodeProviders, remote_masteruri) if topictype and nodeuri and not self._doIgnoreNTP(node, topic, topictype): # register the nodes only once if not ((topic, node, nodeuri) in self.__publisher): publisher_to_register.append((topic, topictype, node, nodeuri)) publisher.append((topic, node, nodeuri)) # unregister not updated publishers for (topic, node, nodeuri) in set(self.__publisher)-set(publisher): own_master_multi.unregisterPublisher(node, topic, nodeuri) handler.append(('upub', topic, node, nodeuri)) #register new publishers for (topic, topictype, node, nodeuri) in publisher_to_register: own_master_multi.registerPublisher(node, topic, topictype, nodeuri) handler.append(('pub', topic, topictype, node, nodeuri)) # sync the subscribers subscriber = [] subscriber_to_register = [] for (topic, nodes) in subscribers: for node in nodes: topictype = self._getTopicType(topic, topicTypes) nodeuri = self._getNodeUri(node, nodeProviders, remote_masteruri) # if remote topictype is None, try to set to the local topic type # if not topictype and not self.__own_state is None: # if topic in self.__own_state.topics: # topictype = self.__own_state.topics[topic].type if not topictype: topictype = self.MSG_ANY_TYPE if topictype and nodeuri and not self._doIgnoreNTS(node, topic, topictype): # register the node as subscriber in local ROS master if not ((topic, node, nodeuri) in self.__subscriber): subscriber_to_register.append((topic, topictype, node, nodeuri)) subscriber.append((topic, node, nodeuri)) # unregister not updated topics for (topic, node, nodeuri) in set(self.__subscriber)-set(subscriber): own_master_multi.unregisterSubscriber(node, topic, nodeuri) handler.append(('usub', topic, node, nodeuri)) #register new subscriber for (topic, topictype, node, nodeuri) in subscriber_to_register: own_master_multi.registerSubscriber(node, topic, topictype, nodeuri) handler.append(('sub', topic, topictype, node, nodeuri)) # sync the services services = [] services_to_register = [] for (service, nodes) in rservices: for node in nodes: serviceuri = self._getServiceUri(service, serviceProviders, remote_masteruri) nodeuri = self._getNodeUri(node, nodeProviders, remote_masteruri) if serviceuri and nodeuri and not self._doIgnoreNS(node, service): # register the node as publisher in local ROS master if not ((service, serviceuri, node, nodeuri) in self.__services): services_to_register.append((service, serviceuri, node, nodeuri)) services.append((service, serviceuri, node, nodeuri)) # unregister not updated services for (service, serviceuri, node, nodeuri) in set(self.__services)-set(services): own_master_multi.unregisterService(node, service, serviceuri) handler.append(('usrv', service, serviceuri, node, nodeuri)) #register new services for (service, serviceuri, node, nodeuri) in services_to_register: own_master_multi.registerService(node, service, serviceuri, nodeuri) handler.append(('srv', service, serviceuri, node, nodeuri)) # execute the multicall and update the current publicher, subscriber and services if not self.__unregistered: with self.__lock_info: self.__sync_info = None self.__publisher = publisher self.__subscriber = subscriber self.__services = services # update the local ROS master socket.setdefaulttimeout(3) result = own_master_multi() # analyze the results of the registration call #HACK param to reduce publisher creation, see line 372 hack_pub = set() for h,(code, statusMessage, r) in zip(handler, result): try: if h[0] == 'sub': if code == -1: rospy.logwarn("SyncThread[%s] topic subscription error: %s (%s), %s %s, node: %s", self.name, h[1], h[2], str(code), str(statusMessage), h[3]) else: rospy.logdebug("SyncThread[%s] topic subscribed: %s, %s %s, node: %s", self.name, h[1], str(code), str(statusMessage), h[3]) if h[0] == 'sub' and code == 1 and len(r) > 0: # Horrible hack: see line 372 hack_pub.add(h[1]) elif h[0] == 'pub': if code == -1: rospy.logwarn("SyncThread[%s] topic advertise error: %s (%s), %s %s", self.name, h[1], h[2], str(code), str(statusMessage)) else: rospy.logdebug("SyncThread[%s] topic advertised: %s, %s %s", self.name, h[1], str(code), str(statusMessage)) elif h[0] == 'usub': rospy.logdebug("SyncThread[%s] topic unsubscribed: %s, %s %s", self.name, h[1], str(code), str(statusMessage)) elif h[0] == 'upub': rospy.logdebug("SyncThread[%s] topic unadvertised: %s, %s %s", self.name, h[1], str(code), str(statusMessage)) elif h[0] == 'srv': if code == -1: rospy.logwarn("SyncThread[%s] service registration error: %s, %s %s", self.name, h[1], str(code), str(statusMessage)) else: rospy.logdebug("SyncThread[%s] service registered: %s, %s %s", self.name, h[1], str(code), str(statusMessage)) elif h[0] == 'usrv': rospy.logdebug("SyncThread[%s] service unregistered: %s, %s %s", self.name, h[1], str(code), str(statusMessage)) except: import traceback rospy.logerr("SyncThread[%s] ERROR while analyzing the results of the registration call [%s]: %s", self.name, h[1], traceback.format_exc()) # Horrible hack: the response from registerSubscriber() can contain a # list of current publishers. But we don't have a way of injecting them # into rospy here. Now, if we get a publisherUpdate() from the master, # everything will work. So, we ask the master if anyone is currently # publishing the topic, grab the advertised type, use it to advertise # ourselves, then unadvertise, triggering a publisherUpdate() along the # way. # We create publisher locally as a hack, to get callback set up properly for already registered local publishers if hack_pub: rospy.loginfo("SyncThread[%s] Horrible hack: create and delete publisher to trigger an update for subscribed topics: %s", self.name, hack_pub) for m in hack_pub: try: topicPub = rospy.Publisher(m, rospy.msg.AnyMsg, queue_size=1) topicPub.unregister() del topicPub except: import traceback rospy.logerr("SyncThread[%s] ERROR while hack subscriber[%s]: %s", self.name, h[1], traceback.format_exc()) # set the last synchronization time self.timestamp = stamp self.timestamp_local = stamp_local rospy.logdebug("SyncThread[%s]: current timestamp %.9f, local %.9f", self.name, stamp, stamp_local) if self.timestamp_remote > stamp_local: rospy.logdebug("SyncThread[%s]: invoke next update, remote ts: %.9f", self.name, self.timestamp_remote) self._update_timer = threading.Timer(random.random() * 2., self._request_remote_state, args=(self._apply_remote_state,)) self._update_timer.start() except: import traceback rospy.logwarn("SyncThread[%s] ERROR: %s", self.name, traceback.format_exc()) finally: socket.setdefaulttimeout(None) def _unreg_on_finish(self): with self.__lock_info: self.__unregistered = True try: rospy.logdebug(" SyncThread[%s] clear all registrations", self.name) socket.setdefaulttimeout(5) own_master = xmlrpclib.ServerProxy(self.localMasteruri) own_master_multi = xmlrpclib.MultiCall(own_master) #end routine if the master was removed for topic, node, uri in self.__subscriber: own_master_multi.unregisterSubscriber(node, topic, uri) for topic, node, uri in self.__publisher: own_master_multi.unregisterPublisher(node, topic, uri) for service, serviceuri, node, uri in self.__services: own_master_multi.unregisterService(node, service, serviceuri) rospy.logdebug(" SyncThread[%s] execute a MultiCall", self.name) r = own_master_multi() rospy.logdebug(" SyncThread[%s] finished", self.name) except: import traceback rospy.logwarn("SyncThread[%s] ERROR while ending: %s", self.name, traceback.format_exc()) socket.setdefaulttimeout(None) def _doIgnoreNTP(self, node, topic, topictype): return self._filter.is_ignored_publisher(node, topic, topictype) def _doIgnoreNTS(self, node, topic, topictype): return self._filter.is_ignored_subscriber(node, topic, topictype) def _doIgnoreNS(self, node, service): return self._filter.is_ignored_service(node, service) def _getTopicType(self, topic, topicTypes): for (topicname, topic_type) in topicTypes: if (topicname == topic): return topic_type.replace('None', '') return None def _getNodeUri(self, node, nodes, remote_masteruri): for (nodename, uri, masteruri, pid, local) in nodes: if (nodename == node) and ((self._filter.sync_remote_nodes() and masteruri == remote_masteruri) or local == 'local'): # the node was registered originally to another ROS master -> do sync if masteruri != self.localMasteruri: return uri return None def _getServiceUri(self, service, nodes, remote_masteruri): for (servicename, uri, masteruri, topic_type, local) in nodes: if (servicename == service) and ((self._filter.sync_remote_nodes() and masteruri == remote_masteruri) or local == 'local'): if masteruri != self.localMasteruri: return uri return None
def __init__(self, name, uri, discoverer_name, monitoruri, timestamp, sync_on_demand=False): ''' Initialization method for the SyncThread. @param name: the name of the ROS master synchronized with. @type name: C{str} @param uri: the URI of the ROS master synchronized with @type uri: C{str} @param discoverer_name: the name of the discovery node running on ROS master synchronized with. @type discoverer_name: C{str} @param monitoruri: The URI of RPC server of the discovery node to get the ROS master state by calling a method only once. @type monitoruri: C{str} @param timestamp: The timestamp of the current state of the ROS master info. @type timestamp: C{float64} @param sync_on_demand: Synchronize topics on demand @type sync_on_demand: bool ''' self.name = name self.uri = uri self.discoverer_name = discoverer_name self.monitoruri = monitoruri self.timestamp = timestamp self.timestamp_local = 0. self.timestamp_remote = 0. self._online = True self._offline_ts = 0 self.masteruri_local = masteruri_from_ros() rospy.logdebug("SyncThread[%s]: create this sync thread", self.name) # synchronization variables self.__lock_info = threading.RLock() self.__lock_intern = threading.RLock() self._use_filtered_method = None # SyncMasterInfo with currently synchronized nodes, publisher (topic, node, nodeuri), subscriber(topic, node, nodeuri) and services self.__sync_info = None self.__unregistered = False # a list with published topics as a tuple of (topic name, node name, node URL) self.__publisher = [] # a list with subscribed topics as a tuple of (topic name, node name, node URL) self.__subscriber = [] # a list with services as a tuple of (service name, service URL, node name, node URL) self.__services = [] # the state of the own ROS master is used if `sync_on_demand` is enabled or # to determine the type of topic subscribed remote with `Empty` type self.__own_state = None # setup the filter self._filter = FilterInterface() self._filter.load(self.name, ['/rosout', rospy.get_name(), self.discoverer_name, '/node_manager', '/zeroconf'], [], ['/rosout', '/rosout_agg'], ['/'] if sync_on_demand else [], ['/*get_loggers', '/*set_logger_level'], [], # do not sync the bond message of the nodelets!! ['bond/Status'], [], [], []) # congestion avoidance: wait for random.random*2 sec. If an update request # is received try to cancel and restart the current timer. The timer can be # canceled for maximal MAX_UPDATE_DELAY times. self._update_timer = None self._delayed_update = 0 self.__on_update = False
class SyncThread(object): ''' A thread to synchronize the local ROS master with a remote master. While the synchronization only the topic of the remote ROS master will be registered by the local ROS master. The remote ROS master will be keep unchanged. ''' MAX_UPDATE_DELAY = 5 # times MSG_ANY_TYPE = '*' def __init__(self, name, uri, discoverer_name, monitoruri, timestamp, sync_on_demand=False): ''' Initialization method for the SyncThread. @param name: the name of the ROS master synchronized with. @type name: C{str} @param uri: the URI of the ROS master synchronized with @type uri: C{str} @param discoverer_name: the name of the discovery node running on ROS master synchronized with. @type discoverer_name: C{str} @param monitoruri: The URI of RPC server of the discovery node to get the ROS master state by calling a method only once. @type monitoruri: C{str} @param timestamp: The timestamp of the current state of the ROS master info. @type timestamp: C{float64} @param sync_on_demand: Synchronize topics on demand @type sync_on_demand: bool ''' self.name = name self.uri = uri self.discoverer_name = discoverer_name self.monitoruri = monitoruri self.timestamp = timestamp self.timestamp_local = 0. self.timestamp_remote = 0. self._online = True self._offline_ts = 0 self.masteruri_local = masteruri_from_ros() rospy.logdebug("SyncThread[%s]: create this sync thread", self.name) # synchronization variables self.__lock_info = threading.RLock() self.__lock_intern = threading.RLock() self._use_filtered_method = None # SyncMasterInfo with currently synchronized nodes, publisher (topic, node, nodeuri), subscriber(topic, node, nodeuri) and services self.__sync_info = None self.__unregistered = False # a list with published topics as a tuple of (topic name, node name, node URL) self.__publisher = [] # a list with subscribed topics as a tuple of (topic name, node name, node URL) self.__subscriber = [] # a list with services as a tuple of (service name, service URL, node name, node URL) self.__services = [] # the state of the own ROS master is used if `sync_on_demand` is enabled or # to determine the type of topic subscribed remote with `Empty` type self.__own_state = None # setup the filter self._filter = FilterInterface() self._filter.load(self.name, ['/rosout', rospy.get_name(), self.discoverer_name, '/node_manager', '/zeroconf'], [], ['/rosout', '/rosout_agg'], ['/'] if sync_on_demand else [], ['/*get_loggers', '/*set_logger_level'], [], # do not sync the bond message of the nodelets!! ['bond/Status'], [], [], []) # congestion avoidance: wait for random.random*2 sec. If an update request # is received try to cancel and restart the current timer. The timer can be # canceled for maximal MAX_UPDATE_DELAY times. self._update_timer = None self._delayed_update = 0 self.__on_update = False def get_sync_info(self): ''' Returns the synchronized publisher, subscriber and services. @rtype: SyncMasterInfo ''' with self.__lock_info: if self.__sync_info is None: # create a sync info result_set = set() result_publisher = [] result_subscriber = [] result_services = [] for (t_n, n_n, n_uri) in self.__publisher: result_publisher.append(SyncTopicInfo(t_n, n_n, n_uri)) result_set.add(n_n) for (t_n, n_n, n_uri) in self.__subscriber: result_subscriber.append(SyncTopicInfo(t_n, n_n, n_uri)) result_set.add(n_n) for (s_n, s_uri, n_n, n_uri) in self.__services: result_services.append(SyncServiceInfo(s_n, s_uri, n_n, n_uri)) result_set.add(n_n) self.__sync_info = SyncMasterInfo(self.uri, list(result_set), result_publisher, result_subscriber, result_services) return self.__sync_info def set_online(self, value, resync_on_reconnect_timeout=0.): if value: if not self._online: with self.__lock_intern: self._online = True offline_duration = time.time() - self._offline_ts if offline_duration >= resync_on_reconnect_timeout: rospy.loginfo("SyncThread[%s]: perform resync after the host was offline (unregister and register again to avoid connection losses to python topic. These does not suppot reconnection!)", self.name) if self._update_timer is not None: self._update_timer.cancel() self._unreg_on_finish() self.__unregistered = False self.__publisher = [] self.__subscriber = [] self.__services = [] self.timestamp = 0. self.timestamp_local = 0. self.timestamp_remote = 0. else: rospy.loginfo("SyncThread[%s]: skip resync after the host was offline because of resync_on_reconnect_timeout=%.2f and the host was only %.2f sec offline", self.name, resync_on_reconnect_timeout, offline_duration) else: self._online = False self._offline_ts = time.time() def update(self, name, uri, discoverer_name, monitoruri, timestamp): ''' Sets a request to synchronize the local ROS master with this ROS master. @note: If currently a synchronization is running this request will be ignored! @param name: the name of the ROS master synchronized with. @type name: C{str} @param uri: the URI of the ROS master synchronized with @type uri: C{str} @param discoverer_name: the name of the discovery node running on ROS master synchronized with. @type discoverer_name: C{str} @param monitoruri: The URI of RPC server of the discovery node to get the ROS master state by calling a method only once. @type monitoruri: C{str} @param timestamp: The timestamp of the current state of the ROS master info. @type timestamp: C{float64} ''' # rospy.logdebug("SyncThread[%s]: update request", self.name) with self.__lock_intern: self.timestamp_remote = timestamp if (self.timestamp_local != timestamp): rospy.logdebug("SyncThread[%s]: update notify new timestamp(%.9f), old(%.9f)", self.name, timestamp, self.timestamp_local) self.name = name self.uri = uri self.discoverer_name = discoverer_name self.monitoruri = monitoruri self._request_update() # rospy.logdebug("SyncThread[%s]: update exit", self.name) def set_own_masterstate(self, own_state, sync_on_demand=False): ''' Sets the state of the local ROS master state. If this state is not None, the topics on demand will be synchronized. @param own_state: the state of the local ROS master state @type own_state: C{master_discovery_fkie/MasterInfo} @param sync_on_demand: if True, sync only topic, which are also local exists (Default: False) @type sync_on_demand: bool ''' with self.__lock_intern: timestamp_local = own_state.timestamp_local if self.__own_state is None or (self.__own_state.timestamp_local != timestamp_local): ownstate_ts = self.__own_state.timestamp_local if self.__own_state is not None else float('nan') rospy.logdebug("SyncThread[%s]: local state update notify new timestamp(%.9f), old(%.9f)", self.name, timestamp_local, ownstate_ts) self.__own_state = own_state if sync_on_demand: self._filter.update_sync_topics_pattern(self.__own_state.topic_names) self._request_update() def stop(self): ''' Stops running thread. ''' rospy.logdebug(" SyncThread[%s]: stop request", self.name) with self.__lock_intern: if self._update_timer is not None: self._update_timer.cancel() self._unreg_on_finish() rospy.logdebug(" SyncThread[%s]: stop exit", self.name) def _request_update(self): with self.__lock_intern: r = random.random() * 2. # start update timer with a random waiting time to avoid a congestion picks on changes of ROS master state if self._update_timer is None or not self._update_timer.isAlive(): del self._update_timer self._update_timer = threading.Timer(r, self._request_remote_state, args=(self._apply_remote_state,)) self._update_timer.start() else: if self._delayed_update < self.MAX_UPDATE_DELAY: # if the timer thread can be canceled start new one self._update_timer.cancel() # if callback (XMLRPC request) is already running the timer is not canceled -> test for `self.__on_update` if not self._update_timer.isAlive() or not self.__on_update: self._delayed_update += 1 self._update_timer = threading.Timer(r, self._request_remote_state, args=(self._apply_remote_state,)) self._update_timer.start() def _request_remote_state(self, handler): self._delayed_update = 0 self.__on_update = True try: # connect to master_monitor rpc-xml server of remote master discovery socket.setdefaulttimeout(20) remote_monitor = xmlrpclib.ServerProxy(self.monitoruri) # determine the getting method: older versions have not a filtered method if self._use_filtered_method is None: try: self._use_filtered_method = 'masterInfoFiltered' in remote_monitor.system.listMethods() except: self._use_filtered_method = False remote_state = None # get the state informations rospy.loginfo("SyncThread[%s] Requesting remote state from '%s'", self.name, self.monitoruri) if self._use_filtered_method: remote_state = remote_monitor.masterInfoFiltered(self._filter.to_list()) else: remote_state = remote_monitor.masterInfo() if not self.__unregistered: handler(remote_state) except: rospy.logerr("SyncThread[%s] ERROR: %s", self.name, traceback.format_exc()) finally: self.__on_update = False socket.setdefaulttimeout(None) def _apply_remote_state(self, remote_state): rospy.loginfo("SyncThread[%s] Applying remote state...", self.name) try: stamp = float(remote_state[0]) stamp_local = float(remote_state[1]) remote_masteruri = remote_state[2] # remote_mastername = remote_state[3] publishers = remote_state[4] subscribers = remote_state[5] rservices = remote_state[6] topicTypes = remote_state[7] nodeProviders = remote_state[8] serviceProviders = remote_state[9] # create a multicall object own_master = xmlrpclib.ServerProxy(self.masteruri_local) own_master_multi = xmlrpclib.MultiCall(own_master) # fill the multicall object handler = [] # sync the publishers publisher = [] publisher_to_register = [] for (topic, nodes) in publishers: for node in nodes: topictype = self._get_topictype(topic, topicTypes) nodeuri = self._get_nodeuri(node, nodeProviders, remote_masteruri) if topictype and nodeuri and not self._do_ignore_ntp(node, topic, topictype): # register the nodes only once if not ((topic, node, nodeuri) in self.__publisher): publisher_to_register.append((topic, topictype, node, nodeuri)) publisher.append((topic, node, nodeuri)) # unregister not updated publishers for (topic, node, nodeuri) in set(self.__publisher) - set(publisher): own_master_multi.unregisterPublisher(node, topic, nodeuri) rospy.logdebug("SyncThread[%s]: UNPUB %s[%s] %s", self.name, node, nodeuri, topic) handler.append(('upub', topic, node, nodeuri)) # register new publishers for (topic, topictype, node, nodeuri) in publisher_to_register: own_master_multi.registerPublisher(node, topic, topictype, nodeuri) rospy.logdebug("SyncThread[%s]: PUB %s[%s] %s[%s]", self.name, node, nodeuri, topic, topictype) handler.append(('pub', topic, topictype, node, nodeuri)) # sync the subscribers subscriber = [] subscriber_to_register = [] for (topic, nodes) in subscribers: for node in nodes: topictype = self._get_topictype(topic, topicTypes) nodeuri = self._get_nodeuri(node, nodeProviders, remote_masteruri) # if remote topictype is None, try to set to the local topic type # if not topictype and not self.__own_state is None: # if topic in self.__own_state.topics: # topictype = self.__own_state.topics[topic].type if not topictype: topictype = self.MSG_ANY_TYPE if topictype and nodeuri and not self._do_ignore_nts(node, topic, topictype): # register the node as subscriber in local ROS master if not ((topic, node, nodeuri) in self.__subscriber): subscriber_to_register.append((topic, topictype, node, nodeuri)) subscriber.append((topic, node, nodeuri)) # unregister not updated topics for (topic, node, nodeuri) in set(self.__subscriber) - set(subscriber): own_master_multi.unregisterSubscriber(node, topic, nodeuri) rospy.logdebug("SyncThread[%s]: UNSUB %s[%s] %s", self.name, node, nodeuri, topic) handler.append(('usub', topic, node, nodeuri)) # register new subscriber for (topic, topictype, node, nodeuri) in subscriber_to_register: own_master_multi.registerSubscriber(node, topic, topictype, nodeuri) rospy.logdebug("SyncThread[%s]: SUB %s[%s] %s[%s]", self.name, node, nodeuri, topic, topictype) handler.append(('sub', topic, topictype, node, nodeuri)) # sync the services services = [] services_to_register = [] for (service, nodes) in rservices: for node in nodes: serviceuri = self._get_serviceuri(service, serviceProviders, remote_masteruri) nodeuri = self._get_nodeuri(node, nodeProviders, remote_masteruri) if serviceuri and nodeuri and not self._do_ignore_ns(node, service): # register the node as publisher in local ROS master if not ((service, serviceuri, node, nodeuri) in self.__services): services_to_register.append((service, serviceuri, node, nodeuri)) services.append((service, serviceuri, node, nodeuri)) # unregister not updated services for (service, serviceuri, node, nodeuri) in set(self.__services) - set(services): own_master_multi.unregisterService(node, service, serviceuri) rospy.logdebug("SyncThread[%s]: UNSRV %s[%s] %s[%s]", self.name, node, nodeuri, service, serviceuri) handler.append(('usrv', service, serviceuri, node, nodeuri)) # register new services for (service, serviceuri, node, nodeuri) in services_to_register: own_master_multi.registerService(node, service, serviceuri, nodeuri) rospy.logdebug("SyncThread[%s]: SRV %s[%s] %s[%s]", self.name, node, nodeuri, service, serviceuri) handler.append(('srv', service, serviceuri, node, nodeuri)) # execute the multicall and update the current publicher, subscriber and services if not self.__unregistered: with self.__lock_info: self.__sync_info = None self.__publisher = publisher self.__subscriber = subscriber self.__services = services # update the local ROS master socket.setdefaulttimeout(3) result = own_master_multi() # analyze the results of the registration call # HACK param to reduce publisher creation, see line 372 hack_pub = set() for h, (code, statusMessage, r) in zip(handler, result): try: if h[0] == 'sub': if code == -1: rospy.logwarn("SyncThread[%s] topic subscription error: %s (%s), %s %s, node: %s", self.name, h[1], h[2], str(code), str(statusMessage), h[3]) else: rospy.logdebug("SyncThread[%s] topic subscribed: %s, %s %s, node: %s", self.name, h[1], str(code), str(statusMessage), h[3]) if h[0] == 'sub' and code == 1 and len(r) > 0: # Horrible hack: see line 372 hack_pub.add(h[1]) elif h[0] == 'pub': if code == -1: rospy.logwarn("SyncThread[%s] topic advertise error: %s (%s), %s %s", self.name, h[1], h[2], str(code), str(statusMessage)) else: rospy.logdebug("SyncThread[%s] topic advertised: %s, %s %s", self.name, h[1], str(code), str(statusMessage)) elif h[0] == 'usub': rospy.logdebug("SyncThread[%s] topic unsubscribed: %s, %s %s", self.name, h[1], str(code), str(statusMessage)) elif h[0] == 'upub': rospy.logdebug("SyncThread[%s] topic unadvertised: %s, %s %s", self.name, h[1], str(code), str(statusMessage)) elif h[0] == 'srv': if code == -1: rospy.logwarn("SyncThread[%s] service registration error: %s, %s %s", self.name, h[1], str(code), str(statusMessage)) else: rospy.logdebug("SyncThread[%s] service registered: %s, %s %s", self.name, h[1], str(code), str(statusMessage)) elif h[0] == 'usrv': rospy.logdebug("SyncThread[%s] service unregistered: %s, %s %s", self.name, h[1], str(code), str(statusMessage)) except: rospy.logerr("SyncThread[%s] ERROR while analyzing the results of the registration call [%s]: %s", self.name, h[1], traceback.format_exc()) # Horrible hack: the response from registerSubscriber() can contain a # list of current publishers. But we don't have a way of injecting them # into rospy here. Now, if we get a publisherUpdate() from the master, # everything will work. So, we ask the master if anyone is currently # publishing the topic, grab the advertised type, use it to advertise # ourselves, then unadvertise, triggering a publisherUpdate() along the # way. # We create publisher locally as a hack, to get callback set up properly for already registered local publishers for m in hack_pub: try: rospy.loginfo("SyncThread[%s] Create and delete publisher to trigger publisherUpdate for %s", self.name, m) topicPub = rospy.Publisher(m, rospy.msg.AnyMsg, queue_size=1) topicPub.unregister() del topicPub except: rospy.logerr("SyncThread[%s] ERROR while hack subscriber[%s]: %s", self.name, h[1], traceback.format_exc()) # set the last synchronization time self.timestamp = stamp self.timestamp_local = stamp_local rospy.logdebug("SyncThread[%s]: current timestamp %.9f, local %.9f", self.name, stamp, stamp_local) if self.timestamp_remote > stamp_local: rospy.logdebug("SyncThread[%s]: invoke next update, remote ts: %.9f", self.name, self.timestamp_remote) self._update_timer = threading.Timer(random.random() * 2., self._request_remote_state, args=(self._apply_remote_state,)) self._update_timer.start() except: rospy.logerr("SyncThread[%s] ERROR: %s", self.name, traceback.format_exc()) finally: socket.setdefaulttimeout(None) rospy.loginfo("SyncThread[%s] remote state applied.", self.name) def _unreg_on_finish(self): with self.__lock_info: self.__unregistered = True try: rospy.logdebug(" SyncThread[%s] clear all registrations", self.name) socket.setdefaulttimeout(5) own_master = xmlrpclib.ServerProxy(self.masteruri_local) own_master_multi = xmlrpclib.MultiCall(own_master) # end routine if the master was removed for topic, node, uri in self.__subscriber: own_master_multi.unregisterSubscriber(node, topic, uri) for topic, node, uri in self.__publisher: own_master_multi.unregisterPublisher(node, topic, uri) for service, serviceuri, node, uri in self.__services: own_master_multi.unregisterService(node, service, serviceuri) rospy.logdebug(" SyncThread[%s] execute a MultiCall", self.name) _ = own_master_multi() rospy.logdebug(" SyncThread[%s] finished", self.name) except: rospy.logerr("SyncThread[%s] ERROR while ending: %s", self.name, traceback.format_exc()) socket.setdefaulttimeout(None) def _do_ignore_ntp(self, node, topic, topictype): return self._filter.is_ignored_publisher(node, topic, topictype) def _do_ignore_nts(self, node, topic, topictype): return self._filter.is_ignored_subscriber(node, topic, topictype) def _do_ignore_ns(self, node, service): return self._filter.is_ignored_service(node, service) def _get_topictype(self, topic, topic_types): for (topicname, topic_type) in topic_types: if (topicname == topic): return topic_type.replace('None', '') return None def _get_nodeuri(self, node, nodes, remote_masteruri): for (nodename, uri, masteruri, pid, local) in nodes: if (nodename == node) and ((self._filter.sync_remote_nodes() and masteruri == remote_masteruri) or local == 'local'): # the node was registered originally to another ROS master -> do sync if masteruri != self.masteruri_local: return uri return None def _get_serviceuri(self, service, nodes, remote_masteruri): for (servicename, uri, masteruri, topic_type, local) in nodes: if (servicename == service) and ((self._filter.sync_remote_nodes() and masteruri == remote_masteruri) or local == 'local'): if masteruri != self.masteruri_local: return uri return None
class SyncThread(threading.Thread): ''' A thread to synchronize the local ROS master with a remote master. While the synchronization only the topic of the remote ROS master will be registered by the local ROS master. The remote ROS master will be keep unchanged. ''' def __init__(self, name, uri, discoverer_name, monitoruri, timestamp, sync_on_demand=False): ''' Initialization method for the SyncThread. @param name: the name of the ROS master synchronized with. @type name: C{str} @param uri: the URI of the ROS master synchronized with @type uri: C{str} @param discoverer_name: the name of the discovery node running on ROS master synchronized with. @type discoverer_name: C{str} @param monitoruri: The URI of RPC server of the discovery node to get the ROS master state by calling a method only once. @type monitoruri: C{str} @param timestamp: The timestamp of the current state of the ROS master info. @type timestamp: C{float64} @param sync_on_demand: Synchronize topics on demand @type sync_on_demand: bool ''' # init thread threading.Thread.__init__(self) self.masterInfo = MasterInfo(name, uri, discoverer_name, monitoruri, timestamp) self.localMasteruri = masteruri_from_ros() rospy.logdebug("SyncThread[%s]: create this sync thread", self.masterInfo.name) # synchronization variables self.__cv = threading.Condition() self.__lock_info = threading.RLock() self.__sync_info = None #SyncMasterInfo with currently synchronized nodes, publisher (topic, node, nodeuri), subscriber(topic, node, nodeuri) and services self.__stop = False # a list with published topics as a tuple of (topic name, node name, node URL) self.__publisher = [] # a list with subscribed topics as a tuple of (topic name, node name, node URL) self.__subscriber = [] # a list with services as a tuple of (service name, service URL, node name, node URL) self.__services = [] self.__own_state = None #setup the filter self._filter = FilterInterface() self._filter.load(self.masterInfo.name, ['/rosout', rospy.get_name().replace('/', '/*')+'*', self.masterInfo.discoverer_name.replace('/', '/*')+'*', '/*node_manager', '/*zeroconf'], [], ['/rosout', '/rosout_agg'], ['/'] if sync_on_demand else [], ['/*get_loggers', '/*set_logger_level'], [], # do not sync the bond message of the nodelets!! ['bond/Status']) # congestion avoidance: wait for minimum 1 sec. If an update request is received wait # for next 1 second. Force update after maximum 5 sec since first update request. self._ts_first_update_request = None self._ts_last_update_request = None self._use_filtered_method = None self.start() def getSyncInfo(self): with self.__lock_info: if self.__sync_info is None: # create a sync info result_set = set() result_publisher = [] result_subscriber = [] result_services = [] for (t_n, n_n, n_uri) in self.__publisher: result_publisher.append(SyncTopicInfo(t_n, n_n, n_uri)) result_set.add(n_n) for (t_n, n_n, n_uri) in self.__subscriber: result_subscriber.append(SyncTopicInfo(t_n, n_n, n_uri)) result_set.add(n_n) for (s_n, s_uri, n_n, n_uri) in self.__services: result_services.append(SyncServiceInfo(s_n, s_uri, n_n, n_uri)) result_set.add(n_n) self.__sync_info = SyncMasterInfo(self.masterInfo.uri, list(result_set), result_publisher, result_subscriber, result_services) return self.__sync_info def update(self, name, uri, discoverer_name, monitoruri, timestamp): ''' Sets a request to synchronize the local ROS master with this ROS master. @note: If currently a synchronization is running this request will be ignored! @param name: the name of the ROS master synchronized with. @type name: C{str} @param uri: the URI of the ROS master synchronized with @type uri: C{str} @param discoverer_name: the name of the discovery node running on ROS master synchronized with. @type discoverer_name: C{str} @param monitoruri: The URI of RPC server of the discovery node to get the ROS master state by calling a method only once. @type monitoruri: C{str} @param timestamp: The timestamp of the current state of the ROS master info. @type timestamp: C{float64} ''' # rospy.logdebug("SyncThread[%s]: update request", self.masterInfo.name) with self.__cv: str_time = str(timestamp) if (str(self.masterInfo.timestamp_local) != str_time): rospy.logdebug("SyncThread[%s]: update notify new timestamp(%s), old(%s)", self.masterInfo.name, str_time, str(self.masterInfo.timestamp_local)) self.masterInfo.name = name self.masterInfo.uri = uri self.masterInfo.discoverer_name = discoverer_name self.masterInfo.monitoruri = monitoruri self.masterInfo.syncts = 0.0 # for congestion avoidance self._ts_last_update_request = time.time() if self._ts_first_update_request is None: self._ts_first_update_request = time.time() self.__cv.notify() # rospy.logdebug("SyncThread[%s]: update exit", self.masterInfo.name) def setOwnMasterState(self, own_state, sync_on_demand=False): ''' Sets the state of the local ROS master state. If this state is not None, the topics on demand will be synchronized. @param own_state: the state of the local ROS master state @type own_state: C{master_discovery_fkie/MasterInfo} @param sync_on_demand: if True, sync only topic, which are also local exists (Default: False) @type sync_on_demand: bool ''' # rospy.logdebug("SyncThread[%s]: setOwnMasterState", self.masterInfo.name) with self.__cv: timestamp_local = own_state.timestamp_local if self.__own_state is None or (self.__own_state.timestamp_local != timestamp_local): rospy.logdebug("SyncThread[%s]: local state update notify new timestamp(%s), old(%s)", self.masterInfo.name, str(timestamp_local), str(self.__own_state.timestamp_local if not self.__own_state is None else 'None')) self.__own_state = own_state if sync_on_demand: self._filter.update_sync_topics_pattern(self.__own_state.topic_names) self.masterInfo.syncts = 0.0 # for congestion avoidance self._ts_last_update_request = time.time() if self._ts_first_update_request is None: self._ts_first_update_request = time.time() self.__cv.notify() # rospy.logdebug("SyncThread[%s]: setOwnMasterState exit", self.masterInfo.name) def stop(self): ''' Stops running thread. ''' rospy.logdebug("SyncThread[%s]: stop request", self.masterInfo.name) with self.__cv: self.__stop = True self.__cv.notify() rospy.logdebug("SyncThread[%s]: stop exit", self.masterInfo.name) def run(self): ''' The synchronization procedure waits for notifications of the L{update()} method. If the remote ROS master is changed, the changes will be performed on the local ROS master. ''' do_wait = False while not self.__stop and not rospy.is_shutdown(): if do_wait: time.sleep(3) with self.__cv: ''' wait for new sync update ''' rospy.logdebug("SyncThread[%s]: run waiting timestamp(%s), syncts(%s)", self.masterInfo.name, str(self.masterInfo.timestamp), str(self.masterInfo.syncts)) if not (self.masterInfo.syncts == 0.0) and self.masterInfo.lastsync != 0.0: self.__cv.wait() rospy.logdebug("SyncThread[%s]: run notify received", self.masterInfo.name) if (not self.__stop): rospy.logdebug("SyncThread[%s]: congestion avoidance...", self.masterInfo.name) # congestion avoidance if not self._ts_first_update_request is None: current_time = time.time() while ((current_time - self._ts_first_update_request) < 5 and (current_time - self._ts_last_update_request) < 1.1): wait = random.random() * 2 print "sleep", self.masterInfo.name, wait time.sleep(wait) current_time = time.time() rospy.logdebug("SyncThread[%s]: run sync", self.masterInfo.name) ''' try to sync ''' try: #connect to master_monitor rpc-xml server socket.setdefaulttimeout(20) remote_monitor = xmlrpclib.ServerProxy(self.masterInfo.monitoruri) if self._use_filtered_method is None: try: self._use_filtered_method = 'masterInfoFiltered' in remote_monitor.system.listMethods() except: self._use_filtered_method = False remote_state = None if self._use_filtered_method: remote_state = remote_monitor.masterInfoFiltered(self._filter.to_list()) else: remote_state = remote_monitor.masterInfo() stamp = float(remote_state[0]) stamp_local = float(remote_state[1]) remote_masteruri = remote_state[2] # remote_mastername = remote_state[3] publishers = remote_state[4] subscribers = remote_state[5] rservices = remote_state[6] topicTypes = remote_state[7] nodeProviders = remote_state[8] serviceProviders = remote_state[9] own_master = xmlrpclib.ServerProxy(self.localMasteruri) own_master_multi = xmlrpclib.MultiCall(own_master) handler = [] # sync the publishers publisher = [] publisher_to_register = [] for (topic, nodes) in publishers: for node in nodes: topictype = self._getTopicType(topic, topicTypes) nodeuri = self._getNodeUri(node, nodeProviders, remote_masteruri) if topictype and nodeuri and not self._doIgnoreNT(node, topic, topictype): # register the nodes only once if not ((topic, node, nodeuri) in self.__publisher): publisher_to_register.append((topic, topictype, node, nodeuri)) publisher.append((topic, node, nodeuri)) # unregister not updated publishers for (topic, node, nodeuri) in set(self.__publisher)-set(publisher): own_master_multi.unregisterPublisher(node, topic, nodeuri) handler.append(('upub', topic, node, nodeuri)) #register new publishers for (topic, topictype, node, nodeuri) in publisher_to_register: own_master_multi.registerPublisher(node, topic, topictype, nodeuri) handler.append(('pub', topic, topictype, node, nodeuri)) # sync the subscribers subscriber = [] subscriber_to_register = [] for (topic, nodes) in subscribers: for node in nodes: topictype = self._getTopicType(topic, topicTypes) nodeuri = self._getNodeUri(node, nodeProviders, remote_masteruri) # if remote topictype is None, try to set to the local topic type if not topictype and not self.__own_state is None: if topic in self.__own_state.topics: topictype = self.__own_state.topics[topic].type if topictype and nodeuri and not self._doIgnoreNT(node, topic, topictype): # register the node as subscriber in local ROS master if not ((topic, node, nodeuri) in self.__subscriber): subscriber_to_register.append((topic, topictype, node, nodeuri)) subscriber.append((topic, node, nodeuri)) # unregister not updated topics for (topic, node, nodeuri) in set(self.__subscriber)-set(subscriber): own_master_multi.unregisterSubscriber(node, topic, nodeuri) handler.append(('usub', topic, node, nodeuri)) #register new subscriber for (topic, topictype, node, nodeuri) in subscriber_to_register: own_master_multi.registerSubscriber(node, topic, topictype, nodeuri) handler.append(('sub', topic, topictype, node, nodeuri)) # sync the services services = [] services_to_register = [] for (service, nodes) in rservices: for node in nodes: serviceuri = self._getServiceUri(service, serviceProviders, remote_masteruri) nodeuri = self._getNodeUri(node, nodeProviders, remote_masteruri) if serviceuri and nodeuri and not self._doIgnoreNS(node, service): # register the node as publisher in local ROS master if not ((service, serviceuri, node, nodeuri) in self.__services): services_to_register.append((service, serviceuri, node, nodeuri)) services.append((service, serviceuri, node, nodeuri)) # unregister not updated services for (service, serviceuri, node, nodeuri) in set(self.__services)-set(services): own_master_multi.unregisterService(node, service, serviceuri) handler.append(('usrv', service, serviceuri, node, nodeuri)) #register new services for (service, serviceuri, node, nodeuri) in services_to_register: own_master_multi.registerService(node, service, serviceuri, nodeuri) handler.append(('srv', service, serviceuri, node, nodeuri)) # exceute the multicall and update the current publicher, subscriber and services with self.__lock_info: self.__sync_info = None self.__publisher = publisher self.__subscriber = subscriber self.__services = services socket.setdefaulttimeout(3) result = own_master_multi() # Horrible hack: the response from registerSubscriber() can contain a # list of current publishers. But we don't have a way of injecting them # into rospy here. Now, if we get a publisherUpdate() from the master, # everything will work. So, we ask the master if anyone is currently # publishing the topic, grab the advertised type, use it to advertise # ourselves, then unadvertise, triggering a publisherUpdate() along the # way. # We create publisher locally as a hack, to get callback set up properly for already registered local publishers for h,(code, statusMessage, r) in zip(handler, result): try: if h[0] == 'sub': if code == -1: rospy.logwarn("SyncThread[%s] topic subscription error: %s (%s), %s %s", self.masterInfo.name, h[1], h[2], str(code), str(statusMessage)) else: rospy.loginfo("SyncThread[%s] topic subscribed: %s, %s %s", self.masterInfo.name, h[1], str(code), str(statusMessage)) if h[0] == 'sub' and code == 1 and len(r) > 0: topicPub = rospy.Publisher(h[1], roslib.message.get_message_class(h[2])) topicPub.unregister() del topicPub elif h[0] == 'pub': if code == -1: rospy.logwarn("SyncThread[%s] topic advertise error: %s (%s), %s %s", self.masterInfo.name, h[1], h[2], str(code), str(statusMessage)) else: rospy.loginfo("SyncThread[%s] topic advertised: %s, %s %s", self.masterInfo.name, h[1], str(code), str(statusMessage)) elif h[0] == 'usub': rospy.loginfo("SyncThread[%s] topic unsubscribed: %s, %s %s", self.masterInfo.name, h[1], str(code), str(statusMessage)) elif h[0] == 'upub': rospy.loginfo("SyncThread[%s] topic unadvertised: %s, %s %s", self.masterInfo.name, h[1], str(code), str(statusMessage)) elif h[0] == 'srv': if code == -1: rospy.logwarn("SyncThread[%s] service registration error: %s, %s %s", self.masterInfo.name, h[1], str(code), str(statusMessage)) else: rospy.loginfo("SyncThread[%s] service registered: %s, %s %s", self.masterInfo.name, h[1], str(code), str(statusMessage)) elif h[0] == 'usrv': rospy.loginfo("SyncThread[%s] service unregistered: %s, %s %s", self.masterInfo.name, h[1], str(code), str(statusMessage)) except: import traceback rospy.logerr("SyncThread[%s] ERROR while hack subscriber[%s]: %s", self.masterInfo.name, h[1], traceback.format_exc()) # set the last synchronization time self.masterInfo.timestamp = stamp self.masterInfo.timestamp_local = stamp_local self.masterInfo.lastsync = stamp self.masterInfo.syncts = stamp self._ts_first_update_request = None rospy.logdebug("SyncThread[%s]: seteeddd timestamp %s, local %s", self.masterInfo.name, str(stamp), str(stamp_local)) socket.setdefaulttimeout(None) do_wait = False except: self.masterInfo.syncts = 0.0 import traceback rospy.logwarn("SyncThread[%s] ERROR: %s", self.masterInfo.name, traceback.format_exc()) do_wait = True try: rospy.logdebug("SyncThread[%s] clear all registrations", self.masterInfo.name) socket.setdefaulttimeout(5) own_master = xmlrpclib.ServerProxy(self.localMasteruri) own_master_multi = xmlrpclib.MultiCall(own_master) #end routine if the master was removed for topic, node, uri in self.__subscriber: own_master_multi.unregisterSubscriber(node, topic, uri) for topic, node, uri in self.__publisher: own_master_multi.unregisterPublisher(node, topic, uri) for service, serviceuri, node, uri in self.__services: own_master_multi.unregisterService(node, service, serviceuri) rospy.logdebug("SyncThread[%s] execute a MultiCall", self.masterInfo.name) r = own_master_multi() rospy.logdebug("SyncThread[%s] finished", self.masterInfo.name) except: import traceback rospy.logwarn("SyncThread[%s] ERROR while ending: %s", self.masterInfo.name, traceback.format_exc()) socket.setdefaulttimeout(None) def _doIgnoreNT(self, node, topic, topictype): return self._filter.is_ignored_topic(node, topic, topictype) def _doIgnoreNS(self, node, service): return self._filter.is_ignored_service(node, service) def _getTopicType(self, topic, topicTypes): for (topicname, type) in topicTypes: if (topicname == topic): return type.replace('None', '') return None def _getNodeUri(self, node, nodes, remote_masteruri): for (nodename, uri, masteruri, pid, local) in nodes: if (nodename == node) and ((self._filter.sync_remote_nodes() and masteruri == remote_masteruri) or local == 'local'): # the node was registered originally to another ROS master -> do sync if masteruri != self.localMasteruri: return uri return None def _getServiceUri(self, service, nodes, remote_masteruri): for (servicename, uri, masteruri, type, local) in nodes: if (servicename == service) and ((self._filter.sync_remote_nodes() and masteruri == remote_masteruri) or local == 'local'): if masteruri != self.localMasteruri: return uri return None