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(),
rospy.Time(r.getRosTimestamp()),
rospy.Time(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(),
rospy.Time(m.getRosTimestamp()),
rospy.Time(m.getRosTimestampLocal()), state,
m.getTXTValue('zname', ''),
m.getTXTValue('rpcuri', ''))))
except Exception:
pass
finally:
self.__lock.release()
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.items():
masters.append(
ROSMaster(str(master.name), master.getMasterUri(),
rospy.Time(master.getRosTimestamp()),
rospy.Time(master.getRosTimestampLocal()),
master.online, master.getTXTValue('zname', ''),
master.getTXTValue('rpcuri', '')))
except Exception:
pass
finally:
self.__lock.release()
return DiscoverMastersResponse(masters)
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(
utf8(self._local_addr), utf8(self._masteruri),
mon_state.timestamp, mon_state.timestamp_local,
True, rospy.get_name(), ''.join([
'http://localhost:',
utf8(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, mce:
self._handle_exception(
"MasterConnectionException while master check loop", mce)
except RuntimeError, ree:
# will thrown on exit of the app while try to emit the signal
self._handle_exception("RuntimeError while master check loop",
ree)
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(),
rospy.Time(master.getRosTimestamp()),
rospy.Time(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 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 fkie_multimaster 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(),
rospy.Time(master_info.getRosTimestamp()),
rospy.Time(
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(),
rospy.Time(master_info.getRosTimestamp()),
rospy.Time(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()
