neighbor_monitor.py

Go to the documentation of this file.

from std_msgs.msg import UInt32
from time import time
 
class NeighborMonitor():
    """Monitors if a neighboring robot is in range
    """
 
    def __init__(self, node, rid, is_enabled, init_delay_sec, max_delay_sec):
        """Initialization
        Args:
            id (int): Robot ID
        """
        self.nodenode = node
        self.robot_idrobot_id = rid
        self.is_enabledis_enabled = is_enabled
        self.origin_robot_idorigin_robot_id = self.robot_idrobot_id
 
        self.init_delay_secinit_delay_sec = init_delay_sec
        self.max_delay_secmax_delay_sec = max_delay_sec
        self.first_heartbeat_receivedfirst_heartbeat_received = False
        self.init_timeinit_time = time()
        self.latest_time_stamplatest_time_stamp = self.init_timeinit_time
        self.last_keyframe_receivedlast_keyframe_received = -1
        self.last_keyframe_sentlast_keyframe_sent = -1
        self.last_match_sentlast_match_sent = -1
 
        self.heartbeat_subscriberheartbeat_subscriber = self.nodenode.create_subscription(
            UInt32, '/r' + str(rid) + '/' + 'cslam/heartbeat',
            self.heartbeat_callbackheartbeat_callback, 10)
 
    def heartbeat_callback(self, msg):
        """Callback to indicate that it is alive
 
        Args:
            msg (UInt32):
        """
        self.origin_robot_idorigin_robot_id = msg.data
        self.latest_time_stamplatest_time_stamp = time()
        if not self.first_heartbeat_receivedfirst_heartbeat_received:
            self.first_heartbeat_receivedfirst_heartbeat_received = True
            self.init_timeinit_time = time()
 
    def is_alive(self):
        """Check if it recently received a heartbeat signal
 
        Returns:
            bool: liveliness indicator
        """
        if self.is_enabledis_enabled:
            now = time()
            return self.first_heartbeat_receivedfirst_heartbeat_received and now - self.init_timeinit_time > self.init_delay_secinit_delay_sec and now - self.latest_time_stamplatest_time_stamp < self.max_delay_secmax_delay_sec
        else:
            True