gateway_node.py

Go to the documentation of this file.

#!/usr/bin/env python
#
# License: BSD
#   https://raw.github.com/robotics-in-concert/rocon_multimaster/license/LICENSE
#
##############################################################################
# Imports
##############################################################################

import rospy
import rocon_gateway
import uuid
import gateway_msgs.msg as gateway_msgs
import gateway_msgs.srv as gateway_srvs
import std_msgs.msg as std_msgs
import std_srvs.srv as std_srvs
from urlparse import urlparse
import rocon_hub_client

from . import gateway
from . import hub_manager

##############################################################################
# Gateway Configuration and Main Loop Class
##############################################################################


class GatewayNode():

    """
      Currently this just provides getup and go for the gateway.
    """
    ##########################################################################
    # Init & Shutdown
    ##########################################################################

    def __init__(self):
        self._param = rocon_gateway.setup_ros_parameters()
        if self._param['disable_uuids']:
            self._unique_name = self._param['name']
            rospy.logwarn("Gateway : uuid's disabled, using possibly non-unique name [%s]" % self._unique_name)
        else:  # append a unique hex string
            # random 16 byte string, alternatively uuid.getnode() returns a hash based
            # on the mac address, uuid.uid1() based on localhost and time
            key = uuid.uuid4()
            self._unique_name = self._param['name'] + key.hex
            rospy.loginfo("Gateway : generated unique hash name [%s]" % self._unique_name)
        self._disallowed_hubs = {}
        self._disallowed_hubs_error_codes = [gateway_msgs.ErrorCodes.HUB_CONNECTION_NOT_IN_NONEMPTY_WHITELIST,
                                             gateway_msgs.ErrorCodes.HUB_CONNECTION_BLACKLISTED,
                                             gateway_msgs.ErrorCodes.HUB_NAME_NOT_FOUND,
                                             # this now has to be permitted as we will often have zeroconf failing for gateways
                                             # that have dropped out of wireless range.
                                             # gateway_msgs.ErrorCodes.HUB_CONNECTION_UNRESOLVABLE
                                             ]
        self._hub_manager = hub_manager.HubManager(
            hub_whitelist=self._param['hub_whitelist'],
            hub_blacklist=self._param['hub_blacklist']
        )
        # Be careful of the construction sequence here, parts depend on others.
        self._gateway_publishers = self._setup_ros_publishers()
        # self._publish_gateway_info needs self._gateway_publishers
        self._gateway = gateway.Gateway(self._hub_manager, self._param, self._unique_name, self._publish_gateway_info)
        self._gateway_services = self._setup_ros_services()  # Needs self._gateway
        self._gateway_subscribers = self._setup_ros_subscribers()  # Needs self._gateway
        # 'ip:port' : (error_code, error_code_str) dictionary of hubs that this gateway has tried to register,
        # but not been permitted (hub is not in whitelist, or is blacklisted)
        direct_hub_uri_list = [self._param['hub_uri']] if self._param['hub_uri'] != '' else []
        self._hub_discovery_thread = rocon_hub_client.HubDiscovery(
            self._hub_ensure_connection, direct_hub_uri_list, self._param['disable_zeroconf'], self._disallowed_hubs)

        # Make local gateway information immediately available
        self._publish_gateway_info()

    def spin(self):
        self._gateway.spin()
        self._shutdown()
        # else the shutdown hook handles it.

    def _shutdown(self):
        '''
          Clears this gateway's information from the redis server.
        '''
        rospy.loginfo("Gateway : shutting down.")
        try:
            # We shouldn't do anything network related here.
            # To be stable against network outages, the shutdown flow and the "crash/network disconnected" flow
            # should be as close as possible.

            # we still need this to cleanup threads locally
            self._hub_discovery_thread.shutdown()

            self._gateway = None
        except Exception as e:
            rospy.logerr("Gateway : unknown error on shutdown [%s][%s]" % (str(e), type(e)))
            raise

    ##########################################################################
    # Hub Discovery & Connection
    ##########################################################################

    def _hub_ensure_connection(self, ip, port):
        '''
        Called when the hub discovery can ping a hub

        :param ip:
        :param port:
        :return:
        '''

        uri = ip + ':' + str(port)
        if uri in self._disallowed_hubs.keys():
            # we already tried this one before, quietly return from here.
            return self._disallowed_hubs[uri]

        hub, error_code, error_code_str = self._hub_manager.is_connected_to_hub(ip, port)
        if error_code == gateway_msgs.ErrorCodes.NO_HUB_CONNECTION:
            error_code, error_code_str = self._register_gateway(hub)

        if error_code == gateway_msgs.ErrorCodes.SUCCESS:  # everything should be fine...
            pass
        elif error_code == gateway_msgs.ErrorCodes.HUB_CONNECTION_ALREADY_EXISTS:  # everything should also be fine...
            pass
        elif error_code in self._disallowed_hubs_error_codes:
            self._disallowed_hubs[uri] = (error_code, error_code_str)
            rospy.logwarn(
                "Gateway : failed to register gateway with the hub [%s][%s][%s]" % (uri, error_code, error_code_str))
        elif error_code == gateway_msgs.ErrorCodes.HUB_CONNECTION_FAILED:
            rospy.logwarn(
                "Gateway : failed to connect to the hub [%s][%s][%s]" % (uri, error_code, error_code_str))
        elif error_code == gateway_msgs.ErrorCodes.HUB_CONNECTION_UNRESOLVABLE:
            # be less noisy about this one - it's a normal error when a gateway has moved out of wireless range
            # but we still discover an 'unresolvable' hub on avahi before avahi eventually removes it.
            rospy.logdebug(
                "Gateway : failed to register gateway with the hub [%s][%s][%s]" % (uri, error_code, error_code_str))
        else:
            rospy.logwarn(
                "Gateway : Unknown error code when insuring gateway connection with the hub [%s][%s][%s]" %
                (uri, error_code, error_code_str))

        return error_code, error_code_str

    def _register_gateway(self, hub):
        '''
          Called when either the hub discovery module finds a hub
          or a request to connect via ros service is made.

          It starts the actual redis connection with the hub and also
          registers the appropriate information about the gateway on
          the hub.

          Note, the return type is only really used by the service callback
          (ros_service_connect_hub).

          @return error code and message
          @rtype gateway_msgs.ErrorCodes, string

          @sa hub_discovery.HubDiscovery
        '''

        existing_advertisements = self._gateway.public_interface.getConnections()
        hub, error_code, error_code_str = \
            self._hub_manager.connect_to_hub(
                hub,  # Hub Details
                self._param['firewall'],  # Gateway Details
                self._unique_name,
                self._disengage_hub,
                self._gateway.ip,
                existing_advertisements
            )
        if hub:
            rospy.loginfo("Gateway : registering on the hub [%s]" % hub.name)
            self._publish_gateway_info()

        return error_code, error_code_str

    def _disengage_hub(self, hub):
        """
          Called whenever gateway_hub detects the connection to the hub has been
          lost.

          This function informs the hub discovery thread that the hub was lost,
          which allows the hub_discovery thread to start looking for the hub.

          @param hub: hub to be disengaged
          @type GatewayHub
        """

        self._hub_discovery_thread.disengage_hub(hub)
        try:
            self._gateway.disengage_hub(hub)
        except AttributeError:
            # caused when self_gateway is NoneType object (can happen as we pass this
            # callback in without guaranteeing we have a gateway object yet)
            pass

    ##########################################################################
    # Ros Pubs, Subs and Services
    ##########################################################################

    def _setup_ros_services(self):
        gateway_services = {}
        gateway_services['connect_hub'] = rospy.Service(
            '~connect_hub', gateway_srvs.ConnectHub, self.ros_service_connect_hub)  # @IgnorePep8
        gateway_services['remote_gateway_info'] = rospy.Service(
            '~remote_gateway_info', gateway_srvs.RemoteGatewayInfo, self.ros_service_remote_gateway_info)  # @IgnorePep8
        gateway_services['advertise'] = rospy.Service(
            '~advertise', gateway_srvs.Advertise, self._gateway.ros_service_advertise)  # @IgnorePep8
        gateway_services['advertise_all'] = rospy.Service(
            '~advertise_all', gateway_srvs.AdvertiseAll, self._gateway.ros_service_advertise_all)  # @IgnorePep8
        gateway_services['flip'] = rospy.Service(
            '~flip', gateway_srvs.Remote, self._gateway.ros_service_flip)  # @IgnorePep8
        gateway_services['flip_all'] = rospy.Service(
            '~flip_all', gateway_srvs.RemoteAll, self._gateway.ros_service_flip_all)  # @IgnorePep8
        gateway_services['pull'] = rospy.Service(
            '~pull', gateway_srvs.Remote, self._gateway.ros_service_pull)  # @IgnorePep8
        gateway_services['pull_all'] = rospy.Service(
            '~pull_all', gateway_srvs.RemoteAll, self._gateway.ros_service_pull_all)  # @IgnorePep8
        #gateway_services['set_watcher_period'] = rospy.Service(
        #    '~set_watcher_period',
        #    gateway_srvs.SetWatcherPeriod,
        #    self._gateway.ros_service_set_watcher_period)  # @IgnorePep8
        return gateway_services

    def _setup_ros_publishers(self):
        gateway_publishers = {}
        gateway_publishers['gateway_info'] = rospy.Publisher('~gateway_info', gateway_msgs.GatewayInfo, latch=True, queue_size=5)
        return gateway_publishers

    def _setup_ros_subscribers(self):
        gateway_subscribers = {}
        #gateway_subscribers['force_update'] = rospy.Subscriber(
        #    '~force_update', std_msgs.Empty, self._gateway.ros_subscriber_force_update)
        return gateway_subscribers

    ##########################################################################
    # Ros Service Callbacks
    ##########################################################################

    def ros_service_connect_hub(self, request):
        """
          Handle incoming requests to connect directly to a gateway hub.

          Requests are of the form of a uri (hostname:port pair) pointing to
          the gateway hub.
        """
        response = gateway_srvs.ConnectHubResponse()
        o = urlparse(request.uri)
        response.result, response.error_message = self._hub_ensure_connection(o.hostname, o.port)
        # Some ros logging
        if response.result == gateway_msgs.ErrorCodes.SUCCESS:
            rospy.loginfo("Gateway : made direct connection to hub [%s]" % request.uri)
        return response

    def _publish_gateway_info(self):
        if self._gateway is None:
            return  # not everything initialised yet
        try:
            gateway_info = gateway_msgs.GatewayInfo()
            gateway_info.name = self._unique_name
            gateway_info.ip = self._gateway.ip
            gateway_info.connected = self._gateway.is_connected()
            gateway_info.hub_names = []
            gateway_info.hub_uris = []
            for hub in self._hub_manager.hubs:
                gateway_info.hub_names.append(hub.name)
                gateway_info.hub_uris.append(hub.uri)
            gateway_info.firewall = self._param['firewall']
            gateway_info.flipped_connections = self._gateway.flipped_interface.get_flipped_connections()
            gateway_info.flipped_in_connections = self._gateway.flipped_interface.getLocalRegistrations()
            gateway_info.flip_watchlist = self._gateway.flipped_interface.getWatchlist()
            gateway_info.pulled_connections = self._gateway.pulled_interface.getLocalRegistrations()
            gateway_info.pull_watchlist = self._gateway.pulled_interface.getWatchlist()
            gateway_info.public_watchlist = self._gateway.public_interface.getWatchlist()
            gateway_info.public_interface = self._gateway.public_interface.getInterface()
            self._gateway_publishers['gateway_info'].publish(gateway_info)
        except AttributeError:
            pass  # occurs if self._gateway is reset to None in the middle of all this.

    def ros_service_remote_gateway_info(self, request):
        """
          Sends out to the hubs to get the remote gateway information for either the specified,
          or the known list of remote gateways.

          :todo: can we optimise this so that hub requests go as a group?
        """
        response = gateway_srvs.RemoteGatewayInfoResponse()
        requested_gateways = request.gateways if request.gateways else self._hub_manager.list_remote_gateway_names()
        for gateway in list(set(requested_gateways)):
            remote_gateway_info = self._hub_manager.remote_gateway_info(gateway)
            if remote_gateway_info:
                response.gateways.append(remote_gateway_info)
            else:
                rospy.logwarn("Gateway : requested gateway info for unavailable gateway [%s]" % gateway)
        return response