Source Code for Module rocon_gateway.gateway_node

  1  #!/usr/bin/env python 
  2  # 
  3  # License: BSD 
  4  #   https://raw.github.com/robotics-in-concert/rocon_multimaster/hydro-devel/rocon_gateway/LICENSE 
  5  # 
  6  ############################################################################## 
  7  # Imports 
  8  ############################################################################## 
  9   
 10  import rospy 
 11  import rocon_gateway 
 12  import uuid 
 13  import gateway_msgs.msg as gateway_msgs 
 14  import gateway_msgs.srv as gateway_srvs 
 15  from urlparse import urlparse 
 16  import rocon_hub_client 
 17   
 18  # Local imports 
 19  import gateway 
 20  import hub_manager 
 21   
 22  ############################################################################## 
 23  # Gateway Configuration and Main Loop Class 
 24  ############################################################################## 
 25   
 26   
 27 -class GatewayNode(): 
 28      ''' 
 29        Currently this just provides getup and go for the gateway. 
 30      ''' 
 31      ########################################################################## 
 32      # Init & Shutdown 
 33      ########################################################################## 
 34   
 35 -    def __init__(self): 
 36          self._param = rocon_gateway.setup_ros_parameters() 
 37          key = uuid.uuid4()  # random 16 byte string, alternatively uuid.getnode() returns a hash based on the mac address, uuid.uid1() based on localhost and time 
 38          self._unique_name = self._param['name'] + key.hex  # append a unique hex string 
 39          rospy.loginfo("Gateway : generated unique hash name [%s]" % self._unique_name) 
 40          self._hub_manager = hub_manager.HubManager( 
 41                               hub_whitelist=self._param['hub_whitelist'], 
 42                               hub_blacklist=self._param['hub_blacklist'] 
 43                               ) 
 44          self._gateway = gateway.Gateway(self._hub_manager, self._param, self._unique_name, self._publish_gateway_info) 
 45          self._gateway_services = self._setup_ros_services() 
 46          self._gateway_publishers = self._setup_ros_publishers() 
 47          direct_hub_uri_list = [self._param['hub_uri']] if self._param['hub_uri'] != '' else [] 
 48          self._hub_discovery_thread = rocon_hub_client.HubDiscovery(self._register_gateway, direct_hub_uri_list, self._param['disable_zeroconf']) 
 49   
 50          # aliases 
 51          self.spin = self._gateway.spin 
 52   
 53 -    def shutdown(self): 
 54          ''' 
 55            Clears this gateway's information from the redis server. 
 56          ''' 
 57          rospy.loginfo("Gateway : shutting down.") 
 58          try: 
 59              self._gateway.shutdown() 
 60              self._hub_manager.shutdown() 
 61              self._hub_discovery_thread.shutdown() 
 62          except Exception as e: 
 63              rospy.logerr("Gateway : unknown error on shutdown [%s][%s]" % (str(e), type(e))) 
 64              raise 
 65   
 66      ########################################################################## 
 67      # Hub Discovery & Connection 
 68      ########################################################################## 
 69   
 70 -    def _register_gateway(self, ip, port): 
 71          ''' 
 72            Called when either the hub discovery module finds a hub 
 73            or a request to connect via ros service is made. 
 74   
 75            It starts the actual redis connection with the hub and also 
 76            registers the appropriate information about the gateway on 
 77            the hub. 
 78   
 79            Note, the return type is only really used by the service callback 
 80            (ros_service_connect_hub). 
 81   
 82            @return error code and message 
 83            @rtype gateway_msgs.ErrorCodes, string 
 84   
 85            @sa hub_discovery.HubDiscovery 
 86          ''' 
 87          hub, error_code, error_code_str = self._hub_manager.connect_to_hub(ip, port) 
 88          if hub: 
 89              hub.register_gateway(self._param['firewall'], 
 90                                   self._unique_name, 
 91                                   self._gateway.remote_gateway_request_callbacks, 
 92                                   self._gateway.disengage_hub,  # hub connection lost hook 
 93                                   self._gateway.ip 
 94                                   ) 
 95              rospy.loginfo("Gateway : registering on the hub [%s]" % hub.name) 
 96              self._publish_gateway_info() 
 97          else: 
 98              rospy.logwarn("Gateway : not registering on the hub [%s]" % error_code_str) 
 99          return error_code, error_code_str 
100   
101      ########################################################################## 
102      # Ros Pubs, Subs and Services 
103      ########################################################################## 
104   
105 -    def _setup_ros_services(self): 
106          gateway_services = {} 
107          gateway_services['connect_hub']   = rospy.Service('~connect_hub',                   gateway_srvs.ConnectHub,       self.ros_service_connect_hub) #@IgnorePep8 
108          gateway_services['remote_gateway_info']  = rospy.Service('~remote_gateway_info',    gateway_srvs.RemoteGatewayInfo,self.ros_service_remote_gateway_info) #@IgnorePep8 
109          gateway_services['advertise']     = rospy.Service('~advertise',                     gateway_srvs.Advertise,        self._gateway.ros_service_advertise) #@IgnorePep8 
110          gateway_services['advertise_all'] = rospy.Service('~advertise_all',                 gateway_srvs.AdvertiseAll,     self._gateway.ros_service_advertise_all) #@IgnorePep8 
111          gateway_services['flip']          = rospy.Service('~flip',                          gateway_srvs.Remote,    self._gateway.ros_service_flip) #@IgnorePep8 
112          gateway_services['flip_all']      = rospy.Service('~flip_all',                      gateway_srvs.RemoteAll, self._gateway.ros_service_flip_all) #@IgnorePep8 
113          gateway_services['pull']          = rospy.Service('~pull',                          gateway_srvs.Remote,    self._gateway.ros_service_pull) #@IgnorePep8 
114          gateway_services['pull_all']      = rospy.Service('~pull_all',                      gateway_srvs.RemoteAll, self._gateway.ros_service_pull_all) #@IgnorePep8 
115          return gateway_services 
116   
117 -    def _setup_ros_publishers(self): 
118          gateway_publishers = {} 
119          gateway_publishers['gateway_info'] = rospy.Publisher('~gateway_info', gateway_msgs.GatewayInfo, latch=True) 
120          return gateway_publishers 
121   
122      ########################################################################## 
123      # Ros Service Callbacks 
124      ########################################################################## 
125   
126 -    def ros_service_connect_hub(self, request): 
127          ''' 
128            Handle incoming requests to connect directly to a gateway hub. 
129   
130            Requests are of the form of a uri (hostname:port pair) pointing to 
131            the gateway hub. 
132          ''' 
133          response = gateway_srvs.ConnectHubResponse() 
134          o = urlparse(request.uri) 
135          response.result, response.error_message = self._register_gateway(o.hostname, o.port) 
136          # Some ros logging 
137          if response.result == gateway_msgs.ErrorCodes.SUCCESS: 
138              rospy.loginfo("Gateway : made direct connection to hub [%s]" % request.uri) 
139          return response 
140   
141 -    def _publish_gateway_info(self): 
142          gateway_info = gateway_msgs.GatewayInfo() 
143          gateway_info.name = self._unique_name 
144          gateway_info.ip = self._gateway.ip 
145          gateway_info.connected = self._gateway.is_connected() 
146          gateway_info.hub_names = [] 
147          gateway_info.hub_uris = [] 
148          for hub in self._hub_manager.hubs: 
149              gateway_info.hub_names.append(hub.name) 
150              gateway_info.hub_uris.append(hub.uri) 
151          gateway_info.firewall = self._param['firewall'] 
152          gateway_info.flipped_connections = self._gateway.flipped_interface.get_flipped_connections() 
153          gateway_info.flipped_in_connections = self._gateway.flipped_interface.getLocalRegistrations() 
154          gateway_info.flip_watchlist = self._gateway.flipped_interface.getWatchlist() 
155          gateway_info.pulled_connections = self._gateway.pulled_interface.getLocalRegistrations() 
156          gateway_info.pull_watchlist = self._gateway.pulled_interface.getWatchlist() 
157          gateway_info.public_watchlist = self._gateway.public_interface.getWatchlist() 
158          gateway_info.public_interface = self._gateway.public_interface.getInterface() 
159          self._gateway_publishers['gateway_info'].publish(gateway_info) 
160   
161 -    def ros_service_remote_gateway_info(self, request): 
162          response = gateway_srvs.RemoteGatewayInfoResponse() 
163          requested_gateways = request.gateways if request.gateways else self._hub_manager.list_remote_gateway_names() 
164          for gateway in list(set(requested_gateways)): 
165              remote_gateway_info = self._hub_manager.remote_gateway_info(gateway) 
166              if remote_gateway_info: 
167                  response.gateways.append(remote_gateway_info) 
168              else: 
169                  rospy.logwarn("Gateway : requested gateway info for unavailable gateway [%s]" % gateway) 
170          return response 
171