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
