00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 import roslib
00011 roslib.load_manifest('rocon_gateway')
00012 import rospy
00013 import rocon_gateway
00014 import redis
00015 import gateway_msgs.msg
00016 import gateway_msgs.srv
00017 from urlparse import urlparse
00018 import zeroconf_msgs.srv
00019
00020
00021
00022
00023
00024
00025 class Gateway():
00026 '''
00027 Currently this just provides getup and go for the gateway.
00028
00029 1. configure ros params
00030 2. setup ros pubsubs, services
00031 3. optionally setup zeroconf if necessary and available
00032 4. loop until a hub connection is made, then spin
00033 5. shutdown
00034 '''
00035 def __init__(self):
00036 self.gateway_sync = None
00037
00038 self.param = rocon_gateway.setup_ros_parameters()
00039 self.gateway_sync = rocon_gateway.GatewaySync(self.param)
00040 self._gateway_services = self._setup_ros_services()
00041
00042 self._zeroconf_services = {}
00043 if not self._attempt_direct_connection():
00044 self._zeroconf_services = rocon_gateway.zeroconf.setup_ros_services()
00045
00046
00047
00048
00049
00050 def spin(self):
00051 previously_found_hubs = []
00052 while not rospy.is_shutdown() and not self.gateway_sync.is_connected:
00053 rospy.sleep(1.0)
00054 if self._zeroconf_services:
00055 self._scan_for_zeroconf_hubs(previously_found_hubs)
00056 else:
00057
00058 rospy.logdebug("Gateway : waiting for hub uri input.")
00059
00060 rospy.loginfo("Gateway : connected to hub [%s][%s]." % (self.gateway_sync.unique_name, self.gateway_sync.hub.name))
00061 rospy.spin()
00062 self._shutdown()
00063
00064 def _shutdown(self):
00065 '''
00066 Clears this gateway's information from the redis server.
00067 '''
00068 try:
00069 self.gateway_sync.shutdown()
00070 except Exception as e:
00071 rospy.logerr("Gateway : error on shutdown [%s]" % str(e))
00072 rospy.logdebug("Gateway : redis server cleared of gateway information")
00073
00074
00075
00076
00077
00078 def _setup_ros_services(self):
00079 gateway_services = {}
00080 gateway_services['connect_hub'] = rospy.Service('~connect_hub', gateway_msgs.srv.ConnectHub, self.ros_service_connect_hub)
00081 gateway_services['gateway_info'] = rospy.Service('~gateway_info', gateway_msgs.srv.GatewayInfo, self.ros_service_gateway_info)
00082 gateway_services['remote_gateway_info'] = rospy.Service('~remote_gateway_info', gateway_msgs.srv.RemoteGatewayInfo,self.ros_service_remote_gateway_info)
00083 gateway_services['advertise'] = rospy.Service('~advertise', gateway_msgs.srv.Advertise, self.gateway_sync.ros_service_advertise)
00084 gateway_services['advertise_all'] = rospy.Service('~advertise_all', gateway_msgs.srv.AdvertiseAll, self.gateway_sync.ros_service_advertise_all)
00085 gateway_services['flip'] = rospy.Service('~flip', gateway_msgs.srv.Remote, self.gateway_sync.ros_service_flip)
00086 gateway_services['flip_all'] = rospy.Service('~flip_all', gateway_msgs.srv.RemoteAll, self.gateway_sync.ros_service_flip_all)
00087 gateway_services['pull'] = rospy.Service('~pull', gateway_msgs.srv.Remote, self.gateway_sync.ros_service_pull)
00088 gateway_services['pull_all'] = rospy.Service('~pull_all', gateway_msgs.srv.RemoteAll, self.gateway_sync.ros_service_pull_all)
00089 return gateway_services
00090
00091
00092
00093
00094
00095 def ros_service_connect_hub(self, request):
00096 '''
00097 Incoming requests are used to then try and connect to the gateway hub
00098 if not already connected.
00099
00100 Requests are of the form of a uri (hostname:port pair) pointing to
00101 the gateway hub.
00102 '''
00103 response = gateway_msgs.srv.ConnectHubResponse()
00104 o = urlparse(request.uri)
00105 response.result = self._connect(o.hostname, o.port)
00106 if response.result == gateway_msgs.msg.Result.SUCCESS:
00107 rospy.loginfo("Gateway : made direct connection to hub [%s]" % request.uri)
00108 elif response.result == gateway_msgs.msg.Result.HUB_CONNECTION_BLACKLISTED:
00109 response.error_message = "cowardly refusing your request since that hub is blacklisted [%s]." % request.uri
00110 elif response.result == gateway_msgs.msg.Result.HUB_CONNECTION_ALREADY_EXISTS:
00111 response.error_message = "(currently) can only connect to one hub at a time [%s]." % self.gateway_sync.hub.name
00112 else:
00113 response.error_message = "direct connection failed, probably not available [%s]." % request.uri
00114 return response
00115
00116 def ros_service_gateway_info(self, msg):
00117 response = gateway_msgs.srv.GatewayInfoResponse()
00118 if self.gateway_sync.unique_name != None:
00119 response.name = self.gateway_sync.unique_name
00120 else:
00121 response.name = self.gateway_sync.unresolved_name
00122 if self.gateway_sync._ip != None:
00123 response.ip = self.gateway_sync._ip
00124 else:
00125 response.ip = 'unavailable'
00126 response.connected = self.gateway_sync.is_connected
00127 response.hub_name = self.gateway_sync.hub.name
00128 response.hub_uri = self.gateway_sync.hub.uri
00129 response.firewall = self.param['firewall']
00130 response.flipped_connections = self.gateway_sync.flipped_interface.get_flipped_connections()
00131 response.flipped_in_connections = self.gateway_sync.flipped_interface.getLocalRegistrations()
00132 response.flip_watchlist = self.gateway_sync.flipped_interface.getWatchlist()
00133 response.pulled_connections = self.gateway_sync.pulled_interface.getLocalRegistrations()
00134 response.pull_watchlist = self.gateway_sync.pulled_interface.getWatchlist()
00135 response.public_watchlist = self.gateway_sync.public_interface.getWatchlist()
00136 response.public_interface = self.gateway_sync.public_interface.getInterface()
00137 return response
00138
00139 def ros_service_remote_gateway_info(self, request):
00140 response = gateway_msgs.srv.RemoteGatewayInfoResponse()
00141 requested_gateways = request.gateways if request.gateways else self.gateway_sync.hub.list_remote_gateway_names()
00142 for gateway in requested_gateways:
00143 remote_gateway_info = self.gateway_sync.hub.remote_gateway_info(gateway)
00144 if remote_gateway_info:
00145 response.gateways.append(remote_gateway_info)
00146 else:
00147 rospy.logwarn("Gateway : requested gateway info for unavailable gateway [%s]" % gateway)
00148 return response
00149
00150
00151
00152
00153
00154 def _attempt_direct_connection(self):
00155 '''
00156 If configured with a static hub_uri, attempt a direct connection.
00157
00158 @return success of the connection
00159 @rtype bool
00160 '''
00161 if self.param['hub_uri'] != '':
00162 o = urlparse(self.param['hub_uri'])
00163 if self._connect(o.hostname, o.port) == gateway_msgs.msg.Result.SUCCESS:
00164 rospy.loginfo("Gateway : made direct connection to hub [%s]" % self.param['hub_uri'])
00165 return True
00166 else:
00167 rospy.logwarn("Gateway : failed direct connection attempt to hub [%s]" % self.param['hub_uri'])
00168 return False
00169
00170 def _scan_for_zeroconf_hubs(self, previously_found_hubs):
00171 '''
00172 Does a quick scan on zeroconf for gateway hubs. If new ones are
00173 found, and it is not on the blacklist, it attempts a connection.
00174
00175 This gets run in the pre-spin part of the spin loop.
00176
00177 @param previously_found_hubs: zeroconf names of previously scanned hubs
00178 @type previously_found_hubs: list of str
00179 '''
00180
00181 req = zeroconf_msgs.srv.ListDiscoveredServicesRequest()
00182 req.service_type = rocon_gateway.zeroconf.gateway_hub_service
00183 resp = self._zeroconf_services["list_discovered_services"](req)
00184 rospy.logdebug("Gateway : checking for autodiscovered gateway hubs")
00185 new_services = lambda l1,l2: [x for x in l1 if x not in l2]
00186 for service in new_services(resp.services,previously_found_hubs):
00187 previously_found_hubs.append(service)
00188 (ip, port) = rocon_gateway.zeroconf.resolve_address(service)
00189 rospy.loginfo("Gateway : discovered hub zeroconf service at " + str(ip) + ":"+str(service.port))
00190 connect_result = self._connect(ip,port)
00191 if connect_result == gateway_msgs.msg.Result.SUCCESS:
00192 break
00193
00194 def _connect(self,ip,port):
00195 if self.gateway_sync.is_connected:
00196 rospy.logwarn("Gateway : gateway is already connected, aborting connection attempt.")
00197 return gateway_msgs.msg.Result.HUB_CONNECTION_ALREADY_EXISTS
00198 try:
00199 hub_name = rocon_gateway.resolve_hub(ip,port)
00200 rospy.loginfo("Gateway : resolved hub name [%s].", hub_name)
00201 except redis.exceptions.ConnectionError:
00202 rospy.logerr("Gateway : couldn't connect to the hub [%s:%s]", ip, port)
00203 return gateway_msgs.msg.Result.HUB_CONNECTION_UNRESOLVABLE
00204 if ip in self.param['hub_blacklist']:
00205 rospy.loginfo("Gateway : ignoring blacklisted hub [%s]",ip)
00206 return gateway_msgs.msg.Result.HUB_CONNECTION_BLACKLISTED
00207 elif hub_name in self.param['hub_blacklist']:
00208 rospy.loginfo("Gateway : ignoring blacklisted hub [%s]",hub_name)
00209 return gateway_msgs.msg.Result.HUB_CONNECTION_BLACKLISTED
00210
00211 if (len(self.param['hub_whitelist']) == 0) or (ip in self.param['hub_whitelist']) or (hub_name in self.param['hub_whitelist']):
00212 if self.gateway_sync.connect_to_hub(ip,port):
00213 return gateway_msgs.msg.Result.SUCCESS
00214 else:
00215 return gateway_msgs.msg.Result.HUB_CONNECTION_FAILED
00216 else:
00217 rospy.loginfo("Gateway : hub/ip not in non-empty whitelist [%s]",hub_name)
00218 return gateway_msgs.msg.Result.HUB_CONNECTION_NOT_IN_NONEMPTY_WHITELIST
