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