def init(self, monitor_port):
'''
Creates the local monitoring. Call start() to run the local monitoring.
@param monitor_port: the port of the XML-RPC Server created by monitoring class.
@type monitor_port: C{int}
'''
self._master_monitor = MasterMonitor(monitor_port)
self._do_pause = True
# self._local_addr = roslib.network.get_local_address()
# self._masteruri = roslib.rosenv.get_master_uri()
self._masteruri = self._master_monitor.getMasteruri()
self._local_addr = self._master_monitor.getMastername()
self._masterMonitorThread = threading.Thread(target = self.mastermonitor_loop)
self._masterMonitorThread.setDaemon(True)
self._masterMonitorThread.start()
def init(self, monitor_port):
'''
Creates the local monitoring. Call start() to run the local monitoring.
@param monitor_port: the port of the XML-RPC Server created by monitoring class.
@type monitor_port: C{int}
'''
self._master_monitor = MasterMonitor(monitor_port, False)
self._do_pause = True
self._do_finish = False
self._masteruri = self._master_monitor.getMasteruri()
self._local_addr = self._master_monitor.getMastername()
self._masterMonitorThread = threading.Thread(
target=self.mastermonitor_loop)
self._masterMonitorThread.setDaemon(True)
self._masterMonitorThread.start()
self._last_error = (time.time(), None)
def init(self, monitor_port):
'''
Creates the local monitoring. Call start() to run the local monitoring.
@param monitor_port: the port of the XML-RPC Server created by monitoring class.
@type monitor_port: C{int}
'''
self._master_monitor = MasterMonitor(monitor_port, False)
self._do_pause = True
self._do_finish = False
self._masteruri = self._master_monitor.getMasteruri()
self._local_addr = self._master_monitor.getMastername()
self._masterMonitorThread = threading.Thread(target=self.mastermonitor_loop)
self._masterMonitorThread.setDaemon(True)
self._masterMonitorThread.start()
self._last_error = (time.time(), None)
def __init__(self, mcast_port, mcast_group, monitor_port):
'''
Initialize method for the Discoverer class
:param mcast_port: The port used to publish and receive the multicast messages.
:type mcast_port: int
:param mcast_group: The IPv4 or IPv6 multicast group used for discovering over nodes.
:type mcast_group: str
:param monitor_port: The port of the RPC Server, used to get more information about the ROS master.
:type monitor_port: int
'''
# threading.Thread.__init__(self)
self.do_finish = False
self.__lock = threading.RLock()
# the list with all ROS master neighbors
self.masters = dict() # (ip, DiscoveredMaster)
# this parameter stores the state of the remote nodes. If the state is changed
# the cache for contacts of remote nodes will be cleared.
self._changed = False
self.ROSMASTER_HZ = rospy.get_param('~rosmaster_hz',
Discoverer.ROSMASTER_HZ)
self.HEARTBEAT_HZ = rospy.get_param('~heartbeat_hz',
Discoverer.HEARTBEAT_HZ)
self.MEASUREMENT_INTERVALS = rospy.get_param(
'~measurement_intervals', Discoverer.MEASUREMENT_INTERVALS)
self.TIMEOUT_FACTOR = rospy.get_param('~timeout_factor',
Discoverer.TIMEOUT_FACTOR)
self.REMOVE_AFTER = rospy.get_param('~remove_after',
Discoverer.REMOVE_AFTER)
self.ACTIVE_REQUEST_AFTER = rospy.get_param(
'~active_request_after', Discoverer.ACTIVE_REQUEST_AFTER)
if self.ACTIVE_REQUEST_AFTER <= 0:
rospy.logwarn("active_request_after [sec]: %s <= 0 set to 60" %
self.ACTIVE_REQUEST_AFTER)
self.ACTIVE_REQUEST_AFTER = 60
self.robots = rospy.get_param('~robot_hosts', [])
self.CHANGE_NOTIFICATION_COUNT = rospy.get_param(
'~change_notification_count', Discoverer.CHANGE_NOTIFICATION_COUNT)
self._current_change_notification_count = 0
self._send_mcast = rospy.get_param('~send_mcast', True)
# for cases with more then one master_discovery on the same host and
# heartbeat rate is less then 0.1. In this case we have to send a multicast
# request reply, because we are bind to the same port. Unicast replies are
# not forward to the same port only once.
self._addresses = dict() # {address : (int) ocurres}
# some parameter checks and info outputs
if not self._send_mcast and not self.robots:
rospy.logwarn(
"This master_discovery is invisible because it send no heart beat messages! Set ~send_mcast to true or add hosts to ~robot_hosts."
)
if not self._send_mcast:
self.HEARTBEAT_HZ = 1. / self.ACTIVE_REQUEST_AFTER
rospy.logwarn(
"Multicast is disabled. Use ~active_request_after(%.2f) ot set ~heartbeat_hz to new value: %.4f"
% (self.ACTIVE_REQUEST_AFTER, self.HEARTBEAT_HZ))
rospy.loginfo("Check the ROS Master[Hz]: " + str(self.ROSMASTER_HZ))
if self.HEARTBEAT_HZ <= 0.:
rospy.logwarn("Heart beat [Hz]: %s is increased to 0.02" %
self.HEARTBEAT_HZ)
self.HEARTBEAT_HZ = 0.02
if self.HEARTBEAT_HZ > 25.5:
rospy.logwarn("Heart beat [Hz]: %s is decreased to 25.5" %
self.HEARTBEAT_HZ)
self.HEARTBEAT_HZ = 25.5
else:
rospy.loginfo("Heart beat [Hz]: %s" % (self.HEARTBEAT_HZ))
rospy.loginfo("Active request after [sec]: %s" %
self.ACTIVE_REQUEST_AFTER)
rospy.loginfo("Remove after [sec]: %s" % self.REMOVE_AFTER)
if self.REMOVE_AFTER <= self.ACTIVE_REQUEST_AFTER:
rospy.logwarn(
"'Active request after' should be less than 'remove after' to avoid removing masters from list!"
)
rospy.loginfo("Robot hosts: " + str(self.robots))
if self.HEARTBEAT_HZ > 0.:
count_packets = len(self.robots) + (1 if self._send_mcast else 0)
netload = self.HEARTBEAT_HZ * self.NETPACKET_SIZE * count_packets
rospy.loginfo("Approx. mininum avg. network load: %.2f bytes/s" %
netload)
self.current_check_hz = self.ROSMASTER_HZ
self.pubstats = rospy.Publisher("~linkstats",
LinkStatesStamped,
queue_size=1)
# test the reachability of the ROS master
local_addr = roslib.network.get_local_address()
if (local_addr in ['localhost', '127.0.0.1']):
rospy.logwarn(
"'%s' is not reachable for other systems. Change the ROS_MASTER_URI!"
% local_addr)
self.mcast_port = mcast_port
self.mcast_group = mcast_group
# initialize the ROS publishers
self.pubchanges = rospy.Publisher("~changes",
MasterState,
queue_size=10)
# initialize the ROS services
rospy.Service('~list_masters', DiscoverMasters,
self.rosservice_list_masters)
rospy.Service('~refresh', std_srvs.srv.Empty, self.rosservice_refresh)
# create a thread to monitor the ROS master state
mgroup = DiscoverSocket.normalize_mgroup(mcast_group)
is_ip6 = self._is_ipv6_group(mgroup)
self.master_monitor = MasterMonitor(monitor_port, ipv6=is_ip6)
# create timer to check for ros master changes
self._timer_ros_changes = threading.Timer(0.1,
self.checkROSMaster_loop)
# init socket for discovering. Exit on errors!
self._init_socket(True)
# create a timer monitor the offline ROS master and calculate the link qualities
self._timer_stats = threading.Timer(1, self.timed_stats_calculation)
# create timer and paramter for heartbeat notifications
self._init_notifications = 0
# disable parameter, if HEARTBEAT_HZ is active (> zero)
if self.HEARTBEAT_HZ > DiscoveredMaster.MIN_HZ_FOR_QUALILTY:
self._init_notifications = self.INIT_NOTIFICATION_COUNT
self._current_change_notification_count = self.CHANGE_NOTIFICATION_COUNT
self._timer_heartbeat = threading.Timer(1.0, self.send_heardbeat)
# set the callback to finish all running threads
rospy.on_shutdown(self.finish)
class Discoverer(object):
'''
The class to publish the current state of the ROS master.
Discovering is done by hearbeats:
Each master discovery node sends to a multicast group periodically messages
with current state. If the frequency is less than 0.3 the detected changes
on ROS master are published immediately.
The current state is described by timestamp of last change. The frequency of
heartbeats can be changed by `~heartbeat_hz` parameter.
If hearbeats are disabled (`~heartbeat_hz` is zero) each master discovery
node sends on start three notification messages and requests.
If for a host no more heartbeat are received while `ACTIVE_REQUEST_AFTER (60 sec)`
a request to this host will be sent as an unicast message. After five
unanswered requests the host state will be changed to `offline`.
After `REMOVE_AFTER (300 sec)` the host will be removed.
:param mcast_port: The port used to publish and receive the multicast messages.
:type mcast_port: int
:param mcast_group: The IPv4 or IPv6 multicast group used for discovering over nodes.
:type mcast_group: str
:param monitor_port: The port of the RPC Server, used to get more information about the ROS master.
:type monitor_port: int
'''
VERSION = 2
'''the version of the packet format described by ``HEARTBEAT_FMT``
:Version 1: 'cBBiiH'
::
one character 'R'
unsigned char: version of the hearbeat message
unsigned char: rate of the heartbeat message in HZ*10. Maximal rate: 25.5 Hz -> value 255
int: secs of the ROS Master state
int: nsecs of the ROS Master state
unsigned short: the port number of the RPC Server of the remote ROS-Core monitor
:Version 2: 'cBBiiHii'
::
``Version 1``
int: secs of the ROS Master state (only local changes). Changes while sync will be ignored.
int: nsecs of the ROS Master state (only local changes). Changes while sync will be ignored.
:Version 3: 'cBBiiHii'
::
``Version 2``
if the timestamp of ROS Master state is zero, the reply as unicast
message will be send to the sender.
'''
HEARTBEAT_FMT = 'cBBiiHii'
''' packet format description, see: http://docs.python.org/library/struct.html '''
HEARTBEAT_HZ = 0.02
''' the send rate of the heartbeat packets in hz. Zero disables the heartbeats. (Default: 0.02 Hz)
Only values between 0.1 and 25.5 are used to detemine the link quality.
'''
MEASUREMENT_INTERVALS = 5
''' the count of intervals (1 sec) used for a quality calculation. If
`HEARTBEAT_HZ` is smaller then 1, `MEASUREMENT_INTERVALS` will be divided
by `HEARTBEAT_HZ` value.
(Default: 5 sec are used to determine the link qaulity)'''
TIMEOUT_FACTOR = 1.4
''' the timeout is defined by calculated measurement duration multiplied by `TIMEOUT_FAKTOR`. '''
ROSMASTER_HZ = 1
''' the test rate of ROS master state in Hz (Default: 1 Hz). '''
REMOVE_AFTER = 300
''' remove an offline host after this time in [sec] (Default: 300 sec). '''
ACTIVE_REQUEST_AFTER = 60
''' send an update request, if after this time no hearbeats are received [sec] (Default: 60 sec). '''
INIT_NOTIFICATION_COUNT = 3
''' the count of heartbeats and update request to send at the start (Default: 3 sec).
It will be send with 1Hz. Only used if `HEARTBEAT_HZ` is zero. '''
OFFLINE_AFTER_REQUEST_COUNT = 5
''' After this unanswered count of requests for update the remote master is set
to offline state (Default: 5 sec).
The requests are send after `ACTIVE_REQUEST_AFTER` with `ROSMASTER_HZ`. '''
CHANGE_NOTIFICATION_COUNT = 3
''' After the ROS master was changed the new state will be sent for
`CHANGE_NOTIFICATION_COUNT` times (Default: 3 sec). The new state will be
sent with `ROSMASTER_HZ` and only if `HEARTBEAT_HZ` is zero. '''
NETPACKET_SIZE = 68
def __init__(self, mcast_port, mcast_group, monitor_port):
'''
Initialize method for the Discoverer class
:param mcast_port: The port used to publish and receive the multicast messages.
:type mcast_port: int
:param mcast_group: The IPv4 or IPv6 multicast group used for discovering over nodes.
:type mcast_group: str
:param monitor_port: The port of the RPC Server, used to get more information about the ROS master.
:type monitor_port: int
'''
# threading.Thread.__init__(self)
self.do_finish = False
self.__lock = threading.RLock()
# the list with all ROS master neighbors
self.masters = dict() # (ip, DiscoveredMaster)
# this parameter stores the state of the remote nodes. If the state is changed
# the cache for contacts of remote nodes will be cleared.
self._changed = False
self.ROSMASTER_HZ = rospy.get_param('~rosmaster_hz',
Discoverer.ROSMASTER_HZ)
self.HEARTBEAT_HZ = rospy.get_param('~heartbeat_hz',
Discoverer.HEARTBEAT_HZ)
self.MEASUREMENT_INTERVALS = rospy.get_param(
'~measurement_intervals', Discoverer.MEASUREMENT_INTERVALS)
self.TIMEOUT_FACTOR = rospy.get_param('~timeout_factor',
Discoverer.TIMEOUT_FACTOR)
self.REMOVE_AFTER = rospy.get_param('~remove_after',
Discoverer.REMOVE_AFTER)
self.ACTIVE_REQUEST_AFTER = rospy.get_param(
'~active_request_after', Discoverer.ACTIVE_REQUEST_AFTER)
if self.ACTIVE_REQUEST_AFTER <= 0:
rospy.logwarn("active_request_after [sec]: %s <= 0 set to 60" %
self.ACTIVE_REQUEST_AFTER)
self.ACTIVE_REQUEST_AFTER = 60
self.robots = rospy.get_param('~robot_hosts', [])
self.CHANGE_NOTIFICATION_COUNT = rospy.get_param(
'~change_notification_count', Discoverer.CHANGE_NOTIFICATION_COUNT)
self._current_change_notification_count = 0
self._send_mcast = rospy.get_param('~send_mcast', True)
# for cases with more then one master_discovery on the same host and
# heartbeat rate is less then 0.1. In this case we have to send a multicast
# request reply, because we are bind to the same port. Unicast replies are
# not forward to the same port only once.
self._addresses = dict() # {address : (int) ocurres}
# some parameter checks and info outputs
if not self._send_mcast and not self.robots:
rospy.logwarn(
"This master_discovery is invisible because it send no heart beat messages! Set ~send_mcast to true or add hosts to ~robot_hosts."
)
if not self._send_mcast:
self.HEARTBEAT_HZ = 1. / self.ACTIVE_REQUEST_AFTER
rospy.logwarn(
"Multicast is disabled. Use ~active_request_after(%.2f) ot set ~heartbeat_hz to new value: %.4f"
% (self.ACTIVE_REQUEST_AFTER, self.HEARTBEAT_HZ))
rospy.loginfo("Check the ROS Master[Hz]: " + str(self.ROSMASTER_HZ))
if self.HEARTBEAT_HZ <= 0.:
rospy.logwarn("Heart beat [Hz]: %s is increased to 0.02" %
self.HEARTBEAT_HZ)
self.HEARTBEAT_HZ = 0.02
if self.HEARTBEAT_HZ > 25.5:
rospy.logwarn("Heart beat [Hz]: %s is decreased to 25.5" %
self.HEARTBEAT_HZ)
self.HEARTBEAT_HZ = 25.5
else:
rospy.loginfo("Heart beat [Hz]: %s" % (self.HEARTBEAT_HZ))
rospy.loginfo("Active request after [sec]: %s" %
self.ACTIVE_REQUEST_AFTER)
rospy.loginfo("Remove after [sec]: %s" % self.REMOVE_AFTER)
if self.REMOVE_AFTER <= self.ACTIVE_REQUEST_AFTER:
rospy.logwarn(
"'Active request after' should be less than 'remove after' to avoid removing masters from list!"
)
rospy.loginfo("Robot hosts: " + str(self.robots))
if self.HEARTBEAT_HZ > 0.:
count_packets = len(self.robots) + (1 if self._send_mcast else 0)
netload = self.HEARTBEAT_HZ * self.NETPACKET_SIZE * count_packets
rospy.loginfo("Approx. mininum avg. network load: %.2f bytes/s" %
netload)
self.current_check_hz = self.ROSMASTER_HZ
self.pubstats = rospy.Publisher("~linkstats",
LinkStatesStamped,
queue_size=1)
# test the reachability of the ROS master
local_addr = roslib.network.get_local_address()
if (local_addr in ['localhost', '127.0.0.1']):
rospy.logwarn(
"'%s' is not reachable for other systems. Change the ROS_MASTER_URI!"
% local_addr)
self.mcast_port = mcast_port
self.mcast_group = mcast_group
# initialize the ROS publishers
self.pubchanges = rospy.Publisher("~changes",
MasterState,
queue_size=10)
# initialize the ROS services
rospy.Service('~list_masters', DiscoverMasters,
self.rosservice_list_masters)
rospy.Service('~refresh', std_srvs.srv.Empty, self.rosservice_refresh)
# create a thread to monitor the ROS master state
mgroup = DiscoverSocket.normalize_mgroup(mcast_group)
is_ip6 = self._is_ipv6_group(mgroup)
self.master_monitor = MasterMonitor(monitor_port, ipv6=is_ip6)
# create timer to check for ros master changes
self._timer_ros_changes = threading.Timer(0.1,
self.checkROSMaster_loop)
# init socket for discovering. Exit on errors!
self._init_socket(True)
# create a timer monitor the offline ROS master and calculate the link qualities
self._timer_stats = threading.Timer(1, self.timed_stats_calculation)
# create timer and paramter for heartbeat notifications
self._init_notifications = 0
# disable parameter, if HEARTBEAT_HZ is active (> zero)
if self.HEARTBEAT_HZ > DiscoveredMaster.MIN_HZ_FOR_QUALILTY:
self._init_notifications = self.INIT_NOTIFICATION_COUNT
self._current_change_notification_count = self.CHANGE_NOTIFICATION_COUNT
self._timer_heartbeat = threading.Timer(1.0, self.send_heardbeat)
# set the callback to finish all running threads
rospy.on_shutdown(self.finish)
def start(self):
self._timer_ros_changes.start()
self._timer_stats.start()
self._timer_heartbeat.start()
def _is_ipv6_group(self, addr):
try:
socket.inet_pton(socket.AF_INET6, addr)
return True
except:
pass
return False
def _init_socket(self, doexit_on_error=False):
# create discovery socket
# if multicast messages are disabled only unicast socket is created
# unicast socket is also created if ~interface is defined
self.socket = DiscoverSocket(self.mcast_port,
self.mcast_group,
unicast_only=(not self._send_mcast))
if self._send_mcast:
if not self.socket.hasEnabledMulticastIface() and doexit_on_error:
sys.exit(
"No enabled multicast interfaces available!\nAdd multicast support e.g. sudo ifconfig eth0 multicast or disable multicast.\nExit"
)
# set callback for received UDP messages
self.socket.set_message_callback(self.recv_udp_msg)
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 send_heardbeat(self):
'''
Sends current state as heartbeat messages to defined multicast group. If the
Discoverer.HEARTBEAT_HZ is greather then zero a timer will be started to
send heartbeat messages periodically. This message will also send on start
of the discoverer.
'''
with self.__lock:
# stop the current running timer, if this method was invoked outside of the timer
try:
self._timer_heartbeat.cancel()
except:
pass
# publish the current state
if not (self.master_monitor.getMasteruri() is None
or rospy.is_shutdown() or self.do_finish):
self._send_current_state2group()
try:
# send update requests to group
if self._init_notifications < self.INIT_NOTIFICATION_COUNT:
self._init_notifications += 1
self._send_request2group()
# send update requests to predefined robot hosts
for remote_master in self.robots:
self._send_request2addr(remote_master)
except Exception as err:
rospy.logwarn(err)
self._init_socket()
if not self.do_finish and (self.HEARTBEAT_HZ > 0.
or self._init_notifications <
self.INIT_NOTIFICATION_COUNT):
sleeptime = 1.0 / self.HEARTBEAT_HZ if self.HEARTBEAT_HZ > 0. else 1.0
self._timer_heartbeat = threading.Timer(
sleeptime, self.send_heardbeat)
self._timer_heartbeat.start()
def _send_current_state2group(self):
try:
msg = self._create_current_state_msg()
if not msg is None:
self.socket.send2group(msg)
except Exception as e:
rospy.logwarn(
'Send current state to mcast group %s:%s failed: %s\n' %
(self.mcast_group, self.mcast_port, e))
self._init_socket()
def _send_current_state2addr(self, address):
try:
msg = self._create_current_state_msg()
if not msg is None:
rospy.logdebug('Send current state to addr %s' % (address))
self.socket.send2addr(msg, address)
if self._is_multi_address(address):
# in case more then one master_discovery runs on the same address
# we send also a multicast message, because no to masters can bind to
# one unicast address
self._send_current_state2group()
except Exception as e:
rospy.logwarn("Send current state to '%s' failed: %s" %
(address, e))
self._init_socket()
def _send_request2group(self):
try:
rospy.logdebug('Send request to mcast group %s:%s' %
(self.mcast_group, self.mcast_port))
current_time = time.time()
for (_, v) in self.masters.iteritems():
v.add_request(current_time)
self.socket.send2group(self._create_request_update_msg())
except Exception as e:
rospy.logwarn("Send request to mcast group %s:%s' failed: %s" %
(self.mcast_group, self.mcast_port, e))
def _send_request2addr(self, address, master=None):
try:
rospy.logdebug('Send a request for update: %s' % address)
self.socket.send2addr(self._create_request_update_msg(), address)
if self._is_multi_address(address):
# in case more then one master_discovery runs on the same address
# we send also a multicast message, because no to masters can bind to
# one unicast address
self._send_request2group()
if not master is None:
master.add_request(time.time())
except Exception as e:
rospy.logwarn("Send to robot host '%s' failed: %s" % (address, e))
def _create_current_state_msg(self):
t = 0
local_t = 0
if not self.master_monitor.getCurrentState() is None:
t = self.master_monitor.getCurrentState().timestamp
local_t = self.master_monitor.getCurrentState().timestamp_local
return struct.pack(Discoverer.HEARTBEAT_FMT, 'R',
Discoverer.VERSION, int(self.HEARTBEAT_HZ * 10),
int(t), int((t - (int(t))) * 1000000000),
self.master_monitor.rpcport, int(local_t),
int((local_t - (int(local_t))) * 1000000000))
return None
def _create_request_update_msg(self):
version = Discoverer.VERSION if Discoverer.VERSION > 2 else 3
msg = struct.pack(Discoverer.HEARTBEAT_FMT, 'R', version,
int(self.HEARTBEAT_HZ * 10), 0, 0,
self.master_monitor.rpcport, 0, 0)
return msg
def checkROSMaster_loop(self):
'''
The method test periodically the state of the ROS master. The new state will
be published as heartbeat messages.
:mod:`master_discovery_fkie.master_monitor.MasterMonitor.checkState()`
'''
import os
try_count = 0
if (not rospy.is_shutdown()) and not self.do_finish:
try:
cputimes = os.times()
cputime_init = cputimes[0] + cputimes[1]
self.update_master_errors()
if self.master_monitor.checkState(self._changed):
# publish the new state if frequetly publishing is disabled
if not self.do_finish and self.HEARTBEAT_HZ < DiscoveredMaster.MIN_HZ_FOR_QUALILTY:
self.send_heardbeat()
self._current_change_notification_count = 0
with self.__lock:
self._changed = False
# repeat the last change for `CHANGE_NOTIFICATION_COUNT` times
if 0 < self._current_change_notification_count < self.CHANGE_NOTIFICATION_COUNT:
self._current_change_notification_count += 1
self.send_heardbeat()
# adapt the check rate to the CPU usage time
cputimes = os.times()
cputime = cputimes[0] + cputimes[1] - cputime_init
if self.current_check_hz * cputime > 0.20:
self.current_check_hz = float(self.current_check_hz) / 2.0
elif self.current_check_hz * cputime < 0.10 and float(
self.current_check_hz) * 2.0 < self.ROSMASTER_HZ:
self.current_check_hz = float(self.current_check_hz) * 2.0
try_count = 0
except MasterConnectionException as conn_err:
try_count = try_count + 1
if try_count == 5:
rospy.logerr("Communication with ROS Master failed: %s",
conn_err)
# remove offline hosts or request updates
self._remove_offline_hosts()
# setup timer for next ROS master state check
self._timer_ros_changes = threading.Timer(
1.0 / self.current_check_hz, self.checkROSMaster_loop)
self._timer_ros_changes.start()
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 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))
class OwnMasterMonitoring(QtCore.QObject):
'''
A class to monitor the state of the master. Will be used, if no master
discovering is available. On changes the 'state_signal' of type
L{master_discovery_fkie.msg.MasterState} will be emitted.
'''
state_signal = QtCore.Signal(MasterState)
'''@ivar: a signal to inform the receiver about new master state.
Parameter: L{master_discovery_fkie.msg.MasterState}'''
err_signal = QtCore.Signal(str)
'''@ivar: a signal to inform about an error.
Parameter: L{str}'''
ROSMASTER_HZ = 1
'''@ivar: the rate to test ROS master for changes.'''
def init(self, monitor_port):
'''
Creates the local monitoring. Call start() to run the local monitoring.
@param monitor_port: the port of the XML-RPC Server created by monitoring class.
@type monitor_port: C{int}
'''
self._master_monitor = MasterMonitor(monitor_port, False)
self._do_pause = True
self._do_finish = False
# self._local_addr = roslib.network.get_local_address()
# self._masteruri = roslib.rosenv.get_master_uri()
self._masteruri = self._master_monitor.getMasteruri()
self._local_addr = self._master_monitor.getMastername()
self._masterMonitorThread = threading.Thread(target = self.mastermonitor_loop)
self._masterMonitorThread.setDaemon(True)
self._masterMonitorThread.start()
def stop(self):
'''
Stop the local master monitoring
'''
print " Shutdown the local master monitoring..."
self._do_finish = True
self._masterMonitorThread.join(15)
self._master_monitor.shutdown()
print " Local master monitoring is off!"
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)
if not rospy.is_shutdown() and not self._do_finish:
time.sleep(1.0/current_check_hz)
class OwnMasterMonitoring(QtCore.QObject):
'''
A class to monitor the state of the master. Will be used, if no master
discovering is available. On changes the 'state_signal' of type
L{master_discovery_fkie.msg.MasterState} will be emitted.
'''
state_signal = QtCore.Signal(MasterState)
'''@ivar: a signal to inform the receiver about new master state.
Parameter: L{master_discovery_fkie.msg.MasterState}'''
err_signal = QtCore.Signal(str)
'''@ivar: a signal to inform about an error.
Parameter: L{str}'''
ROSMASTER_HZ = 1
'''@ivar: the rate to test ROS master for changes.'''
def init(self, monitor_port):
'''
Creates the local monitoring. Call start() to run the local monitoring.
@param monitor_port: the port of the XML-RPC Server created by monitoring class.
@type monitor_port: C{int}
'''
self._master_monitor = MasterMonitor(monitor_port, False)
self._do_pause = True
self._do_finish = False
# self._local_addr = roslib.network.get_local_address()
# self._masteruri = roslib.rosenv.get_master_uri()
self._masteruri = self._master_monitor.getMasteruri()
self._local_addr = self._master_monitor.getMastername()
self._masterMonitorThread = threading.Thread(
target=self.mastermonitor_loop)
self._masterMonitorThread.setDaemon(True)
self._masterMonitorThread.start()
self._last_error = (time.time(), None)
def stop(self):
'''
Stop the local master monitoring
'''
self._do_finish = True
if self._master_monitor.is_running():
print " Shutdown the local master monitoring..."
self._masterMonitorThread.join(15)
self._master_monitor.shutdown()
print " Local master monitoring is off!"
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, 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)
if not rospy.is_shutdown() and not self._do_finish:
time.sleep(1.0 / current_check_hz)