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